#include "SPI.h"
#include "Adafruit_GFX.h"
#include "Adafruit_ILI9341.h"
#include <SoftwareSerial.h>
#include <OBD2UART.h>

// Define the pins for the TFT display
#define TFT_DC 7
#define TFT_CS 8
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

// Define RX and TX pins for SoftwareSerial (ELM327 simulation)
SoftwareSerial ELMSerial(10, 11);  // RX, TX

#define BTN_PIN 5
#define POTEN_1_PIN A0
#define POTEN_2_PIN A1
#define NUM_SCREENS 3 // Now we have 3 screens

// Simulated message index for OBD2 data
int messageIndex = 0;
int screenToggle = 0;
String simulatedMessage = "";

unsigned long previousOBD2Millis = 0;  // Store last OBD2 update time
const long obd2Interval = 2000;  // 2 seconds between OBD2 data updates

int lastProgressValue = -1;
int lastDialValue = -1;
String lastOBD2Message = "";

// Function prototypes
void drawStaticElements();
void drawDial(int x, int y, int radius, float inputMin, float inputMax, float inputValue, float guageMin, float guageMax);
void drawProgressBar(int x, int y, int w, int h, float inputMin, float inputMax, float inputValue, float guageMin, float guageMax);
void drawOBD2Data(String obd2Data);
String getSimulatedMessage();

void setup() {
  tft.begin();                      // Initialize the TFT display
  tft.setRotation(3);               // Set display rotation (you can adjust this)
  tft.fillScreen(ILI9341_BLACK);    // Clear the screen with black

  Serial.begin(115200);
  ELMSerial.begin(9600);
  pinMode(POTEN_1_PIN, INPUT);
  pinMode(BTN_PIN, INPUT_PULLUP);

  Serial.println("Simulating OBD2 Data...");
  delay(2000);  // Initial delay to let everything settle

  drawStaticElements();  // Draw elements that do not change frequently
}

void loop() {
  // Handle screen toggling with the button
  int toggleSwitch = digitalRead(BTN_PIN);
  if (toggleSwitch == LOW) {
    screenToggle = (screenToggle + 1) % NUM_SCREENS;  // Now cycles through 3 screens
    delay(100);  // Debounce delay for button press
    tft.fillScreen(ILI9341_BLACK);  // Clear screen when toggling
    drawStaticElements();           // Redraw static elements for new screen
  }

  // Update OBD2 data every 2 seconds without blocking the display refresh
  unsigned long currentMillis = millis();
  if (currentMillis - previousOBD2Millis >= obd2Interval) {
    previousOBD2Millis = currentMillis;
    simulatedMessage = getSimulatedMessage();
    ELMSerial.println(simulatedMessage);
    Serial.println("Simulated Data: " + simulatedMessage);


    // Example OBD-II data
    int rpm = 3200;  // Example RPM from OBD-II
    int engineLoad = 75;  // Example engine load percentage from OBD-II

    // Estimate torque based on RPM and engine load
    float torque = estimateTorque(rpm, engineLoad);

    Serial.print("Torque: ");
     Serial.println(torque);

  }

  // Render the current screen
  if (screenToggle == 0) {
    int readValue = analogRead(POTEN_1_PIN);
    if (readValue != lastProgressValue) {  // Only redraw if value changes
      lastProgressValue = readValue;
      drawProgressBar(0, 50, 240, 20, 0, 1023, readValue, 0, 100);
    }
  }
  else if (screenToggle == 1) {
    int readValue = analogRead(POTEN_2_PIN);
    if (readValue != lastDialValue) {  // Only redraw if value changes
      lastDialValue = readValue;
      tft.fillCircle(120, 160, 82, ILI9341_BLACK);  // Clear dial area
      drawDial(120, 160, 80, 0, 1023, readValue, 0, 100);
    }
  }
  else if (screenToggle == 2) {
    if (simulatedMessage != lastOBD2Message) {  // Only redraw if OBD2 message changes
      lastOBD2Message = simulatedMessage;
      tft.fillRect(0, 50, 240, 80, ILI9341_BLACK);  // Clear OBD2 data area
      drawOBD2Data(simulatedMessage);
    }
  }
}

void drawStaticElements() {
  // Draw elements that do not change on screen toggles
  if (screenToggle == 0) {
    tft.setCursor(0, 20);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(2);
    tft.print("Progress Bar Screen");
  }
  else if (screenToggle == 1) {
    tft.setCursor(0, 20);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(2);
    tft.print("Dial Screen");
  }
  else if (screenToggle == 2) {
    tft.setCursor(0, 20);
    tft.setTextColor(ILI9341_WHITE);
    tft.setTextSize(2);
    tft.print("OBD2 Data Screen");
  }
}

void drawDial(int x, int y, int radius, float inputMin, float inputMax, float inputValue, float guageMin, float guageMax) {
  float DIAL_TRAVEL_DEGS = 270.00;
  float DIAL_START_POSITION = -225.00;
  int progress = getGuageValue(inputMin, inputMax, inputValue, guageMin, guageMax);
  progress = DIAL_TRAVEL_DEGS / guageMax * progress;

  float angle = float(progress) + DIAL_START_POSITION;
  float radAngle = radians(angle);

  int endX = x + (cos(radAngle) * (radius - 3));
  int endY = y + (sin(radAngle) * (radius - 3));

  // Draw the dial background and needle
  tft.drawCircle(x, y, radius, ILI9341_WHITE);
  tft.fillCircle(x, y, 4, ILI9341_RED);  // Small red center circle
  tft.drawLine(x, y, endX, endY, ILI9341_RED);  // Dial needle
}

void drawProgressBar(int x, int y, int w, int h, float inputMin, float inputMax, float inputValue, float guageMin, float guageMax) {
  int progress = getGuageValue(inputMin, inputMax, inputValue, guageMin, guageMax);
  int filledWidth = (w * progress) / 100;

  // Draw the progress bar background
  tft.drawRect(x, y, w, h, ILI9341_WHITE);

  tft.fillRect(filledWidth, y+2, w-filledWidth-4, h-4, ILI9341_BLACK);  // Clear progress bar area

  // Draw the filled portion of the progress bar
  tft.fillRect(x+2, y+2, filledWidth-4, h-4, ILI9341_GREEN);

  // Display text label for the progress bar
  tft.setCursor(x, y + h + 10);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print("Progress: ");
  tft.print(progress);
  tft.print("%");
}

int getGuageValue(float inputMin, float inputMax, float inputValue, float guageMin, float guageMax) {
  float value = inputValue;
  value = value / (inputMax - inputMin);
  return int(value * (guageMax - guageMin));
}

// Simulating different types of OBD2 messages
String getSimulatedMessage() {
  switch (messageIndex) {
    case 0:
      messageIndex = (messageIndex + 1) % 3;
      return "41 42 12.5V";  // Battery Voltage
    case 1:
      messageIndex = (messageIndex + 1) % 3;
      return "41 05 90C";  // Engine Temperature
    case 2:
      messageIndex = (messageIndex + 1) % 3;
      return "41 0D 60 km/h";  // Vehicle Speed
    default:
      return "No Data";
  }
}

// Function to draw OBD2 data on the screen
void drawOBD2Data(String obd2Data) {
  tft.setCursor(0, 50);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print("OBD2 Data:");

  // Overwrite previous message with a black background
  tft.setCursor(0, 90);
  tft.setTextColor(ILI9341_GREEN, ILI9341_BLACK);
  tft.setTextSize(3);
  tft.print(obd2Data);
}

// Define RPM and corresponding Torque arrays (from the green curve)
const int numPoints = 19; // Number of data points in the table
int rpm_table[numPoints] = {1500, 1750, 2000, 2250, 2500, 2750, 3000, 3250, 3500, 3750,
                            4000, 4250, 4500, 4750, 5000, 5250, 5500, 5750, 6000
                           };
int torque_table[numPoints] = {240, 250, 260, 270, 280, 285, 290, 295, 300, 300,
                               295, 290, 280, 270, 260, 245, 225, 205, 180
                              };

// Function to estimate torque based on OBD-II RPM and engine load
float estimateTorque(int rpm, int engineLoad) {
  // Ensure engine load is between 0 and 100%
  if (engineLoad < 0) engineLoad = 0;
  if (engineLoad > 100) engineLoad = 100;

  // Find the torque for the given RPM by interpolation
  float interpolatedTorque = 0;

  // If RPM is below the range of the table
  if (rpm <= rpm_table[0]) {
    interpolatedTorque = torque_table[0];
  }
  // If RPM is above the range of the table
  else if (rpm >= rpm_table[numPoints - 1]) {
    interpolatedTorque = torque_table[numPoints - 1];
  }
  // If RPM is within the range of the table
  else {
    for (int i = 0; i < numPoints - 1; i++) {
      if (rpm >= rpm_table[i] && rpm < rpm_table[i + 1]) {
        // Linear interpolation between two points
        float rpmDiff = rpm_table[i + 1] - rpm_table[i];
        float torqueDiff = torque_table[i + 1] - torque_table[i];
        float rpmFraction = (float)(rpm - rpm_table[i]) / rpmDiff;
        interpolatedTorque = torque_table[i] + (torqueDiff * rpmFraction);
        break;
      }
    }
  }

  // Adjust torque based on engine load percentage
  float estimatedTorque = interpolatedTorque * (engineLoad / 100.0);

  return estimatedTorque;
}