#include "hoverboard-esc-serial-comm.h"



ESCSerialComm::ESCSerialComm(HardwareSerial &_serialRef) { //constructor
    serialRef=&_serialRef;
    wheelcircumference=0.5278; //8.4cm radius -> 0.084m*2*Pi
}

void ESCSerialComm::init() {
    serialRef->begin(SERIAL_CONTROL_BAUD);
}

void ESCSerialComm::setSpeed(int16_t uSpeedLeft, int16_t uSpeedRight)
{
    Motorparams.cmdL=uSpeedLeft;
    Motorparams.cmdR=uSpeedRight;
}
int16_t ESCSerialComm::getCmdL() {
    return Motorparams.cmdL;
}
int16_t ESCSerialComm::getCmdR() {
    return Motorparams.cmdR;
}

bool ESCSerialComm::sendPending(long millis) {
    return (millis - last_send > SENDPERIOD);
}

bool ESCSerialComm::update(long millis) //returns true if something was sent or received
{
    loopmillis=millis;
    bool flag_sent=false;
    bool flag_received=ReceiveSerial();
    if (flag_received) {
        updateMotorparams();
    }


    if ( loopmillis > Motorparams.millis+FEEDBACKRECEIVETIMEOUT  ) { //controller disconnected
      if (controller_connected) { //just got disconnected
        controller_connected=false;
        Serial.println("Controller Front feedback timeout");
      }
    }else if(!controller_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
      controller_connected=true;
      Serial.println("Controller Front connected");
    }


    if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers    
        last_send=loopmillis;
        
        if (controller_connected) {
            SendSerial(Motorparams.cmdL,Motorparams.cmdR);
            

            flag_sent=true;
            //Serial.print(cmd_send); Serial.print(", "); Serial.print(throttle_pos); Serial.print(", "); Serial.print(filtered_curFL*1000); Serial.print(", "); Serial.print(filtered_curFR*1000); Serial.print(", "); Serial.print(filtered_currentAll*1000);  Serial.println()
            
        }

        //Update speed and trip
        float _meanRPM=(Feedback.speedL_meas-Feedback.speedR_meas)/2.0;
        meanSpeedms=_meanRPM*wheelcircumference/60.0;   // Units: 1/min * m / 60s
        trip+=abs(meanSpeedms)* (SENDPERIOD/1000.0);

        //mah consumed
        currentConsumed += (Motorparams.filtered_curL+Motorparams.filtered_curR)* (SENDPERIOD/1000.0)/3600.0; //amp hours
    }
    return flag_received || flag_sent;
}

bool ESCSerialComm::feedbackAvailable() 
{
    return flag_received;
}



void ESCSerialComm::SendSerial(int16_t uSpeedLeft, int16_t uSpeedRight)
{
  // Create command
  Command.start    = (uint16_t)START_FRAME;
  Command.speedLeft    = (int16_t)uSpeedLeft;
  Command.speedRight    = (int16_t)uSpeedRight;
  Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight);
  
  serialRef->write((uint8_t *) &Command, sizeof(Command));
  
}

bool ESCSerialComm::ReceiveSerial()
{
  
  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
}


void ESCSerialComm::updateMotorparams() {
  Motorparams.cur_pos++;
  Motorparams.cur_pos%=CURRENT_FILTER_SIZE;
  Motorparams.curL_DC[Motorparams.cur_pos] = -Feedback.curL_DC; //invert so positive current is consumed current. negative then means regenerated
  Motorparams.curR_DC[Motorparams.cur_pos] = -Feedback.curR_DC;
  Motorparams.millis=loopmillis;
}

int sort_desc(const void *cmp1, const void *cmp2) //compare function for qsort
{
  float a = *((float *)cmp1);
  float b = *((float *)cmp2);
  return a > b ? -1 : (a < b ? 1 : 0);
}

float filterMedian(int16_t* values) {
  float copied_values[CURRENT_FILTER_SIZE];
  for(int i=0;i<CURRENT_FILTER_SIZE;i++) {
    copied_values[i] = values[i]; //TODO: maybe some value filtering/selection here
  }
  float copied_values_length = sizeof(copied_values) / sizeof(copied_values[0]);
  qsort(copied_values, copied_values_length, sizeof(copied_values[0]), sort_desc);

  float mean=copied_values[CURRENT_FILTER_SIZE/2];
  for (uint8_t i=1; i<=CURRENT_MEANVALUECOUNT;i++) {
    mean+=copied_values[CURRENT_FILTER_SIZE/2-i]+copied_values[CURRENT_FILTER_SIZE/2+i]; //add two values around center
  }
  mean/=(1+CURRENT_MEANVALUECOUNT*2);

  return mean;
}