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

RTC_DS3231 rtc;

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C

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

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

float sensorValues[][tableSize] = {
  {0, 1, 2, 3, 4, 5},
  {0, 1, 2, 3, 4, 5},
  {0, 1, 2, 3, 4, 5},
  {0, 1, 2, 3, 4, 5},
};

int tableData[][tableSize] = {
  {10, 12, 14, 16, 18, 20},
  {0, 20, 40, 60, 80, 100},
  {0, 8, 16, 24, 32, 40},
  {100, 300, 500, 700, 900, 1100},
};

const char* sensorLabels[] = {"LAMBDA", "ETHANOL", "BOOST", "EGT"};
const char* sensorSymb[] = {"afr", "  %", "psi", "  c"};

const int buttonPin = 2;
int currentSensor = 0;
bool buttonState = HIGH;
bool lastButtonState = HIGH;

void setup() {
  rtc.begin();
  display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  
  pinMode(buttonPin, INPUT_PULLUP);
}

void loop() {
  buttonState = digitalRead(buttonPin);
  display.clearDisplay();
  
  if (buttonState != lastButtonState) {
    if (buttonState == LOW) {
      currentSensor = (currentSensor + 1) % (numSensors + 1); // Including the time/date screen
    }
  }

  lastButtonState = buttonState;

  if (currentSensor < numSensors) {
    int sensorReading = analogRead(sensorPins[currentSensor]);
    float voltage = (sensorReading / 1023.0) * 5.0;
    float interpolatedData = interpolate(sensorValues[currentSensor], tableData[currentSensor], tableSize, voltage);

    display.setCursor(5, 5);
    display.setTextSize(2);
    display.print(sensorLabels[currentSensor]);

    display.setCursor(SCREEN_WIDTH - 110, SCREEN_HEIGHT / 2 - 10);
    display.setTextSize(3);
    display.print(interpolatedData, 1);
    display.setCursor(88, SCREEN_HEIGHT - 20);
    display.setTextSize(2);
    display.print(sensorSymb[currentSensor]);

    display.setCursor(10, SCREEN_HEIGHT - 15);
    display.setTextSize(1);
    display.print(voltage, 2);
    display.print("v");
  } else {
    // Display time and date
    DateTime now = rtc.now();
    
    
    // Center the date
    display.setTextSize(2);
    display.setCursor(5, 35);
    display.println(now.toString("MM.DD.YYYY"));

    // Center the time
    display.setTextSize(3);
    display.setCursor(SCREEN_WIDTH - 103, 10);
    
    int hour = now.hour();
    if (hour > 12) {
      hour -= 12;
    }
    display.print(hour);
    display.print(":");
    if (now.minute() < 10) {
      display.print("0");
    }
    display.print(now.minute());
    if (hour < 12) {
      display.print(""); //"AM" - Display after time digits
    } else {
      display.print(""); //"PM" - Display after time digits
    }
  }

  display.display();
  delay(100);
}

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] and x <= xValues[i + 1]) {
        float slope = (yValues[i + 1] - yValues[i]) / (xValues[i + 1] - xValues[i]);
        return yValues[i] + slope * (x - xValues[i]);
      }
    }
  }
}