//TODO:
/*
 - reset Trip to 0 by button press or something. function: resetTrip() 
*/


#include <Arduino.h>


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

#include <TimeLib.h> //for teensy rtc 
#include "helpfunctions.h"
#include "hoverboard-esc-serial-comm.h"
#include "led.h"
#include "temperature.h"

//#include "comms.h"
String getLogFilename();
#include "display.h"
#include "logging.h"

#include "ADS1X15.h"







ESCSerialComm escFront(Serial2);
ESCSerialComm escRear(Serial3);
//Serial1 = TX1=1, RX1=0
//Serial2 = TX2=10, RX2=9
//Serial3 = TX3=8, RX3=7

ADS1115 ADS(0x48);

/*
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)
*/



void readADS();
void readADC();
void failChecks();
//void sendCMD();
void calculateSetSpeed(unsigned long timediff);
void checkLog();

void leds();

void readButtons();
void readADSButtons();

uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending);


time_t getTeensy3Time();


  // ########################## 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_FAN,OUTPUT);
  digitalWrite(PIN_FAN,HIGH); //Turn fan on during startup for debugging purposes
  

  //TODO: remove mode button things
  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


  init_led();
  led_testLEDSBlocking();
  
  
  delay(2000);
  Serial.println("Init Functions");
  led_simpeProgress(0,1);

  bool initResult=false;
  
  initResult=display_init();
  led_simpeProgress(1,initResult);
  

  
  initResult=initLogging();
  led_simpeProgress(2,initResult);
  
  

  escFront.init();
  led_simpeProgress(3,true);
  escRear.init();
  led_simpeProgress(4,true);

  delay(2000);
  Serial.println("Wait finished. Booting..");
  led_simpeProgress(5,true);

  //init ADS1115
  
  if (!ADS.begin()) {
    Serial.println("Error:"); delay(2000); Serial.println("ADS1115 Init Error!");
    led_simpeProgress(6,false);
    writeLogComment((unsigned long)millis(), "Error ADS1115 Init");
  }else{
    ADS.setGain(0);
    ADS.setDataRate(7);// Read Interval: 7-> 2ms, 6-> 3-4ms , 5-> 5-6ms,  4-> 9ms, 0-> 124ms
      // also set ADSREADPERIOD to at least the read interval
    ADS.requestADC(0); //Start requesting a channel
    led_simpeProgress(6,true);
  }
  delay(10);
  for (uint8_t i=0;i<4;i++){ //read all channels once to have adc readings ready in first loop (to prevent premature failsafe)
    readADS();
    delay(10);
  }
  

  setSyncProvider(getTeensy3Time); //See https://www.pjrc.com/teensy/td_libs_Time.html#teensy3
  if (timeStatus()!= timeSet) {
    Serial.println("Unable to sync with the RTC");
    led_simpeProgress(7,false);
  } else {
    Serial.println("RTC has set the system time");
    led_simpeProgress(7,true);
  }


  if (datalogging) { //sd init was successful
    initResult=loadTripSD();
  }else{
    initResult=false;
  }
  led_simpeProgress(8,initResult);


  initTemperature();
  led_simpeProgress(9,true);
  


  writeLogComment(millis(), "Setup Finished");
  led_simpeProgress(15,true);
  
  led_simpleProgressWait(); //wait longer if any errors were displayed with led_simpeProgress()
}



// ########################## LOOP ##########################
void loop() {
  //Serial.print("Loopduration="); Serial.println(); //loopduration is at max 11ms

  loopmillis=millis(); //read millis for this cycle


  /*
  // ____ Debugging pending serial byted for feedback
  static int s2availmax=0;
  int _a2=Serial2.available();
  if (_a2>s2availmax) {
    s2availmax=_a2;
    //Serial.print("new s2availmax"); Serial.println(s2availmax);
    String _text="Serial2 Bytes Available Max=";
    _text+=s2availmax;
    writeLogComment(Serial1,loopmillis, _text);
  }

  static int s3availmax=0;
  int _a3=Serial3.available();
  if (_a3>s3availmax) {
    s3availmax=_a3;
    //Serial.print("new s3availmax"); Serial.println(s3availmax);
    String _text="Serial3 Bytes Available Max=";
    _text+=s3availmax;
    writeLogComment(Serial1,loopmillis, _text);
  }
  // ----- End of debug



  bool newData2=ReceiveSerial(SerialcomFront,FeedbackFront, NewFeedbackFront, Serial2); 
  bool newData3=ReceiveSerial(SerialcomRear,FeedbackRear, NewFeedbackRear, Serial3);
  //Max (40) or 22 available/pending bytes

  if (newData2) {
    updateMotorparams(motorparamsFront,FeedbackFront,loopmillis);
  }
  if (newData3) {
    updateMotorparams(motorparamsRear,FeedbackRear,loopmillis);
  }
  */


  

  
  if (ADS.isConnected() && (loopmillis - last_adsread > ADSREADPERIOD) ) { //read teensy adc and filter
    last_adsread=loopmillis;
    if (ADS.isBusy() == false) //reads a register on ads
    {
      readADS();
    }else{
      Serial.println("Unnecessary ADS poll. Increase ADSREADPERIOD");
    }
  }
  
  static unsigned long last_adcread=0;
  if (loopmillis - last_adcread > ADCREADPERIOD) { //read teensy adc and filter
    last_adcread=loopmillis;

    readADC();
  }

  static unsigned long last_buttonread=0;
  if (loopmillis - last_buttonread > BUTTONREADPERIOD) { //read digital input states
    last_buttonread=loopmillis;
    readButtons();
    readADSButtons();
  }


  failChecks();  

  static unsigned long last_calculateSetSpeed=0;
  if (loopmillis - last_calculateSetSpeed > SENDPERIOD) {
    unsigned long _timediff=loopmillis-last_calculateSetSpeed;
    last_calculateSetSpeed=loopmillis;
    calculateSetSpeed(_timediff);    

    //Update Statistics

    max_filtered_currentAll=max(max_filtered_currentAll,filtered_currentAll);
    min_filtered_currentAll=min(min_filtered_currentAll,filtered_currentAll);
    max_filtered_wattAll=max(max_filtered_wattAll,filtered_currentAll*(escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0);
    min_filtered_wattAll=min(min_filtered_wattAll,filtered_currentAll*(escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0);
    max_meanSpeed=max(max_meanSpeed,(escFront.getMeanSpeed()+escRear.getMeanSpeed())/2);
    if (!armed) { //reset statistics if disarmed
      max_filtered_currentAll=0;
      min_filtered_currentAll=0;
      max_filtered_wattAll=0;
      min_filtered_wattAll=0;
      max_meanSpeed=0;
    }
  }

  escFront.update(loopmillis);
  escRear.update(loopmillis);

  static unsigned long last_statsupdate=0;
  #define STATSUPDATEINTERVAL 100
  if (loopmillis-last_statsupdate>STATSUPDATEINTERVAL) {
    
    minSpeedms=min(escFront.getWheelspeed_L(),min(escFront.getWheelspeed_R(),min(escRear.getWheelspeed_L(),escRear.getWheelspeed_R()))); //take speed of slowest wheel
    float _tripincrease=abs(minSpeedms) * ((loopmillis-last_statsupdate)/1000.0);
    trip+=_tripincrease;
    overallTrip+=_tripincrease;
    
    float _currentIncrease=(escFront.getFiltered_curL()+escFront.getFiltered_curR()+escRear.getFiltered_curL()+escRear.getFiltered_curR())* ((loopmillis-last_statsupdate)/1000.0)/3600.0; //amp hours
    float _watthoursIncrease=((escFront.getFiltered_curL()+escFront.getFiltered_curR())*escFront.getFeedback_batVoltage()+(escRear.getFiltered_curL()+escRear.getFiltered_curR())*escRear.getFeedback_batVoltage())* ((loopmillis-last_statsupdate)/1000.0)/3600.0; //amp hours
    currentConsumed += _currentIncrease;
    overallCurrentConsumed += _currentIncrease;

    watthoursConsumed += _watthoursIncrease;
    overallWatthoursConsumed += _watthoursIncrease;

    last_statsupdate=loopmillis;
  }

  /*  TODO: remove this if, because everything contained in esc.update()
  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(); //TODO remove
  loggingLoop(loopmillis,escFront,escRear);
  if (!armed && !statswritten) { //write stats only once when disarmed
    statswritten=true;
    writeTrip(loopmillis,escFront,escRear);
  }
  if (statswritten && armed) {
    statswritten=false;
  }

  leds();
  led_update(loopmillis,escFront,escRear); //ws2812 led ring

  static unsigned long last_display_update=0;
  if (loopmillis - last_display_update > DISPLAYUPDATEPERIOD) {
    last_display_update=loopmillis;
    display_update(escFront,escRear);
  }

  //Temperature
  temperatureLoop(loopmillis);

  //Fan
  static unsigned long last_fan_update=0;
  #define FANUPDATEPERIOD 5000
  float fan_turn_on_temp=45;
  float fan_turn_off_temp=32;
  if (loopmillis - last_fan_update > FANUPDATEPERIOD) {
    last_fan_update=loopmillis;
    boolean fanstatus=digitalRead(PIN_FAN);
    //float temp=max(escFront.getFeedback_boardTemp(),escRear.getFeedback_boardTemp());
    float temp=max(temp_ESCFront,temp_ESCRear);
    if (temp_ESCFront==DEVICE_DISCONNECTED_C || temp_ESCRear==DEVICE_DISCONNECTED_C ) { //temperature error
      digitalWrite(PIN_FAN,HIGH); //force fan on

    }else{ //normal temperature control
      
      if (!fanstatus) { //fan is off
        if (temp>=fan_turn_on_temp){
          digitalWrite(PIN_FAN,HIGH);
        }
      }else{ //fan is on
        if (temp<=fan_turn_off_temp){
          digitalWrite(PIN_FAN,LOW);
        }
      }
    }
  
    
  }

  looptime_duration=max(looptime_duration,millis()-loopmillis);

}


time_t getTeensy3Time()
{
  return Teensy3Clock.get();
}





void readADS() { //sequentially read ads and write to variable
  /*static unsigned long _lastReadADS=0;
  Serial.print("readADS Interval="); Serial.println(millis()-_lastReadADS);
  _lastReadADS=millis();*/
  static uint8_t ads_input_switch=0;

  int16_t ads_val = ADS.getValue(); //get value from last selected channel


  switch (ads_input_switch) {
    case 0: //Throttle Sensor A
      ads_throttle_A_raw=ads_val;
      break;
    case 1: //Throttle Sensor B
      ads_throttle_B_raw=ads_val;
      break;
    case 2: //Brake
      ads_brake_raw=ads_val;
      break;
    case 3: //Buttons
      ads_control_raw=ads_val;
      break;
  }

  ads_input_switch++;
  ads_input_switch%=4; //max 4 channels
  ADS.requestADC(ads_input_switch);  // request a new one
}


// #### LOOPFUNCTIONS


void readADC() {
  
  //Serial.print(ads_throttle_A_raw); Serial.print('\t'); 
  //Serial.print(ads_throttle_B_raw); Serial.print('\t'); 
  //Serial.print(ads_brake_raw); Serial.print('\t'); 
  //Serial.print(ads_control_raw); Serial.println();
  //throttle_raw = (ads_throttle_A_raw+ads_throttle_B_raw)/2.0*THROTTLE_ADC_FILTER + throttle_raw*(1-THROTTLE_ADC_FILTER); //apply filter

  throttle_rawA=ads_throttle_A_raw;
  throttle_rawB=ads_throttle_B_raw;


  //maps throttle curve to be linear
  //throttle_pos=max(0,min(1000,linearizeThrottle(throttle_raw))); //map and constrain
  throttle_posA=max(0,min(1000, linearizeThrottle(ads_throttle_A_raw, throttleCurvePerMM_A, sizeof(throttleCurvePerMM_A)/sizeof(throttleCurvePerMM_A[0]), throttleCurvePerMM_A_Descending ) )); //map and constrain
  throttle_posB=max(0,min(1000, linearizeThrottle(ads_throttle_B_raw, throttleCurvePerMM_B, sizeof(throttleCurvePerMM_B)/sizeof(throttleCurvePerMM_B[0]), throttleCurvePerMM_B_Descending ) )); //map and constrain
  
  //Serial.print(throttle_posA); Serial.print('\t'); 
  //Serial.print(throttle_posB); Serial.print('\t'); 
  
  int16_t throttle_posMean = (throttle_posA+throttle_posB)/2.0;   
  
  
  throttle_pos = throttle_posMean*THROTTLE_ADC_FILTER + throttle_pos*(1-THROTTLE_ADC_FILTER); //apply filter
 
    
  brake_raw=ads_brake_raw;
  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 || ((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0) >0.5 || (!reverse_enabled && brake_pos>0)) { //reset idle time on these conditions (disables reverse driving)
    last_notidle=loopmillis;
    reverse_enabled=false;
  }
  
  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

  //throttlemax=750 - cmd ist aber 543
  //throttlemax=250 - cmd ist aber 117

  //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() {
  static bool laststate_Front_getControllerConnected;
  if ( !escFront.getControllerConnected() && laststate_Front_getControllerConnected) { //controller got disconnected and was connected before
    laststate_Front_getControllerConnected=false;
    writeLogComment(loopmillis, "Controller Front feedback timeout");  
  }else if( escFront.getControllerConnected() && !laststate_Front_getControllerConnected) { //controller was disconnected and is now connected
    laststate_Front_getControllerConnected=true;
    writeLogComment(loopmillis, "Controller Front connected");
  }

  static bool laststate_Rear_getControllerConnected;
  if ( !escRear.getControllerConnected() && laststate_Rear_getControllerConnected) { //controller got disconnected and was connected before
    laststate_Rear_getControllerConnected=false;
    writeLogComment(loopmillis, "Controller Rear feedback timeout");  
  }else if( escRear.getControllerConnected() && !laststate_Rear_getControllerConnected) { //controller was disconnected and is now connected
    laststate_Rear_getControllerConnected=true;
    writeLogComment(loopmillis, "Controller Rear connected");
  }


  controllers_connected=escFront.getControllerConnected() & escRear.getControllerConnected();

  //ADC Range Check
  static unsigned long throttle_ok_time=0;
  if ((ads_throttle_A_raw >= failsafe_throttle_min_A) & (ads_throttle_A_raw <= failsafe_throttle_max_A) & (ads_throttle_B_raw >= failsafe_throttle_min_B) & (ads_throttle_B_raw <= failsafe_throttle_max_B)) { //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(loopmillis, "Error Throttle ADC Out of Range. A="+(String)ads_throttle_A_raw+" B="+(String)ads_throttle_B_raw);
    }
    //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
  }

  static unsigned long throttlediff_ok_time=0;
  if (abs(throttle_posA-throttle_posB) <= failsafe_throttle_maxDiff) { //inside safe range (to check if wire got disconnected)
    throttlediff_ok_time=loopmillis;
  }
  if (loopmillis>throttlediff_ok_time+ADC_DIFFHIGH_TIME) { //not ok for too long
    if (!error_throttle_difftoohigh) {
      error_throttle_difftoohigh=true;
      writeLogComment(loopmillis, "Error Throttle Diff too High. A="+(String)ads_throttle_A_raw+" B="+(String)ads_throttle_B_raw);
    }
    //Serial.print("Error Throttle ADC Out of Range="); Serial.println(throttle_raw);
  }


  static unsigned long brake_ok_time=0;
  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(loopmillis, "Error Brake ADC Out of Range. ADC="+(String)brake_raw);
    }
    //Serial.print("Error Brake ADC Out of Range="); Serial.println(brake_raw);
  }
  #define ADS_MAX_READ_INTERVAL 100
  if (loopmillis-last_adsread > ADS_MAX_READ_INTERVAL) {
    if (!error_ads_max_read_interval) {
      error_ads_max_read_interval=true;
      writeLogComment(loopmillis, "Error ADS Max read interval");
    }
    //Serial.print("Error ADS Max read interval="); Serial.println(loopmillis-last_adsread);
  }

  boolean logged_error_sdfile_unavailable=false;
  if (error_sdfile_unavailable && !logged_error_sdfile_unavailable) {
    logged_error_sdfile_unavailable=true;
    writeLogComment(loopmillis, "Error SDFile Unavailable");      
  }

  if (!controllers_connected || error_brake_outofrange || error_throttle_outofrange || error_throttle_difftoohigh || error_ads_max_read_interval) { //any errors?
    armed=false; //disarm
    throttle_pos=0;
    brake_pos=0;
  }
}

void calculateSetSpeed(unsigned long timediff){

  int16_t adjusted_throttle_pos=constrain(throttle_pos*(throttle_max/1000.0),0,throttle_max);

  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*timediff/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 filtered_currentFront=max(escFront.getFiltered_curL(),escFront.getFiltered_curR());
  float filtered_currentRear=max(escRear.getFiltered_curL(),escRear.getFiltered_curR());

  filtered_currentAll=filtered_currentFront+filtered_currentRear; //positive value is current Drawn from battery. negative value is braking current
  

  if(adjusted_throttle_pos<last_cmd_send){ //freewheeling or braking
    if (filtered_currentAll>freewheel_current) { //drive current too high
      cmd_send-= max(0, (filtered_currentAll-freewheel_current)*freewheel_break_factor*(timediff/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
    
  }

  //acceleration
  cmd_send += constrain(adjusted_throttle_pos-cmd_send,0,(int16_t)(max_acceleration_rate*(timediff/1000.0)) ); //if throttle higher than last applied value, apply throttle directly

  cmd_send=constrain(cmd_send,0,throttle_max);

  last_cmd_send=cmd_send;

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

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

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

  escFront.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
  escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor);

  log_update=true;
}

/*
void sendCMD() { //TODO: remove complete function because replaced by calculateSetSpeed()
  // ## 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*REVERSE_SPEED;
  }

  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() { //TODO: remove
  if (!log_header_written && loopmillis>=WRITE_HEADER_TIME){ //write header for log file after logger booted up
    writeLogInfo(Serial1);
    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 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() {
  bool button_start_longpress_flag=false;
  bool button_start_shortpress_flag=false;

  static bool button_start_wait_release_flag=false;
  bool last_button_start_state=button_start_state;

  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_wait_release_flag) { //action not prohibited currently
    if (button_start_state) { //button is pressed
      if ( (loopmillis> button_start_lastchange + LONG_PRESS_ARMING_TIME)) { //pressed long
        button_start_longpress_flag=true;
        button_start_wait_release_flag=true; //do not trigger again until button released
      }
    }else if(!button_start_state && last_button_start_state) { //just released
      button_start_shortpress_flag=true;
    }
  }

  if (!button_start_state) { //release wait flag at end if button released
    button_start_wait_release_flag=false;
  }


  if (button_start_shortpress_flag) {
    armed=false; //disarm
    writeLogComment(loopmillis, "Disarmed by button");
  }
  if (button_start_longpress_flag) {
    if (escFront.getControllerConnected() && escRear.getControllerConnected()) {
      armed=true; //arm if button pressed long enough
      writeLogComment(loopmillis, "Armed by button");
      if (control_buttonA) { //button A is held down during start button press
        throttle_max=1000;
        reverse_speed=0.25;
      }else if (control_buttonB) { //button B is held down during start button press
        throttle_max=750;
        reverse_speed=0.25;
      }else { //no control button pressed during start
        throttle_max=250;
        reverse_speed=0.15;
      }

    }else{
      writeLogComment(loopmillis, "Unable to arm");
    }
    
  }

}

void readADSButtons() {
  bool last_control_buttonA=control_buttonA;
  bool last_control_buttonB=control_buttonB;
  if ( (ads_control_raw > (calib_control_buttonA-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonA+calib_control_treshold) ) ) {
    control_buttonA=true;
    control_buttonB=false;
  }else if ( (ads_control_raw > (calib_control_buttonB-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonB+calib_control_treshold) ) ) {
    control_buttonA=false;
    control_buttonB=true;
  }else if ( (ads_control_raw > (calib_control_buttonAB-calib_control_treshold)) && (ads_control_raw < (calib_control_buttonAB+calib_control_treshold) ) ) {
    control_buttonA=true;
    control_buttonB=true;
  }else if ( ads_control_raw > calib_control_max){
    control_buttonA=false;
    control_buttonB=false;
  }

  if (control_buttonA && !last_control_buttonA) { //button A was just pressed
    writeLogComment(loopmillis, "Button A Pressed");
  }

  if (control_buttonB && !last_control_buttonB) { //button B was just pressed
    writeLogComment(loopmillis, "Button B Pressed");    
  }
}


uint16_t linearizeThrottle(uint16_t v, const uint16_t *pthrottleCurvePerMM, int arraysize,bool sorteddescending) {
  //input is raw adc value from hall sensor
  //uses pthrottleCurvePerMM array to find linear approximation of actual throttle travel
  //array has to be sorted ! if sorteddescending=false then sorted ascending, if true then array should be sorted descending
  uint8_t _searchpos=0;
  //uint8_t arraysize = sizeof(pthrottleCurvePerMM)/sizeof(pthrottleCurvePerMM[0]);
  while (_searchpos < arraysize && v>pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)]) { //find arraypos with value above input value
    _searchpos++; //try next value
  }

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

  uint16_t nextLower=pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)-(sorteddescending?0:1)];
  uint16_t nextHigher=pthrottleCurvePerMM[(sorteddescending?(arraysize-1-_searchpos):_searchpos)-(sorteddescending?1:0)];
  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
  if (sorteddescending){
    _linearThrottle=1000-_linearThrottle; //invert result
  }
  return (uint16_t)_linearThrottle;
}