#include <Arduino.h>

// ########################## DEFINES ##########################
#define SERIAL_CONTROL_BAUD   115200       // [-] 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         0xABCD       // [-] Start frme definition for reliable serial communication


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

#define PIN_THROTTLE A0
const uint16_t calib_throttle_min = 350;
const uint16_t calib_throttle_max = 810;

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;
unsigned long last_receive = 0;

float avg_currentL=0;
float avg_currentR=0;


// 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;
} 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, HardwareSerial &SerialRef);
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);

void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
{
  // 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);
  
  SerialRef.write((uint8_t *) &scom, sizeof(scom));
  
}

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

  // If DEBUG_RX is defined print all incoming bytes
  #ifdef DEBUG_RX
    Serial.print(sread.incomingByte);
  #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();
    } else {
      _result=0;
    }
    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;

  return _result; //new data was available

}

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

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

  Serial2.begin(SERIAL_CONTROL_BAUD); //control
  Serial3.begin(SERIAL_CONTROL_BAUD); //control

  pinMode(PIN_THROTTLE, INPUT_PULLUP);

}


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

  bool newData=false;
  newData=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
  
  
  //Serial.print("fo="); Serial.println(count);
  //count++;

  if (newData) {
    float _current = (FeedbackFront.curL_DC+FeedbackFront.curR_DC)/2.0 / 50; 
    
    /*
    Serial.print(FeedbackFront.curL_DC); Serial.print(", ");
    Serial.print(FeedbackFront.curR_DC); Serial.print(", ");

    Serial.print(", mean="); Serial.print(_current);
    Serial.print(", ");
    Serial.print(FeedbackFront.cmd1); Serial.print(", ");
    Serial.print(FeedbackFront.cmd2); Serial.print(", ");
    Serial.println();
    */

    #define CURRENTFILTER 0.95
    avg_currentL=avg_currentL*CURRENTFILTER+FeedbackFront.curL_DC*(1-CURRENTFILTER);
    avg_currentR=avg_currentR*CURRENTFILTER+FeedbackFront.curR_DC*(1-CURRENTFILTER);
  }

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

    uint16_t throttle_raw = analogRead(PIN_THROTTLE);
    //Serial.print("Analog: "); Serial.print(throttle_raw);
    out_speedFR=max(0,min(1000,map(throttle_raw,calib_throttle_min,calib_throttle_max,0,1000)));
    
    //Serial.print(", Send: "); Serial.println(out_speedFL);
    SendSerial(CommandFront,out_speedFL,out_speedFR,Serial2);   

    Serial.print(out_speedFL); Serial.print(", "); Serial.print(out_speedFR); Serial.print(", "); Serial.print(avg_currentR); Serial.print(", "); Serial.println(avg_currentL);
    
  }
}