#include <Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

// ---- Pins
const int PIN_SERVO_PALM   = 3;
const int PIN_SERVO_FINGER = 4;
const int PIN_LED          = 7;   // READY indicator (HIGH=READY)
const int PIN_BTN_FAULT    = 6;   // press -> FAULT (freeze)
const int PIN_BTN_RESET    = 12;  // press -> exit FAULT (home or hold)
const int PIN_F_FWD        = 8;   // Finger forward (hold)
const int PIN_F_BACK       = 9;   // Finger back   (hold)
const int PIN_P_FWD        = 10;  // Palm forward  (hold)
const int PIN_P_BACK       = 11;  // Palm back     (hold)

// ---- Servos
Servo servoPalm, servoFinger;

// ---- Motion config
const int SERVO_MIN     = 10;
const int SERVO_MAX     = 170;
const int SERVO_HOME    = 90;
const int STEP_DEG      = 2;
const unsigned long STEP_MS = 15;

// ---- State
enum SysState { READY_STATE, FAULT_STATE };
SysState state = READY_STATE;

bool attachedServos = false;
int anglePalm   = SERVO_HOME;
int angleFinger = SERVO_HOME;

// After FAULT, if any move button is pressed, we go HOME on reset
bool goHomeOnReset = false;

unsigned long lastStepMs = 0;

// ---- LCD status
char statusMsg[17] = "Ready";
unsigned long statusExpiryMs = 0;
const unsigned long STATUS_HOLD_MS = 900;

inline bool pressed(int pin) { return digitalRead(pin) == LOW; }

void setStatus(const char* msg) {
  snprintf(statusMsg, sizeof(statusMsg), "%-16.16s", msg);
  statusExpiryMs = millis() + STATUS_HOLD_MS;
}

void lcdAnglesLine() {
  lcd.setCursor(0,0);
  char line1[17];
  snprintf(line1, sizeof(line1), "P:%3d  F:%3d   ", anglePalm, angleFinger);
  lcd.print(line1);
}
void lcdStatusLine() {
  if (millis() > statusExpiryMs) {
    snprintf(statusMsg, sizeof(statusMsg),
             (state==READY_STATE) ? "Ready          " : (goHomeOnReset ? "FAULT: HomeArm " : "FAULT           "));
  }
  lcd.setCursor(0,1);
  lcd.print(statusMsg);
}
void updateLCD() { lcdAnglesLine(); lcdStatusLine(); }

void clampAngles() {
  if (anglePalm   < SERVO_MIN) anglePalm   = SERVO_MIN;
  if (anglePalm   > SERVO_MAX) anglePalm   = SERVO_MAX;
  if (angleFinger < SERVO_MIN) angleFinger = SERVO_MIN;
  if (angleFinger > SERVO_MAX) angleFinger = SERVO_MAX;
}

void attachIfNeeded() {
  if (!attachedServos) {
    servoPalm.attach(PIN_SERVO_PALM);
    servoFinger.attach(PIN_SERVO_FINGER);
    attachedServos = true;
  }
}
void detachIfAttached() {
  if (attachedServos) {
    servoPalm.detach();
    servoFinger.detach();
    attachedServos = false;
  }
}
void writeServos() {
  if (!attachedServos) return;
  servoPalm.write(anglePalm);
  servoFinger.write(angleFinger);
}

// ----- movements (READY only)
void handleMovements() {
  if (state != READY_STATE) return;

  bool moved = false;
  if (pressed(PIN_F_FWD))  { attachIfNeeded(); angleFinger += STEP_DEG; setStatus("Finger FWD");  moved = true; }
  if (pressed(PIN_F_BACK)) { attachIfNeeded(); angleFinger -= STEP_DEG; setStatus("Finger BACK"); moved = true; }
  if (pressed(PIN_P_FWD))  { attachIfNeeded(); anglePalm   += STEP_DEG; setStatus("Palm FWD");    moved = true; }
  if (pressed(PIN_P_BACK)) { attachIfNeeded(); anglePalm   -= STEP_DEG; setStatus("Palm BACK");   moved = true; }

  if (moved) { clampAngles(); writeServos(); }
}

// ----- FAULT: freeze here (do not go home)
void handleFault() {
  static bool last = false;
  bool now = pressed(PIN_BTN_FAULT);
  if (!last && now && state != FAULT_STATE) {
    state = FAULT_STATE;
    digitalWrite(PIN_LED, LOW);
    goHomeOnReset = false;       // not armed yet
    // stay attached so angles hold; or detachIfAttached() if you want limp
    setStatus("FAULT (frozen)");
  }
  last = now;

  // While in FAULT, watch for a tap on any move button to arm "home on reset"
  if (state == FAULT_STATE) {
    if (pressed(PIN_F_FWD) || pressed(PIN_F_BACK) || pressed(PIN_P_FWD) || pressed(PIN_P_BACK)) {
      goHomeOnReset = true;
      setStatus("Home armed");
    }
  }
}

// ----- RESET: if armed -> HOME; else -> keep frozen angles
void handleReset() {
  static bool last = false;
  bool now = pressed(PIN_BTN_RESET);
  if (!last && now) {
    if (state == FAULT_STATE) {
      if (goHomeOnReset) { anglePalm = SERVO_HOME; angleFinger = SERVO_HOME; }
      // else keep angles as-is (frozen)
      state = READY_STATE;
      digitalWrite(PIN_LED, HIGH);
      attachIfNeeded();
      writeServos();
      setStatus(goHomeOnReset ? "Reset->HOME" : "Reset->Hold");
      goHomeOnReset = false; // clear for next fault
    } else {
      // optional: allow reset in READY to go home
      anglePalm = SERVO_HOME; angleFinger = SERVO_HOME; writeServos();
      setStatus("HOME");
    }
  }
  last = now;
}

void setup() {
  // inputs
  pinMode(PIN_BTN_FAULT, INPUT_PULLUP);
  pinMode(PIN_BTN_RESET, INPUT_PULLUP);
  pinMode(PIN_F_FWD,     INPUT_PULLUP);
  pinMode(PIN_F_BACK,    INPUT_PULLUP);
  pinMode(PIN_P_FWD,     INPUT_PULLUP);
  pinMode(PIN_P_BACK,    INPUT_PULLUP);

  // outputs
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED, HIGH); // READY at start

  // servos
  servoPalm.attach(PIN_SERVO_PALM);
  servoFinger.attach(PIN_SERVO_FINGER);
  attachedServos = true;
  anglePalm = SERVO_HOME;
  angleFinger = SERVO_HOME;
  writeServos();

  // lcd
  lcd.init(); lcd.backlight(); lcd.clear();
  lcd.setCursor(0,0); lcd.print("Assistive Hand ");
  lcd.setCursor(0,1); lcd.print("READY          ");
  delay(500); lcd.clear();

  setStatus("Ready");
  updateLCD();
}

void loop() {
  handleFault();
  handleReset();

  unsigned long now = millis();
  if (now - lastStepMs >= STEP_MS) {
    lastStepMs = now;
    handleMovements();
    updateLCD();
  }
}