#include <Arduino.h>
#include <Encoder.h>
#include "Adafruit_GFX.h"    // Core graphics library
#include "SPI.h"       // this is needed for display
#include "Wire.h"      // this is needed for FT6206
#include "Adafruit_ILI9341.h"
#include "Adafruit_FT6206.h"




/* Thermistor Analog Pin */
#define THERMISTORPIN A1

/* How many samples to take and average, more takes longer but is more 'smooth' */
const int NUMSAMPLES = 5;
const int OILTEMPOFFSET = 93;          // OIL TEMP OFFSET
const float OILTEMPFACTOR = 0.111;       // OIL TEMP CONVERSION FACTOR

/* Stepper pins */
const int STEPPIN = 4;
const int DIRECTIONPIN = 6;

/* Relay pin */
const int RELAY_PIN = 7; // Pin for the relay control

/* Haldex constants */
const int HALDEX_INCREMENT = 25;
const int HALDEX_MAX_TEMP = 80;
const unsigned long OIL_TIME = 1000;
const int noOfSteps = 20;

/* Rotary encoder pins */
const int pinA = 2; // Connect to CLK on the rotary encoder
const int pinB = 3; // Connect to DT on the rotary encoder

Encoder encoder(pinA, pinB);

// The FT6206 uses hardware I2C (SCL/SDA)
Adafruit_FT6206 ts = Adafruit_FT6206();

/* Touchscreen settings */
#define TFT_CS 10
#define TFT_RST 9
#define TFT_DC 8
Adafruit_ILI9341 tft = Adafruit_ILI9341(TFT_CS, TFT_DC);

boolean RecordOn = false;

// Variables
int currentHaldexPercentage = 0;
int samples[NUMSAMPLES];
int currentTemp;
unsigned long oilTime;

void setup();
void loop();
void disableHaldex();
void writeTempToLCD();
void writeHaldexMessage();
void moveMotorForwards(int noOfSteps);
void moveMotorBackwards(int noOfSteps);
void moveMotor(int noOfSteps);
float readTemperature();
void checkRotaryEncoder();
void controlRelay(); // Function to control the relay
void setupLCD();
void setupMotor();
void RaceButton();
void UpButton();
void DownButton();
void checkTouch();

void setup() {
  Serial.begin(9600);
  setupLCD();
  setupMotor();
  pinMode(RELAY_PIN, OUTPUT);
  digitalWrite(RELAY_PIN, LOW); // Ensure relay is off initially
  oilTime = 0;
  RaceButton();
  UpButton();
  DownButton();
}

void loop() {  


    // See if there's any  touch data for us
if (ts.touched()) {
    TS_Point p = ts.getPoint();
    p.x = map(p.x, 0, 240, 240, 0);
    p.y = map(p.y, 0, 320, 320, 0);

    Serial.print("Touch X: "); Serial.print(p.x);
    Serial.print(" Touch Y: "); Serial.println(p.y);

    if (RecordOn) {
        // Check Race Button
        if ((p.x >= 100) && (p.x <= 140) && (p.y >= 20) && (p.y <= 100)) {
           if (currentHaldexPercentage < 25) {
                Serial.println("Race btn hit");
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
           }
            else if (currentHaldexPercentage < 50) {
                Serial.println("Race btn hit");
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();

           }
            else if (currentHaldexPercentage < 75) {
                Serial.println("Race btn hit");
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();

           }
            else if (currentHaldexPercentage < 100) {
                Serial.println("Race btn hit");
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
           }  
            else if (currentHaldexPercentage > 99) {
                Serial.println("Race btn hit");
                moveMotorBackwards(noOfSteps);
                writeHaldexMessage();
                moveMotorBackwards(noOfSteps);
                writeHaldexMessage();
                moveMotorBackwards(noOfSteps);
                writeHaldexMessage();
                moveMotorBackwards(noOfSteps);
                writeHaldexMessage();
           }
    }
        // Check Up Button
        if ((p.x >= 40) && (p.x <= 80) && (p.y >= 20) && (p.y <= 100)) {
            if (currentHaldexPercentage < 100) {
              Serial.println("Up btn hit");
                moveMotorForwards(noOfSteps);
                writeHaldexMessage();
            }
        }
        // Check Down Button
        if ((p.x >= 160) && (p.x <= 200) && (p.y >= 20) && (p.y <= 100)) {
            if (currentHaldexPercentage >= HALDEX_INCREMENT) {
              Serial.println("Down btn hit");
                moveMotorBackwards(noOfSteps);
                writeHaldexMessage();
            }
        }
    }


    else //Record is off (RecordOn == false)
    Serial.println(RecordOn);
  }  



  if (millis() - oilTime > OIL_TIME) {
    oilTime = millis();
    currentTemp = readTemperature();
    writeTempToLCD();
  }

  if (currentTemp > HALDEX_MAX_TEMP) {
    disableHaldex();
  } else {
    writeHaldexMessage();
    checkRotaryEncoder();
    controlRelay();
  }
}

void disableHaldex() {
  writeHaldexMessage();
  while (currentHaldexPercentage > 0) {
    moveMotorBackwards(noOfSteps);
  }
}

void writeTempToLCD() {
  tft.setCursor(0, 0);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.fillRect(0, 0, 200, 25, ILI9341_BLACK); // Clear the previous temperature
  tft.print("Temp : ");
  tft.print(currentTemp);
  tft.print("*C");
}

void writeHaldexMessage() {
  tft.setCursor(0, 40);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.fillRect(0, 40, 200, 25, ILI9341_BLACK); // Clear previous message
  
  if (currentTemp > HALDEX_MAX_TEMP) {
    tft.print("Haldex Disabled");
  } else {
    tft.print("Haldex ");
    tft.print(currentHaldexPercentage);
    tft.print("%");
  }
  tft.setCursor(0, 220);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(3);
  tft.fillRect(0, 220, 300, 25, ILI9341_BLACK); // Clear previous message

  if (currentHaldexPercentage < 99) {
  tft.print("Why Are You Gay");
    } else { 
  if (currentHaldexPercentage > 99)
  tft.print("Wir Suchen Dich");
}
}


void moveMotorForwards(int noOfSteps) {
  digitalWrite(DIRECTIONPIN, LOW);
  currentHaldexPercentage += HALDEX_INCREMENT;
  moveMotor(noOfSteps);
}

void moveMotorBackwards(int noOfSteps) {
  digitalWrite(DIRECTIONPIN, HIGH);
  currentHaldexPercentage -= HALDEX_INCREMENT;
  moveMotor(noOfSteps);
}

void moveMotor(int noOfSteps) {
  for (int i = 0; i < noOfSteps; i++) {
    digitalWrite(STEPPIN, HIGH);
    delay(10);
    digitalWrite(STEPPIN, LOW);
    delay(10);
  }
}

float readTemperature() {
  uint8_t i;
  float average;

  for (i = 0; i < NUMSAMPLES; i++) {
    samples[i] = analogRead(THERMISTORPIN);
    delay(10);
  }

  average = 0;
  for (i = 0; i < NUMSAMPLES; i++) {
    average += samples[i];
  }
  average /= NUMSAMPLES;
  
  return OILTEMPOFFSET - (average * OILTEMPFACTOR);
}

void checkRotaryEncoder() {
  static long lastPosition = 0;
  long newPosition = encoder.read() / 4; // Adjust as necessary for your encoder

  if (newPosition != lastPosition) {
    if (newPosition > lastPosition) { // Rotated clockwise
      if (currentHaldexPercentage < 100) {
        moveMotorForwards(noOfSteps);
        writeHaldexMessage();
      }
    } else { // Rotated counter-clockwise
      if (currentHaldexPercentage >= HALDEX_INCREMENT) {
        moveMotorBackwards(noOfSteps);
        writeHaldexMessage();
      }
    }
    lastPosition = newPosition;
  }
}

void controlRelay() {
  if (currentHaldexPercentage > 20) {
    digitalWrite(RELAY_PIN, HIGH); // Turn on the relay
  } else if (currentHaldexPercentage < 19) {
    digitalWrite(RELAY_PIN, LOW); // Turn off the relay
  }
}

void setupLCD() {
  tft.begin();
    if (!ts.begin(40)) { 
    Serial.println("Unable to start touchscreen.");
  } 
  else { 
    Serial.println("Touchscreen started."); 
  }
  tft.setRotation(1); // Adjust display orientation as necessary
  tft.fillScreen(ILI9341_BLACK); // Clear screen
  writeHaldexMessage();
}

void setupMotor() {
  pinMode(DIRECTIONPIN, OUTPUT);
  pinMode(STEPPIN, OUTPUT);
  digitalWrite(DIRECTIONPIN, LOW);
  digitalWrite(STEPPIN, LOW);
}


void UpButton() {

  tft.fillRect(220, 40, 80, 40, ILI9341_BLUE); // Draw button
  tft.setCursor(240, 52);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print("Up");
  RecordOn = true;
}

void RaceButton() {

  tft.fillRect(220, 100, 80, 40, ILI9341_RED); // Draw button
  tft.setCursor(240, 112);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print("Race");
  RecordOn = true;
}

void DownButton() {

  tft.fillRect(220, 160, 80, 40, ILI9341_BLUE); // Draw button
  tft.setCursor(240, 172);
  tft.setTextColor(ILI9341_WHITE);
  tft.setTextSize(2);
  tft.print("Down");
  RecordOn = true;

}