#include <Arduino.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>

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

// ─── PINS ───────────────────────────────────────────────
#define BTN_PIN     2
#define LED_G       4
#define LED_Y       5
#define LED_R       19

#define SERVO_A_PIN 13
#define SERVO_B_PIN 14
#define SERVO_C_PIN 32

#define STEP_PIN    25
#define DIR_PIN     26
#define EN_PIN      27

// ─── SERVOS ─────────────────────────────────────────────
Servo servoA, servoB, servoC;

// ─── STEPPER CONFIG ─────────────────────────────────────
#define STEPS_PER_REV 200
#define STEP_DELAY_US 1200

// ─── FAKE PRESSURE VALUES (kPa) ─────────────────────────
float ps1 = 101.3;
float ps2 =  99.8;
float ps3 = 102.1;
float ps4 = 100.5;

// ─── STATE ──────────────────────────────────────────────
bool activated    = false;
bool lastBtnState = HIGH;

// ────────────────────────────────────────────────────────
void stepMotor(int steps, bool forward) {
  digitalWrite(DIR_PIN, forward ? HIGH : LOW);
  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(STEP_DELAY_US);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(STEP_DELAY_US);
  }
}

// ────────────────────────────────────────────────────────
void updateDisplay() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(10, 0);
  display.println("-- HATCH MONITOR --");
  display.drawLine(0, 9, 127, 9, SSD1306_WHITE);

  display.setCursor(0, 13);
  display.print("PS1 TubeA: ");
  display.print(ps1, 1);
  display.println(" kPa");

  display.setCursor(0, 23);
  display.print("PS2 TubeB: ");
  display.print(ps2, 1);
  display.println(" kPa");

  display.setCursor(0, 33);
  display.print("PS3 TubeC: ");
  display.print(ps3, 1);
  display.println(" kPa");

  display.setCursor(0, 43);
  display.print("PS4 Hab:   ");
  display.print(ps4, 1);
  display.println(" kPa");

  display.drawLine(0, 54, 127, 54, SSD1306_WHITE);

  display.setCursor(0, 56);
  if (activated) {
    display.println("STATUS: SEAL ACTIVE");
  } else {
    display.println("STATUS: STANDBY    ");
  }

  display.display();
}

// ────────────────────────────────────────────────────────
void setLEDs(bool green, bool yellow, bool red) {
  digitalWrite(LED_G, green  ? HIGH : LOW);
  digitalWrite(LED_Y, yellow ? HIGH : LOW);
  digitalWrite(LED_R, red    ? HIGH : LOW);
}

// ────────────────────────────────────────────────────────
void activateSequence() {
  setLEDs(false, true, false);
  updateDisplay();

  servoA.write(90);
  servoB.write(90);
  servoC.write(90);
  delay(600);

  digitalWrite(EN_PIN, LOW);
  stepMotor(STEPS_PER_REV, true);

  setLEDs(true, false, false);
  activated = true;
  updateDisplay();
}

// ────────────────────────────────────────────────────────
void deactivateSequence() {
  setLEDs(false, true, false);
  updateDisplay();

  servoA.write(0);
  servoB.write(0);
  servoC.write(0);
  delay(600);

  stepMotor(STEPS_PER_REV, false);
  digitalWrite(EN_PIN, HIGH);

  setLEDs(false, false, true);
  activated = false;
  updateDisplay();
}

// ────────────────────────────────────────────────────────
void setup() {
  Serial.begin(115200);

  pinMode(LED_G,    OUTPUT);
  pinMode(LED_Y,    OUTPUT);
  pinMode(LED_R,    OUTPUT);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(DIR_PIN,  OUTPUT);
  pinMode(EN_PIN,   OUTPUT);
  pinMode(BTN_PIN,  INPUT);

  digitalWrite(EN_PIN, HIGH);

  servoA.attach(SERVO_A_PIN);
  servoB.attach(SERVO_B_PIN);
  servoC.attach(SERVO_C_PIN);
  servoA.write(0);
  servoB.write(0);
  servoC.write(0);

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 init failed");
    while (true);
  }
  display.clearDisplay();
  display.display();

  setLEDs(false, false, true);
  updateDisplay();
}

// ────────────────────────────────────────────────────────
void loop() {
  bool btnState = digitalRead(BTN_PIN);

  if (btnState == HIGH && lastBtnState == LOW) {
    delay(50);
    if (!activated) {
      activateSequence();
    } else {
      deactivateSequence();
    }
  }
  lastBtnState = btnState;

  ps1 += random(-10, 11) * 0.01;
  ps2 += random(-10, 11) * 0.01;
  ps3 += random(-10, 11) * 0.01;
  ps4 += random(-10, 11) * 0.01;

  updateDisplay();
  delay(300);
}