#ifndef _COMMS_H_
#define _COMMS_H_


#include "definitions.h"
#include "structs.h"

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

void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis);

void writeLogInfo(HardwareSerial &SerialRef);
void writeLogHeader(HardwareSerial &SerialRef);
void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake);
void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg);


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=false; //return true if new full data frame received
  // 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 false; //nothing new
  }

  // 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();
      _result=true;
    } else {
      _result=false;
    }
    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
}



void updateMotorparams( MotorParameter &mp, SerialFeedback &fb,unsigned long _loopmillis) {
  mp.cur_pos++;
  mp.cur_pos%=CURRENT_FILTER_SIZE;
  mp.curL_DC[mp.cur_pos] = -fb.curL_DC; //invert so positive current is consumed current. negative then means regenerated
  mp.curR_DC[mp.cur_pos] = -fb.curR_DC;
  mp.millis=_loopmillis;
  log_update=true;
}


/*
void writeLogInfo(HardwareSerial &SerialRef) { //first line of file
  SerialRef.print("#TIMESTAMP:");
  SerialRef.println(now()); 
}

void writeLogHeader(HardwareSerial &SerialRef) {
  SerialRef.print("time,cmd_FrontL,cmd_FrontR,cmd_RearL,cmd_RearR,");
  SerialRef.print("current_FrontL,current_FrontR,current_RearL,current_RearR,");
  SerialRef.print("rpm_FrontL,rpm_FrontR,rpm_RearL,rpm_RearR,");
  SerialRef.print("temp_Front,temp_Rear,vbat_Front,vbat_Rear,");
  SerialRef.println("currentAll,throttle,brake,speed,trip,currentConsumed");
}

void writeLog(HardwareSerial &SerialRef, unsigned long time, MotorParameter &mpfront, MotorParameter &mprear, SerialFeedback &fbfront, SerialFeedback &fbrear, float currentAll, int16_t throttle, int16_t brake)
{
  SerialRef.print(time/1000.0,3); SerialRef.print(","); //time in seconds
  SerialRef.print(mpfront.cmdL); SerialRef.print(",");
  SerialRef.print(mpfront.cmdR); SerialRef.print(",");
  SerialRef.print(mprear.cmdL); SerialRef.print(",");
  SerialRef.print(mprear.cmdR); SerialRef.print(",");

  SerialRef.print(mpfront.filtered_curL,3); SerialRef.print(",");
  SerialRef.print(mpfront.filtered_curR,3); SerialRef.print(",");
  SerialRef.print(mprear.filtered_curL,3); SerialRef.print(",");
  SerialRef.print(mprear.filtered_curR,3); SerialRef.print(",");

  SerialRef.print(fbfront.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
  SerialRef.print(-fbfront.speedR_meas); SerialRef.print(",");
  SerialRef.print(fbrear.speedL_meas); SerialRef.print(","); //invert speed, because left wheels are negated
  SerialRef.print(-fbrear.speedR_meas); SerialRef.print(",");

  SerialRef.print(fbfront.boardTemp/10.0,1); SerialRef.print(","); //in degC
  SerialRef.print(fbrear.boardTemp/10.0,1); SerialRef.print(","); //in degC
  SerialRef.print(fbfront.batVoltage/100.0); SerialRef.print(","); //in V
  SerialRef.print(fbrear.batVoltage/100.0); SerialRef.print(","); //in V

  SerialRef.print(currentAll,3); SerialRef.print(",");
  SerialRef.print(throttle); SerialRef.print(",");
  SerialRef.print(brake); SerialRef.print(",");
  SerialRef.print(meanSpeedms); SerialRef.print(","); // m/s
  SerialRef.print(trip); SerialRef.print(","); //in m
  SerialRef.print(currentConsumed,3); SerialRef.println(); //in Ah (Amphours)
  
}

void writeLogComment(HardwareSerial &SerialRef, unsigned long time, String msg) 
{
  SerialRef.print("#"); SerialRef.print(time/1000.0,3); SerialRef.print(","); SerialRef.print(msg); SerialRef.println();
}
*/


#endif