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

// Розміри дисплея
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Кнопки
#define BTN1 6   // Next / Select
#define BTN2 7   // Down
#define BTN3 8   // Back

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// ====================== Peugeot Logo ======================
const unsigned char peugeot_logo [] PROGMEM = {
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xff, 0xff, 0xff, 0xff, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x70, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x02, 0x9c, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0xef, 0x4a, 0xdc, 0x92, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x8e, 0x30, 0x8c, 0x10, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x1f, 0xfc, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x18, 0x1f, 0x00, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x38, 0x07, 0x80, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0x78, 0x41, 0xe0, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x00, 0xf1, 0xa1, 0xe0, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x07, 0x80, 0x20, 0x78, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x08, 0x0e, 0x00, 0x00, 0x18, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x1e, 0x00, 0x00, 0x0c, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x3e, 0x00, 0x03, 0x0c, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x3e, 0x00, 0x03, 0x8c, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x30, 0x44, 0x01, 0xc0, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x48, 0x01, 0xc8, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0xd8, 0x00, 0xc0, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x03, 0xb0, 0x00, 0x70, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x06, 0x71, 0x00, 0x60, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x61, 0x00, 0x40, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x80, 0xc3, 0x00, 0x41, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0x83, 0x00, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc1, 0x82, 0x00, 0xc3, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x86, 0x01, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x20, 0xc6, 0x02, 0x06, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x62, 0x04, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x18, 0x32, 0x68, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x19, 0x60, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x09, 0x20, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0xc0, 0x03, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xe0, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x30, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0c, 0x30, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

// === МОЧІ СПРАЙТИ (32x32) ===
const unsigned char mochihappy [] PROGMEM = {
  B00000000,B00000000,B00000000,B00000000,
  B00011110,B00000000,B01100011,B00000000,
  B00100001,B00000000,B01011101,B00000000,
  B01000000,B10000000,B01011101,B00000000,
  B01000000,B10000000,B01100011,B00000000,
  B00100001,B00000000,B00011110,B00000000
};

const unsigned char mochisquint [] PROGMEM = {
  B00000000,B00000000,B00000000,B00000000,
  B00011110,B00000000,B01011011,B00000000,
  B00100101,B00000000,B01011011,B00000000,
  B01000001,B00000000,B01011011,B00000000,
  B00100101,B00000000,B01011011,B00000000,
  B00011110,B00000000,B00000000,B00000000
};

const unsigned char mochisleepy [] PROGMEM = {
  B00000000,B00000000,B00000000,B00000000,
  B00011110,B00000000,B01000001,B00000000,
  B00100101,B00000000,B01000001,B00000000,
  B01000001,B00000000,B01000001,B00000000,
  B00100101,B00000000,B01000001,B00000000,
  B00011110,B00000000,B00000000,B00000000
};

const unsigned char mochiblink [] PROGMEM = {
  B00000000,B00000000,B00000000,B00000000,
  B00011110,B00000000,B01111111,B00000000,
  B01111111,B10000000,B01111111,B00000000,
  B01111111,B10000000,B01111111,B00000000,
  B00011110,B00000000,B00000000,B00000000,
  B00000000,B00000000,B00000000,B00000000
};

// Debounce
unsigned long lastDebounceTime1 = 0, lastDebounceTime2 = 0, lastDebounceTime3 = 0;
const unsigned long debounceDelay = 50;
bool lastButtonState1 = HIGH, lastButtonState2 = HIGH, lastButtonState3 = HIGH;

// Меню
const int menuCount = 4;
int menuIndex = 0;
const char* menuItems[menuCount] = {"zahalna infa", "palevo", "pomylca obd", "Mochi"};
bool inZahalnaMenu = false, inPalevoMenu = false, inPomylcaMenu = false, inMochiMenu = false;

// Дані "Palevo"
int bak1 = 0, bak2 = 10, zalyshok = 120;

// Mochi Animation
unsigned long lastBlinkTime = 0, lastEmotionTime = 0;
bool showEyes = true;
int emotionState = 0; // 0 — happy, 1 — squint, 2 — sleepy

// === Ініціалізація ===
void setup() {
  Wire.begin(4, 5);
  pinMode(BTN1, INPUT_PULLUP);
  pinMode(BTN2, INPUT_PULLUP);
  pinMode(BTN3, INPUT_PULLUP);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    for (;;);
  }

  // Показати логотип Peugeot по центру
  display.clearDisplay();
  display.drawBitmap(
    (SCREEN_WIDTH - 128) / 2,         // по центру по X
    (SCREEN_HEIGHT - 32) / 2,         // по центру по Y
    peugeot_logo,                     // масив бітів лого
    128,                              // ширина лого
    32,                               // висота лого
    SSD1306_WHITE
  );
  display.display();
  delay(2500);

  drawMenu(); // Показати меню після логотипу
}

// === Читання кнопок з debounce ===
bool readButton(int pin, bool &lastState, unsigned long &lastTime) {
  bool reading = digitalRead(pin);
  if (reading != lastState) lastTime = millis();
  if ((millis() - lastTime) > debounceDelay) {
    if (reading == LOW) {
      lastState = reading;
      return true;
    }
  }
  lastState = reading;
  return false;
}

// === Основний цикл ===
void loop() {
  if (inMochiMenu) {
    if (readButton(BTN3, lastButtonState3, lastDebounceTime3)) {
      inMochiMenu = false;
      menuIndex = 0;
      drawMenu();
      delay(200);
      return;
    }
    updateMochi();
    delay(100);
    return;
  }

  if (inZahalnaMenu || inPalevoMenu || inPomylcaMenu) {
    if (readButton(BTN3, lastButtonState3, lastDebounceTime3)) {
      inZahalnaMenu = inPalevoMenu = inPomylcaMenu = false;
      menuIndex = 0;
      drawMenu();
      delay(200);
      return;
    }
  }

  if (inPalevoMenu) {
    static unsigned long btn1PressTime = 0;
    if (digitalRead(BTN1) == LOW) {
      if (btn1PressTime == 0) btn1PressTime = millis();
    } else if (btn1PressTime != 0) {
      if (millis() - btn1PressTime > 1000) {
        bak1 = bak2 = zalyshok = 0;
        displayAction("Reset done");
      }
      btn1PressTime = 0;
      drawPalevoMenu();
    }
    return;
  }

  if (readButton(BTN1, lastButtonState1, lastDebounceTime1)) {
    menuIndex = (menuIndex + 1) % menuCount;
    drawMenu();
  }

  if (readButton(BTN2, lastButtonState2, lastDebounceTime2)) {
    menuIndex = (menuIndex - 1 + menuCount) % menuCount;
    drawMenu();
  }

  if (readButton(BTN3, lastButtonState3, lastDebounceTime3)) {
    selectMenuItem();
  }
}

// === Меню ===
void drawMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("=== MENU ===");
  
  for (int i = 0; i < menuCount; i++) {
    int y = 15 + i * 12;
    if (i == menuIndex) {
      display.fillRect(0, y - 1, SCREEN_WIDTH, 10, SSD1306_WHITE);
      display.setTextColor(SSD1306_BLACK);
    } else {
      display.setTextColor(SSD1306_WHITE);
    }
    display.setCursor(2, y);
    display.println(menuItems[i]);
  }
  display.display();
}

void selectMenuItem() {
  inZahalnaMenu = inPalevoMenu = inPomylcaMenu = inMochiMenu = false;
  switch (menuIndex) {
    case 0: drawZahalnaMenu(); inZahalnaMenu = true; break;
    case 1: drawPalevoMenu(); inPalevoMenu = true; break;
    case 2: drawPomylcaMenu(); inPomylcaMenu = true; break;
    case 3: drawMochiFace(); inMochiMenu = true; break;
  }
}

void drawZahalnaMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("=== Zahalna Infa ===");
  display.setCursor(0, 15); display.println("Speed:       65 km/h");
  display.setCursor(0, 25); display.println("Litry:       7./100km");
  display.setCursor(0, 35); display.println("Power:      12.4 V");
  display.setCursor(0, 55); display.println("BTN3: Back");
  display.display();
}

void drawPalevoMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0); display.println("=== Palevo ===");
  display.setCursor(0, 15); display.print("Bak1: "); display.print(bak1); display.println(" km");
  display.setCursor(0, 25); display.print("Bak2: "); display.print(bak2); display.println(" km");
  display.setCursor(0, 35); display.print("Zalyshok: "); display.print(zalyshok); display.println(" km");
  display.setCursor(0, 55); display.println("BTN1: Reset"); display.println("BTN3: Back");
  display.display();
}

void drawPomylcaMenu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0); display.println("=== Pomylca OBD ===");
  display.setCursor(0, 15); display.println("Code:  0000");
  display.setCursor(0, 25); display.println("Name:  LATALIZATOR");
  display.setCursor(0, 35); display.println("Info:   P0130");
  display.setCursor(0, 55); display.println("BTN3: Back");
  display.display();
}

void displayAction(const char* msg) {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(msg);
  display.display();
  delay(1500);
  drawMenu();
}

// === Mochi ===
void drawMochiFace() {
  display.clearDisplay();
  const unsigned char* sprite;

  if (!showEyes) {
    sprite = mochiblink;   // кліпання
  } else {
    switch (emotionState) {
      case 0: sprite = mochihappy; break;
      case 1: sprite = mochisquint; break;
      default: sprite = mochisleepy; break;
    }
  }

  display.drawBitmap((SCREEN_WIDTH - 32) / 2, (SCREEN_HEIGHT - 32) / 2 - 5, sprite, 32, 32, SSD1306_WHITE);
  display.setTextSize(1);
  display.setCursor(0, SCREEN_HEIGHT - 9);
  display.println("BTN3: Back");
  display.display();
}

void updateMochi() {
  unsigned long now = millis();

  if (now - lastBlinkTime > 3000) {
    showEyes = !showEyes;
    lastBlinkTime = now;
  }

  if (now - lastEmotionTime > 5000) {
    emotionState = (emotionState + 1) % 3;
    lastEmotionTime = now;
  }

  drawMochiFace();
}