#include <U8g2lib.h>
#include <TimerOne.h>

#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;

// Initialize the U8g2 library for BTT Mini 12864
U8G2_ST7920_128X64_F_SW_SPI u8g2(U8G2_R0, /* clock=*/ 13, /* data=*/ 11, /* CS=*/ 10, /* reset=*/ 9);

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);

  u8g2.begin();

  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
}

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);
  }

  u8g2.clearBuffer();

  if (settingWarning) {
    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.drawStr(0, 10, "WARNING");

    u8g2.setFont(u8g2_font_fur20_tf);
    u8g2.setCursor(20, 40);
    u8g2.print(warningThreshold / 100.0, 2);

    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.drawStr(90, 60, "x100 KPA");
  } else {
    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.drawStr(0, 10, "BOOST x100 KPA");

    u8g2.setFont(u8g2_font_fur20_tf);
    u8g2.setCursor(20, 40);
    u8g2.print(currentBoost / 100.0, 2);

    u8g2.setFont(u8g2_font_ncenB08_tr);
    u8g2.setCursor(0, 60);
    u8g2.print(targetBoost / 100.0, 2);

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

  u8g2.sendBuffer();
  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
}