#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_standingOffDisplay(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 (escFront.getControllerConnected() && escRear.getControllerConnected()) {
    if (loopmillis-last_notidle>1000) {
      display_standingDisplay(escFront,escRear);
    }else{
      display_drivingDisplay(escFront,escRear);
    }
    
  }else{
    display_standingOffDisplay(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;
  //_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");

  
  //## Trip / Current Consumed Display
  display.setCursor(1,SCREEN_HEIGHT-7);
  if (((millis()/2500)%2)==0) {
    //## Trip
    dtostrf(escFront.getTrip(),1,0,buf);
    display.print((String)buf);
    display.print("m + ");

    dtostrf(escRear.getTrip(),1,0,buf);
    display.print((String)buf);
    display.print("m");
  }else{
    //## Current Consumed    
    dtostrf(escFront.getCurrentConsumed(),1,1,buf);
    display.print((String)buf);
    display.print("Ah + ");

    dtostrf(escRear.getCurrentConsumed(),1,1,buf);
    display.print((String)buf);
    display.print("Ah");
  }
}

void display_standingDisplay(ESCSerialComm& escFront, ESCSerialComm& escRear) {
  char buf[8];
  display.setFont();
  display.setCursor(0,0);

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

  display.print(F("Trip: "));
  dtostrf(escFront.getTrip(),1,0,buf);
  display.print((String)buf);
  display.print("/");
  dtostrf(escRear.getTrip(),1,0,buf);
  display.print((String)buf);
  display.print(" m");
  display.println();

  display.print(F("Cons. "));
  dtostrf(escFront.getCurrentConsumed(),1,2,buf);
  display.print((String)buf);
  display.print("/");

  dtostrf(escRear.getCurrentConsumed(),1,2,buf);
  display.print((String)buf);
  display.print(" Ah");
  display.println();

  
}


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

}

#endif