/*
HamHead CI-V Alpha Module

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. 

This now gets the current VFO mode, writes it to a variable, then
switch cases it to set a string. I may do something similar to
automatically handle "valid" CI-V frames. I need to anyway for
transcieve data.

*/
#define DEBUG 0 //enables or disables debug(), debug2(), and debugln() debugging output
#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; // VFO A
unsigned long vfob; // VFO B
unsigned long vfos; // starting VFO
byte civ[9] = {0xFE, 0xFE, 0x88, 0xE0}; // civ array with preamble
byte endbyte = 0xFD; // I'm not keeping the endbyte in the array.
unsigned long *v; // pointer for whatever VFO we're in
String *m; // pointer for current VFO's mode
boolean newdata = false;
int bc;
long bcd[6];
String vam;
String vbm;

void setup() {
Serial1.begin(19200);
Serial.begin(9600);
Serial.println("Initalizing VFO Variables.");
debugln();
debugln("Getting Starting VFO...");
v = &vfos;
getvfo();
v = &vfoa;
m = &vam;
setvfo(0x00);
getvfo();
getmode();
v = &vfob;
m = &vbm;
setvfo(0x01);
getvfo();
getmode();
Serial.print("VFO A: ");
Serial.print(vfoa, DEC);
Serial.print(" - ");
Serial.print(vam);
Serial.println();
Serial.print("VFO B: ");
Serial.print(vfob, DEC);
Serial.print(" - ");
Serial.print(vbm);
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) { // check the array for echo at index 0 or ACK at index 3
        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 // The IC-731/735, and I assume the 765/CI-IV, only send 4 bytes for frequency. We only need to account for index starting at 0.
      bc--;
    #else
    bc -=2; // Everything after the 735 sends 5 bytes (unless you put the radio in 731 mode). We're HF only; this ditches the extra byte and index adjustment.
    #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 swapvfo() {
  debugln("Sending VFO Swap");
  send7(0x07, 0xB0); // VFO, SWAP A/B
  Serial1.flush();
  delay(50); // I'm assuming this is needed to wait for the radio to send data
  getdata(); // I might be able to avoid all delay() if I do something with my loop.
  }
void setvfo(byte vfo) {

  if (vfo == 0x00) debugln ("Setting VFO A");
  else debugln("Setting VFO B");
  send7(0x07, vfo); // VFO, A or B (0x00 or 0x01)
  Serial1.flush();
  delay(50);
  getdata(); // drop the echo
}

/*

the send# functions just replace the appropiate array index with whatever
was passed to it. It'd be nice if I could have one function with an additional
value of bytes; but something something "expects 2, sends 1" arguement error.

*/
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); // 03 is VFO read
  Serial1.flush();
  delay(50);
  getdata(); // preload the vfo
  *v = gogovfo(); // process and update
 }

void getmode() {
  int mode;
  debugln("Getting Mode.");
  send6(0x04); // 04 is mode read
  getdata();
  mode = (int) rxbytes[3]; // it's easy enough to just deal with mode as integer
  switch (mode) { 
    case 0:
      *m = "LSB";
      break;
    case 1:
      *m = "USB";
      break;
    case 2:
      *m = "AM";
      break;
    case 3:
      *m = "CW";
      break;
    case 4:
      *m = "RTTY";
      break;
    case 5:
      *m = "FM";
      break;
    case 7:
      *m = "CW-R";
      break;
    case 8:
      *m = "RTTY-R";
      break;
    case 23:
      *m = "D-Star DV";
      break;
    default:
      *m = "INVALID";
      break;
  }
  newdata = false;
  }
  
void setstartvfo() {
    Serial.println();
  if (vfos == vfoa) {
    Serial.println("Started on A");
    setvfo(0x00);}
  else Serial.println("Staying on B");
}