#include <SoftwareSerial.h>

#define STATE_IDLE            0
#define STATE_SEND_TARGETS    1
#define GET_TARGET_ACK        2
#define CALIBRATE_FREQ        3
#define GET_FREQ_ACK          4
#define CALIBRATE_VALUES      5
#define GET_VALUE_ACK         6
#define GET_ADJ_VALUES        7
#define DISPLAY_ADJ_VALUES    8
#define GET_GAIN_VALUES       9
#define DISPLAY_GAIN_VALUES   10
#define GET_RANGE_VALUES      11
#define DISPLAY_RANGE_VALUES  12
#define GET_SYSTEM_CONFIG     13
#define DISPLAY_SYSTEM_CONFIG 14
#define STATE_SEND_PGAS       15
#define GET_PGA_ACK           16
#define WRITE_RANGES          17
#define GET_RANGES_ACK        18
#define SAVE_GAINS_TO_FLASH   19
#define GET_FLASH_WRITE_ACK   20

#define STATE_EXIT

byte SetTargetRegisters[] = {0xA5, 0x17, 0x41, 0x00, 0x86, 0x4D, 0x8E, 0x20, 0x00, 0x00, 0xB0, 0x04, 0x10, 0x27, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x60, 0xEA, 0xEA};
byte SetPGARegisters[] = {0xA5, 0x0C, 0x41, 0x00, 0x7A, 0x4D, 0x04, 0x00, 0x00, 0x00, 0x01, 0xBE};
byte SetRangeRegisters[] = {0xA5, 0x0C, 0x41, 0x00, 0x82, 0x4D, 0x04, 0x12, 0x0A, 0x11, 0x00, 0xF2};
byte CalibrateFrequency[] = {0xA5, 0x04, 0x76, 0x1F};
byte SaveRegistersToFlash[] = {0xA5, 0x04, 0x53, 0xFC};
byte CalibrateGains[] = {0xA5, 0x04, 0x5A, 0x03};
byte ReadValues[] = {0xA5, 0x08, 0x41, 0x00, 0x06, 0x4E, 0x10, 0x52};
byte ReadGainCalValues[] = {0xA5, 0x08, 0x41, 0x00, 0x60, 0x4E, 0x06, 0xA2};
byte ReadRangeValues[] = {0xA5, 0x08, 0x41, 0x00, 0x82, 0x4E, 0x04, 0xC2};
byte ReadSystemConfiguration[] = {0xA5, 0x08, 0x41, 0x00, 0x7A, 0x4E, 0x06, 0xBC};
byte RxData[30];
byte state = STATE_IDLE;
unsigned int Vrms = 0;
unsigned int Frequency = 0;
unsigned long Irms = 0;
unsigned long power = 0;
unsigned long temp = 0;
unsigned int gain_current_rms = 0;
unsigned int gain_voltage_rms = 0;
unsigned int gain_active_power = 0;
byte range_value_current_rms = 0;
byte range_value_voltage_rms = 0;
byte range_value_active_power = 0;
unsigned long system_config = 0;
unsigned int event_config = 0;


void setup() {
  //Serial.begin(9600);
  Serial.begin(115200);
  state = WRITE_RANGES;            // redundant
  delay(100);
}


void loop() {
  switch (state) {

  case STATE_IDLE:
    break;

  case WRITE_RANGES:
    Serial_write_array(SetRangeRegisters, sizeof(SetRangeRegisters)); //Send the byte array
    state = GET_RANGES_ACK;
    delay(5);
    break;

  case GET_RANGES_ACK:
    if (Serial.available() > 0) {

      temp = Serial.read();

      if (temp == 0x06) {
        state = STATE_SEND_PGAS;
        Serial.println("Write Ranges - Passed");
        break;
      }

      if (temp == 0x15) {
        state = STATE_SEND_PGAS;
        Serial.println("Write Ranges - Failed");
        break;
      }

      if (temp == 0x51) {
        state = STATE_SEND_PGAS;
        Serial.println("Write Ranges - CRC Failed");
        break;
      }
    }
    break;

  case STATE_SEND_PGAS:
    Serial_write_array(SetPGARegisters, sizeof(SetPGARegisters)); //Send the byte array
    state = GET_PGA_ACK;
    delay(5);
    break;

  case GET_PGA_ACK:
    if (Serial.available() > 0) {

      temp = Serial.read();

      if (temp == 0x06) {
        state = STATE_SEND_TARGETS;
        Serial.println("Send PGAs - Passed");
        break;
      }

      if (temp == 0x15) {
        state = STATE_SEND_TARGETS;
        Serial.println("Send PGAs - Failed");
        break;
      }

      if (temp == 0x51) {
        state = STATE_SEND_TARGETS;
        Serial.println("Send PGAs - CRC Failed");
        break;
      }
    }
    break;

  case STATE_SEND_TARGETS:
    Serial_write_array(SetTargetRegisters, sizeof(SetTargetRegisters)); //Send the byte array
    state = GET_TARGET_ACK;
    delay(5);
    break;

  case GET_TARGET_ACK:
    if (Serial.available() > 0) {

      temp = Serial.read();

      if (temp == 0x06) {
        state = CALIBRATE_FREQ;
        Serial.println("Send Target Values Packet - Passed");
        break;
      }

      if (temp == 0x15) {
        state = CALIBRATE_FREQ;
        Serial.println("Send Target Values Packet - Failed");
        break;
      }

      if (temp == 0x51) {
        state = CALIBRATE_FREQ;
        Serial.println("Send Target Values packet - CRC Failed");
        break;
      }
    }
    break;

  case CALIBRATE_FREQ:
    Serial_write_array(CalibrateFrequency, sizeof(CalibrateFrequency)); //Send the byte array
    state = GET_FREQ_ACK;
    delay(1);
    break;

  case GET_FREQ_ACK:
    if (Serial.available() > 0) {

      temp = Serial.read();

      if (temp == 0x06) {
        state = SAVE_GAINS_TO_FLASH;
        Serial.println("Send Calibrate Frequency Packet - Passed");
        break;
      }

      if (temp == 0x15) {
        state = SAVE_GAINS_TO_FLASH;
        Serial.println("Send Calibrate Frequency Packet - Failed");
        break;
      }

      if (temp == 0x51) {
        state = SAVE_GAINS_TO_FLASH;
        Serial.println("Send Calibrate Frequency Packet - CRC Failed");
        break;
      }
    }
    break;


  case SAVE_GAINS_TO_FLASH:
    Serial_write_array(SaveRegistersToFlash, sizeof(SaveRegistersToFlash)); //Send the byte array
    state = GET_FLASH_WRITE_ACK;
    delay(5);
    break;

  case GET_FLASH_WRITE_ACK:
    if (Serial.available() > 0) {

      temp = Serial.read();

      if (temp == 0x06) {
        state = CALIBRATE_VALUES;
        Serial.println("Write to Flash - Passed");
        break;
      }

      if (temp == 0x15) {
        state = CALIBRATE_VALUES;
        Serial.println("Write to Flash - Failed");
        break;
      }

      if (temp == 0x51) {
        state = CALIBRATE_VALUES;
        Serial.println("Write to Flash - CRC Failed");
        break;
      }
    }
    break;

  case CALIBRATE_VALUES:
    Serial_write_array(CalibrateGains, sizeof(CalibrateGains)); //Send the byte array
    state = GET_VALUE_ACK;
    delay(1);
    break;

  case GET_VALUE_ACK:
    if (Serial.available() > 0) {

      temp = Serial.read();

      if (temp == 0x06) {
        state = GET_ADJ_VALUES;
        Serial.println("Send Calibrate Values Packet - Passed");
        break;
      }

      if (temp == 0x15) {
        state = GET_ADJ_VALUES;
        Serial.println("Send Calibrate Values Packet - Failed");
        break;
      }

      if (temp == 0x51) {
        state = GET_ADJ_VALUES;
        Serial.println("Send Calibrate Values Packet - CRC Failed");
        break;
      }
    }
    break;

  case GET_ADJ_VALUES:
    Serial_write_array(ReadValues, sizeof(ReadValues)); //Send the byte array
    state = DISPLAY_ADJ_VALUES;
    delay(1);
    break;

  case DISPLAY_ADJ_VALUES:
    if (Serial.available() > 18) {
      for (int i = 0; i < 19; i++) {
        RxData[i] = Serial.read();
      }

      if (RxData[0] == 0x06) {
        Serial.println("Values Packet - Passed");
      }

      if (RxData[0] == 0x15) {
        Serial.println("Values Packet - Failed");
      }

      if (RxData[0] == 0x51) {
        Serial.println("Packet - CRC Failed");
      }

      Serial.print("ACK   ");
      Serial.println(RxData[0], HEX);
      Serial.print("BYTES ");
      Serial.println(RxData[1], HEX);
      Serial.print("0x06 ");
      Serial.println(RxData[2], HEX);
      Serial.print("0x07 ");
      Serial.println(RxData[3], HEX);
      Serial.print("0x08 ");
      Serial.println(RxData[4], HEX);
      Serial.print("0x09 ");
      Serial.println(RxData[5], HEX);
      Serial.print("0x0A ");
      Serial.println(RxData[6], HEX);
      Serial.print("0x0B ");
      Serial.println(RxData[7], HEX);
      Serial.print("0x0C ");
      Serial.println(RxData[8], HEX);
      Serial.print("0x0D ");
      Serial.println(RxData[9], HEX);
      Serial.print("0x0E ");
      Serial.println(RxData[10], HEX);
      Serial.print("0x0F ");
      Serial.println(RxData[11], HEX);
      Serial.print("0x10 ");
      Serial.println(RxData[12], HEX);
      Serial.print("0x11 ");
      Serial.println(RxData[13], HEX);
      Serial.print("0x12 ");
      Serial.println(RxData[14], HEX);
      Serial.print("0x13 ");
      Serial.println(RxData[15], HEX);
      Serial.print("0x14 ");
      Serial.println(RxData[16], HEX);
      Serial.print("0x15 ");
      Serial.println(RxData[17], HEX);
      Serial.print("CRC  ");
      Serial.println(RxData[18], HEX);

      temp = (long)RxData[3];
      Vrms = (int)(temp << 8);
      Vrms = Vrms + (int)RxData[2];

      temp = (long)RxData[5];
      Frequency = (int)(temp << 8);
      Frequency = Frequency + (int)RxData[4];

      temp = (long) RxData[13];
      Irms = temp << 24;
      temp = (long) RxData[12];
      Irms = Irms + (temp << 16);
      temp = (long) RxData[11];
      Irms = Irms + (temp << 8);
      Irms = Irms + (long)RxData[10];

      temp = (long)RxData[17];
      power = temp << 24;
      temp = (long) RxData[16];
      power = power + (temp << 16);
      temp = (long) RxData[15];
      power = power + (temp << 8);
      power = power + RxData[14];

      Serial.print("Vrms = ");
      Serial.println(Vrms);
      Serial.print("Irms = ");
      Serial.println(Irms);
      Serial.print("Power = ");
      Serial.println(power);
      Serial.print("Frequency = ");
      Serial.println(Frequency);

      state = GET_GAIN_VALUES;
    }
    break;

  case GET_GAIN_VALUES:
    Serial_write_array(ReadGainCalValues, sizeof(ReadGainCalValues)); //Send the byte array
    state = DISPLAY_GAIN_VALUES;
    delay(1);
    break;

  case DISPLAY_GAIN_VALUES:
    if (Serial.available() > 8) {
      for (int i = 0; i < 9; i++) {
        RxData[i] = Serial.read();
      }

      if (RxData[0] == 0x06) {
        Serial.println("Gain Values Packet - Passed");
      }

      if (RxData[0] == 0x15) {
        Serial.println("Gain Values Packet - Failed");
      }

      if (RxData[0] == 0x51) {
        Serial.println("Gain Packet - CRC Failed");
      }

      Serial.print("ACK   ");
      Serial.println(RxData[0], HEX);
      Serial.print("BYTES ");
      Serial.println(RxData[1], HEX);
      Serial.print("0x60 ");
      Serial.println(RxData[2], HEX);
      Serial.print("0x61 ");
      Serial.println(RxData[3], HEX);
      Serial.print("0x62 ");
      Serial.println(RxData[4], HEX);
      Serial.print("0x63 ");
      Serial.println(RxData[5], HEX);
      Serial.print("0x64 ");
      Serial.println(RxData[6], HEX);
      Serial.print("0x65 ");
      Serial.println(RxData[7], HEX);
      Serial.print("CRC  ");
      Serial.println(RxData[8], HEX);


      temp = (long)RxData[3];
      gain_current_rms = (int)(temp << 8);
      gain_current_rms = gain_current_rms + (int)RxData[2];

      temp = (long)RxData[5];
      gain_voltage_rms = (int)(temp << 8);
      gain_voltage_rms = gain_voltage_rms + (int)RxData[4];

      temp = (long)RxData[7];
      gain_active_power = (int)(temp << 8);
      gain_active_power = gain_active_power + (int)RxData[6];

      Serial.print("Gain current rms = ");
      Serial.println(gain_current_rms);
      Serial.print("Gain voltage rms = ");
      Serial.println(gain_voltage_rms);
      Serial.print("Gain active power = ");
      Serial.println(gain_active_power);

      state = GET_RANGE_VALUES;
    }
    break;

  case GET_RANGE_VALUES:
    Serial_write_array(ReadRangeValues, sizeof(ReadRangeValues)); //Send the byte array
    state = DISPLAY_RANGE_VALUES;
    delay(1);
    break;

  case DISPLAY_RANGE_VALUES:
    if (Serial.available() > 6) {
      for (int i = 0; i < 7; i++) {
        RxData[i] = Serial.read();
      }

      if (RxData[0] == 0x06) {
        Serial.println("Range Values Packet - Passed");
      }

      if (RxData[0] == 0x15) {
        Serial.println("Range Values Packet - Failed");
      }

      if (RxData[0] == 0x51) {
        Serial.println("Range Packet - CRC Failed");
      }

      Serial.print("ACK   ");
      Serial.println(RxData[0], HEX);
      Serial.print("BYTES ");
      Serial.println(RxData[1], HEX);
      Serial.print("Voltage Range ");
      Serial.println(RxData[2], HEX);
      Serial.print("Current Range ");
      Serial.println(RxData[3], HEX);
      Serial.print("Power Range   ");
      Serial.println(RxData[4], HEX);
      Serial.print("Rsvd Range    ");
      Serial.println(RxData[5], HEX);
      Serial.print("CRC           ");
      Serial.println(RxData[6], HEX);

      range_value_current_rms = RxData[2];
      range_value_voltage_rms = RxData[3];
      range_value_active_power = RxData[4];

      state = GET_SYSTEM_CONFIG;
    }
    break;

  case GET_SYSTEM_CONFIG:
    Serial_write_array(ReadSystemConfiguration, sizeof(ReadSystemConfiguration)); //Send the byte array
    state = DISPLAY_SYSTEM_CONFIG;
    delay(1);
    break;

  case DISPLAY_SYSTEM_CONFIG:
    if (Serial.available() > 8) {
      for (int i = 0; i < 9; i++) {
        RxData[i] = Serial.read();
      }

      if (RxData[0] == 0x06) {
        Serial.println("System and Event Values Packet - Passed");
      }

      if (RxData[0] == 0x15) {
        Serial.println("System and Event Range Values Packet - Failed");
      }

      if (RxData[0] == 0x51) {
        Serial.println("System and Event Range Packet - CRC Failed");
      }

      Serial.print("ACK          ");
      Serial.println(RxData[0], HEX);
      Serial.print("BYTES        ");
      Serial.println(RxData[1], HEX);
      Serial.print("System[0]    ");
      Serial.println(RxData[2], HEX);
      Serial.print("System[1]    ");
      Serial.println(RxData[3], HEX);
      Serial.print("System[2]    ");
      Serial.println(RxData[4], HEX);
      Serial.print("System[3]    ");
      Serial.println(RxData[5], HEX);
      Serial.print("Event[0]    ");
      Serial.println(RxData[6], HEX);
      Serial.print("Event[1]   ");
      Serial.println(RxData[7], HEX);
      Serial.print("CRC        ");
      Serial.println(RxData[8], HEX);

      temp = (long)RxData[5];
      system_config = temp << 24;
      temp = (long) RxData[4];
      system_config = system_config + (temp << 16);
      temp = (long) RxData[3];
      system_config = system_config + (temp << 8);
      system_config = system_config + RxData[2];

      temp = (long)RxData[7];
      event_config = (int)(temp << 8);
      event_config = event_config + (int)RxData[6];

      Serial.print("System Config = ");
      Serial.println(system_config);
      Serial.print("Event Config  = ");
      Serial.println(event_config);

      state = STATE_IDLE;
    }
    break;
  }
}


void Serial_write_array(byte incoming_array[], byte characters_to_transmit) {
  byte index = 0;

  for (index = 0; index < characters_to_transmit; index++) {

    Serial.write(incoming_array[index]);
    Serial.print("                    Writing ");
    Serial.print(incoming_array[index], HEX);
    Serial.println();
    delay(1);
  }
}