#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Fonts/FreeSerif9pt7b.h>
#include <Fonts/FreeSerif12pt7b.h>
//#include <Fonts/FreeMono9pt7b.h>
//#include <Fonts/FreeMonoBold9pt7b.h>
//#include <Fonts/FreeMonoBoldOblique9pt7b.h>
//#include <Fonts/FreeMonoOblique9pt7b.h>

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

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)
// On an arduino LEONARDO:   2(SDA),  3(SCL), ...
// ESP32 i2c:   GPIO 22 (SCL), GPIO 21 (SDA)          lolin32lite: SDA 19, SCL 23
// ESP32 SPI:   GPIO 18(CLK), GPIO 19(MISO), GPIO 23(MOSI), GPIO 5(CS)

#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    !CALE I had 0x3C for 128x64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);



void GetXYAdvance(char symb, int16_t &xAdvance, int16_t &yAdvance){
  auto &oled = display;
  int16_t cx0 = 20;
  int16_t cy0 = 40;
  oled.setCursor(cx0, cy0);
  oled.print(symb);
  xAdvance = oled.getCursorX()-cx0;
  oled.println();
  yAdvance = oled.getCursorY()-cy0;
}


class CaleSSD1306FloatWidget{
public:
	CaleSSD1306FloatWidget(Adafruit_SSD1306 &tftRef, uint16_t x, uint16_t y, const GFXfont *f=NULL, int16_t n_integer_digits=3, int16_t precision = 2, const char *unitNameStr=NULL, bool debugF=false):tft(tftRef),font(f),xpos(x),ypos(y),num_digits_after_decimal(precision),debug(debugF){
		unitName[0]='\0';
		buff[0]='\0';
		min_width = n_integer_digits + (precision > 0 ? 1 + precision : 0);
		minVal = -pow(10.0f,n_integer_digits-1) + 1.0f/pow(10.0f,precision);
		maxVal = pow(10.0f,n_integer_digits) - 1.0f/pow(10.0f,precision);
		valDelta = 0.5/pow(10.0f,precision);
		if(unitNameStr && strlen(unitNameStr)<sizeof(unitName)){
			strcpy(unitName, unitNameStr);
		}else {
			unitName[0]='\0';
			unitsPrinted = true;
		}
    Serial.print("CaleSSD1306FloatWidget,"); Serial.print(xpos); Serial.print(","); Serial.print(ypos); Serial.print(","); Serial.print(minVal); Serial.print(","); Serial.print(maxVal); Serial.print(","); Serial.println(unitName); 
	}
	bool update(float val){
	
		if(val>maxVal) val = maxVal;
		if(val<minVal) val = minVal;

		if(abs(val-lastPrintedVal)>valDelta){
			if(font) tft.setFont(font);
			ErasePreviouslyPrinted();

			tft.setTextColor(SSD1306_WHITE);
			tft.setCursor(xpos, ypos);
			dtostrf(val, min_width, num_digits_after_decimal, buff);
			tft.print(buff);

			if(!unitsPrinted) { tft.print(unitName); unitsPrinted = true; }
			if(font) tft.setFont();

			lastPrintedVal = val;
			//if(debug) { Serial.print('['); Serial.print(buff); Serial.println(']'); }
			return true;
		}
		return false;
	}
	private:
	void ErasePreviouslyPrinted(){
		tft.setTextColor(SSD1306_BLACK);
		tft.setCursor(xpos, ypos);
		tft.print(buff);
	}
	void ErasePreviouslyPrintedN(){
		int16_t  x1, y1;
		uint16_t w, h;
		tft.getTextBounds(buff, xpos, ypos, &x1, &y1, &w, &h); //doesn't need to draw on display
		tft.fillRect(x1, y1, w, h, SSD1306_BLACK);
	}
	
	Adafruit_SSD1306& tft;
	const GFXfont *font;
	uint16_t xpos;
	uint16_t ypos;
	bool debug;

	float lastPrintedVal = -999.99;
	char buff[8]; //last printed value
	float valDelta;

	int16_t min_width;//minimal field width of the output string (n_integer_digits + 1(.) + num_digits_after_decimal)
	int16_t num_digits_after_decimal; //length of fractional part
	float minVal;
	float maxVal;

	char unitName[8];
	bool unitsPrinted = false;
};

CaleSSD1306FloatWidget *wFWTest;


void setup(){
  Serial.begin(115200);
  
  Serial.print("SDA:"); Serial.println(SDA);
  Serial.print("SCL:"); Serial.println(SCL);

  Serial.println(sizeof(CaleSSD1306FloatWidget));

  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  int16_t xAdvanceTemp, yAdvanceTemp;
  GetXYAdvance('0', xAdvanceTemp, yAdvanceTemp);
  Serial.print("xAdvance:"); Serial.println(xAdvanceTemp);
  Serial.print("yAdvance:"); Serial.println(yAdvanceTemp);

  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  if(wFWTest) delete wFWTest;  
  wFWTest = new CaleSSD1306FloatWidget(display, 80, 35, NULL, 1, 2, "A");

}


size_t idx=0;
CaleSSD1306FloatWidget *wTemp;
CaleSSD1306FloatWidget *wStatus;
CaleSSD1306FloatWidget *wTimeM;
CaleSSD1306FloatWidget *wTimeS;
void CaleTest0(){
  display.drawLine(0, SCREEN_HEIGHT/2-1, SCREEN_WIDTH-1, SCREEN_HEIGHT/2-1, SSD1306_WHITE); //128x64 area

  /*
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(40, 10);
  display.setTextSize(2);
  display.print(F("W5"));
  */


  if(wTemp==NULL) wTemp = new CaleSSD1306FloatWidget(display, 0, 18, &FreeSerif12pt7b, 3, 1, "F");
  wTemp->update(123.4+float(idx)/2.0);

  if(wStatus==NULL) wStatus = new CaleSSD1306FloatWidget(display, 118, 12, &FreeSerif9pt7b, 1, 0, NULL);
  wStatus->update(1+float(idx)/10.0);

  if(wTimeM==NULL) wTimeM = new CaleSSD1306FloatWidget(display, 75, 30, &FreeSerif9pt7b, 2, 0, ":");//69 for m
  wTimeM->update(12.4+float(idx)/10.0);

  if(wTimeS==NULL) wTimeS = new CaleSSD1306FloatWidget(display, 98, 30, &FreeSerif9pt7b, 2, 0, NULL); //103 for s
  wTimeS->update( int(25.4+float(idx))%60 );


  display.display();
  ++idx;
  delay(1000);
}

void CaleTest1(){

  display.clearDisplay();

  unsigned long startMs = millis();

  display.setTextSize(1); // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(0, 20);
  display.print(F("W5"));

  display.setTextSize(2);

  display.setCursor(20, 20);
  display.print(F("W5"));

  display.setTextSize(1);

  /*display.setFont(&FreeMono9pt7b);
  display.setCursor(60, 20);
  display.print(F("W5"));

  display.setFont(&FreeMonoBold9pt7b);
  display.setCursor(40, 40);
  display.print(F("W5"));

  display.setFont(&FreeMonoBoldOblique9pt7b);
  display.setCursor(60, 40);
  display.print(F("W5"));

  display.setFont(&FreeMonoOblique9pt7b);
  display.setCursor(40, 60);
  display.print(F("W5"));*/

  unsigned long printMs = millis();

  display.display();

  Serial.print(printMs-startMs); Serial.print('\t');
  Serial.print(millis()-startMs); Serial.println("ms");
  delay(1000);

}


void CaleTest2(){

  display.clearDisplay();


  display.setTextSize(1); // Normal 1:1 pixel scale
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(0, 20);
  display.print(F("W="));
  display.println(2.3);
  display.display();
  delay(500);

  display.println(5.36);
  display.display();
  delay(500);

  display.setCursor(60, 30);
  display.write(37); //WriteASCII
  display.display();
  delay(500);

  display.startscrollright(0x00, 0x07);
  delay(4000);
  display.stopscroll();

  
  delay(1000);

}

void CaleTest3(){
  //https://lastminuteengineers.com/oled-display-esp32-tutorial/
  // Clear the buffer.
  display.clearDisplay();

  // Display Text
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,28);
  display.println("Hello world!");
  display.display();
  delay(2000);
  display.clearDisplay();

  // Display Inverted Text
  display.setTextColor(BLACK, WHITE); // 'inverted' text
  display.setCursor(0,28);
  display.println("Hello world!");
  display.display();
  delay(2000);
  display.clearDisplay();

  // Changing Font Size
  display.setTextColor(WHITE);
  display.setCursor(0,24);
  display.setTextSize(2);
  display.println("Hello!");
  display.display();
  delay(2000);
  display.clearDisplay();

  // Display Numbers
  display.setTextSize(1);
  display.setCursor(0,28);
  display.println(123456789);
  display.display();
  delay(2000);
  display.clearDisplay();

  // Specifying Base For Numbers
  display.setCursor(0,28);
  display.print("0x"); display.print(0xFF, HEX); 
  display.print("(HEX) = ");
  display.print(0xFF, DEC);
  display.println("(DEC)"); 
  display.display();
  delay(2000);
  display.clearDisplay();

  // Display ASCII Characters
  display.setCursor(0,24);
  display.setTextSize(2);
  display.write(3);
  display.display();
  delay(2000);
  display.clearDisplay();

  // Scroll full screen
  display.setCursor(0,0);
  display.setTextSize(1);
  display.println("Full");
  display.println("screen");
  display.println("scrolling!");
  display.display();
  display.startscrollright(0x00, 0x07);
  delay(2000);
  display.stopscroll();
  delay(1000);
  display.startscrollleft(0x00, 0x07);
  delay(2000);
  display.stopscroll();
  delay(1000);    
  display.startscrolldiagright(0x00, 0x07);
  delay(2000);
  display.startscrolldiagleft(0x00, 0x07);
  delay(2000);
  display.stopscroll();
  display.clearDisplay();

  // Scroll part of the screen
  display.setCursor(0,0);
  display.setTextSize(1);
  display.println("Scroll");
  display.println("some part");
  display.println("of the screen.");
  display.display();
  display.startscrollright(0x00, 0x00);
}



void CaleTest4(){
  //display.setFont(&FreeSerif12pt7b);
  //display.setFont();

  const int16_t PERC_X=80;
  const int16_t PERC_Y=35;
  const int16_t xAdvance = 6;//12;

  display.clearDisplay();
  display.setCursor(80, 35);
  display.setTextColor(SSD1306_WHITE);
  display.print(4);
  display.setTextColor(SSD1306_WHITE);
  display.print(0);
  display.print('%');
  //display.drawLine(PERC_X+xAdvance, PERC_Y+5, PERC_X+2*xAdvance, PERC_Y+5, SSD1306_WHITE);
  display.display();
  delay(200);

  /*display.setCursor(80, 35);
  display.setTextColor(SSD1306_BLACK);
  display.print(4);
  display.display();
  delay(500);*/

  display.setCursor(80, 35);
  display.setTextColor(SSD1306_WHITE, SSD1306_BLACK);
  display.print(5);
  display.display();
  delay(200);
}

//test current got grabbled due to either negative or some noise?
float currentA = -0.99;
void CaleTest5(){
  wFWTest->update(currentA);
  display.display();

  currentA+=0.01;
  if(currentA>0){
    currentA+=0.12;
    delay(200);
  }  
}

bool wInitialized = false;
int16_t xAdvance;
//profile position
const int16_t PROF_X=0;
const int16_t PROF_Y=22;
//percentage position:
const int16_t PERC_X=85;
const int16_t PERC_Y=18;
CaleSSD1306FloatWidget *wPerc;
CaleSSD1306FloatWidget *wVoltage;
CaleSSD1306FloatWidget *wCurrent;
CaleSSD1306FloatWidget *wTime;
CaleSSD1306FloatWidget *wMAH;
CaleSSD1306FloatWidget *wMAH2; //for debugging

void CaleTest6(){
  if(!wInitialized){
    wInitialized = true;
    auto &oled=display;
    
    oled.setFont(&FreeSerif12pt7b);
    int16_t yAdvanceTemp;
    GetXYAdvance('0', xAdvance, yAdvanceTemp);
    oled.setFont();
    oled.clearDisplay();

    oled.setCursor(0, 1);
    oled.print("MARIA"); 
    oled.setCursor(0, 11);
    oled.print(23);  oled.print('%');
    oled.setCursor(0, 21);
    oled.print(1456); oled.print('s');
    oled.setCursor(0, 31);
    oled.print(static_cast<uint16_t>(1353+0.5)); oled.print("mah");
    oled.display();

    if(wPerc) delete wPerc;
    //wPerc = new CaleSSD1306FloatWidget(oled, PERC_X-2*xAdvance+3, PERC_Y, &FreeSerif12pt7b, 2, 1, "%");
    if(wVoltage) delete wVoltage;
    wVoltage = new CaleSSD1306FloatWidget(oled, 60, 24, NULL, 1, 2, "V");
    if(wCurrent) delete wCurrent;
    wCurrent = new CaleSSD1306FloatWidget(oled, 92, 24, NULL, 1, 2, "A");
    if(wTime) delete wTime;
    wTime = new CaleSSD1306FloatWidget(oled, 75, 34, NULL, 4, 0, "s");
    if(wMAH) delete wMAH;
    wMAH = new CaleSSD1306FloatWidget(oled, 75, 44, NULL, 4, 0, "mah");
    wMAH2 = new CaleSSD1306FloatWidget(oled, 75, 64, NULL, 4, 0, "h"); //DONOTDISPLAY
  }

  if(wTime) wTime->update(1234);
  if(wPerc) wPerc->update(40.5);
  if(wVoltage) wVoltage->update(5.03);
  if(wCurrent) wCurrent->update(1.12);
  if(wMAH) wMAH->update(3512);
  display.display();

  delay(1000);  
}


void loop() {
  CaleTest0();
  //CaleTest6();

  delay(50);
}