#ifndef ESCSERIALCOMM_H
#define ESCSERIALCOMM_H

#include <Arduino.h>

int _sort_desc(const void *cmp1, const void *cmp2);

#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 (bobbycar?)
   int16_t  speedR_meas; //right speed is negatie when driving forward (bobbycar?)
   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(unsigned long loopmillis);
    bool feedbackAvailable();
    void setSpeed(int16_t uSpeedLeft, int16_t uSpeedRight);
    int16_t getCmdL();
    int16_t getCmdR();
    bool sendPending(unsigned long loopmillis);

    int16_t getFeedback_cmd1();
    int16_t getFeedback_cmd2();
    int16_t getFeedback_speedL_meas();
    int16_t getFeedback_speedR_meas();
    float getFeedback_batVoltage();
    float getFeedback_boardTemp();
    float getFeedback_curL_DC();
    float getFeedback_curR_DC();
    float getFiltered_curL();
    float getFiltered_curR();

    float getMinBatVoltage();
    float getMaxBoardTemp();
    double getCurrentConsumed();
    double getMeanSpeed();
    double getTrip();

    float getMincurL();
    float getMincurR();
    float getMaxcurL();
    float getMaxcurR();

    unsigned long getFeedbackInterval(); //get time from last received feedback
    unsigned long getTripTime(unsigned long loopmillis);

    bool getControllerConnected();

    void resetStatistics();

    float getWheelspeed_L();
    float getWheelspeed_R();

    

  private:
    long _millis_lastinput; //declare private variable


    HardwareSerial *serialRef;


    bool controller_connected=false;

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

    double currentConsumed; //Ah

    float minBatVoltage=1000;
    float maxBoardTemp=0;


    float mincurL=0;
    float mincurR=0;

    float maxcurL=0;
    float maxcurR=0;



    unsigned long last_send;
    unsigned long last_receive;
    unsigned long last_reset_time;

    unsigned long feedback_interval_timed;

    unsigned long last_update_trip;


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

    bool flag_received=false;


    void updateMotorparams(unsigned long loopmillis);

    void SendSerial(int16_t uSpeedLeft, int16_t uSpeedRight);
    bool ReceiveSerial();

    
    float filterMedian(int16_t* values);


};
#endif