#include <SoftwareSerial.h>

#define Rx    10
#define Tx    11
#define BAUD  115200
#define SERIALOUT 57600
SoftwareSerial UM7Data(Rx, Tx);
//AltSoftSerial UM7Data;

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 EULER_PHI_THETA  0x70
#define EULER_PSI  0x71

#define PT_HAS_DATA  0b10000000
#define PT_IS_BATCH  0b01000000
#define PT_COMM_FAIL 0b00000001
#define PT_BATCH_LEN       0x08  //Batch length = 1

#define DATA_BUFF_LEN  128

byte Data[DATA_BUFF_LEN];
int um7 = 0;
bool xy;
byte charBuffer = 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(SERIALOUT);
}

void loop(){

  um7 = UM7Data.available();
  if (um7 > 0){
    charBuffer = UM7Data.read();
    switch(nState){
      case STATE_ZERO : // Begin. Look for 's'.
        Reset();
        if (charBuffer == 's'){ //0x73 = 's'
          nState = STATE_S;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_S : // Have 's'. Look for 'n'.
        if (charBuffer == 'n'){ //0x6E = 'n'
          nState = STATE_SN;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SN : // Have 'sn'. Look for 'p'.
        if (charBuffer == 'p'){ //0x70 = 'p'
          nState = STATE_SNP;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength.
        UM7_Packet.HasData = 1 && (charBuffer & PT_HAS_DATA);
        UM7_Packet.IsBatch = 1 && (charBuffer & PT_IS_BATCH);
        UM7_Packet.BatchLength = ((charBuffer >> 2) & 0b00001111);
        UM7_Packet.CommFail = 1 && (charBuffer & 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.
        UM7_Packet.Address = charBuffer;
        nDataByteCount = 0;
        nState = STATE_READ_DATA;
        break;
      case STATE_READ_DATA : // Read Data. (UM7_PT.BatchLength * 4) bytes.
        Data[nDataByteCount] = charBuffer;
        nDataByteCount++;
        if (nDataByteCount >= UM7_Packet.DataLength){
          nState = STATE_CHK1;
        }
        break;
      case STATE_CHK1 : // Read Checksum 1
        UM7_Packet.Checksum1 = charBuffer;
        nState = STATE_CHK0;
        break;
      case STATE_CHK0 : // Read Checksum 0
        UM7_Packet.Checksum0 = charBuffer;
        nState = STATE_DONE;
        break;
      case STATE_DONE : // Entire packet consumed. Process packet
        PrintOSC();

        Reset();
        nState = STATE_ZERO;

        xy = !xy;
        pollAddress(xy? EULER_PHI_THETA : EULER_PSI);
        break;
      }
    }
    else{ //Serial.println("no data");
    }
  }

void PrintOSC() {
  if (UM7_Packet.Address == EULER_PHI_THETA) {
      Serial.println(BitShiftCombine (Data[0], Data[1]) ); // X
      Serial.println(BitShiftCombine (Data[2], Data[3]) ); // Z
  }
  if (UM7_Packet.Address == EULER_PSI) {
      Serial.println(BitShiftCombine(Data[0], Data[1]) ); // Y
  }
}

int32_t BitShiftCombine( unsigned char x_high, unsigned char x_low)
{
  return (x_high << 8) | x_low;         //shift x_high over to leftmost 8 bits
}

void PrintDebug(){
  Serial.print("HasData = ");
  Serial.print(UM7_Packet.HasData,BIN);
  Serial.print("; IsBatch = ");
  Serial.print(UM7_Packet.IsBatch,BIN);
  Serial.print("; BatchLength = ");
  Serial.print(UM7_Packet.BatchLength,DEC);
  Serial.print("; ComFail = ");
  Serial.print(UM7_Packet.CommFail,BIN);
  Serial.print("; Address = 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("; DataLength = ");
  Serial.print(UM7_Packet.DataLength,DEC);

  Serial.print("; Data = ");
  for (int i=0;i<nDataByteCount;i++)
    Serial.write(Data[i]);
  Serial.print("\n");
}

void pollAddress(int address){  //This void function makes a request for a packet from the IMU
  byte chksum0 = 0, chksum1 = 0;
  unsigned int chksum = 0;

  chksum = 's' + 'n' + 'p' + (PT_IS_BATCH | PT_BATCH_LEN) + address;
  chksum1 = chksum >> 8;
  chksum0 = chksum & 0xFF;

  UM7Data.write('s');
  UM7Data.write('n');
  UM7Data.write('p');
  UM7Data.write(PT_IS_BATCH | PT_BATCH_LEN);
  UM7Data.write(address);
  UM7Data.write(chksum1);
  UM7Data.write(chksum0);
}

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;
}