#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);

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 Routinges here
    
    if (armed) {
      if (loopmillis-last_notidle>5000) {
        display_standingDisplay(escFront,escRear);
      }else{
        if (!control_buttonA) {
          display_drivingDisplay(escFront,escRear);
        }else{
          display_debugDisplay(escFront,escRear);
        }
      }
      
    }else{
      display_standingDisarmedDisplay(escFront,escRear);
    }
    
  }
  

  
  
  display.display();
}


void display_drivingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
  
  //## 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);
  
  //if (((millis()/2500)%2)==0) {
  if (control_buttonB) {
    //## Speed statistic
    display.print("max: ");
    dtostrf(max_meanSpeed*3.6,1,0,buf);
    display.print((String)buf);
    display.print("km/h");
  }else{
    //## Current Consumed    
    /*
    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 Hours Consumed
    dtostrf(min_filtered_wattAll,1,0,buf);
    display.print("min:");
    display.print((String)buf);
    display.print("W max:");

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

}

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) {
  double _displaytrip=trip;
  //double _displaycurrent=currentConsumed;
  double _displaywatthours=watthoursConsumed;
  //bool _displayOverall= ((millis()/3000)%2==0); //switch based on time
  bool _displayOverall=control_buttonB; //switch with button
  bool _displayParameters=control_buttonA;

  char buf[8];

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

  if (_displayParameters) {
    
    display.print(F("cmdred min=")); display.print(minimum_constant_cmd_reduce);
    display.print(F(" p=")); display.print(brake_cmdreduce_proportional);
    display.println();


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

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

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

    display.print(F("Vbat:")); display.print(escFront.getFeedback_batVoltage());
    display.print(F("/")); display.print(escRear.getFeedback_batVoltage());
    display.print(" V");
    display.println();

    //display.print(F("Temp:")); display.print(escFront.getFeedback_boardTemp());
    //display.print(F("/")); display.print(escRear.getFeedback_boardTemp());
    display.print(F("T:")); display.print(temp_ESCFront,0);
    display.print(F("/")); display.print(temp_ESCRear,0);
    display.print(F("/")); display.print(temp_Air,0);
    display.print(" C");
    display.println();

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

    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
  char buf[8];
  display.setFont();
  display.setCursor(0,0);

  display.print(getLogFilename());
    
  display.print(F(" ")); display.print(loopmillis/1000);
  display.print(F("s"));
  display.println();

  display.print(F("ESC F="));
  display.print(escFront.getControllerConnected());
  display.print(F(" R="));
  display.print(escRear.getControllerConnected());
  display.println();


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

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

  if (control_buttonA){
    display.print(" A");
  }
  if (control_buttonB){
    display.print(" B");
  }


}

#endif