#include <Arduino.h>
#include <QuickPID.h>

// Pin Definitions
const int compressorPin = 7;        // Pin to control the compressor
const int deflateValvePin = 8;      // Pin to control the deflation valve
const int pressureSensorPin = 4;    // Analog pin for the pressure sensor

// Constants
const int maxCompressorRunTime = 10000;  // Maximum time to run the compressor (ms)
const int minCompressorRunTime = 1000;   // Minimum time to run the compressor (ms)
const int stabilizationTime = 2000;     // Time to stabilize (ms)
const float tolerance = 0.5;            // Pressure tolerance
float setpoint = 50.0;            // Desired pressure

// Variables
float currentPressure = 0.0;
float output = 0.0;                     // PID output
float kp = 1.0, ki = 0.1, kd = 0.01;    // PID constants
bool pressureMet = false;

// QuickPID Instance
QuickPID myPID(&currentPressure, &output, &setpoint, kp, ki, kd, QuickPID::Action::direct);

// Function to read pressure from a sensor
float readPressureSensor() {
    int sensorValue = analogRead(pressureSensorPin);  // Read analog input
    float pressure = map(sensorValue, 0, 4095, 0, 100);  // Map to pressure range (0-100 psi)
    Serial.print("Pressure Read: ");
    Serial.println(pressure);
    return pressure;
}

// Function to calculate compressor/deflation run time based on gap
int calculateCompressorRunTime(float gap) {
    float normalizedGap = gap / setpoint;  // Normalize gap to a 0-1 scale
    int runTime = minCompressorRunTime + (maxCompressorRunTime - minCompressorRunTime) * normalizedGap;
    Serial.print("Calculated Compressor Run Time: ");
    Serial.println(runTime);
    return constrain(runTime, minCompressorRunTime, maxCompressorRunTime);
}

// Compressor control functions
void turnCompressorOn() {
    digitalWrite(compressorPin, HIGH);
    digitalWrite(deflateValvePin, LOW);  // Ensure deflation valve is off
    Serial.println("Compressor ON");
}

void turnDeflateValveOn() {
    digitalWrite(deflateValvePin, HIGH);
    digitalWrite(compressorPin, LOW);  // Ensure compressor is off
    Serial.println("Deflate Valve ON");
}

void turnCompressorOff() {
    digitalWrite(compressorPin, LOW);
    digitalWrite(deflateValvePin, LOW);
    Serial.println("System OFF");
}

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

    // Initialize pins
    pinMode(compressorPin, OUTPUT);
    pinMode(deflateValvePin, OUTPUT);
    pinMode(pressureSensorPin, INPUT);
    turnCompressorOff();  // Ensure everything is off initially

    // Initialize QuickPID
    myPID.SetMode(QuickPID::Control::automatic);
    myPID.SetOutputLimits(-1.0, 1.0);  // Output range for deflate (-1.0) to inflate (1.0)

    Serial.println("System Initialized.");
}

// Arduino loop function
void loop() {
   float gap = abs(setpoint - currentPressure1);

 if(currentPressure1 > setpoint)
 {
  Stop();
  delay(1000);
  return;
 }

 if(gap > 1)
 {
  pressureMet = false;
 }

 if (!pressureMet) {

        myPID.Compute();
        if(gap <= tolerance)
        {
          pressureMet = true;
          Stop();
          delay(stabilizationTime);
          return;
        }
        int runTime = calculateCompressorRunTime(gap);
        if(output > 0.0)
        {
          Inflate();
          delay(runTime);
        }
        else if  (output < 0.0) // deflate
        {
          Deflate();
          delay(runTime);
        }
        else
        {
          Stop();
          delay(1000);
        }
        Stop();
        delay(stabilizationTime);

        // Read the pressure sensor after stabilization
        currentPressure = tempSensor1.pressure();
        Serial.print("Current Pressure (after stabilization): ");
        Serial.println(currentPressure);
    } else {
        // If pressure is met, keep the compressor off
        Stop();
        delay(1000);  // Add a delay to avoid spamming the loop
    }
}