#include "RigState.h"

/**********************************************************************/
// Handle the case of the TeensyDSP.

#ifdef TEENSYDUINO

/**********************************************************************/

bool readVFOA(uint32_t* d) {
  unsigned freq = (vfoActive == VFO_A) ? frequency : vfoA;
  if (*d == freq) {
    return false
  } else {
    *d = freq;
    return true;
  }
}

void writeVFOA(uint32_t d) {
  if (vfoActive == VFO_A) {
    setFrequency(d);
  } else {
    vfoA = frequency;    
  }
}

bool readVFOB(uint32_t* d) {
  unsigned freq = (vfoActive == VFO_B) ? frequency : vfoB;
  if (*d == freq) {
    return false
  } else {
    *d = freq;
    return true;
  }
}

void writeVFOB(uint32_t d) {
  if (vfoActive == VFO_B) {
    setFrequency(d);
  } else {
    vfoB = frequency;    
  }
}

bool readRIT(uint32_t* d) {
  int freq = ritRxFrequency - frequency;
  if (*d == (uint32_t)freq) {
    return false;
  } else {
    *d = (uint32_t)freq;
    return true;
  }
}

void writeRIT(uint32_t d) {
  ritRxFrequency = (int)d + ritTxFrequency;
  if ((ritOn == 1) && (inTx == 0)) {
    setFrequency(ritRxFrequency);
  }
}

bool readXIT(uint32_t* d) {
  return false;
}

void writeXIT(uint32_t d) {
}

bool readFlags(uint32_t* d) {
  uint32_t flags = 0
  flags = 0;
  flags |= (vfoActive == VFO_B ? UBITX_VFOB_FLAG : 0);
  flags |= (cwMode != 0 ? UBITX_CW_FLAG : 0);
  flags |= (isUSB != 0 ? UBITX_USB_FLAG : 0);
  flags |= (splitOn != 0 ? UBITX_SPLIT_FLAG : 0);
  flags |= (ritOn != 0 ? UBITX_RIT_FLAG : 0);
  //flags |= (xitOn != 0 ? UBITX_XIT_FLAG : 0);
  if (*d == flags) {
    return false;
  } else {
    *d = flags;
    return true;
  }
}

void writeFlags(uint32_t d) {
  char prev = vfoActive;
  vfoActive = (d & UBITX_VFOB_FLAG ? VFO_B : VFO_A);
  if (vfoActive != prev) {
    if (vfoActive == VFO_A) {
      if (vfoA != frequency) {
        setFrequency(vfoA);      
      } 
    } else if (vfoActive == VFO_B) {
      if (vfoB != frequency) {
        setFrequency(vfoB);
      }
    }
  }  
  
  splitOn = rigState.flags & UBITX_SPLIT_FLAG ? 1 : 0;
  
  prev = ritOn;  
  ritOn = rigState.flags & UBITX_RIT_FLAG ? 1 : 0;
  if (ritOn != prev) {
    if ((ritOn == 1) && (inTx == 0)) {
      setFrequency(ritRxFrequency);
    }
  }

  char prev = (cwMode << 1) | isUSB;
  isUSB = rigState.flags & UBITX_USB_FLAG ? 1 : 0;
  if (rigState.flags & UBITX_CW_FLAG) {    
    cwMode = isUSB ? 2 : 1; // 2 = cwu / 1 = cwl
  } else {
    cwMode = 0;
  }
  if ((cwMode << 1) | isUSB != prev) {
    setFrequency(frequency);    
  }
}

/**********************************************************************/

RigState inState;
RigState outState;

void RigState::begin() {
  field[WIREBUS_VFO_A].data = vfoA;
  field[WIREBUS_VFO_B].data = vfoB;
  field[WIREBUS_RIT_OFS].data = (uint32_t)(ritRxFrequency - frequency);
  field[WIREBUS_XIT_OFS].data = 0;
  field[WIREBUS_FLAGS].data = 0;
  field[WIREBUS_FLAGS].data |= (vfoActive == VFO_B ? UBITX_VFOB_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (cwMode != 0 ? UBITX_CW_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (isUSB != 0 ? UBITX_USB_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (splitOn != 0 ? UBITX_SPLIT_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (ritOn != 0 ? UBITX_RIT_FLAG : 0);
  //field[WIREBUS_FLAGS].data |= (xitOn != 0 ? UBITX_XIT_FLAG : 0);

  for (byte i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    field[i].dirty = true;   // Set true to force an initial send to the TeensyDSP.
  }

  readFunc[WIREBUS_VFO_A] = &readVFOA;
  readFunc[WIREBUS_VFO_B] = &readVFOB;
  readFunc[WIREBUS_RIT_OFS] = &readRIT;
  readFunc[WIREBUS_XIT_OFS] = &readXIT;
  readFunc[WIREBUS_FLAGS] = &readFlags;
  writeFunc[WIREBUS_VFO_A] = &writeVFOA;
  writeFunc[WIREBUS_VFO_B] = &writeVFOB;
  writeFunc[WIREBUS_RIT_OFS] = &writeRIT;
  writeFunc[WIREBUS_XIT_OFS] = &writeXIT;
  writeFunc[WIREBUS_FLAGS] = &writeFlags;
}

void RigState::update() {
  // First we need to determine which fields have changed (and are
  // thus dirty and need to be sent to the TeensyDSP).
  for (byte i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    if (read(i)) {
      field[i].dirty = true;
      numDirty++;
    }
  }    

  // Next we need to send the current (changed) Raduino information
  // to the TeensyDSP.  The expected response is the number of fields
  // (in bytes) that the TeensyDSP needs to send in response.
  Wire.beginTransmission(I2CMETER_ADDR);
  Wire.write(I2CMETER_RIGINF);
  Wire.write(numDirty * sizeof(Field), 1);    // Write the number of dirty fields (in bytes).
  for (int i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    if (field[i].dirty) {                     // Write each field that is dirty to the bus.
      Wire.write((byte*)(&(field[i].data)), sizeof(Field));
      field[i].dirty = false;
      numDirty--;
    }
  }
  Wire.endTransmission();  

  // Now we're going to read the response from the TeensyDSP.  All
  // fields should be marked as clean at this point (unless there's
  // something that has been updated via interrupt???).
  Wire.requestFrom(I2CMETER_ADDR, 1);
  byte numBytes;
  while (Wire.available()) {
    numBytes = Wire.read();    // Should only get executed for one byte... but just in case.
  }
  if (numBytes == 0) return;

  // Let the TeensyDSP know that we want it to send its deltas now.
  Wire.beginTransmission(I2CMETER_ADDR);
  Wire.write(I2CMETER_RIGREQ);
  Wire.endTransmission();

  // Retrieve all of the deltas.  Mark any received field as dirty.
  Wire.requestFrom(I2CMETER_ADDR, numBytes);
  int index = -1;
  byte* ptr;
  while (Wire.available()) {
    byte b = Wire.read();
    if (index = -1) {
      ptr = (byte*)(&(field[b].data));
      field[b].dirty = true;
      numDirty++;
      index = 0;
    } else {
      ptr[index++] = b;
      if (index == 4) {
        index = -1;
      }
    }
  }

  // Perform the corresponding update for each dirty field.
  for (byte i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    if (field[i].dirty) {
      write(i);
      field[i].dirty = false;
      numDirty--;
    }    
  }
}

/**********************************************************************/
// Handle the case of the Raduino

#else

/**********************************************************************/

bool readVFOA(uint32_t* d) {
  unsigned freq = (vfoActive == VFO_A) ? frequency : vfoA;
  if (*d == freq) {
    return false
  } else {
    *d = freq;
    return true;
  }
}

void writeVFOA(uint32_t d) {
  if (vfoActive == VFO_A) {
    setFrequency(d);
  } else {
    vfoA = frequency;    
  }
}

bool readVFOB(uint32_t* d) {
  unsigned freq = (vfoActive == VFO_B) ? frequency : vfoB;
  if (*d == freq) {
    return false
  } else {
    *d = freq;
    return true;
  }
}

void writeVFOB(uint32_t d) {
  if (vfoActive == VFO_B) {
    setFrequency(d);
  } else {
    vfoB = frequency;    
  }
}

bool readRIT(uint32_t* d) {
  int freq = ritRxFrequency - frequency;
  if (*d == (uint32_t)freq) {
    return false;
  } else {
    *d = (uint32_t)freq;
    return true;
  }
}

void writeRIT(uint32_t d) {
  ritRxFrequency = (int)d + ritTxFrequency;
  if ((ritOn == 1) && (inTx == 0)) {
    setFrequency(ritRxFrequency);
  }
}

bool readXIT(uint32_t* d) {
  return false;
}

void writeXIT(uint32_t d) {
}

bool readFlags(uint32_t* d) {
  uint32_t flags = 0
  flags = 0;
  flags |= (vfoActive == VFO_B ? UBITX_VFOB_FLAG : 0);
  flags |= (cwMode != 0 ? UBITX_CW_FLAG : 0);
  flags |= (isUSB != 0 ? UBITX_USB_FLAG : 0);
  flags |= (splitOn != 0 ? UBITX_SPLIT_FLAG : 0);
  flags |= (ritOn != 0 ? UBITX_RIT_FLAG : 0);
  //flags |= (xitOn != 0 ? UBITX_XIT_FLAG : 0);
  if (*d == flags) {
    return false;
  } else {
    *d = flags;
    return true;
  }
}

void writeFlags(uint32_t d) {
  char prev = vfoActive;
  vfoActive = (d & UBITX_VFOB_FLAG ? VFO_B : VFO_A);
  if (vfoActive != prev) {
    if (vfoActive == VFO_A) {
      if (vfoA != frequency) {
        setFrequency(vfoA);      
      } 
    } else if (vfoActive == VFO_B) {
      if (vfoB != frequency) {
        setFrequency(vfoB);
      }
    }
  }  
  
  splitOn = rigState.flags & UBITX_SPLIT_FLAG ? 1 : 0;
  
  prev = ritOn;  
  ritOn = rigState.flags & UBITX_RIT_FLAG ? 1 : 0;
  if (ritOn != prev) {
    if ((ritOn == 1) && (inTx == 0)) {
      setFrequency(ritRxFrequency);
    }
  }

  char prev = (cwMode << 1) | isUSB;
  isUSB = rigState.flags & UBITX_USB_FLAG ? 1 : 0;
  if (rigState.flags & UBITX_CW_FLAG) {    
    cwMode = isUSB ? 2 : 1; // 2 = cwu / 1 = cwl
  } else {
    cwMode = 0;
  }
  if ((cwMode << 1) | isUSB != prev) {
    setFrequency(frequency);    
  }
}

/**********************************************************************/

RigState rigState;

void RigState::begin() {
  field[WIREBUS_VFO_A].data = vfoA;
  field[WIREBUS_VFO_B].data = vfoB;
  field[WIREBUS_RIT_OFS].data = (uint32_t)(ritRxFrequency - frequency);
  field[WIREBUS_XIT_OFS].data = 0;
  field[WIREBUS_FLAGS].data = 0;
  field[WIREBUS_FLAGS].data |= (vfoActive == VFO_B ? UBITX_VFOB_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (cwMode != 0 ? UBITX_CW_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (isUSB != 0 ? UBITX_USB_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (splitOn != 0 ? UBITX_SPLIT_FLAG : 0);
  field[WIREBUS_FLAGS].data |= (ritOn != 0 ? UBITX_RIT_FLAG : 0);
  //field[WIREBUS_FLAGS].data |= (xitOn != 0 ? UBITX_XIT_FLAG : 0);

  for (byte i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    field[i].dirty = true;   // Set true to force an initial send to the TeensyDSP.
  }

  readFunc[WIREBUS_VFO_A] = &readVFOA;
  readFunc[WIREBUS_VFO_B] = &readVFOB;
  readFunc[WIREBUS_RIT_OFS] = &readRIT;
  readFunc[WIREBUS_XIT_OFS] = &readXIT;
  readFunc[WIREBUS_FLAGS] = &readFlags;
  writeFunc[WIREBUS_VFO_A] = &writeVFOA;
  writeFunc[WIREBUS_VFO_B] = &writeVFOB;
  writeFunc[WIREBUS_RIT_OFS] = &writeRIT;
  writeFunc[WIREBUS_XIT_OFS] = &writeXIT;
  writeFunc[WIREBUS_FLAGS] = &writeFlags;
}

void RigState::update() {
  // First we need to determine which fields have changed (and are
  // thus dirty and need to be sent to the TeensyDSP).
  for (byte i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    if (read(i)) {
      field[i].dirty = true;
      numDirty++;
    }
  }    

  // Next we need to send the current (changed) Raduino information
  // to the TeensyDSP.  The expected response is the number of fields
  // (in bytes) that the TeensyDSP needs to send in response.
  Wire.beginTransmission(I2CMETER_ADDR);
  Wire.write(I2CMETER_RIGINF);
  Wire.write(numDirty * sizeof(Field), 1);    // Write the number of dirty fields (in bytes).
  for (int i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    if (field[i].dirty) {                     // Write each field that is dirty to the bus.
      Wire.write((byte*)(&(field[i].data)), sizeof(Field));
      field[i].dirty = false;
      numDirty--;
    }
  }
  Wire.endTransmission();  

  // Now we're going to read the response from the TeensyDSP.  All
  // fields should be marked as clean at this point (unless there's
  // something that has been updated via interrupt???).
  Wire.requestFrom(I2CMETER_ADDR, 1);
  byte numBytes;
  while (Wire.available()) {
    numBytes = Wire.read();    // Should only get executed for one byte... but just in case.
  }
  if (numBytes == 0) return;

  // Let the TeensyDSP know that we want it to send its deltas now.
  Wire.beginTransmission(I2CMETER_ADDR);
  Wire.write(I2CMETER_RIGREQ);
  Wire.endTransmission();

  // Retrieve all of the deltas.  Mark any received field as dirty.
  Wire.requestFrom(I2CMETER_ADDR, numBytes);
  int index = -1;
  byte* ptr;
  while (Wire.available()) {
    byte b = Wire.read();
    if (index = -1) {
      ptr = (byte*)(&(field[b].data));
      field[b].dirty = true;
      numDirty++;
      index = 0;
    } else {
      ptr[index++] = b;
      if (index == 4) {
        index = -1;
      }
    }
  }

  // Perform the corresponding update for each dirty field.
  for (byte i = 0; i < WIREBUS_NUM_FIELDS; i++) {
    if (field[i].dirty) {
      write(i);
      field[i].dirty = false;
      numDirty--;
    }    
  }
}

#endif



/**********************************************************************
 * EOF                                                                *
 **********************************************************************/