#include <Arduino.h>


/*
Connections:
Tennsy Pin, Pin Name, Connected to
10, Tx2, Hoverboard RX(Green)
9, Rx2, Hoverboard TX(Blue)
8, Tx3, Hoverboard RX(Green)
7, Rx3, Hoverboard TX(Blue)
*/

// ########################## 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 SERIAL_LOG_BAUD 115200 // baud rate for logging output
bool log_update=true;
unsigned long last_log_send=0;

#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
#define LOGMININTERVAL 20 //minimum interval (ms) to send logs
#define LOGMAXINTERVAL 10000  //maximum time (ms) after which data is send

#define WRITE_HEADER_TIME 400  //just before FEEDBACKRECEIVETIMEOUT, so header gets written before error comments
bool log_header_written = false;


#define FEEDBACKRECEIVETIMEOUT 500

bool controllerFront_connected=false;
bool controllerRear_connected=false;
bool controllers_connected=false;

#define PIN_THROTTLE A7
//const uint16_t calib_throttle_min = 420; //better a bit too high than too low
//const uint16_t calib_throttle_max = 790;
const uint16_t failsafe_throttle_min = 20; //if adc value falls below this failsafe is triggered
const uint16_t failsafe_throttle_max = 1000; //if adc value goes above this failsafe is triggered
const uint16_t throttleCurvePerMM[] = {414,460,490,511,527,539,548,555,561,567,573,578,584,590,599,611,630,657,697,754,789,795}; //adc values for every unit (mm) of linear travel
#define PIN_BRAKE A8
const uint16_t calib_brake_min = 100;//better a bit too high than too low
const uint16_t calib_brake_max = 600;
const uint16_t failsafe_brake_min = 20; //if adc value falls below this failsafe is triggered
const uint16_t failsafe_brake_max = 1000; //if adc value goes above this failsafe is triggered

int16_t throttle_pos=0;
int16_t brake_pos=0;

unsigned long last_adcread=0;
#define ADCREADPERIOD 10
uint16_t throttle_raw=0; 
uint16_t brake_raw=0;
#define ADC_OUTOFRANGE_TIME 100
unsigned long throttle_ok_time=0;
unsigned long brake_ok_time=0;
bool error_throttle_outofrange=false;
bool error_brake_outofrange=false;

#define NORMAL_MAX_ACCELERATION_RATE 10000
#define SLOW_MAX_ACCELERATION_RATE 500
int16_t max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE; //maximum cmd send increase per second


float meanSpeedms=0;
float trip=0; //trip distance in meters
float wheelcircumference=0.5278; //wheel diameter in m. 8.4cm radius -> 0.084m*2*Pi

float currentConsumed=0; //Ah


//Driving parameters
int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
int16_t brake_cmdreduce_proportional=500; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading

bool reverse_enabled=false;
unsigned long last_notidle=0; //not rolling to fast, no pedal pressed

#define PIN_START A9
#define PIN_LED_START 2 //Enginge start led

#define PIN_LATCH_ENABLE A6

#define PIN_MODE_SWITCH 3
#define PIN_MODE_LEDG 4
#define PIN_MODE_LEDR 5



unsigned long last_send = 0;
unsigned long last_receive = 0;

float filtered_currentAll=0;

int16_t cmd_send=0;
int16_t last_cmd_send=0;

uint8_t speedmode=0;
#define SPEEDMODE_SLOW 1
#define SPEEDMODE_NORMAL 0


unsigned long button_start_lastchange=0;
bool button_start_state=false;
#define LONG_PRESS_ARMING_TIME 2000
#define DEBOUNCE_TIME 50

bool armed = false; //cmd output values forced to 0 if false


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


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


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;
SerialFeedback FeedbackFront;
SerialFeedback NewFeedbackFront;
SerialFeedback FeedbackRear;
SerialFeedback NewFeedbackRear;

#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;
MotorParameter motorparamsFront;
MotorParameter motorparamsRear;


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

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

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, String msg);

void readADC();
void failChecks();
void sendCMD();
void checkLog();
void updateMotorparams( MotorParameter &mp, SerialFeedback &fb);
void leds();

void readButtons();

uint16_t linearizeThrottle(uint16_t v);

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

  Serial1.begin(SERIAL_LOG_BAUD); //TX1=1, RX1=0

  Serial2.begin(SERIAL_CONTROL_BAUD); //control, TX2=10, RX2=9
  Serial3.begin(SERIAL_CONTROL_BAUD); //control, TX3=8, RX3=7

  pinMode(PIN_THROTTLE, INPUT);
  pinMode(PIN_BRAKE, INPUT);
  pinMode(PIN_START, INPUT_PULLUP); //Pressed=High

  pinMode(PIN_LED_START, OUTPUT); //Active High
  
  pinMode(PIN_MODE_LEDG, OUTPUT); //Active Low
  digitalWrite(PIN_MODE_LEDG,LOW);
  pinMode(PIN_MODE_LEDR, OUTPUT); //Active Low
  digitalWrite(PIN_MODE_LEDR,LOW);

  pinMode(PIN_LATCH_ENABLE, OUTPUT);
  digitalWrite(PIN_LATCH_ENABLE,HIGH); //latch on
  pinMode(PIN_MODE_SWITCH, INPUT_PULLUP);
}


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

  bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2);
  bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);

  if (newData2) {
    updateMotorparams(motorparamsFront,FeedbackFront);
  }
  if (newData3) {
    updateMotorparams(motorparamsRear,FeedbackRear);
  }
  
  if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
    last_adcread=loopmillis;
    readADC();

    readButtons();

    
  }


  failChecks();  

  if (loopmillis - last_send > SENDPERIOD) { //Calculate motor stuff and send to motor controllers    
    last_send=loopmillis;
    sendCMD();    

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

    //mah consumed
    currentConsumed += (motorparamsFront.filtered_curL+motorparamsFront.filtered_curR+motorparamsRear.filtered_curL+motorparamsRear.filtered_curR)* (SENDPERIOD/1000.0)/3600.0; //amp hours
  }

  //If needed write log to serial port
  checkLog();

  leds();

}


// ##### HELPFUNCTIONS


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



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();
}



// #### LOOPFUNCTIONS


void readADC() {
  throttle_raw = analogRead(PIN_THROTTLE);
   //maps throttle curve to be linear
  throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain
  brake_raw = analogRead(PIN_BRAKE);
  brake_pos=max(0,min(1000,map(brake_raw,calib_brake_min,calib_brake_max,0,1000))); //map and constrain
  //brake_pos = (int16_t)(pow((brake_pos/1000.0),2)*1000);

  if (throttle_pos>0 || meanSpeedms>0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving)
    last_notidle=loopmillis;
    reverse_enabled=false;
  }
  #define REVERSE_ENABLE_TIME 1000
  if (loopmillis-last_notidle > REVERSE_ENABLE_TIME) {
    reverse_enabled=true;
  }

  int16_t throttlebreak_pos = throttle_pos-brake_pos*2; //reduce throttle_when applying brake
  throttle_pos=constrain(throttlebreak_pos,0,1000);
  brake_pos=constrain(-throttlebreak_pos/2,0,1000); //rescale brake value from throttlebreak_pos

  //Serial.print(throttle_raw); Serial.print(", "); Serial.print(brake_raw); Serial.print(", ");
  //Serial.print(throttle_pos); Serial.print(", "); Serial.print(brake_pos); Serial.println();

  
  if (digitalRead(PIN_MODE_SWITCH)) { //pushed in, also high if cable got disconnected
    if (speedmode!=SPEEDMODE_SLOW) {
      speedmode=SPEEDMODE_SLOW;
      max_acceleration_rate=SLOW_MAX_ACCELERATION_RATE;
      if (loopmillis>WRITE_HEADER_TIME) {
        writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_SLOW");
      }
    }
  }else{ //button not pushed in
    if (speedmode!=SPEEDMODE_NORMAL) {
      speedmode=SPEEDMODE_NORMAL;
      max_acceleration_rate=NORMAL_MAX_ACCELERATION_RATE;
      if (loopmillis>WRITE_HEADER_TIME) {
        writeLogComment(Serial1,loopmillis, "Mode switched to SPEEDMODE_NORMAL");
      }
    }
  } 


  if (speedmode==SPEEDMODE_SLOW) {
    throttle_pos/=2;
  }


}

void failChecks() {
  if ( loopmillis > motorparamsFront.millis+FEEDBACKRECEIVETIMEOUT  ) { //controller disconnected
    if (controllerFront_connected) { //just got disconnected
      controllerFront_connected=false;
      writeLogComment(Serial1,loopmillis, "Controller Front feedback timeout");
    }
  }else if(!controllerFront_connected && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
    controllerFront_connected=true;
    writeLogComment(Serial1,loopmillis, "Controller Front connected");
  }

  if ( loopmillis > motorparamsRear.millis+FEEDBACKRECEIVETIMEOUT  ) { //controller disconnected
    if (controllerRear_connected) { //just got disconnected
      controllerRear_connected=false;
      writeLogComment(Serial1,loopmillis, "Controller Rear feedback timeout");
    }
  }else if(!controllerRear_connected  && loopmillis > FEEDBACKRECEIVETIMEOUT) { //not timeouted but was before
    controllerRear_connected=true;
    writeLogComment(Serial1,loopmillis, "Controller Rear connected");
  }

  controllers_connected=controllerFront_connected & controllerRear_connected;

  //ADC Range Check
  if ((throttle_raw >= failsafe_throttle_min) & (throttle_raw <= failsafe_throttle_max)) { //inside safe range (to check if wire got disconnected)
    throttle_ok_time=loopmillis;
  }
  if (loopmillis>throttle_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
    if (!error_throttle_outofrange) {
      error_throttle_outofrange=true;
      writeLogComment(Serial1,loopmillis, "Error Throttle ADC Out of Range");
    }
  }
  if ((brake_raw >= failsafe_brake_min) & (brake_raw <= failsafe_brake_max)) { //outside safe range. maybe wire got disconnected
    brake_ok_time=loopmillis;
  }
  if (loopmillis>brake_ok_time+ADC_OUTOFRANGE_TIME) { //not ok for too long
    if(!error_brake_outofrange) {
      error_brake_outofrange=true;
      writeLogComment(Serial1,loopmillis, "Error Brake ADC Out of Range");
    }
  }



  if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange) { //any errors?
    throttle_pos=0;
    brake_pos=0;
  }
}

void sendCMD() {
  /* ## FOR REFERENCE:
  int16_t minimum_constant_cmd_reduce=1; //reduce cmd every loop by this constant amount when freewheeling/braking
  int16_t brake_cmdreduce_proportional=100; //cmd gets reduced by an amount proportional to brake position (ignores freewheeling). cmd_new-=brake_cmdreduce_proportional / second @ full brake. with BREAK_CMDREDUCE_CONSTANT=1000 car would stop with full brake at least after a second (ignoring influence of brake current control/freewheeling)
  float startbrakecurrent=3; //Ampere. "targeted brake current @full brake". at what point to start apply brake proportional to brake_pos. for everything above that cmd is reduced by freewheel_break_factor
  float startbrakecurrent_offset=0.1; //offset start point for breaking, because of reading fluctuations around 0A. set this slightly above idle current reading
  */
  int16_t brake_pos_expo = (int16_t)(pow((brake_pos/1000.0),2)*1000);

  float brakepedal_current_multiplier=startbrakecurrent/1000.0; //how much breaking (in Ampere) for unit of brake_pos (0<=brake_pos<=1000)

  int16_t cmdreduce_constant=map(brake_pos_expo,0,1000,0,(int16_t)(brake_cmdreduce_proportional*SENDPERIOD/1000)); //reduce cmd value every cycle

  float freewheel_current=startbrakecurrent_offset-brake_pos_expo*brakepedal_current_multiplier; //above which driving current cmd send will be reduced more. increase value to decrease breaking. values <0 increases breaking above freewheeling
  float freewheel_break_factor=500.0; //speed cmd units per amp per second. 1A over freewheel_current decreases cmd speed by this amount (on average)
  motorparamsFront.filtered_curL=filterMedian(motorparamsFront.curL_DC)/50.0; //in Amps
  motorparamsFront.filtered_curR=filterMedian(motorparamsFront.curR_DC)/50.0; //in Amps
  motorparamsRear.filtered_curL=filterMedian(motorparamsRear.curL_DC)/50.0; //in Amps
  motorparamsRear.filtered_curR=filterMedian(motorparamsRear.curR_DC)/50.0; //in Amps

  float filtered_currentFront=max(motorparamsFront.filtered_curL,motorparamsFront.filtered_curR);
  float filtered_currentRear=max(motorparamsRear.filtered_curL,motorparamsRear.filtered_curR);

  filtered_currentAll=max(filtered_currentFront,filtered_currentRear); //positive value is current Drawn from battery. negative value is braking current

  if (throttle_pos>=last_cmd_send) { //accelerating
    cmd_send += constrain(throttle_pos-cmd_send,0,max_acceleration_rate*SENDPERIOD/1000); //if throttle higher than last applied value, apply throttle directly
  }else{ //freewheeling or braking
    if (filtered_currentAll>freewheel_current) { //drive current too high
      cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(SENDPERIOD/1000.0)); //how much current over freewheel current, multiplied by factor. reduces cmd_send value
    }
    cmd_send-=max(minimum_constant_cmd_reduce,cmdreduce_constant); //reduce slowly anyways
    
  }

  

  cmd_send=constrain(cmd_send,0,1000);

  last_cmd_send=cmd_send;

  int16_t cmd_send_toMotor=constrain(cmd_send*  (1.0-(brake_pos*0.5/1000.0) ) ,0,1000); //brake "ducking"

  if (reverse_enabled) {
    cmd_send_toMotor-=brake_pos*0.1;
  }

  if (!controllers_connected || !armed) { //controllers not connected or not armed
    cmd_send=0;
    cmd_send_toMotor=0; //safety off
  }

  //apply throttle command to all motors
  motorparamsFront.cmdL=cmd_send_toMotor;
  motorparamsFront.cmdR=cmd_send_toMotor;
  motorparamsRear.cmdL=cmd_send_toMotor;
  motorparamsRear.cmdR=cmd_send_toMotor;

  if (controllers_connected) {
    SendSerial(CommandFront,motorparamsFront.cmdL,motorparamsFront.cmdR,Serial2);
    SendSerial(CommandRear,motorparamsRear.cmdL,motorparamsRear.cmdR,Serial3);

    log_update=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()
    
  }else if(loopmillis>last_log_send+LOGMININTERVAL){
    //Serial.print(throttle_raw);  Serial.println();
    
    Serial.print(linearizeThrottle(throttle_raw));  Serial.println();
    last_log_send=loopmillis;
  }
}


void checkLog() {
  if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up
    writeLogHeader(Serial1);
    log_header_written=true;
  }
  
  if (log_header_written && ( (log_update && loopmillis>last_log_send+LOGMININTERVAL) || loopmillis>last_log_send+LOGMAXINTERVAL) ) {
    last_log_send=loopmillis;
    log_update=false;
    writeLog(Serial1,loopmillis, motorparamsFront,motorparamsRear, FeedbackFront, FeedbackRear, filtered_currentAll, throttle_pos, brake_pos);
  }
}

void updateMotorparams( MotorParameter &mp, SerialFeedback &fb) {
  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 leds() {
  //Start LED
  if (!armed) { //disarmed
    digitalWrite(PIN_LED_START,((loopmillis/1000)%2 == 0)); //high is on for LED_START. blink every second. loopmillis 0 - 1000 led is on.
  }else{ //armed
    digitalWrite(PIN_LED_START,HIGH); //LED On
  }
  

  if (speedmode==SPEEDMODE_SLOW) {
    digitalWrite(PIN_MODE_LEDG,LOW); //Green, low is on
    digitalWrite(PIN_MODE_LEDR,HIGH);
  }else if (speedmode==SPEEDMODE_NORMAL) {
    digitalWrite(PIN_MODE_LEDG,HIGH);
    digitalWrite(PIN_MODE_LEDR,LOW); //Red
  } 
}

void readButtons() {

  if (loopmillis > button_start_lastchange+DEBOUNCE_TIME) { //wait some time after last change
    if (digitalRead(PIN_START) && !button_start_state) { //start engine button pressed and was not pressed before
      button_start_state=true; //pressed
      button_start_lastchange=loopmillis; //save time for debouncing
    }else if (!digitalRead(PIN_START) && button_start_state) { //released an was pressed before
      button_start_state=false; // not pressed
      button_start_lastchange=loopmillis; //save time for debouncing
    }
  }

  if (button_start_state) { //pressed
    if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
      if (throttle_pos<=0 && brake_pos<=0 && controllers_connected && !armed) {  //brake or thottle not pressed, controllers connected
        armed=true; //arm if button pressed long enough
        writeLogComment(Serial1,loopmillis, "Armed by button");
      }      
    }else if (armed){ //not pressed long enough and is armed
      armed=false; //disarm
      writeLogComment(Serial1,loopmillis, "Disarmed by button");
    }
  }

}

uint16_t linearizeThrottle(uint16_t v) {
  //input is raw adc value from hall sensor
  //uses throttleCurvePerMM array to find linear approximation of actual throttle travel
  //array has to be sorted !
  uint8_t _searchpos=0;
  uint8_t arraysize = sizeof(throttleCurvePerMM)/sizeof(throttleCurvePerMM[0]);
  while (_searchpos < arraysize && v>throttleCurvePerMM[_searchpos]) { //find arraypos with value above input value
    _searchpos++; //try next value
  }

  if (_searchpos <=0) { //lower limit
    return 0;
  }
  if (_searchpos >= arraysize) { //upper limit
    return 1000;
  }

  uint16_t nextLower=throttleCurvePerMM[_searchpos-1];
  uint16_t nextHigher=throttleCurvePerMM[_searchpos];
  float _linearThrottle = _searchpos+map(v*1.0,nextLower,nextHigher,0.0,1.0);
  _linearThrottle/=arraysize; //scale to 0-1
  _linearThrottle*=1000; //scale to 0-1000
  return (uint16_t)_linearThrottle;
}