#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>

// Pin definitions for the ILI9341 display
#define TFT_CS     40
#define TFT_RST    41  // Or set to -1 and connect to Arduino RESET pin
#define TFT_DC     42
#define TFT_MOSI   43
#define TFT_CLK    44
#define TFT_MISO   45

// Create the display object with a higher SPI clock speed (e.g., 40 MHz)
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_MOSI, TFT_CLK, TFT_RST, TFT_MISO);

// Pin definitions
#define BUTTON_INC_PIN 2  // Increase height button
#define BUTTON_DEC_PIN 3  // Decrease height button
#define BUTTON_INHIBIT_PIN 25 // Inhibit button
#define BUTTON_IGNITION_PIN 27 // Ignition button

// LED Pins for each wheel
#define LED_WHEEL_1_LOW 4
#define LED_WHEEL_1_HIGH 5
#define LED_WHEEL_2_LOW 6
#define LED_WHEEL_2_HIGH 7
#define LED_WHEEL_3_LOW 8
#define LED_WHEEL_3_HIGH 9
#define LED_WHEEL_4_LOW 10
#define LED_WHEEL_4_HIGH 11

// LED Pins for ride height levels
#define LED_HEIGHT_0 12
#define LED_HEIGHT_1 13
#define LED_HEIGHT_2 22
#define LED_HEIGHT_3 23
#define LED_HEIGHT_4 24

// LED Pin for Inhibit mode indication
#define LED_INHIBIT 26

// Analog pins for potentiometers (wheel height sensors and speed sensor)
#define POT_WHEEL_1 A0
#define POT_WHEEL_2 A1
#define POT_WHEEL_3 A2
#define POT_WHEEL_4 A3
#define POT_SPEED A4  // Speed potentiometer

// Variables to store current height levels
int height1 = 0;
int height2 = 0;
int height3 = 0;
int height4 = 0;
int speed = 0;  // Speed of the car

// Variables to store average height levels
int averageHeight1 = 0;
int averageHeight2 = 0;
int averageHeight3 = 0;
int averageHeight4 = 0;

// Car height levels (0 to 4)
int carHeight = 0;

// Adjusted height variables
int adjustedHeight1 = 0;
int adjustedHeight2 = 0;
int adjustedHeight3 = 0;
int adjustedHeight4 = 0;

// Inhibit mode variables
bool inhibit = false;
unsigned long inhibitLedToggleTime = 0;
bool inhibitLedState = LOW;
const unsigned long inhibitLedBlinkInterval = 500; // 500 ms blink interval

// Ignition state and timer variables
bool ignitionOn = true;  // Assume ignition starts as on
unsigned long ignitionOffStartTime = 0;
unsigned long timeIgnitionOff = 0; // Time since ignition was turned off in seconds

// Adjustable parameters
const int incrementIncrease = 10;   // Increment to adjust height when RED LED is on
const int incrementDecrease = 5;    // Increment to adjust height when BLUE LED is on
const int speedThresholdLevel2 = 25;  // Speed threshold for reducing height to level 2
const int speedThresholdLevel1 = 5;   // Speed threshold for increasing height to level 1
const float alpha = 0.1;              // Smoothing factor for running average

// Variables to track previous values for optimized display updates
int previousHeight1 = -1;
int previousHeight2 = -1;
int previousHeight3 = -1;
int previousHeight4 = -1;
int previousSpeed = -1;
int previousCarHeight = -1;
bool previousInhibit = false;

void setup() {
  // Initialize Serial communication
  Serial.begin(115200);

  // Initialize display
  tft.begin();
  tft.setRotation(1);  // Adjust orientation as needed
  tft.setTextSize(1);
  tft.setTextColor(ILI9341_WHITE);

  // Set pin modes for buttons
  pinMode(BUTTON_INC_PIN, INPUT_PULLUP);
  pinMode(BUTTON_DEC_PIN, INPUT_PULLUP);
  pinMode(BUTTON_INHIBIT_PIN, INPUT_PULLUP);
  pinMode(BUTTON_IGNITION_PIN, INPUT_PULLUP);

  // Set pin modes for LEDs
  initializeLEDPins();
}

void loop() {
  handleButtonInputs();
  readSensors();

  if (!inhibit) {
    adjustRideHeightBasedOnSpeed();
  }

  updateRideHeightLEDs();
  updateWheelLEDs();
  updateInhibitLED();
  adjustHeights();  // Adjust heights based on LED status

  checkIgnitionStatus(); // Check and update ignition status
  optimizedDisplayInfo();  // Optimized display update function
  printDebugInfo();

  delay(100);
}

void handleButtonInputs() {
  // Update car height based on button presses
  if (digitalRead(BUTTON_INC_PIN) == LOW) {
    increaseCarHeight();
    delay(200);  // Debounce delay
  }
  if (digitalRead(BUTTON_DEC_PIN) == LOW) {
    decreaseCarHeight();
    delay(200);  // Debounce delay
  }

  // Toggle inhibit mode
  if (digitalRead(BUTTON_INHIBIT_PIN) == LOW) {
    inhibit = !inhibit;
    delay(200);  // Debounce delay
  }

  // Check the ignition button state
  ignitionOn = digitalRead(BUTTON_IGNITION_PIN) == LOW;
}

void checkIgnitionStatus() {
  if (!ignitionOn) {
    // If ignition is off, calculate the time it's been off
    if (ignitionOffStartTime == 0) {
      ignitionOffStartTime = millis(); // Start the timer
    }
    timeIgnitionOff = (millis() - ignitionOffStartTime) / 1000; // Convert to seconds
  } else {
    // Reset the timer if ignition is on
    ignitionOffStartTime = 0;
    timeIgnitionOff = 0;
  }
}

void readSensors() {
  // Read wheel heights
  height1 = analogRead(POT_WHEEL_1) + adjustedHeight1;
  height2 = analogRead(POT_WHEEL_2) + adjustedHeight2;
  height3 = analogRead(POT_WHEEL_3) + adjustedHeight3;
  height4 = analogRead(POT_WHEEL_4) + adjustedHeight4;

  // Read car speed
  speed = analogRead(POT_SPEED);
  speed = map(speed, 0, 1023, 0, 200);  // Map speed to range 0-200
}

void updateWheelLEDs() {
  // Determine the permitted range based on carHeight (5 levels)
  int rangeMin = carHeight * 204;
  int rangeMax = rangeMin + 203;

  // Update LEDs based on average wheel heights
  updateLEDs(averageHeight1, height1, LED_WHEEL_1_LOW, LED_WHEEL_1_HIGH, rangeMin, rangeMax);
  updateLEDs(averageHeight2, height2, LED_WHEEL_2_LOW, LED_WHEEL_2_HIGH, rangeMin, rangeMax);
  updateLEDs(averageHeight3, height3, LED_WHEEL_3_LOW, LED_WHEEL_3_HIGH, rangeMin, rangeMax);
  updateLEDs(averageHeight4, height4, LED_WHEEL_4_LOW, LED_WHEEL_4_HIGH, rangeMin, rangeMax);
}

void updateLEDs(int &averageHeight, int height, int ledLow, int ledHigh, int rangeMin, int rangeMax) {
  // Update the running average height
  averageHeight = (alpha * height) + ((1 - alpha) * averageHeight);

  // Error handling: clamp the average height within expected sensor ranges
  averageHeight = constrain(averageHeight, 0, 1023);

  // Determine LED state based on the average height
  if (averageHeight < rangeMin) {
    digitalWrite(ledLow, HIGH);
    digitalWrite(ledHigh, LOW);
  } else if (averageHeight > rangeMax) {
    digitalWrite(ledHigh, HIGH);
    digitalWrite(ledLow, LOW);
  } else {
    digitalWrite(ledLow, LOW);
    digitalWrite(ledHigh, LOW);
  }
}

void updateRideHeightLEDs() {
  digitalWrite(LED_HEIGHT_0, carHeight == 0 ? HIGH : LOW);
  digitalWrite(LED_HEIGHT_1, carHeight == 1 ? HIGH : LOW);
  digitalWrite(LED_HEIGHT_2, carHeight == 2 ? HIGH : LOW);
  digitalWrite(LED_HEIGHT_3, carHeight == 3 ? HIGH : LOW);
  digitalWrite(LED_HEIGHT_4, carHeight == 4 ? HIGH : LOW);
}

void increaseCarHeight() {
  if (carHeight < 4) {  // Update for 5 levels
    carHeight++;
  }
}

void decreaseCarHeight() {
  if (carHeight > 0) {
    carHeight--;
  }
}

void adjustRideHeightBasedOnSpeed() {
  if (carHeight >= 3 && speed > speedThresholdLevel2) {
    carHeight = 2;  // Reduce height to level 2 if speed exceeds threshold
  } else if (carHeight < 1 && speed > speedThresholdLevel1) {
    carHeight = 1;  // Increase height to level 1 if speed exceeds threshold
  }
}

void adjustHeights() {
  // Adjust heights based on LED status
  if (digitalRead(LED_WHEEL_1_LOW) == HIGH) {
    adjustedHeight1 += incrementIncrease;
  } else if (digitalRead(LED_WHEEL_1_HIGH) == HIGH) {
    adjustedHeight1 -= incrementDecrease;
  }
  adjustedHeight1 = constrain(adjustedHeight1, 0, 1023);

  if (digitalRead(LED_WHEEL_2_LOW) == HIGH) {
    adjustedHeight2 += incrementIncrease;
  } else if (digitalRead(LED_WHEEL_2_HIGH) == HIGH) {
    adjustedHeight2 -= incrementDecrease;
  }
  adjustedHeight2 = constrain(adjustedHeight2, 0, 1023);

  if (digitalRead(LED_WHEEL_3_LOW) == HIGH) {
    adjustedHeight3 += incrementIncrease;
  } else if (digitalRead(LED_WHEEL_3_HIGH) == HIGH) {
    adjustedHeight3 -= incrementDecrease;
  }
  adjustedHeight3 = constrain(adjustedHeight3, 0, 1023);

  if (digitalRead(LED_WHEEL_4_LOW) == HIGH) {
    adjustedHeight4 += incrementIncrease;
  } else if (digitalRead(LED_WHEEL_4_HIGH) == HIGH) {
    adjustedHeight4 -= incrementDecrease;
  }
  adjustedHeight4 = constrain(adjustedHeight4, 0, 1023);
}

void updateInhibitLED() {
  if (inhibit) {
    unsigned long currentMillis = millis();
    if (currentMillis - inhibitLedToggleTime >= inhibitLedBlinkInterval) {
      inhibitLedToggleTime = currentMillis;
      inhibitLedState = !inhibitLedState;
      digitalWrite(LED_INHIBIT, inhibitLedState);
    }
  } else {
    digitalWrite(LED_INHIBIT, LOW);  // Turn off the inhibit LED when not in inhibit mode
  }
}

void initializeLEDPins() {
  // Set pin modes for wheel LEDs
  pinMode(LED_WHEEL_1_LOW, OUTPUT);
  pinMode(LED_WHEEL_1_HIGH, OUTPUT);
  pinMode(LED_WHEEL_2_LOW, OUTPUT);
  pinMode(LED_WHEEL_2_HIGH, OUTPUT);
  pinMode(LED_WHEEL_3_LOW, OUTPUT);
  pinMode(LED_WHEEL_3_HIGH, OUTPUT);
  pinMode(LED_WHEEL_4_LOW, OUTPUT);
  pinMode(LED_WHEEL_4_HIGH, OUTPUT);

  // Set pin modes for ride height LEDs
  pinMode(LED_HEIGHT_0, OUTPUT);
  pinMode(LED_HEIGHT_1, OUTPUT);
  pinMode(LED_HEIGHT_2, OUTPUT);
  pinMode(LED_HEIGHT_3, OUTPUT);
  pinMode(LED_HEIGHT_4, OUTPUT);

  // Set pin mode for Inhibit LED
  pinMode(LED_INHIBIT, OUTPUT);
}

void optimizedDisplayInfo() {
  // Only update if the value has changed to minimize redrawing
  tft.setTextColor(ILI9341_YELLOW);

  if (height1 != previousHeight1 || adjustedHeight1 != previousHeight1) {
    tft.fillRect(10, 50, 320, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 50);
    tft.print("Wheel 1 Height: ");
    tft.print(height1);
    tft.print(" Adjusted: ");
    tft.println(adjustedHeight1);
    previousHeight1 = height1;
  }

  if (height2 != previousHeight2 || adjustedHeight2 != previousHeight2) {
    tft.fillRect(10, 70, 320, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 70);
    tft.print("Wheel 2 Height: ");
    tft.print(height2);
    tft.print(" Adjusted: ");
    tft.println(adjustedHeight2);
    previousHeight2 = height2;
  }

  if (height3 != previousHeight3 || adjustedHeight3 != previousHeight3) {
    tft.fillRect(10, 90, 320, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 90);
    tft.print("Wheel 3 Height: ");
    tft.print(height3);
    tft.print(" Adjusted: ");
    tft.println(adjustedHeight3);
    previousHeight3 = height3;
  }

  if (height4 != previousHeight4 || adjustedHeight4 != previousHeight4) {
    tft.fillRect(10, 110, 320, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 110);
    tft.print("Wheel 4 Height: ");
    tft.print(height4);
    tft.print(" Adjusted: ");
    tft.println(adjustedHeight4);
    previousHeight4 = height4;
  }

  tft.setTextColor(ILI9341_WHITE);

  if (speed != previousSpeed) {
    tft.fillRect(10, 140, 240, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 140);
    tft.print("Speed: ");
    tft.println(speed);
    previousSpeed = speed;
  }

  if (carHeight != previousCarHeight) {
    tft.fillRect(10, 20, 240, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 20);
    tft.setTextColor(ILI9341_WHITE);
    tft.print("Car Height: ");
    tft.println(carHeight);
    previousCarHeight = carHeight;
  }

  if (inhibit != previousInhibit) {
    tft.fillRect(10, 170, 240, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 170);
    tft.print("Inhibit Mode: ");
    tft.println(inhibit ? "ON" : "OFF");
    previousInhibit = inhibit;
  }

  if (!ignitionOn) {
    tft.fillRect(10, 200, 240, 20, ILI9341_BLACK);  // Clear the area
    tft.setCursor(10, 200);
    tft.print("Ignition Off Time: ");
    tft.print(timeIgnitionOff);
    tft.println(" s");
  } else {
    tft.fillRect(10, 200, 240, 20, ILI9341_BLACK);  // Clear the area if ignition is on
  }
}

void printDebugInfo() {
  // Debugging output
  Serial.print("Hght1: ");
  Serial.print(height1);
  Serial.print(" | Adj Hght1: ");
  Serial.print(adjustedHeight1);
  Serial.print(" | Hght2: ");
  Serial.print(height2);
  Serial.print(" | Adj Hght2: ");
  Serial.print(adjustedHeight2);
  Serial.print(" | Hght3: ");
  Serial.print(height3);
  Serial.print(" | Adj Hght3: ");
  Serial.print(adjustedHeight3);
  Serial.print(" | Hght4: ");
  Serial.print(height4);
  Serial.print(" | Adj Hght4: ");
  Serial.print(adjustedHeight4);
  Serial.print(" | Car Hght: ");
  Serial.print(carHeight);
  Serial.print(" | Speed: ");
  Serial.print(speed);
  Serial.print(" | Inhibit: ");
  Serial.print(inhibit ? "ON" : "OFF");
  Serial.print(" | Ignition On: ");
  Serial.print(ignitionOn ? "Yes" : "No");
  Serial.print(" | Ignition Off Time: ");
  Serial.print(timeIgnitionOff);
  Serial.println(" s");
}