#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>
#include <OneWire.h>
#include <DallasTemperature.h>
#include <esp_task_wdt.h>

#define i2c_Address 0x3C // Change this if your OLED has a different I2C address
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
#define ONE_WIRE_BUS1 2 // D2 pin for the first DS18B20 sensor
#define ONE_WIRE_BUS2 7 // D7 pin for the second DS18B20 sensor
#define FAN1_PIN 5      // D3 pin for FAN1
#define FAN2_PIN 6      // D4 pin for FAN2
#define OVERRIDE_FAN1_PIN 9 // D5 pin for overriding FAN1 (pulled to ground to activate)
#define OVERRIDE_FAN2_PIN 10 // D6 pin for overriding FAN2 (pulled to ground to activate)

Adafruit_SH1106G display = Adafruit_SH1106G(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
OneWire oneWire1(ONE_WIRE_BUS1);
OneWire oneWire2(ONE_WIRE_BUS2);
DallasTemperature sensors1(&oneWire1);
DallasTemperature sensors2(&oneWire2);

bool fan1Active = false;
bool fan2Active = false;

float pressure = 0.0;
float temperature = 0.0;

int M3200address = 0x28;   // 0x28, 0x36 or 0x46, depending on the sensor.
float maxPressure = 100;   // pressure in PSI for this sensor, 100, 250, 500, ... 10k.
byte status;

unsigned long pressureMillis = 0;
const int pressureTiming = 500; // 500ms every data acquisition

void fanControl(float averageTemperature) {
  if (averageTemperature >= 99.0 && !fan1Active) {
    digitalWrite(FAN1_PIN, HIGH); // Turn ON FAN1
    fan1Active = true;
  }

  if (averageTemperature >= 106.0 && !fan2Active) {
    digitalWrite(FAN2_PIN, HIGH); // Turn ON FAN2
    fan2Active = true;
  }

  if (averageTemperature <= 95.0) {
    digitalWrite(FAN1_PIN, LOW); // Turn OFF FAN1
    fan1Active = false;
  }

  if (averageTemperature <= 99.0) {
    digitalWrite(FAN2_PIN, LOW); // Turn OFF FAN2
    fan2Active = false;
  }
}

void displayError(const char *message) {
  display.setCursor(0, 20);
  display.print(message);
}

void setup() {
  Serial.begin(9600);

  pinMode(LED_BUILTIN, OUTPUT);

  pinMode(FAN1_PIN, OUTPUT);
  pinMode(FAN2_PIN, OUTPUT);
  pinMode(OVERRIDE_FAN1_PIN, INPUT_PULLUP);
  pinMode(OVERRIDE_FAN2_PIN, INPUT_PULLUP);

  if (!display.begin(i2c_Address)) {
    Serial.println(F("SH110X allocation failed"));
    for (;;);
  }

  sensors1.begin(); // Initialize the first DS18B20 sensor
  sensors2.begin(); // Initialize the second DS18B20 sensor

  display.display();
  delay(2000);
  display.clearDisplay();
}

void loop() {
  digitalWrite(LED_BUILTIN, HIGH);
  
  display.clearDisplay();

  // Set text size and color
  display.setTextSize(1);
  display.setTextColor(SH110X_WHITE);

  // Read temperatures from both DS18B20 sensors
  sensors1.requestTemperatures();
  sensors2.requestTemperatures();

  float temperatureC1 = sensors1.getTempCByIndex(0);
  float temperatureC2 = sensors2.getTempCByIndex(0);

  // Validate temperature readings
  if (isValidTemperature(temperatureC1)) {
    // Display temperatures on the screen in Celsius with one decimal place
    display.setCursor(0, 0);
    display.print("Clt1 Temp: ");
    display.print(temperatureC1, 1);
    display.print(" C");
  } else {
    displayError("Clt1 Temp: Invalid");
  }

  if (isValidTemperature(temperatureC2)) {
    display.setCursor(0, 10);
    display.print("Clt2 Temp: ");
    display.print(temperatureC2, 1);
    display.print(" C");
  } else {
    displayError("Clt2 Temp: Invalid");
  }

  // Check for override inputs
  bool overrideFan1 = digitalRead(OVERRIDE_FAN1_PIN) == LOW; // Pulled to ground to activate (active LOW)
  bool overrideFan2 = digitalRead(OVERRIDE_FAN2_PIN) == LOW; // Pulled to ground to activate (active LOW)

  // Control FANs based on override inputs and temperature thresholds
  if (overrideFan2) {
    digitalWrite(FAN1_PIN, HIGH); // Turn ON FAN1
    digitalWrite(FAN2_PIN, HIGH); // Turn ON FAN2
    fan1Active = true;
    fan2Active = true;
  } else if (overrideFan1) {
    digitalWrite(FAN1_PIN, HIGH); // Turn ON FAN1
    fan1Active = true;
    fan2Active = false; // Deactivate FAN2 if FAN1 override is active
  } else {
    // Control FANs based on average temperature threshold
    float averageTemperature = getAverageTemperature(temperatureC1, temperatureC2);

    if (isValidTemperature(averageTemperature)) {
      display.setCursor(0, 20);
      display.print("CltAvgTmp: ");
      display.print(averageTemperature, 1);
      display.print(" C");

      fanControl(averageTemperature);
    } else {
      displayError("CltAvgTmp: Invalid");
    }
  }

  // Display FAN status
  display.setCursor(0, 30);
  display.print("FAN1: ");
  display.print(fan1Active || overrideFan1 ? "ON" : "OFF");

  display.setCursor(64, 30);
  display.print("FAN2: ");
  display.print(fan2Active || overrideFan2 ? "ON" : "OFF");

  // Read data from the M3200 sensor
  readM3200Sensor();

  // Display M3200 sensor data
  display.setCursor(0, 43);
  display.print("Oil Pres: ");
  display.print(pressure, 1);
  display.print(" PSI");

  display.setCursor(0, 53);
  display.print("Oil Temp: ");
  display.print(temperature);
  display.print(" C");

  // Update the display
  display.display();

  // Delay for a while
  delay(2000);
}

void readM3200Sensor() {
  Wire.requestFrom(M3200address, 4); // request 4 bytes
  int n = Wire.available();
  if (n == 4) {
    status = 0;
    uint16_t rawP;     // pressure data from sensor
    uint16_t rawT;     // temperature data from sensor
    rawP = (uint16_t)Wire.read(); // upper 8 bits
    rawP <<= 8;
    rawP |= (uint16_t)Wire.read(); // lower 8 bits
    rawT = (uint16_t)Wire.read(); // upper 8 bits
    rawT <<= 8;
    rawT |= (uint16_t)Wire.read(); // lower 8 bits

    status = rawP >> 14; // The status is 0, 1, 2 or 3
    rawP &= 0x3FFF;      // keep 14 bits, remove status bits

    rawT >>= 5; // the lowest 5 bits are not used

    // The math could be done with integers, but I choose float for now
    pressure = ((rawP - 1000.0) / (15000.0 - 1000.0)) * maxPressure;
    temperature = ((rawT - 512.0) / (1075.0 - 512.0)) * 55.0;
  } else {
    displayError("M3200 Pressure Transducer not Detected");
  }
}

float getAverageTemperature(float temp1, float temp2) {
  // Calculate average temperature
  if (isValidTemperature(temp1) && isValidTemperature(temp2)) {
    return (temp1 + temp2) / 2.0;
  } else if (isValidTemperature(temp1)) {
    return temp1;
  } else if (isValidTemperature(temp2)) {
    return temp2;
  } else {
    return -127.0; // Invalid temperature
  }
}

bool isValidTemperature(float temp) {
  // Check if temperature is within a reasonable range
  return temp > -100.0 && temp < 150.0;
}
