#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <RTClib.h>
#include <EEPROM.h>

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;

const int tableSize = 6;
const int sensorPins[] = {A0, A1, A2, A3};
const int numSensors = sizeof(sensorPins) / sizeof(sensorPins[0]);

const char* sensorNames[] = {"* LAMBDA", "* ETHANOL", "* OIL", "* BOOST"};
const char* sensorSymbols[] = {"afr", "% ", "psi", "psi"};

float sensorValues[numSensors][tableSize] = {
    {0.0, 1.0, 2.0, 3.0, 4.0, 5.0},
    {0.0, 1.0, 2.0, 3.0, 4.0, 5.0},
    {0.5, 1.3, 2.1, 2.9, 3.7, 4.5},
    {0.0, 1.0, 2.0, 3.0, 4.0, 5.0},
};

int tableData[numSensors][tableSize] = {
    {10, 12, 14, 16, 18, 20},
    {0, 20, 40, 60, 80, 100},
    {0, 20, 40, 60, 80, 100},
    {-12, 0, 12, 24, 36, 48},
};

int currentInputIndex = 0;
bool buttonPressed = false;
bool displaySensorData = false;

int lastDisplayState = 0;

void saveStateToEEPROM() {
  EEPROM.put(0, currentInputIndex);
  EEPROM.put(sizeof(int), displaySensorData);
  EEPROM.put(2 * sizeof(int), lastDisplayState);
}

void readStateFromEEPROM() {
  EEPROM.get(0, currentInputIndex);
  EEPROM.get(sizeof(int), displaySensorData);
  EEPROM.get(2 * sizeof(int), lastDisplayState);
}

void setup() {
  pinMode(buttonPinForward, INPUT_PULLUP);
  pinMode(buttonPinBackward, INPUT_PULLUP);
  pinMode(buttonPinDateTime, INPUT_PULLUP);
  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);
  }

  // Check if the RTC lost power and if so, set the RTC to the date & time this sketch was compiled
  if (rtc.lostPower()) {
    display.println("RTC lost power, setting the time!");
    rtc.adjust(DateTime(F(__DATE__), F(__TIME__)));
  }

  // Read the state from EEPROM and restore it
  readStateFromEEPROM();
}

void loop() {
  int inputValue = analogRead(sensorPins[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) {
    if (!buttonPressed) {
      buttonPressed = true;

      if (buttonForwardState == LOW) {
        currentInputIndex = (currentInputIndex + 1) % numSensors;
      }
      if (buttonBackwardState == LOW) {
        currentInputIndex = (currentInputIndex - 1 + numSensors) % numSensors;
      }
    }
  } else {
    buttonPressed = false;
  }

  if (buttonDateTimeState == LOW && !buttonPressed) {
    buttonPressed = true;
    lastDisplayState = 1 - lastDisplayState; // Toggle between 0 and 1
    saveStateToEEPROM();
  }

  if (lastDisplayState == 1) {
    // Display sensor data
    float outputValue = interpolate(sensorValues[currentInputIndex], tableData[currentInputIndex], tableSize, voltage);
    display.clearDisplay();

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

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

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

    display.setTextSize(0);
    display.setCursor(5, 52);
    display.print(voltage, 2);
    display.println(" v");

    display.display();
  } else {
    // Display date and time
    display.clearDisplay();
    DateTime now = rtc.now();
    display.setTextSize(2);
    display.setCursor(5, 42);
    display.print(now.month(), DEC);
    display.print('.');
    display.print(now.day(), DEC);
    display.print('.');
    display.println(now.year(), DEC);

    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(':');
    }

    int minute = now.minute();
    if (minute < 10) {
      display.print("0");
    }
    display.print(minute);

    display.setTextSize(1);
    display.setCursor(5, 10);
    if (hour < 12) {
      display.print("AM");
    } else {
      display.print("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;
}