#include <Arduino.h>

// ########################## DEFINES ##########################
#define SERIAL_CONTROL_BAUD   38400       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)
#define START_FRAME         0xAAAA       // [-] Start frme definition for reliable serial communication


#define SENDPERIOD 50 //ms. delay for sending speed and steer data to motor controller via serial

int16_t out_speedFL=0;
int16_t out_speedFR=0;
int16_t out_speedRL=0;
int16_t out_speedRR=0;
unsigned long last_send = 0;

void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight);
void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback);

// Global variables for serial communication
typedef struct{
    uint8_t idx = 0;                        // Index for new data pointer
    uint16_t bufStartFrame;                 // Buffer Start Frame
    byte *p;                                // Pointer declaration for the new received data
    byte incomingByte;
    byte incomingBytePrev;
    long lastValidDataSerial_time;
    long boardlastPoweron=0; //mainly for failcheck
    long boardlastPoweroff=0;
} SerialRead;
SerialRead SerialcomFront;


typedef struct{
   uint16_t start;
   int16_t  speedLeft;
   int16_t  speedRight;
   uint16_t checksum;
} SerialCommand;
SerialCommand CommandFront;

typedef struct{
   uint16_t start;
   int16_t  cmd1;
   int16_t  cmd2;
   int16_t  speedL_meas;
   int16_t  speedR_meas;
   int16_t  batVoltage;
   int16_t  boardTemp;
   int16_t  curL_DC;
   int16_t  curR_DC;
   uint16_t cmdLed;
   uint16_t  checksum;
} SerialFeedback;
SerialFeedback FeedbackFront;
SerialFeedback NewFeedbackFront;

void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight)
{
  // Create command
  scom.start    = (uint16_t)START_FRAME;
  scom.speedLeft    = (int16_t)uSpeedLeft;
  scom.speedRight    = (int16_t)uSpeedRight;
  scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
  Serial1.write((uint8_t *) &scom, sizeof(scom));  //TODO: use correct serial object
}

void ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback)
{
  // Check for new data availability in the Serial buffer
  if ( Serial1.available() ) {
    sread.incomingByte    = Serial1.read();                                 // Read the incoming byte
    sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) |  sread.incomingBytePrev;  // Construct the start frame    
  }
  else {
    return;
  }

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
    Serial.print(sread.incomingByte);
    return;
  #endif      
  
  // Copy received data
  if (sread.bufStartFrame == START_FRAME) {                     // Initialize if new data is detected
    sread.p     = (byte *)&NewFeedback;
    *sread.p++  = sread.incomingBytePrev;
    *sread.p++  = sread.incomingByte;   
    sread.idx   = 2;    
  } else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) {  // Save the new received data
    *sread.p++  = sread.incomingByte; 
    sread.idx++;
  } 
  
  // Check if we reached the end of the package
  if (sread.idx == sizeof(SerialFeedback)) {    
    uint16_t checksum;
  
    checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
          ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
  
    // Check validity of the new data
    if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
      // Copy the new data
      memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
      sread.lastValidDataSerial_time = millis();
    }
    sread.idx = 0;  // Reset the index (it prevents to enter in this if condition in the next cycle)
    /*
      // Print data to built-in Serial
      Serial.print("1: ");   Serial.print(Feedback.cmd1);
      Serial.print(" 2: ");  Serial.print(Feedback.cmd2);
      Serial.print(" 3: ");  Serial.print(Feedback.speedR);
      Serial.print(" 4: ");  Serial.print(Feedback.speedL);
      Serial.print(" 5: ");  Serial.print(Feedback.speedR_meas);
      Serial.print(" 6: ");  Serial.print(Feedback.speedL_meas);
      Serial.print(" 7: ");  Serial.print(Feedback.batVoltage);
      Serial.print(" 8: ");  Serial.println(Feedback.boardTemp);
    } else {
      Serial.println("Non-valid data skipped");
    }*/

    // Update previous states
    sread.incomingBytePrev = sread.incomingByte;
    
  }

  // ########################## SETUP ##########################
void setup() 
{

  Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)

  Serial1.begin(SERIAL_CONTROL_BAUD); //control.  A2=TX2, A3=RX2 (Serial1 is Usart 2). Marked with "1" on connector (Rear)
  Serial2.begin(SERIAL_CONTROL_BAUD); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3). Marked with "II" on connector (Front)


}

// ########################## LOOP ##########################
void loop() {
  loopmillis=millis(); //read millis for this cycle

  ReceiveSerial();


  if (loopmillis - last_send > SENDPERIOD) {
    last_send=loopmillis;


    SendSerial(out_speedRL,out_speedRR);

    float _current = (Feedback1.curL_DC+Feedback1.curR_DC+Feedback2.curL_DC+Feedback2.curR_DC)/4.0 / 50; 
    
    Serial.print(Feedback1.curL_DC); Serial.print(", "); //1 is rear
    Serial.print(Feedback1.curR_DC); Serial.print(", ");
    Serial.print(Feedback2.curL_DC); Serial.print(", "); //2 is front
    Serial.print(Feedback2.curR_DC);
    Serial.print(", mean="); Serial.print(_current);
    Serial.println();
    
  }
}