#include "Wheel.h"
#include <Adafruit_GFX.h>
#include <Adafruit_ILI9341.h>
#include <Arduino.h>

// Pin definitions for the ILI9341 display
#define TFT_CS     40
#define TFT_RST    41
#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);

// Define colors for different text and background
#define BACKGROUND_COLOR ILI9341_BLACK
#define TEXT_COLOR ILI9341_WHITE
#define HIGHLIGHT_COLOR ILI9341_YELLOW

// Pin definitions for potentiometers (simulating weight sensors)
#define POT_WHEEL_SIM1 A0
#define POT_WHEEL_SIM2 A1
#define POT_WHEEL_SIM3 A2
#define POT_WHEEL_SIM4 A3

// Pin definitions for potentiometers (reding the wheel height)
#define POT_WHEEL_HEIGHT_1 A4
#define POT_WHEEL_HEIGHT_2 A5
#define POT_WHEEL_HEIGHT_3 A6
#define POT_WHEEL_HEIGHT_4 A7

// Pin definitions for solenoids (inflate and deflate)
#define SOLENOID_INFLATE_WHEEL_1 2
#define SOLENOID_DEFLATE_WHEEL_1 3
#define SOLENOID_INFLATE_WHEEL_2 4
#define SOLENOID_DEFLATE_WHEEL_2 5
#define SOLENOID_INFLATE_WHEEL_3 6
#define SOLENOID_DEFLATE_WHEEL_3 7
#define SOLENOID_INFLATE_WHEEL_4 8
#define SOLENOID_DEFLATE_WHEEL_4 9

// Target ride height and margin of error
const int targetHeight = 511;
const int marginOfError = 10;

const boolean SIMULATE = true;

// Create Wheel objects for each wheel
Wheel wheel1("FR", SOLENOID_INFLATE_WHEEL_1, SOLENOID_DEFLATE_WHEEL_1, POT_WHEEL_HEIGHT_1, POT_WHEEL_SIM1);
Wheel wheel2("FL", SOLENOID_INFLATE_WHEEL_2, SOLENOID_DEFLATE_WHEEL_2, POT_WHEEL_HEIGHT_2, POT_WHEEL_SIM2);
Wheel wheel3("RR", SOLENOID_INFLATE_WHEEL_3, SOLENOID_DEFLATE_WHEEL_3, POT_WHEEL_HEIGHT_3, POT_WHEEL_SIM3);
Wheel wheel4("RL", SOLENOID_INFLATE_WHEEL_4, SOLENOID_DEFLATE_WHEEL_4, POT_WHEEL_HEIGHT_4, POT_WHEEL_SIM4);

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

  // Initialize display
  tft.begin();
  tft.setRotation(1);  // Adjust orientation as needed
  tft.fillScreen(BACKGROUND_COLOR); // Set the background color
  tft.setTextSize(2);
  tft.setTextColor(TEXT_COLOR);

  // Set pin modes for solenoids
  pinMode(SOLENOID_INFLATE_WHEEL_1, OUTPUT);
  pinMode(SOLENOID_DEFLATE_WHEEL_1, OUTPUT);
  pinMode(SOLENOID_INFLATE_WHEEL_2, OUTPUT);
  pinMode(SOLENOID_DEFLATE_WHEEL_2, OUTPUT);
  pinMode(SOLENOID_INFLATE_WHEEL_3, OUTPUT);
  pinMode(SOLENOID_DEFLATE_WHEEL_3, OUTPUT);
  pinMode(SOLENOID_INFLATE_WHEEL_4, OUTPUT);
  pinMode(SOLENOID_DEFLATE_WHEEL_4, OUTPUT);

  // Initialize solenoids to OFF
  deactivateAllSolenoids();
}

void loop() {
  // Simulate the effect of weight on height

  wheel1.setTargetHeight(targetHeight);
  wheel2.setTargetHeight(targetHeight);
  wheel3.setTargetHeight(targetHeight);
  wheel4.setTargetHeight(targetHeight);

  adjustWheel(wheel1);
  adjustWheel(wheel2);
  adjustWheel(wheel3);
  adjustWheel(wheel4);
}

void adjustWheel(Wheel &wheel) {

  if (SIMULATE) {
    Serial.print("SIM ");
    // Simulate the effect of weight and pressure on height
    int weight = analogRead(wheel.getSimPosPin());
    int weightOffset = calculateWeightOffset(weight);

    int pressure = wheel.getPressure();
    int pressureOffset = calculatePressureOffset(pressure);

    float height = (weightOffset + pressureOffset);
    wheel.setCurrentHeight(height);
  }
  else{
    Serial.print("READ ");
    float height = analogRead(wheel.getWheelPosPin());
    wheel.setCurrentHeight(height);
  }

  int adjustPressure = wheel.adjustHeight();
  if (adjustPressure > 0) {    
    inflate(wheel);
  }
  else if (adjustPressure < 0) {
    deflate(wheel);
  }
  else {
    //Serial.print(", LEVEL");
    deactivateSolenoids(wheel);
  }

  wheel.printData();

}

// Inflate the wheel
void inflate(Wheel &wheel) {
  // Make sure deflation is stopped
  digitalWrite(wheel.getDeflatePin(), LOW);
  digitalWrite(wheel.getInflatePin(), HIGH);

  float pressureStep_ = 5.0;
  float p = wheel.getPressure();
  p += pressureStep_;
  wheel.setPressure(p);
}

// Deflate the wheel
void deflate(Wheel &wheel) {
  // Make sure inflation is stopped
  digitalWrite(wheel.getInflatePin(), LOW);
  digitalWrite(wheel.getDeflatePin(), HIGH);

  float pressureStep_ = 5.0;
  float p = wheel.getPressure();
  p -= pressureStep_;
  wheel.setPressure(p);
}

// Function to deactivate a Wheel solenoids
void deactivateSolenoids(Wheel &wheel) {
  digitalWrite(wheel.getInflatePin(), LOW);
  digitalWrite(wheel.getDeflatePin(), LOW);
}

// Function to deactivate all solenoids
void deactivateAllSolenoids() {
  deactivateSolenoids(wheel1);
  deactivateSolenoids(wheel2);
  deactivateSolenoids(wheel3);
  deactivateSolenoids(wheel4);
}

// Function to calculate the height based on weight
float calculateWeightOffset(int weight) {
  float weightEffect = (weight + random(-3, 4)) * -1; // Adding randomness, Approximate the drop in height due to weight
  return weightEffect;
}

float calculatePressureOffset(int pressure) {
  float pressureEffect = (pressure + random(-3, 4)) * 1; // Adding randomness, Approximate the raise in height due to pressure
  return pressureEffect;
}