/*
HamHead CI-V Alpha Module - Initialization

This is a test of multiple things, an initialization routine
as well as working with pointers. Pointers was easy; getting
communication working boiled down to catching all my forgotten
variables and such. Pointers are used so that we can just work
on one VFO variable that's mapped to whatever the current one
is. 

Echo and ACK data are simply dropped right now. I have some
plans of maybe using echos to keep VFO in sync. But I will
determine this as I actually build functions. 

We now automatically drop the Echo and ACK ($FB), but you
MUST call getdata() to do so. 

*/
#define DEBUG 0
#if DEBUG == 1
#define debug(x) Serial.print(x)
#define debug2(x,y) Serial.print(x,y)
#define debugln(x) Serial.println(x)
#else
#define debug(x)
#define debug2(x,y)
#define debugln(x)
#endif


//#define MODE731 1 // Uncoment for 731/735 mode.

const byte numBytes = 32;
long dec[6];
byte rxbytes[numBytes];
unsigned long vfoa;
unsigned long vfob;
unsigned long vfos;
//byte civ[9];
byte civ[9] = {0xFE, 0xFE, 0x88, 0xE0, 0xFF, 0xFF};
byte endbyte = 0xFD;
unsigned long *v;
boolean newdata = false;
int bc;
long bcd[6];

void setup() {
Serial1.begin(19200);
Serial.begin(9600);
Serial.println("Initalizing VFO Variables.");
debugln();
debugln("Getting Starting VFO...");
v = &vfos;
getvfo();
v = &vfoa;
setvfo(0x00);
getvfo();
//debugln();
v = &vfob;
setvfo(0x01);
getvfo();
Serial.print("VFO A: ");
Serial.print(vfoa, DEC);
Serial.println();
Serial.print("VFO B: ");
Serial.print(vfob, DEC);
debugln();
debugln("Reverting to initial VFO.");
setstartvfo();
debugln("Done.");
}

void loop() {

}

void getdata() {
  byte rb;
    debug("RX Bytes: ");
  while (Serial1.available() > 0 && newdata == false) {
    rb = Serial1.read();
    if (rb == 0xFE) {  // if it's a start byte we don't need it. 
      newdata = false; // make sure we keep looping
      bc = 0; // i don't trust myself
    } else if (rb == 0xFD) { // end of the frame
      rxbytes[bc] = '\0'; // terminate the string
      if (rxbytes[0]==0x88 || rxbytes[2] == 0xFB) {
        newdata = false;  // auto-echo ditch&ack
        bc = 0;
      }
      else newdata = true;} // indicate there's new data}
      else {
    rxbytes[bc] = rb; // write the byte to the array
    bc++; // increment byte counter
    debug2(rb, HEX);
    debug(" "); }
  }
  debugln();
  }

long gogovfo() {
    int i = 0;
    long ggv;
    #ifdef MODE731
      bc--;
    #else
    bc -=2;
    #endif
    for (int x = bc; x > 2; x--) {
      bcd[i] = (((rxbytes[x] >> 4) *10) + (rxbytes[x]&0xF));
      i++; }
    ggv = ((bcd[0]*1000000)+(bcd[1]*10000)+(bcd[2]*100)+(bcd[3]));
    newdata = false;
    return ggv;
    }

void setvfo(byte vfo) {
  //byte zz;
  if (vfo == 0x00) debugln ("Setting VFO A");
  else debugln("Setting VFO B");
  send7(0x07, vfo);
  Serial1.flush();
  delay(50);
  getdata(); // drop the echo
 // newdata=false;
//  getdata(); // drop the ack
//  newdata=false; 
}

void send6(byte cmd) {
  civ[4]=cmd;
  Serial1.write(civ,5);
  Serial1.write(endbyte);
  Serial1.flush();
  delay(50);
}

void send7(byte cmd, byte data) {
  civ[4] = cmd;
  civ[5] = data;
  Serial1.write(civ,6);
  Serial1.write(endbyte);
  Serial1.flush();
  delay(50);
  }
  
void getvfo() {
  debugln("Sending VFO Read");
  send6(0x03);
  Serial1.flush();
  delay(50);
 // getdata(); // drop the echo
 // newdata=false;
  getdata(); // preload the vfo
  *v = gogovfo(); // process and update
 }

void setstartvfo() {
    Serial.println();
  if (vfos == vfoa) {
    Serial.println("Started on A");
    setvfo(0x00);}
  else Serial.println("Staying on B");
}