#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>

#define LCD_ADDR     0x27
#define LCD_COLS     20
#define LCD_ROWS     4

// Motor control pins (example)
#define DOOR1_MOTOR_PWM      14    // PWM channel for Door 1
#define DOOR1_MOTOR_DIR      12    // direction pin
#define DOOR2_MOTOR_PWM      27    // PWM channel for Door 2
#define DOOR2_MOTOR_DIR      26    // direction pin

// Limit switches
#define DOOR1_LIMIT_OPEN     32
#define DOOR1_LIMIT_CLOSED   33
#define DOOR2_LIMIT_OPEN     25
#define DOOR2_LIMIT_CLOSED   13

// Command input pins
#define CMD_OPEN_DOOR1       4
#define CMD_OPEN_DOOR2       5
#define CMD_CLOSE_DOOR1      16
#define CMD_CLOSE_DOOR2      17
#define CMD_OPEN_BOTH        18
#define CMD_CLOSE_BOTH       19
#define CMD_EMERGENCY_OPEN   23

// Menu buttons
#define BTN_MENU  34
#define BTN_UP    35
#define BTN_DOWN  36
#define BTN_ENTER 39

// Objects
LiquidCrystal_I2C lcd(LCD_ADDR, LCD_COLS, LCD_ROWS);
Adafruit_MPU6050 mpuDoor1;
Adafruit_MPU6050 mpuDoor2A;   // door 2 uses two sensors to ensure levelling
Adafruit_MPU6050 mpuDoor2B;

// Calibration variables (loaded from NVM)
float door1Home = 0, door1Open = 90;
float door2AHom = 0, door2AOpen = 90;
float door2BHom = 0, door2BOpen = 90;

// Delay settings (seconds)
unsigned long openDelay1 = 0, openDelay2 = 0;
unsigned long closeDelay1 = 0, closeDelay2 = 0;
unsigned long emergencyDelay = 0;

// State variables
enum DoorState {CLOSED, OPENING, OPEN, CLOSING, STOPPED} door1State = CLOSED, door2State = CLOSED;
unsigned long commandTimestamp = 0;

void setup() {
  Serial.begin(115200);
  // Initialise LCD
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Hatch Door Control");

  // Initialise MPU sensors
  if (!mpuDoor1.begin()) { Serial.println("Door1 MPU not found!"); }
  if (!mpuDoor2A.begin()) { Serial.println("Door2A MPU not found!"); }
  if (!mpuDoor2B.begin()) { Serial.println("Door2B MPU not found!"); }

  // Set up motors (use LEDC for PWM)
  ledcAttach(DOOR1_MOTOR_PWM, 1000, 10);  // pin, frequency (Hz), resolution (bits)
  // to set the PWM duty cycle:
  ledcWrite(DOOR1_MOTOR_PWM, dutyValue);  // dutyValue is 0–1023 for 10‑bit resolution
  pinMode(DOOR1_MOTOR_DIR, OUTPUT);
  pinMode(DOOR2_MOTOR_DIR, OUTPUT);

  // Configure limit switches and input pins
  pinMode(DOOR1_LIMIT_OPEN, INPUT_PULLUP);
  pinMode(DOOR1_LIMIT_CLOSED, INPUT_PULLUP);
  pinMode(DOOR2_LIMIT_OPEN, INPUT_PULLUP);
  pinMode(DOOR2_LIMIT_CLOSED, INPUT_PULLUP);
  pinMode(CMD_OPEN_DOOR1, INPUT_PULLUP);
  pinMode(CMD_OPEN_DOOR2, INPUT_PULLUP);
  pinMode(CMD_CLOSE_DOOR1, INPUT_PULLUP);
  pinMode(CMD_CLOSE_DOOR2, INPUT_PULLUP);
  pinMode(CMD_OPEN_BOTH, INPUT_PULLUP);
  pinMode(CMD_CLOSE_BOTH, INPUT_PULLUP);
  pinMode(CMD_EMERGENCY_OPEN, INPUT_PULLUP);

  // Configure menu buttons
  pinMode(BTN_MENU,  INPUT_PULLUP);
  pinMode(BTN_UP,    INPUT_PULLUP);
  pinMode(BTN_DOWN,  INPUT_PULLUP);
  pinMode(BTN_ENTER, INPUT_PULLUP);

  // Load calibration values & delays from NVM (e.g., Preferences)
  // not shown here
}

void loop() {
  // Update command inputs
  handleCommands();

  // Manage door movements
  updateDoor(1);
  updateDoor(2);

  // Update display periodically
  static unsigned long lastDisplay = 0;
  if (millis() - lastDisplay > 500) {
    updateDisplay();
    lastDisplay = millis();
  }

  // Handle menu navigation
  handleMenu();
}

void handleCommands() {
  // Emergency open overrides everything
  if (!digitalRead(CMD_EMERGENCY_OPEN)) {
    commandTimestamp = millis();
    door1State = OPENING;
    door2State = OPENING;
    // Wait for emergencyDelay before starting (user adjustable)
    return;
  }
  // Individual commands
  if (!digitalRead(CMD_OPEN_DOOR1)) door1State = OPENING;
  if (!digitalRead(CMD_CLOSE_DOOR1)) door1State = CLOSING;
  if (!digitalRead(CMD_OPEN_DOOR2)) door2State = OPENING;
  if (!digitalRead(CMD_CLOSE_DOOR2)) door2State = CLOSING;
  if (!digitalRead(CMD_OPEN_BOTH)) { door1State = OPENING; door2State = OPENING; }
  if (!digitalRead(CMD_CLOSE_BOTH)) { door1State = CLOSING; door2State = CLOSING; }
}

void updateDoor(int door) {
  // Read angle(s) and limit switches
  float angle, angle2 = 0;
  bool limitOpen, limitClosed;

  if (door == 1) {
    sensors_event_t a, g, t;
    mpuDoor1.getEvent(&a, &g, &t);
    angle = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI; // simple pitch
    limitOpen   = !digitalRead(DOOR1_LIMIT_OPEN);
    limitClosed = !digitalRead(DOOR1_LIMIT_CLOSED);
  } else {
    sensors_event_t aA, gA, tA, aB, gB, tB;
    mpuDoor2A.getEvent(&aA, &gA, &tA);
    mpuDoor2B.getEvent(&aB, &gB, &tB);
    float angleA = atan2(aA.acceleration.y, aA.acceleration.z) * 180 / PI;
    float angleB = atan2(aB.acceleration.y, aB.acceleration.z) * 180 / PI;
    angle  = (angleA + angleB) / 2.0;   // average angle
    angle2 = angleA - angleB;           // difference for levelling
    limitOpen   = !digitalRead(DOOR2_LIMIT_OPEN);
    limitClosed = !digitalRead(DOOR2_LIMIT_CLOSED);
  }

  // Choose calibration variables
  float home, fullOpen;
  if (door == 1) { home = door1Home; fullOpen = door1Open; }
  else           { home = (door == 2 ? door2AHom : 0); fullOpen = (door == 2 ? door2AOpen : 90); }

  // Compute percentage open
  float percentOpen = (angle - home) / (fullOpen - home);
  if (percentOpen < 0) percentOpen = 0;
  if (percentOpen > 1) percentOpen = 1;

  // Determine state machine
  DoorState &state = (door == 1 ? door1State : door2State);
  int pwmChannel = (door == 1 ? 0 : 1);
  int dirPin     = (door == 1 ? DOOR1_MOTOR_DIR : DOOR2_MOTOR_DIR);

  switch (state) {
    case OPENING:
      // Ramp up PWM until near full open
      if (limitOpen || percentOpen >= 1.0) {
        state = OPEN; 
        ledcWrite(pwmChannel, 0); 
        break;
      }
      digitalWrite(dirPin, HIGH); // direction to open
      // Increase duty cycle gradually (implement ramp)
      ledcWrite(pwmChannel, computeRampDuty(percentOpen, true));
      break;
    case CLOSING:
      if (limitClosed || percentOpen <= 0.0) {
        state = CLOSED; 
        ledcWrite(pwmChannel, 0); 
        break;
      }
      digitalWrite(dirPin, LOW); // direction to close
      ledcWrite(pwmChannel, computeRampDuty(percentOpen, false));
      break;
    case OPEN:
    case CLOSED:
      ledcWrite(pwmChannel, 0);
      break;
    default:
      break;
  }
}

uint32_t computeRampDuty(float percentOpen, bool opening) {
  // Return a duty cycle between 0 and 1023 (10‑bit) based on door position.
  // When opening: ramp up until 30%, run at full speed, ramp down at 90%.
  if (opening) {
    if (percentOpen < 0.1) {
      return (uint32_t)(percentOpen * 10.0 * 1023); // 0→10% duty ramp
    } else if (percentOpen > 0.9) {
      return (uint32_t)((1.0 - percentOpen) * 10.0 * 1023); // ramp down
    } else {
      return 1023; // full speed
    }
  } else {
    // Closing: invert ramp based on (1 - percentOpen)
    float p = 1.0 - percentOpen;
    if (p < 0.1) {
      return (uint32_t)(p * 10.0 * 1023);
    } else if (p > 0.9) {
      return (uint32_t)((1.0 - p) * 10.0 * 1023);
    } else {
      return 1023;
    }
  }
}

void updateDisplay() {
  // Show door angles and status on LCD
  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("D1:");
  lcd.print(door1State == OPEN ? "OPEN " : (door1State == CLOSED ? "CLOSED" : "MOVING"));
  lcd.print(" ");
  lcd.print("D2:");
  lcd.print(door2State == OPEN ? "OPEN" : (door2State == CLOSED ? "CLOSED" : "MOVING"));

  lcd.setCursor(0,1);
  lcd.print("Ang1:");
  lcd.print(door1Home); // replace with actual angle variable if stored

  lcd.setCursor(10,1);
  lcd.print("Ang2:");
  // display average angle for door2
  lcd.print("N/A");
}

void handleMenu() {
  // Simple menu handling (pseudo)
  static int menuIndex = 0;
  static bool inMenu = false;
  if (buttonPressed(BTN_MENU)) {
    inMenu = !inMenu;
    lcd.clear();
    return;
  }
  if (!inMenu) return;
  // Navigate menu with UP/DOWN and adjust values with ENTER
  // e.g., calibrate sensors by reading current angle and storing to home/fullOpen variables
}

bool buttonPressed(int pin) {
  static unsigned long lastDebounce = 0;
  static bool lastState = HIGH;
  bool reading = digitalRead(pin);
  if (reading != lastState) {
    lastDebounce = millis();
  }
  if ((millis() - lastDebounce) > 50) {
    if (reading == LOW && lastState == HIGH) {
      lastState = reading;
      return true;
    }
  }
  lastState = reading;
  return false;
}