#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <TimerOne.h> // Include TimerOne library

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define ENCODER_CLK 2
#define ENCODER_DT 3
#define ENCODER_SW 4

#define PRESSURE_SENSOR_PIN A0
#define SOLENOID_PIN 9
#define BUZZER_PIN 8

volatile int encoderPos = 0;
volatile bool encoderChanged = false;

unsigned long lastButtonPress = 0;
const unsigned long debounceDelay = 50;

int targetBoost = 0;
int currentBoost = 0;
int warningThreshold = 20; // Example threshold value
bool inMenu = false;
bool settingWarning = false;

void setup() {
  Serial.begin(9600);

  pinMode(ENCODER_CLK, INPUT);
  pinMode(ENCODER_DT, INPUT);
  pinMode(ENCODER_SW, INPUT_PULLUP);

  pinMode(SOLENOID_PIN, OUTPUT);
  pinMode(BUZZER_PIN, OUTPUT);
  digitalWrite(BUZZER_PIN, LOW);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  attachInterrupt(digitalPinToInterrupt(ENCODER_CLK), updateEncoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENCODER_DT), updateEncoder, CHANGE);

  Timer1.initialize(1000000 / 100); // Initialize Timer1 with PWM frequency (100 Hz)
  Timer1.pwm(SOLENOID_PIN, 0); // Start PWM on SOLENOID_PIN with 0% duty cycle

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
}

void loop() {
  if (encoderChanged) {
    if (settingWarning) {
      warningThreshold = encoderPos; // Adjust warning threshold in warning setting mode
      encoderChanged = false;
      beep(); // Add beep sound when encoder turns
    } else if (inMenu) {
      targetBoost = encoderPos; // Adjust target boost in main menu
      encoderChanged = false;
      beep(); // Add beep sound when encoder turns
    } else {
      targetBoost = encoderPos; // Adjust target boost in normal mode
    }
  }

  if (digitalRead(ENCODER_SW) == LOW) {
    unsigned long currentMillis = millis();
    if (currentMillis - lastButtonPress > debounceDelay) {
      lastButtonPress = currentMillis;
      settingWarning = !settingWarning; // Toggle warning setting mode
      Serial.println("Encoder Button Pressed");
      beep(); // Beep on button press
    }
  }

  currentBoost = readPressureSensor();

  int pwmDutyCycle = 0;
  if (currentBoost >= targetBoost) {
    // If current boost is at or above target boost, activate solenoid with 90% duty cycle
    pwmDutyCycle = 90;
  } else if (currentBoost > targetBoost - 5) {
    // If current boost is within 5 units of target boost, adjust duty cycle
    int error = targetBoost - currentBoost;
    pwmDutyCycle = map(error, 0, 50, 10, 90); // Adjust duty cycle based on error
  } else {
    // If current boost is well below target boost, set duty cycle to 0%
    pwmDutyCycle = 0;
  }

  Timer1.setPwmDuty(SOLENOID_PIN, map(pwmDutyCycle, 0, 100, 0, 1023)); // Update PWM duty cycle

  if (currentBoost > warningThreshold) {
    digitalWrite(BUZZER_PIN, HIGH);
  } else {
    digitalWrite(BUZZER_PIN, LOW);
  }

  display.clearDisplay();

  if (settingWarning) {
    display.setTextSize(1);
    display.setCursor(0, 0);
    display.print("WARNING");

    display.setTextSize(3);
    display.setCursor(20, 20);
    display.print(warningThreshold / 100.0, 2);

    display.setTextSize(1);
    display.setCursor(90, 56);
    display.print("x100 KPA");
  } else {
    display.setTextSize(1);
    display.setCursor(0, 0);
    display.print("BOOST x100 KPA");

    display.setTextSize(3);
    display.setCursor(20, 20);
    display.print(currentBoost / 100.0, 2);

    display.setTextSize(1);
    display.setCursor(0, 56);
    display.print(targetBoost / 100.0, 2);

    display.setCursor(60, 56); // Adjusted to the left
    display.print("DUTY ");
    display.print(pwmDutyCycle);
    display.println("%");
  }

  display.display();
  delay(100);
}

void updateEncoder() {
  static int lastClkState = HIGH;
  static int lastDtState = HIGH;
  int clkState = digitalRead(ENCODER_CLK);
  int dtState = digitalRead(ENCODER_DT);

  if (clkState != lastClkState) {
    if (clkState == LOW && dtState == HIGH) {
      encoderPos++;
    } else if (clkState == LOW && dtState == LOW) {
      encoderPos--;
    }
    encoderChanged = true;
  }

  lastClkState = clkState;
  lastDtState = dtState;
}

int readPressureSensor() {
  int sensorValue = analogRead(PRESSURE_SENSOR_PIN);
  float voltage = sensorValue * (5.0 / 1023.0);
  int pressure = (voltage - 0.2) * 50; // Example conversion for MPX4250
  return pressure;
}

void beep() {
  // Add beep functionality if needed
}