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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define BTN_SENSOR 2  // Button to switch sensors
#define BTN_MODE 3    // Button to switch display mode

const int sensorPins[4] = {A0, A1, A2, A3};
const char* sensorNames[4] = {"OIL PRESSURE", "FUEL PRESSURE", "BOOST", "AIR FUEL"};
const char* sensorSymbols[4] = {"psi", "psi", "psi", "afr"};
float sensorTable[4][2] = { {0.0, 100.0}, {0.0, 100.0}, {-12.0, 43.5}, {10.0, 20.0} }; // Min & Max Values

int currentSensor = 0;
int displayMode = 0; // 0 = Single, 1 = Dual, 2 = All

void setup() {
    pinMode(BTN_SENSOR, INPUT_PULLUP);
    pinMode(BTN_MODE, INPUT_PULLUP);
    
    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address may need adjustment
        for(;;);
    }
    display.clearDisplay();
    display.display();
}

float readSensor(int index) {
    int raw = analogRead(sensorPins[index]);
    float voltage = raw * (5.0 / 1023.0);
    float value = mapFloat(voltage, 0.0, 5.0, sensorTable[index][0], sensorTable[index][1]);
    return value;
}

void loop() {
    if (digitalRead(BTN_SENSOR) == LOW) {
        currentSensor = (currentSensor + 1) % 4;
        delay(200);
    }
    if (digitalRead(BTN_MODE) == LOW) {
        displayMode = (displayMode + 1) % 3;
        delay(200);
    }
    updateDisplay();
}

void updateDisplay() {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    
    if (displayMode == 0) {
        displaySingleSensor(currentSensor);
    } else if (displayMode == 1) {
        displayDualSensor();
    } else {
        displayAllSensors();
    }
    display.display();
}

void displaySingleSensor(int index) {
    display.setCursor(0, 0);
    display.print(sensorNames[index]);
    display.print(": ");
    display.print(readSensor(index));
    display.print(sensorSymbols[index]);
}

void displayDualSensor() {
    int secondSensor = (currentSensor + 1) % 4;
    display.setCursor(0, 0);
    display.print(sensorNames[currentSensor]);
    display.print(": ");
    display.print(readSensor(currentSensor));
    display.print(sensorSymbols[currentSensor]);
    display.setCursor(0, 10);
    display.print(sensorNames[secondSensor]);
    display.print(": ");
    display.print(readSensor(secondSensor));
    display.print(sensorSymbols[secondSensor]);
}

void displayAllSensors() {
    for (int i = 0; i < 4; i++) {
        display.setCursor(0, i * 10);
        display.print(sensorNames[i]);
        display.print(": ");
        display.print(readSensor(i));
        display.print(sensorSymbols[i]);
    }
}

float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}