#ifndef _DISPLAY_H_
#define _DISPLAY_H_

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeMonoBold18pt7b.h>
#include <Fonts/FreeSansBold9pt7b.h>


#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 32 // OLED display height, in pixels

#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

uint8_t standingDisplayScreen=0;
#define NUM_STANDINGDISPLAYSCREEN 4

#define DISPLAYSTANDSTILLTIME 5000

bool display_init();
void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);
void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);

void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear);


bool display_init(){
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    return false;
  }
  display.clearDisplay();
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.display();
  return true;
}

void display_update(ESCSerialComm& escFront, ESCSerialComm& escRear){
  display.clearDisplay();
  /*
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,0);             // Start at top-left corner
  display.print(F("Speed   : ")); display.println((escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0);
  display.print(F("Thr: ")); display.print(throttle_pos);
  display.print(F(" Brk : ")); display.println(brake_pos);
  display.print(F("Current : ")); display.println(filtered_currentAll);
  display.print(F("Vbat : ")); display.print(escFront.getFeedback_batVoltage());
  display.print(F(" / ")); display.println(escRear.getFeedback_batVoltage());
  */

  if ( (error_brake_outofrange || error_throttle_outofrange || error_throttle_difftoohigh || error_ads_max_read_interval || error_sdfile_unavailable) && ((loopmillis/2000)%2==0)) {
    //Error Messages
    
    display.setFont();
    display.setTextSize(1);             // Normal 1:1 pixel scale
    display.setTextColor(SSD1306_WHITE);        // Draw white text
    display.setCursor(0,0);             // Start at top-left corner
    display.print(F("Error!"));
    display.println();

    String errorstring="";
    if (error_brake_outofrange) {
      errorstring+="brake_outofrange\n";
    }
    if (error_throttle_outofrange) {
      errorstring+="throttle_outofrange\n";
    }
    if (error_throttle_difftoohigh) {
      errorstring+="throttle_difftoohigh\n";
    }
    if (error_ads_max_read_interval) {
      errorstring+="ads_max_read_interval\n";
    }if (error_sdfile_unavailable) {
      errorstring+="error_sdfile_unavailable\n";
    }
    display.print(errorstring);

  }else{
    //Normal Display Routines here
    
    if (armed) {
      if (loopmillis-last_notidle>DISPLAYSTANDSTILLTIME) {
        display_standingDisplay(escFront,escRear);
      }else{
        display_drivingDisplay(escFront,escRear);
        //display_debugDisplay(escFront,escRear);        
      }
      
    }else{
      display_standingDisarmedDisplay(escFront,escRear);
    }
    
  }
  

  
  
  display.display();
}


void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
  //Display stuff here for when bobbycar is moving or was moving recently and is armed


  //## Km/h Display
  display.setFont(&FreeMonoBold18pt7b);
  display.setTextSize(1);             // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  display.setCursor(0,SCREEN_HEIGHT-(SCREEN_HEIGHT-18)/2 - 3);             // Start at top-left corner


  //float _speeddisplay=(escFront.getMeanSpeed()+escRear.getMeanSpeed())/2.0*3.6;
  float _speeddisplay=minSpeedms*3.6;
  
  //_speeddisplay=(millis()/1000)%21; //debugging
  char buf[8];
  dtostrf(_speeddisplay,1,1,buf);
  String strbuf=buf;
  
  if (strbuf.length()<4) { //pad spaces on the left
    strbuf=" "+strbuf;
  }
  display.print(strbuf);
  
  display.setFont();
  display.setCursor(SCREEN_WIDTH-25,SCREEN_HEIGHT-16);
  display.print("km/h");

  //A
  display.setCursor(SCREEN_WIDTH-37,1);
  static float averaged_filtered_currentAll;
  #define CURRENT_FILTER 0.8
  averaged_filtered_currentAll=averaged_filtered_currentAll*CURRENT_FILTER+(1-CURRENT_FILTER)*filtered_currentAll; //filter over time
  float averaged_filtered_wattAll=averaged_filtered_currentAll*(escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0;
  //dtostrf(averaged_filtered_currentAll,1,2,buf);
  dtostrf(averaged_filtered_wattAll,1,0,buf);
  strbuf=buf;
  if (strbuf.length()<5) { //pad spaces on the left
    strbuf=" "+strbuf;
  }
  display.print(strbuf);
  //display.print("A");
  display.print("W");

  
  //## Trip / Current Consumed Display
  display.setCursor(1,SCREEN_HEIGHT-7);

  //## Current    
  /*
  dtostrf(min_filtered_currentAll,1,1,buf);
  display.print("min:");
  display.print((String)buf);
  display.print("A max:");

  dtostrf(max_filtered_currentAll,1,1,buf);
  display.print((String)buf);
  display.print("A");
  */

  //## Watt
  dtostrf(min_filtered_wattAll,1,0,buf);
  display.print((String)buf);
  display.print(" / ");

  dtostrf(max_filtered_wattAll,1,0,buf);
  display.print((String)buf);
  display.print("W ");

  display.setCursor(SCREEN_WIDTH-6*6,SCREEN_HEIGHT-7);
  dtostrf(max_meanSpeed*3.6,1,0,buf);
  display.print((String)buf);
  display.print("km/h");

}


void display_debugDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
  //Debug
  display.setTextSize(2);             // Normal 1:1 pixel scale
  display.setFont();
  display.setTextColor(SSD1306_WHITE);        // Draw white text
  char buf[8];

  display.setCursor(1,0);
  dtostrf(filtered_currentAll,1,3,buf);
  display.print((String)buf);
  display.print(" A > ");
  display.println();


  //all wheel getCMD
  
  display.setTextSize(1);
  display.setCursor(25,SCREEN_HEIGHT-8*2);
  display.print(escFront.getCmdL());
  display.setCursor(SCREEN_WIDTH-4*7-25,SCREEN_HEIGHT-8*2);
  display.print(escFront.getCmdR());
  display.setCursor(25,SCREEN_HEIGHT-8*1);
  display.print(escRear.getCmdL());
  display.setCursor(SCREEN_WIDTH-4*7-25,SCREEN_HEIGHT-8*1);
  display.print(escRear.getCmdR());
  
  /*
  display.setTextSize(1);
  display.setCursor(1,SCREEN_HEIGHT-8*2);
  display.print("minCMD=");
  display.print(min(min(min(escFront.getCmdL(),escFront.getCmdR()),escRear.getCmdL()),escRear.getCmdR()));

  display.print(" thrpos=");
  display.print(throttle_pos);
  */
}


void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
  //Display stuff here when ESCs are powered on and bobbycar is armed but was not moving for a while

  double _displaytrip=trip;
  //double _displaycurrent=currentConsumed;
  double _displaywatthours=watthoursConsumed;
  bool _displayOverall= ((millis()/2500)%2==0); //switch based on time
  
  
  

  char buf[8];

  display.setFont();
  display.setCursor(0,0);

    

  if (_displayOverall) { //alternate between this trip and overall trip
    _displaytrip=overallTrip;
    //_displaycurrent=overallCurrentConsumed;
    _displaywatthours=overallWatthoursConsumed;
  }


  //Row 1
  if (!_displayOverall) {
    display.print(F("Vbat:")); display.print(escFront.getFeedback_batVoltage());
    display.print(F("/")); display.print(escRear.getFeedback_batVoltage());
  }else{
    display.print(F("Vmin:")); display.print(min_voltage);
  }
  display.print(" V");
  display.println();
  


  //Row 2
  //display.print(F("Temp:")); display.print(escFront.getFeedback_boardTemp());
  //display.print(F("/")); display.print(escRear.getFeedback_boardTemp());
  display.print(F("T:")); display.print(temp_Front,0);
  display.print(F("/")); display.print(temp_Rear,0);
  display.print(F("/")); display.print(temp_Air,0);
  display.print(" C");
  display.println();

  //Row 3
  display.print(F("Trip:"));
  dtostrf(_displaytrip,1,0,buf);
  display.print((String)buf);
  display.print("m ");
  //dtostrf(_displaycurrent,1,2,buf);
  dtostrf(_displaywatthours,1,2,buf);
  display.print((String)buf);
  //display.print(" Ah");
  display.print("Wh");
  display.println();

  //Row 4

  display.print(F(""));
  //dtostrf( _displaytrip/1000/_displaycurrent ,1,2,buf);
  dtostrf( _displaywatthours/_displaytrip*100,1,2,buf);
  display.print((String)buf);
  //display.print(" km/Ah");
  display.print(" kWh/100km");

  if (_displayOverall){
    display.print(" sum");
  }


  display.println();



}


void display_standingDisarmedDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
  //Displayed stuff here when escs are powered off / disconnected or bobbycar is not armed
  char buf[8];
  display.setFont();
  display.setCursor(0,0);

  

  switch(standingDisplayScreen) 
  {
    case 0:
    //Quick overview of inputs and status

    //Row1
    if (getDatalogging()) {
      display.print(getLogFilename());
    }else{
      display.print("LOG DISABLED");
    }
      
    display.print(F(" ")); display.print(loopmillis/1000);
    display.print(F("s"));
    display.println();

    //Row2

    display.print(F("Throttle "));
    dtostrf((throttle_posA+throttle_posB)/2.0,4,0,buf);
    display.print((String)buf);
    display.print(F(" ("));
    if (throttle_posA>throttle_posB) { //use values acutally taken for diff check
      display.print(F("+")); //add + for better formatting
    }
    dtostrf((throttle_posA-throttle_posB),1,0,buf);
    display.print((String)buf);
    display.print(F(")"));
    display.println();

    //Row3

    display.print("Brake ");
    dtostrf(brake_linear,4,0,buf); //use only linearized values (not constrained)
    display.print((String)buf);  

    display.setCursor(8*10,2*8);
    dtostrf((escFront.getFeedback_batVoltage()+escRear.getFeedback_batVoltage())/2.0,1,1,buf);
    display.print((String)buf);  
    display.print(F("V"));
    display.println();

    

    
    /*display.print(" c=");
    dtostrf(ads_control_raw,1,0,buf);
    display.print((String)buf);  */


    break;

  case 1:
    //Trip Stats (trip / overall)

    //Row 1
    display.print(F("Trip ")); display.print(trip,0); display.print(F(" / ")); display.print(overallTrip,0); display.print(F("m"));
    display.println();
    //Row 2
    display.print(F("Curr ")); display.print(currentConsumed*1000,0); display.print(F(" / "));  display.print(overallCurrentConsumed*1000,0); display.print(F("mAh"));
    display.println();
    //Row 3
    display.print(F("Pow ")); display.print(watthoursConsumed,0); display.print(F(" / "));  display.print(overallWatthoursConsumed,0); display.print(F("Wh"));
    display.println();

    break;

  case 2:
    //Configuration parameters

    //Row 1
    display.print(F("cmdred min=")); display.print(minimum_constant_cmd_reduce);
    display.print(F(" p=")); display.print(brake_cmdreduce_proportional);
    display.println();

    //Row 2

    display.print(F("brkI="));
    dtostrf(startbrakecurrent,1,1,buf);
    display.print((String)buf);
    display.print("A");
    display.print(F(" free="));
    dtostrf(freewheel_break_factor,1,1,buf);
    display.print((String)buf);
    display.println();

    //Row 3

    display.print(F("thrmax=")); display.print(throttle_max);
    display.print(F(" rev="));
    dtostrf(reverse_speed,1,2,buf);
    display.print((String)buf);
    display.println();

    break;

    case 3:
    //Raw inputs and Input Debug

    //TODO: show deviation (and max deviation), show resulting throttle and brake pos
    

    //Row1

    display.print(F("Throttle "));
    dtostrf(ads_throttle_A_raw,1,0,buf);
    display.print((String)buf);
    display.print(F("/"));
    dtostrf(ads_throttle_B_raw,1,0,buf);
    display.print((String)buf);
    display.println();

    //Row2
    static int16_t _throttle_maxdiff = 0;
    if (control_buttonA || control_buttonB) { //any button press
      _throttle_maxdiff=0; //reset maxdiff for this display
    }
    _throttle_maxdiff=max(_throttle_maxdiff,abs(throttle_posA-throttle_posB));
    display.print(F("Thr.maxdiff "));
    dtostrf(_throttle_maxdiff,1,0,buf);
    display.print((String)buf);
    display.println();

    //Row3

    display.print("Brake ");
    dtostrf(ads_brake_raw,1,0,buf);
    display.print((String)buf);  
    display.println();

    
    break;

  }

  //Put User Inputs in last row
  //Row 4
  display.setCursor(0,24);
  if (button_start_state){
    display.print("S");
  }else{
    display.print(" ");
  }
  if (control_buttonB){
    display.print("B");
  }else{
    display.print(" ");
  }
  if (control_buttonA){
    display.print("A");
  }else{
    display.print(" ");
  }

  if (escFront.getControllerConnected()){
    display.print("F");
  }else{
    display.print(" ");
  }
  if (escRear.getControllerConnected()){
    display.print("R");
  }else{
    display.print(" ");
  }

  display.setCursor(SCREEN_WIDTH-6*3,24);
  display.print(standingDisplayScreen);
  display.print("/"); display.print(NUM_STANDINGDISPLAYSCREEN-1);

  
  


}

#endif