#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

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

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET 4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define LOGO_HEIGHT 32
#define LOGO_WIDTH 128

#define line 12

/*
  static const unsigned char PROGMEM logo_bmp[] =
  {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x03, 0xC0, 0x78, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x80, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x07, 0x00, 0x0E, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0F, 0x80, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x0C, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x18, 0x00, 0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x30, 0x00, 0x01, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x70, 0x00, 0x00, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x0F, 0xC0, 0x60, 0x00, 0xF0, 0xE1, 0xE0, 0xF8, 0x1F, 0xC0, 0xF1, 0x87, 0x87, 0x83, 0xC0, 0xFC,
  0x3F, 0xF0, 0xE0, 0x01, 0xF8, 0x61, 0xE0, 0xF8, 0x7F, 0xE0, 0xF7, 0x8F, 0x87, 0x83, 0xE1, 0xCE,
  0x60, 0xF0, 0xE0, 0x01, 0xF8, 0x61, 0xE0, 0xF8, 0x41, 0xF0, 0xF7, 0x87, 0x87, 0x83, 0xE3, 0x86,
  0x00, 0xF8, 0xE0, 0x01, 0xF8, 0x71, 0xE0, 0xF8, 0x00, 0xF0, 0xFF, 0x87, 0x87, 0x83, 0xC3, 0x80,
  0x00, 0x78, 0xC0, 0x01, 0xF8, 0x71, 0xE0, 0xF8, 0x00, 0xF0, 0xF8, 0x07, 0x87, 0x83, 0xC3, 0xC0,
  0x00, 0xF8, 0xE0, 0x79, 0xF8, 0x71, 0xE0, 0xF8, 0x01, 0xF0, 0xF0, 0x07, 0x87, 0x83, 0xC3, 0xF8,
  0x0F, 0xF8, 0xE1, 0xFD, 0xF8, 0x71, 0xE0, 0xF8, 0x1F, 0xF0, 0xF0, 0x07, 0x87, 0x83, 0xC3, 0xFE,
  0x7C, 0x78, 0xE1, 0xCF, 0xF8, 0xF1, 0xE0, 0xF8, 0x78, 0xF0, 0xF0, 0x07, 0x8F, 0x83, 0xC1, 0xFF,
  0xF8, 0x78, 0xE3, 0xFF, 0xF8, 0xE1, 0xE0, 0xF8, 0xF0, 0xF0, 0xF0, 0x07, 0x8F, 0x83, 0xC0, 0x3F,
  0xF0, 0x78, 0xF3, 0xFE, 0xF9, 0xE1, 0xE0, 0xF8, 0xF0, 0xF0, 0xF0, 0x07, 0x87, 0x83, 0xC0, 0x0F,
  0xF0, 0xF8, 0x79, 0x8F, 0xF9, 0xE1, 0xF0, 0xF9, 0xF0, 0xF0, 0xF0, 0x07, 0x87, 0x87, 0xC2, 0x07,
  0xF8, 0xF8, 0x7D, 0x87, 0xFB, 0xC1, 0xFF, 0xF8, 0xF1, 0xF0, 0xF0, 0x0F, 0x87, 0xFF, 0xE3, 0x0E,
  0x7F, 0x7C, 0x3F, 0x00, 0x1F, 0x80, 0xFE, 0xF8, 0xFE, 0xFC, 0xF0, 0x0F, 0x83, 0xFB, 0xE3, 0xFC,
  0x1C, 0x38, 0x13, 0xFF, 0xF9, 0x80, 0x38, 0x00, 0x3C, 0x38, 0x00, 0x00, 0x00, 0xE0, 0x00, 0xF0,
  0x00, 0x00, 0x1F, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x03, 0xFF, 0xF8, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x7F, 0x1F, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x03, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x01, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0xFF, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xFF, 0xFF, 0xE0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
  };
*/

String OledLine1 = "";
String OledLine2 = "";
String OledLine3 = "";
String OledLine4 = "";
String OledLine5 = "";
String OledLine6 = "";

void oled_init()
{

  Serial.println("Init Oled");

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

  display.display();
  // delay(2000); // Pause for 2 seconds
  // displayText("Init Finished");
  OledLine5 = String("Init Finished");
  OledLine4 = String("Version: ") + myVersion;
  display.setRotation(2);
  ScrollInText();
}

void ShowSerialScreen(void)
{
  sprintf(tmpstr, "Ver: %d-%d %d", myVersion, parm[1], parm[64]);
  OledLine4 = String(tmpstr);
  OledLine5 = makeTimeStr();
  OledLine6 = String("Batt: ") + String(voltageNow / 10) + String("V");

  ScrollInText();

  Serial.println(OledLine4);
  Serial.println(OledLine5);
  Serial.println(OledLine6);
}

void showAddrScreen(void)
{
  char tmpstr[30];

  sprintf(tmpstr, "Addr %d Next = ADDR", parm[1]);
  OledLine4 = String(tmpstr);
  OledLine5 = String("        Save = BOOT");
  OledLine6 = String("        Exit = RESET");
  ScrollInText();
}

void displayText(String str)
{
  // if(Timers[OLEDHOLDTIMER] < 50) return;
  OledLine4 = String("");
  OledLine5 = str;
  OledLine6 = String("");
  ScrollInText();
  Serial.println(str);
}

/*
  void ShowSensorScreen(long serial, int dat, int ssi)
  {
  OledLine4 = String(serial);
  OledLine4 += "         ";
  OledLine4 += make_time_str();
  //OledLine4 += String(tme / 100);
  //OledLine4 += String(":");
  //OledLine4 += String(tme % 100);
  OledLine5 = "DATA: ";
  OledLine5 += String(dat);
  OledLine6 = "RSSI: ";
  OledLine6 += String(ssi);
  OledLine6 += " Q: ";
  OledLine6 += atQ_size();
  ScrollInText();
  //  Serial.println(OledLine4);
  //  Serial.println(OledLine5);
  //  Serial.println(OledLine6);
  }
*/

#define SCROLL 0 // leave 0 because scroll times out power relay
void ScrollInText(void)
{

  Timers[OLEDBLANKTIMER] = 0;

  display.setTextSize(1);
  display.setTextColor(WHITE);

  if (SCROLL == 1)
    for (int i = 1; i < 33; i++)
    {
      display.clearDisplay();
      display.setCursor(1, 1 - i);
      display.println(OledLine1);
      display.setCursor(1, (line * 1) - i);
      display.println(OledLine2);
      display.setCursor(1, (line * 2) - i);
      display.println(OledLine3);
      display.setCursor(1, (line * 3) - i);
      display.println(OledLine4);
      display.setCursor(1, (line * 4) - i);
      display.println(OledLine5);
      display.setCursor(1, (line * 5) - i);
      display.println(OledLine6);
      display.display();
      delay(20);
    }

  OledLine1 = OledLine4;
  OledLine2 = OledLine5;
  OledLine3 = OledLine6;

  display.clearDisplay();
  display.setCursor(1, 1);
  display.println(OledLine1);
  display.setCursor(1, (line * 1));
  display.println(OledLine2);
  display.setCursor(1, (line * 2));
  display.println(OledLine3);
  display.display();
}

void displayVoltage(void)
{
  int v;
  v = io_array[BATT_V]; // / 10;
  sprintf(tmpstr, "V:  %2d.%d", v / 10, v % 10);
  displayTextXY(tmpstr, 40, 0, 1, 1);
  v = motorGetCurrent(0);
  sprintf(tmpstr, "M0: %2d.%d", v / 10, v % 10);
  displayTextXY(tmpstr, 40, 10, 0, 1);
  v = motorGetCurrent(1);
  sprintf(tmpstr, "M1: %2d.%d", v / 10, v % 10);
  displayTextXY(tmpstr, 40, 20, 0, 1);
  v = motorGetPosition(0);
  sprintf(tmpstr, "%4d", v);
  displayTextXY(tmpstr, 100, 10, 0, 1);
  v = motorGetPosition(1);
  sprintf(tmpstr, "%4d", v);
  displayTextXY(tmpstr, 100, 20, 0, 1);
}

void displayTextXY(char *buff, uint8_t x, uint8_t y, bool clear, bool dispNow)
{
  if (Timers[OLEDHOLDTIMER] < 50)
    return;

  if (clear)
    display.clearDisplay();
  display.setCursor(x, y);
  display.println(buff);
  if (dispNow)
    display.display();
}

/*
  void drawbitmap(void) {
  display.clearDisplay();

  display.drawBitmap(
    (display.width()  - LOGO_WIDTH ) / 2,
    (display.height() - LOGO_HEIGHT) / 2,
    logo_bmp, LOGO_WIDTH, LOGO_HEIGHT, 1);
  display.display();
  delay(1000);
  }
*/

//Draws A Loading Bar While Updating From OTA
void drawLoadingBar(int current, int total)
{
  static int lastProgress = -1; // Track the last progress value

  int progress = static_cast<int>(static_cast<float>(current) / total * 100);
  progress = constrain(progress, 0, 100);

  // Only update the display if progress has changed
  if (progress != lastProgress)
  {
    const int TEXT_X = (SCREEN_WIDTH - 17 * 6) / 2;
    int rectWidth = map(progress, 0, 100, 0, SCREEN_WIDTH);
    Serial.println(progress);
    //digitalWrite(HEARTBEATLED,!digitalRead(HEARTBEATLED));
    display.clearDisplay();

    // Draw the progress bar
    display.fillRect(0, 12, rectWidth, 20, WHITE);

    // Draw "Downloading" text
    display.setTextColor(WHITE);
    display.setCursor(TEXT_X, 0);
    display.print("Downloading ");
    display.print(progress);
    display.print(" %");

    // Update the display
    display.display();




    lastProgress = progress;
  }
}