// CHUMKI_LFR_V1.ino
#include <Wire.h>
#include <OneButton.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

/* ================= OLED ================= */
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define SCREEN_ADDRESS 0x3C

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

/* ================= LED ================= */
#define LED 13

/* ================= MOTOR PINS ================= */
#define RIGHT_FWD 7
#define RIGHT_BWD 6
#define LEFT_FWD 8
#define LEFT_BWD 12
#define RIGHT_PWM 9
#define LEFT_PWM 10

/* ================= MULTIPLEXER ================= */
#define S0 2
#define S1 3
#define S2 4
#define S3 5
#define SIG_PIN A0

/* ================= BUTTONS ================= */
#define BTN_UP     A2
#define BTN_DOWN   11
#define BTN_SELECT A3
#define BTN_CANCEL A1

/* ------------------- ONEBUTTON SETUP ------------------- */
OneButton btn_up(BTN_UP, true); 
OneButton btn_down(BTN_DOWN, true);
OneButton btn_select(BTN_SELECT, true);
OneButton btn_cancel(BTN_CANCEL, true);

/* ================= BUTTON STATES FLAG ================= */
bool btn_up_clicked, btn_up_doubleclicked, btn_up_longpressed, btn_up_longpressing, btn_up_longpressstopped;
bool btn_down_clicked, btn_down_doubleclicked, btn_down_longpressed, btn_down_longpressing, btn_down_longpressstopped;
bool btn_select_clicked, btn_select_doubleclicked, btn_select_longpressed, btn_select_longpressing, btn_select_longpressstopped;
bool btn_cancel_clicked, btn_cancel_doubleclicked, btn_cancel_longpressed, btn_cancel_longpressing, btn_cancel_longpressstopped;



uint8_t settings_Pos = 1;  // Which setting is selected (1=Speed, 2=Kp, 3=Kd)
bool editMode = false;     // Are we editing a value?
bool settingsNeedsRedraw = true;

// Settings values
int speedValue = 150;
float kpValue = 2.5;
float kdValue = 1.2;



/* ================= PAGES ================= */
enum pageType { 
  ROOT_PAGE, 
  MENU_PAGE, 
  CALIBRATION_PAGE, 
  TEST_CAR_PAGE, 
  SETTINGS_PAGE, 
  SENSOR_PAGE 
};

pageType currPage = ROOT_PAGE;
uint8_t set_Pos = 1;

/* ------------------- SETUP ------------------- */
void setup() {
  Serial.begin(9600);
  Wire.begin();

  // Motor pins
  pinMode(RIGHT_FWD, OUTPUT);
  pinMode(RIGHT_BWD, OUTPUT);
  pinMode(LEFT_FWD, OUTPUT);
  pinMode(LEFT_BWD, OUTPUT);
  pinMode(RIGHT_PWM, OUTPUT);
  pinMode(LEFT_PWM, OUTPUT);

  // Multiplexer pins
  pinMode(S0, OUTPUT);
  pinMode(S1, OUTPUT);
  pinMode(S2, OUTPUT);
  pinMode(S3, OUTPUT);
  pinMode(SIG_PIN, INPUT);

  // Button pins
  pinMode(BTN_UP, INPUT_PULLUP);
  pinMode(BTN_DOWN, INPUT_PULLUP);
  pinMode(BTN_SELECT, INPUT_PULLUP);
  pinMode(BTN_CANCEL, INPUT_PULLUP);
  btn_setup();

  // OLED init
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("OLED not found"));
    while (true);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println(F("Chumki LFR v1"));
  display.println(F("Initializing..."));
  display.display();
  delay(2000);

  Serial.println(F("System Ready"));
  resetButtonStates();
}

/* ------------------- LOOP ------------------- */
void loop() {
  btn_up.tick();
  btn_down.tick();
  btn_select.tick();
  btn_cancel.tick();
  delay(5);

  switch (currPage) {
    case ROOT_PAGE: root_page(); break;
    case MENU_PAGE: menu_page(); break;
    case CALIBRATION_PAGE: calibration_page(); break;
    case TEST_CAR_PAGE: test_car_page(); break;
    case SETTINGS_PAGE: settings_page(); break;
    case SENSOR_PAGE: sensor_page(); break;
  }
}