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

// Define the pins for the ILI9341
#define TFT_CS 10
#define TFT_RST 9
#define TFT_DC 8
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC, TFT_RST);

// Define pin numbers for other components
const int lhUp = 2;
const int lhDown = 3;
const int rhUp = 4;
const int rhDown = 5;
const int input1 = A4; // LHF
const int input2 = A5; // LHR
const int input3 = A6; // RHF
const int input4 = A7; // RHR
const int button1 = 6;
const int button2 = 7;
const int button3 = A0;
const int button4 = A1;
const int led1 = A2;
const int led2 = A3;
const int led3 = 14; // Use an analog pin as digital
const int led4 = 15; // Use an analog pin as digital

// Define height settings and names
const int accessHeight = 10;       // Example value for Access height
const int lowProfileHeight = 20;   // Example value for Low Profile height
const int standardHeight = 30;     // Example value for Standard height
const int highProfileHeight = 40;  // Example value for High Profile height
const char* heightNames[] = {"Access", "Low Profile", "Standard", "High Profile"};
const int heightValues[] = {accessHeight, lowProfileHeight, standardHeight, highProfileHeight};

// Current selected height
int currentHeightIndex = -1;
bool heightSelected = false;
unsigned long startTime;
const int controlPeriod = 1000; // Control period for pulsing (1 second)
unsigned long lastControlTime = 0;

void setup() {
  // Initialize the ILI9341 display
  tft.begin();
  tft.setRotation(1); // Set rotation to landscape
  tft.fillScreen(ILI9341_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ILI9341_WHITE);

  // Initialize pins
  pinMode(lhUp, OUTPUT);
  pinMode(lhDown, OUTPUT);
  pinMode(rhUp, OUTPUT);
  pinMode(rhDown, OUTPUT);
  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);
  pinMode(button3, INPUT_PULLUP);
  pinMode(button4, INPUT_PULLUP);
  pinMode(led1, OUTPUT);
  pinMode(led2, OUTPUT);
  pinMode(led3, OUTPUT);
  pinMode(led4, OUTPUT);

  // Display initial message
  tft.setCursor(0, 0);
  tft.println("ELECTRONIC AIR SUSPENSION");

  // Display headings for input and output values
  tft.setCursor(0, 60);
  tft.print("Input Values:");
  tft.drawLine(0, 80, 320, 80, ILI9341_RED); // Underline

  tft.setCursor(0, 140);
  tft.print("Output Values:");
  tft.drawLine(0, 160, 320, 160, ILI9341_RED); // Underline
}

void loop() {
  // Read buttons and select height
  if (digitalRead(button1) == LOW) {
    currentHeightIndex = 0;
    updateLEDs(1);
    heightSelected = true;
    startTime = millis();
  } else if (digitalRead(button2) == LOW) {
    currentHeightIndex = 1;
    updateLEDs(2);
    heightSelected = true;
    startTime = millis();
  } else if (digitalRead(button3) == LOW) {
    currentHeightIndex = 2;
    updateLEDs(3);
    heightSelected = true;
    startTime = millis();
  } else if (digitalRead(button4) == LOW) {
    currentHeightIndex = 3;
    updateLEDs(4);
    heightSelected = true;
    startTime = millis();
  }

  // Read input values
  int lhf = analogRead(input1);
  int lhr = analogRead(input2);
  int rhf = analogRead(input3);
  int rhr = analogRead(input4);

  // Calculate average input values
  int avgInput1 = (lhf + lhr) / 2;
  int avgInput2 = (rhf + rhr) / 2;

  if (heightSelected) {
    // Control the outputs based on the current height
    int targetHeight = heightValues[currentHeightIndex];
    
    bool lhUpState = avgInput1 < targetHeight;
    bool lhDownState = avgInput1 > targetHeight;
    bool rhUpState = avgInput2 < targetHeight;
    bool rhDownState = avgInput2 > targetHeight;

    // Pulse logic for getting closer to the target height
    unsigned long currentTime = millis();
    if (currentTime - lastControlTime > controlPeriod) {
      lastControlTime = currentTime;

      if (abs(avgInput1 - targetHeight) < 5) {
        lhUpState = !lhUpState;
        lhDownState = !lhDownState;
      }
      if (abs(avgInput2 - targetHeight) < 5) {
        rhUpState = !rhUpState;
        rhDownState = !rhDownState;
      }
    }

    digitalWrite(lhUp, lhUpState ? HIGH : LOW);
    digitalWrite(lhDown, lhDownState ? HIGH : LOW);
    digitalWrite(rhUp, rhUpState ? HIGH : LOW);
    digitalWrite(rhDown, rhDownState ? HIGH : LOW);

    if (millis() - startTime > 10000) {
      // Turn off all outputs after 10 seconds
      digitalWrite(lhUp, LOW);
      digitalWrite(lhDown, LOW);
      digitalWrite(rhUp, LOW);
      digitalWrite(rhDown, LOW);
      heightSelected = false;
    }
  } else {
    // Ensure all outputs are off when no height is selected
    digitalWrite(lhUp, LOW);
    digitalWrite(lhDown, LOW);
    digitalWrite(rhUp, LOW);
    digitalWrite(rhDown, LOW);
  }

  // Clear and update the current height setting display
  tft.setCursor(0, 30);
  tft.fillRect(0, 30, 320, 20, ILI9341_BLACK); // Clear previous height value
  if (currentHeightIndex != -1) {
    tft.setTextColor(ILI9341_RED);
    tft.print("Target Height: ");
    tft.println(heightNames[currentHeightIndex]);
    tft.setTextColor(ILI9341_WHITE);
  }

  // Clear and update the input values display
  tft.setCursor(0, 90);
  tft.fillRect(0, 90, 320, 40, ILI9341_BLACK); // Clear previous input values
  tft.print("LHF: ");
  tft.print(lhf);
  tft.print("  LHR: ");
  tft.println(lhr);
  tft.setCursor(0, 110);
  tft.print("RHF: ");
  tft.print(rhf);
  tft.print("  RHR: ");
  tft.println(rhr);

  // Clear and update the output states display
  tft.setCursor(0, 170);
  tft.fillRect(0, 170, 320, 40, ILI9341_BLACK); // Clear previous output values
  tft.print("LH Up: ");
  if (digitalRead(lhUp)) {
    tft.setTextColor(ILI9341_RED);
    tft.print("ON");
  } else {
    tft.setTextColor(ILI9341_WHITE);
    tft.print("OFF");
  }
  tft.setTextColor(ILI9341_WHITE);
  tft.print("  LH Down: ");
  if (digitalRead(lhDown)) {
    tft.setTextColor(ILI9341_RED);
    tft.print("ON");
  } else {
    tft.setTextColor(ILI9341_WHITE);
    tft.print("OFF");
  }
  tft.setCursor(0, 190);
  tft.setTextColor(ILI9341_WHITE);
  tft.print("RH Up: ");
  if (digitalRead(rhUp)) {
    tft.setTextColor(ILI9341_RED);
    tft.print("ON");
  } else {
    tft.setTextColor(ILI9341_WHITE);
    tft.print("OFF");
  }
  tft.setTextColor(ILI9341_WHITE);
  tft.print("  RH Down: ");
  if (digitalRead(rhDown)) {
    tft.setTextColor(ILI9341_RED);
    tft.print("ON");
  } else {
    tft.setTextColor(ILI9341_WHITE);
    tft.print("OFF");
  }
  tft.setTextColor(ILI9341_WHITE);

  delay(100); // Small delay to stabilize readings
}

// Function to update LED status
void updateLEDs(int selectedButton) {
  digitalWrite(led1, selectedButton == 1 ? HIGH : LOW);
  digitalWrite(led2, selectedButton == 2 ? HIGH : LOW);
  digitalWrite(led3, selectedButton == 3 ? HIGH : LOW);
  digitalWrite(led4, selectedButton == 4 ? HIGH : LOW);
}