#include <SoftwareSerial.h>

#define Rx    10
#define Tx    11
#define BAUD  115200
SoftwareSerial UM7Data(Rx, Tx);

int nState = 0;
#define STATE_ZERO         0
#define STATE_S            1
#define STATE_SN           2
#define STATE_SNP          3
#define STATE_PT           4
#define STATE_READ_DATA    5
#define STATE_CHK1         6
#define STATE_CHK0         7
#define STATE_DONE         8

#define UM6_GET_DATA 0xAE
#define UM6_QUAT_AB  0x64
#define UM6_QUAT_CD  0x65
#define PT_HAS_DATA  0b10000000
#define PT_IS_BATCH  0b01000000
#define PT_COMM_FAIL 0b00000001

#define DATA_BUFF_LEN  16

byte aPacketData[DATA_BUFF_LEN];
int n = 0;
byte c = 0;
int nDataByteCount = 0;

typedef struct {
  boolean HasData;
  boolean IsBatch;
  byte BatchLength;
  boolean CommFail;
  byte Address;
  byte Checksum1;
  byte Checksum0;
  byte DataLength;
} UM7_PacketStruct ;

UM7_PacketStruct UM7_Packet;

void setup(){
  UM7Data.begin(BAUD);
  Serial.begin(9600);
  while(!Serial);
// REQUEST FIRMWARE VERSION
  UM7Data.write('s');
  UM7Data.write('n');
  UM7Data.write('p');
  UM7Data.write((byte)0x00);
  UM7Data.write(0xAA); // GET_FW_REVISION register
  UM7Data.write(0x01); // CHECKSUM HIGH BYTE
  UM7Data.write(0xFB); // CHECKSUM LOW BYTE
}

void loop(){

  n = UM7Data.available();
  if (n > 0){
    c = UM7Data.read();
    Serial.println(c, DEC);
    switch(nState){
      case STATE_ZERO : // Begin. Look for 's'.
        //Serial.print("ZERO ");
        Reset();
        if (c == 's'){ //0x73 = 's'
          nState = STATE_S;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_S : // Have 's'. Look for 'n'.
        Serial.print("S");
        if (c == 'n'){ //0x6E = 'n'
          nState = STATE_SN;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SN : // Have 'sn'. Look for 'p'.
        Serial.print("N");
        if (c == 'p'){ //0x70 = 'p'
          nState = STATE_SNP;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength.
        Serial.print("P");
        UM7_Packet.HasData = 1 && (c & PT_HAS_DATA);
        UM7_Packet.IsBatch = 1 && (c & PT_IS_BATCH);
        UM7_Packet.BatchLength = ((c >> 2) & 0b00001111);
        UM7_Packet.CommFail = 1 && (c & PT_COMM_FAIL);
        nState = STATE_PT;
        if (UM7_Packet.IsBatch){
          UM7_Packet.DataLength = UM7_Packet.BatchLength * 4;
        } else {
          UM7_Packet.DataLength = 4;
        }
        break;
      case STATE_PT : // Have PacketType. Read Address.
        Serial.println("PACKETBYTE");
        UM7_Packet.Address = c;
        nDataByteCount = 0;
        nState = STATE_READ_DATA;
        break;
      case STATE_READ_DATA : // Read Data. (UM7_PT.BatchLength * 4) bytes.
        Serial.println("READ_DATA");
        aPacketData[nDataByteCount] = c;
        nDataByteCount++;
        if (nDataByteCount >= UM7_Packet.DataLength){
          nState = STATE_CHK1;
        }
        break;
      case STATE_CHK1 : // Read Checksum 1
        Serial.println("CHK1");
        UM7_Packet.Checksum1 = c;
        nState = STATE_CHK0;
        break;
      case STATE_CHK0 : // Read Checksum 0
        Serial.println("CHK0");
        UM7_Packet.Checksum0 = c;
        nState = STATE_DONE;
        break;
      case STATE_DONE : // Entire packet consumed. Process packet
        Serial.println("STATE_DONE");
        ProcessPacket();
        nState = STATE_ZERO;
        break;
      }
    }
    else{ //Serial.println("no data");
    }
  }

void ProcessPacket(){
int DataA = 0;
int DataB = 0;
int DataC = 0;
int DataD = 0;

  PrintDebug();
  switch(UM7_Packet.Address){
    case UM6_QUAT_AB :
      if (UM7_Packet.HasData && !UM7_Packet.CommFail){
        DataA = (aPacketData[0] << 8) | aPacketData[1];
        DataB = (aPacketData[2] << 8) | aPacketData[3];
        if (UM7_Packet.DataLength > 4){
          DataC = (aPacketData[4] << 8) | aPacketData[5];
          DataD = (aPacketData[6] << 8) | aPacketData[7];
        }
      }
      Serial.print("N = ");
      Serial.print(n,DEC);
      Serial.print(" A = ");
      Serial.print(DataA,DEC);
      Serial.print(" B = ");
      Serial.print(DataB,DEC);
      Serial.print(" C = ");
      Serial.print(DataC,DEC);
      Serial.print(" D = ");
      Serial.print(DataD,DEC);
      Serial.println(".");
      break;
    Serial.println("unknown packet");
  }
}

void Reset(){
  UM7_Packet.HasData = false;
  UM7_Packet.IsBatch = false;
  UM7_Packet.BatchLength = 0;
  UM7_Packet.CommFail = false;
  UM7_Packet.Address = 0;
  UM7_Packet.Checksum1 = 0;
  UM7_Packet.Checksum0 = 0;
  UM7_Packet.DataLength = 0;
}

void PrintDebug(){
// PT = has_data=1 is_batch=1 batch=0010 (2) res=0 fail=0
// AD = 0x64
  Serial.print("N = ");
  Serial.print(n,DEC);
  Serial.print(" HD = ");
  Serial.print(UM7_Packet.HasData,BIN);
  Serial.print(" IB = ");
  Serial.print(UM7_Packet.IsBatch,BIN);
  Serial.print(" BL = ");
  Serial.print(UM7_Packet.BatchLength,DEC);
  Serial.print(" CF = ");
  Serial.print(UM7_Packet.CommFail,BIN);
  Serial.print(" AD = 0x");
  Serial.print(UM7_Packet.Address,HEX);
  Serial.print(" CS1 = 0x");
  Serial.print(UM7_Packet.Checksum1,HEX);
  Serial.print(" CS0 = 0x");
  Serial.print(UM7_Packet.Checksum0,HEX);
  Serial.print(" DL = ");
  Serial.print(UM7_Packet.DataLength,DEC);
  Serial.println(".");
}