#ifndef ESCSERIALCOMM_H
#define ESCSERIALCOMM_H

#include <Arduino.h>


int sort_desc(const void *cmp1, const void *cmp2);
float filterMedian(int16_t* values);

#define SERIAL_CONTROL_BAUD   115200       // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
#define START_FRAME         0xABCD       // [-] Start frme definition for reliable serial communication


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


#define FEEDBACKRECEIVETIMEOUT 500



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



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


typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
   uint16_t start;
   int16_t  cmd1;
   int16_t  cmd2;
   int16_t  speedL_meas; //left speed is positive when driving forward
   int16_t  speedR_meas; //right speed is negatie when driving forward
   int16_t  batVoltage;
   int16_t  boardTemp;
   int16_t  curL_DC; //negative values are current consumed. positive values mean generated current
   int16_t  curR_DC;
   uint16_t cmdLed;
   uint16_t  checksum;
} SerialFeedback;

#define CURRENT_FILTER_SIZE 60 //latency is about CURRENT_FILTER_SIZE/2*MEASURE_INTERVAL (measure interval is defined by hoverboard controller)
#define CURRENT_MEANVALUECOUNT 20 //0<= meanvaluecount < CURRENT_FILTER_SIZE/2. how many values will be used from sorted weight array from the center region. abour double this values reading are used

typedef struct{
  int16_t curL_DC[CURRENT_FILTER_SIZE] = {0}; //current will be inverted for this so positive value means consumed current
  int16_t curR_DC[CURRENT_FILTER_SIZE] = {0};
  uint8_t cur_pos=0;
  int16_t cmdL=0;
  int16_t cmdR=0;
  float filtered_curL=0;
  float filtered_curR=0;
  unsigned long millis=0; //time when last message received
} MotorParameter;




class ESCSerialComm
{
  public:
    ESCSerialComm(HardwareSerial& _SerialRef); //constructor
    void init();
    bool update(long millis);
    bool feedbackAvailable();
    void setSpeed(int16_t uSpeedLeft, int16_t uSpeedRight);
    int16_t getCmdL();
    int16_t getCmdR();
    bool sendPending(long millis);

  private:
    unsigned long loopmillis;

    long _millis_lastinput; //declare private variable


    HardwareSerial *serialRef;


    bool controller_connected=false;

    float meanSpeedms;
    float trip; //trip distance in meters
    float wheelcircumference; //wheel diameter in m. 

    float currentConsumed; //Ah


    unsigned long last_send;
    unsigned long last_receive;


    SerialCommand Command;
    SerialRead SRead;
    SerialFeedback Feedback;
    SerialFeedback NewFeedback;
    MotorParameter Motorparams;

    bool flag_received=false;


    void updateMotorparams();

    void SendSerial(int16_t uSpeedLeft, int16_t uSpeedRight);
    bool ReceiveSerial();
};
#endif