#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <RTClib.h>
#include <EEPROM.h> // Include EEPROM library

RTC_DS3231 rtc;

#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);

const int buttonPinForward = 2;
const int buttonPinBackward = 3;
const int buttonPinDateTime = 4; // New button for switching display modes

const int tableSize = 6;
const int numSensors = 5; // Including one for PWM input

const char* sensorNames[] = {"* LAMBDA", "* TEMP", "* OIL", "* BOOST", "* ETHANOL"};

const char* sensorSymbols[] = {"afr", "f  ", "psi", "psi", "%  "};

float sensorValues[numSensors][tableSize] = {
  {0.0, 1.0, 2.0, 3.0, 4.0, 5.0}, // Input voltage A0
  {0.0, 1.0, 2.0, 3.0, 4.0, 5.0}, // Input voltage A1
  {0.5, 1.3, 2.1, 2.9, 3.7, 4.5}, // Input voltage A2
  {0.39, .849, 1.308, 2.455, 3.603, 4.75}, // Input voltage A3
  {50, 70, 90, 110, 130, 150}  // PWM input "Hz"
};

int tableData[numSensors][tableSize] = {
  {10, 12, 14, 16, 18, 20},   // Interpolated output value A0 
  {-40, -20, 0, 20, 40, 80},   // Interpolated output value A1
  {0, 20, 40, 60, 80, 100},   // Interpolated output value A2
  {-23.968, -12.2 ,.004, 14.51, 29.01, 43.52},  // Interpolated output value A3
  {0, 20, 40, 60, 80, 100}  // PWM input interpolation (assuming proportional)
};

int currentInputIndex = 0;
bool buttonPressed = false;
bool displaySensorData = false; // Variable to track the display mode
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50; // Debounce time in milliseconds

void setup() {
  pinMode(buttonPinForward, INPUT_PULLUP);
  pinMode(buttonPinBackward, INPUT_PULLUP);
  pinMode(buttonPinDateTime, INPUT_PULLUP); // Set the new button as input with pull-up resistor
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(5, 15);
  display.println("Modded by @jaymangsr");
  display.display();
  delay(1000);
  display.clearDisplay();
  if (!rtc.begin()) {
    display.println("RTC not found");
    display.display();
    while (1);
  }

  if (rtc.lostPower()) {
    display.println("RTC lost power, setting time...");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  }
  
  // Load display state from EEPROM
  loadDisplayState();
}

void loop() {
  int inputValue;
  if (currentInputIndex == numSensors - 1) {
    inputValue = analogRead(A6); // Read PWM input for the last sensor
  } else {
    inputValue = analogRead(currentInputIndex);
  }
  float voltage = float(inputValue) * 5.0 / 1023.0;

  int buttonForwardState = digitalRead(buttonPinForward);
  int buttonBackwardState = digitalRead(buttonPinBackward);
  int buttonDateTimeState = digitalRead(buttonPinDateTime);

  if ((buttonForwardState == LOW || buttonBackwardState == LOW || buttonDateTimeState == LOW) &&
      (millis() - lastDebounceTime > debounceDelay)) {
    lastDebounceTime = millis();

    if (!buttonPressed) {
      buttonPressed = true;

      if (buttonForwardState == LOW) {
        currentInputIndex = (currentInputIndex + 1) % numSensors;
      }
      if (buttonBackwardState == LOW) {
        currentInputIndex = (currentInputIndex - 1 + numSensors) % numSensors;
      }
      if (buttonDateTimeState == LOW) {
        displaySensorData = !displaySensorData;
      }
      
      // Save display state to EEPROM
      saveDisplayState();
    }
  } else {
    buttonPressed = false;
  }

  if (displaySensorData) {
    float outputValue;
    if (currentInputIndex == numSensors - 1) {
      // No need for interpolation for PWM input, just use the voltage directly
      outputValue = voltage;
    } else {
      outputValue = interpolate(sensorValues[currentInputIndex], tableData[currentInputIndex], tableSize, voltage);
    }

    display.clearDisplay();

    display.setTextSize(2); // Sensor name
    display.setCursor(5, 5);
    display.println(sensorNames[currentInputIndex]);

    display.setTextSize(3); // Interpolated value 
    display.setCursor(18, 24);
    display.print(outputValue, 1);

    display.setTextSize(2); // Sensor symbol
    display.setCursor(90, 45);
    display.println(sensorSymbols[currentInputIndex]);

    display.setTextSize(0); // Input voltage or PWM value
    display.setCursor(5, 52);
    display.print(currentInputIndex == numSensors - 1 ? inputValue : voltage, 2);
    display.println(currentInputIndex == numSensors - 1 ? "" : " V");

    display.display();
  } else {
    // Display date
    display.clearDisplay();
    DateTime now = rtc.now();
    display.setTextSize(2);
    display.setCursor(5, 42);
    if (now.month() < 10) {
      display.print(" "); // Leading zero for month
    }
    display.print(now.month(), DEC);
    display.print('.');
    if (now.day() < 10) {
      display.print("0"); // Leading zero for day
    }
    display.print(now.day(), DEC);
    display.print('.');
    display.println(now.year(), DEC);

    // Convert and display time as 12-hour format
    display.setTextSize(3);
    display.setCursor(24, 8);
    int hour = now.hour();
    if (hour == 0) {
      display.print("12:");
    } else if (hour <= 12) {
      display.print(hour);
      display.print(':');
    } else {
      display.print(hour - 12);
      display.print(':');
    }

    if (now.minute() < 10) {
      display.print("0"); // Leading zero for minute
    }
    display.print(now.minute(), DEC);

    // Display AM or PM
    display.setTextSize(1);
    display.setCursor(5, 10);
    if (hour < 12) {
      display.print("AM"); //Am
    } else {
      display.print("PM"); //Pm
    }

    display.display();
  }
}

float interpolate(float xValues[], int yValues[], int size, float x) {
  if (x <= xValues[0]) {
    return yValues[0];
  } else if (x >= xValues[size - 1]) {
    return yValues[size - 1];
  } else {
    for (int i = 0; i < size - 1; i++) {
      if (x >= xValues[i] && x <= xValues[i + 1]) {
        float slope = float(yValues[i + 1] - yValues[i]) / float(xValues[i + 1] - xValues[i]);
        return yValues[i] + slope * (x - xValues[i]);
      }
    }
  }
  return 0.0;
}

// Function to save display state to EEPROM
void saveDisplayState() {
  EEPROM.update(0, currentInputIndex);
  EEPROM.update(1, displaySensorData ? 1 : 0);
}

// Function to load display state from EEPROM
void loadDisplayState() {
  currentInputIndex = EEPROM.read(0);
  displaySensorData = EEPROM.read(1) == 1;
}