/*
  04 Tires Control Pressure
*/

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SETTING_1_BUTTON 2
#define SETTING_2_BUTTON 3

Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &Wire);

// Define the analog input pins
const int analogInputPins[] = {A0, A1, A2, A3};
const int numAnalogInputs = 4;
const int analogSetpoint = A6;
// Define the digital output pings
const int controlOutputPins_I[] = {4, 5, 6, 7};
const int controlOutputPins_D[] = {8, 9, 10, 11};
int controlOutputState[] = {0, 0, 0, 0};
unsigned long controlOutputDelay[] = {0, 0, 0, 0};

// Conversion factors
const float voltageReference = 5.0;  // Voltage reference of Arduino Nano (in volts)
const float maxPressure = 10.3421;   // Maximum pressure corresponding to 5V - 150 PIS - 10.3421 bar)
// Variables for pressure
float pressureValues[numAnalogInputs] = {0};

// Setpoint and control variables for each analog input
int setpointsInt[numAnalogInputs] = {495, 495, 495, 495};  // Default setpoint values
float setpoints[numAnalogInputs] = {5.0, 5.0, 5.0, 5.0};  // Default setpoint values
int setpointInt = 0;
float setpoint = 0;

int buttonState[2] = {1, 1};          // Pull up
int lastButtonState[2] = {1, 1};      // Pull up
unsigned long lastPressTime[2] = {0, 0};
unsigned long DebounceTime[2] = {0, 0};
unsigned long debounceDelay = 50;
unsigned long longpress = 1000;

enum State {
  SETTING_SETPOINT,
  RUNNING_CONTROL
};

State currentState = SETTING_SETPOINT;

void setup() 
{
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);

  pinMode(SETTING_1_BUTTON, INPUT_PULLUP);
  pinMode(SETTING_2_BUTTON, INPUT_PULLUP);

    // Display initialization
  display.display();
  delay(2000);
  display.clearDisplay();

  // Print a welcome message
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Automatic Pressure");
  display.display();
  delay(50);

  // Set digital output pins for control values
  for (int i = 0; i < numAnalogInputs; i++) {
    pinMode(controlOutputPins_I[i], OUTPUT);
    digitalWrite(controlOutputPins_I[i], LOW);

    pinMode(controlOutputPins_D[i], OUTPUT);
    digitalWrite(controlOutputPins_D[i], LOW);
  }
}

void loop() 
{

  // Read the analog values
  int sensorValues[numAnalogInputs];
  for (int i = 0; i < numAnalogInputs; i++) {
    // Only read tires when no loading/unloading pressure
    if (controlOutputState[i] == 0 ){
      sensorValues[i] = analogRead(analogInputPins[i]);
      // Convert analog readings to pressure values
      float pressureValue = map(sensorValues[i], 0, 1023, 0, maxPressure * 1000) / 1000.0;  // Scale from 0-1023 to 0-maxPressure
      pressureValues[i] = pressureValue;
    }
  }
  setpointInt = analogRead(analogSetpoint);
  setpoint = map(setpointInt, 0, 1023, 0, maxPressure * 1000) / 1000.0;  // Scale from 0-1023 to 0-maxPressure

  // Read the button state with debouncing
  int reading[2] = {0, 0};  
  reading[0] = digitalRead(SETTING_1_BUTTON);
  reading[1] = digitalRead(SETTING_2_BUTTON);
  for(int i = 0; i<2; i++){
    if (reading[i] != lastButtonState[i]) {
      if (reading[i] == HIGH){
        lastPressTime[i] = millis() - DebounceTime[i];
      }
      DebounceTime[i] = millis();
    }

    // Debounce time for both buttoms
    if ((millis() - DebounceTime[i]) > debounceDelay) {
      if (reading[i] != buttonState[i]) {
        buttonState[i] = reading[i];
      }
    }
    // Update state
    lastButtonState[i] = reading[i];
  }

  // Buttom states - State Change
  if ((buttonState[0] == 1) && (buttonState[1] == 1) && (lastPressTime[0] > longpress) && (lastPressTime[1] > longpress)){
    // One Shot
    lastPressTime[0] = 0;
    lastPressTime[1] = 0;

    // Default to measure pressure
    controlOutputState[0] = 0;
    controlOutputState[1] = 0;
    controlOutputState[2] = 0;
    controlOutputState[3] = 0;

    if (currentState == SETTING_SETPOINT) {
      currentState = RUNNING_CONTROL;
    } else{
      currentState = SETTING_SETPOINT;
    }
  }else if (currentState == SETTING_SETPOINT){
    if (buttonState[0] == 1 &&  lastPressTime[0] > 100){
      lastPressTime[0] = 0;
      // Update Front Tires
      setpointsInt[0] = setpointInt; 
      setpoints[0] = setpoint;  // Scale from 0-1023 to 0-maxPressure

      setpointsInt[1] = setpointInt;
      setpoints[1] = setpoint;
    }else if (buttonState[1] == 1 &&  lastPressTime[1] > 100){
      lastPressTime[1] = 0;
      // Update Rear Tires
      setpointsInt[2] = setpointInt;
      setpoints[2] = setpoint;  // Scale from 0-1023 to 0-maxPressure

      setpointsInt[3] = setpointInt;
      setpoints[3] = setpoint;
    }  
  }

  // Control loop
  if (currentState == RUNNING_CONTROL) {
    // Compare the analog values with the setpoint
    for (int i = 0; i < numAnalogInputs; i++) {
      if (abs(sensorValues[i] - setpointsInt[i]) < 10){
        digitalWrite(controlOutputPins_I[i], 0);
        digitalWrite(controlOutputPins_D[i], 0);
        controlOutputState[i] = 0;
      }else{
        if (controlOutputState[i] == 1 && (millis() - controlOutputDelay[i] > 5000)){
          controlOutputState[i] = 0;
          controlOutputDelay[i] = millis();
          digitalWrite(controlOutputPins_I[i], LOW);
          digitalWrite(controlOutputPins_D[i], LOW);
        }
        if (controlOutputState[i] == 0 && (millis() - controlOutputDelay[i] > 1000)){
          controlOutputState[i] = 1;
          controlOutputDelay[i] = millis();
        }

        if (controlOutputState[i] == 1 && (millis() - controlOutputDelay[i] < 5000)){
          if (sensorValues[i] < setpointsInt[i]) {
            digitalWrite(controlOutputPins_I[i], 1);
            digitalWrite(controlOutputPins_D[i], 0);
          }else {
            digitalWrite(controlOutputPins_I[i], 0);
            digitalWrite(controlOutputPins_D[i], 1);
          }
        }
      }
    }
  }else{
    // Set digital output pins for control values
    for (int i = 0; i < numAnalogInputs; i++) {
      digitalWrite(controlOutputPins_I[i], LOW);
      digitalWrite(controlOutputPins_D[i], LOW);
      controlOutputState[i] = 0;
    }
  }

  // Display depending of STATE
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Automatic Pressure");
  if (currentState == SETTING_SETPOINT) {
    display.println("Setting Pressure");
    for (int i = 0; i < numAnalogInputs; i++) {
      display.print("SP");
      display.print(i+1);
      display.print(": ");
      display.print(pressureValues[i], 1);  // Display pressure with 3 decimal places
      display.print("/");
      display.print(setpoints[i], 1);
      display.println(" bar");
    }
    display.print("Setpoint: ");
    display.print(setpoint, 1);  // Display pressure with 3 decimal places
    display.println(" bar");
  }else{
    display.println("Controlling Pressure");
    for (int i = 0; i < numAnalogInputs; i++) {
      display.print("CP");
      display.print(i+1);
      display.print(": ");
      display.print(pressureValues[i], 1);  // Display pressure with 3 decimal places
      display.print("/");
      display.print(setpoints[i], 1);
      if (abs(sensorValues[i] - setpointsInt[i]) < 10){
        display.println("bar -OK");
      }
      else{
        display.println("bar");
      }
    }
  }
  display.display();
  // Wait for a short duration before updating again
  delay(50);
}