/*
  Eye Mechanism Control - Animation Only
  
  Wiring:
  - Servos: LR=D22, UD=D23, TL=D24, BL=D25, TR=D26, BR=D27
  - No external inputs (switches/pots) required.
*/

#include <Servo.h>

// --- Pin Definitions ---
const int PIN_SERVO_LR = 22;
const int PIN_SERVO_UD = 23;
const int PIN_SERVO_TL = 24;
const int PIN_SERVO_BL = 25;
const int PIN_SERVO_TR = 26;
const int PIN_SERVO_BR = 27;
const int PIN_SERVO_JAW_LEFT = 28;
const int PIN_SERVO_JAW_RIGHT = 29;
const int PIN_SERVO_BROW_LL = 30;
const int PIN_SERVO_BROW_LR = 31;
const int PIN_SERVO_BROW_RR = 32;
const int PIN_SERVO_BROW_RL = 33;

// --- Servo Objects ---
Servo servoLR;
Servo servoUD;
Servo servoTL;
Servo servoBL;
Servo servoTR;
Servo servoBR;
Servo servoJawLeft;
Servo servoJawRight;
Servo servoBrowLL;
Servo servoBrowLR;
Servo servoBrowRR;
Servo servoBrowRL;

// --- Limits & State ---
struct ServoLimit {
  int minAngle;
  int maxAngle;
};

// Default Limits
ServoLimit limitLR = {60, 120};
ServoLimit limitUD = {60, 120};
ServoLimit limitTL = {110, 40}; // Min=Closed(110), Max=Open(40)
ServoLimit limitBL = {70, 130}; // Min=Closed(70), Max=Open(130)
ServoLimit limitTR = {50, 140}; // Min=Closed(50), Max=Open(140)
ServoLimit limitBR = {120, 70}; // Min=Closed(120), Max=Open(70)

// Jaw Limits (reduced travel for safer, subtler motion)
ServoLimit limitJawLeft = {0, 15}; // Min=Closed(0), Max=Open(15)
// Eyebrow Limits (Min=Bottom, Max=Top)
// Left-Left: 70 -> 30
// Left-Right: 60 -> 90
// Right-Right: 130 -> 180
// Right-Left: 30 -> 0
// Eyebrow safe ranges normalized to 35 degrees of travel each,
// using current aligned posture as reference and avoiding 0-deg endpoints.
ServoLimit limitBrowLL = {42, 7};   // 35 deg travel
ServoLimit limitBrowLR = {35, 70};  // 35 deg travel, shifted 15 deg downward overall
ServoLimit limitBrowRR = {137, 172}; // 35 deg travel
ServoLimit limitBrowRL = {36, 1};   // 35 deg travel
// Right jaw is calculated relative to left (180 - left)

// --- Serial Control State ---
enum MotionMode {
  MOTION_IDLE,
  MOTION_WAIT,
  MOTION_TALK
};

MotionMode currentMode = MOTION_IDLE;
int talkJawPercent = 0;
unsigned long lastTalkUpdateMs = 0;
const unsigned long TALK_TIMEOUT_MS = 350;
unsigned long nextTalkBrowUpdateMs = 0;
char serialLine[64];
uint8_t serialLineLen = 0;

void serviceSerial();
void smartDelay(unsigned long delayMs);
void applyJawPercent(int percent);
void setMotionMode(MotionMode mode);
void handleSerialCommand(const char* command);
void updateLidsForUD(int udAngle);
void centerEyes();
void moveEyesNatural(int targetLR, int targetUD, int durationMs);
void moveLidsTo(int targetTL, int targetTR, int targetBL, int targetBR, int durationMs);
void blink();
void moveBrowsSplitSmooth(int leftPercent, int rightPercent, int durationMs);
float smoothStep(float t);
int lerpInt(int start, int end, float t);
void setBrowsSplit(int leftPercent, int rightPercent);
void setBrows(int percent);
void setBrowsNeutral();
void runIdleAnimationStep();
void runWaitAnimationStep();
void runTalkAnimationStep();

void setup() {
  Serial.begin(9600);
  
  // Initialize Random Seed
  // On Mega, A0 is a good floating pin if nothing is connected
  randomSeed(analogRead(A0)); 

  // Attach Servos
  servoLR.attach(PIN_SERVO_LR);
  servoUD.attach(PIN_SERVO_UD);
  servoTL.attach(PIN_SERVO_TL);
  servoBL.attach(PIN_SERVO_BL);
  servoTR.attach(PIN_SERVO_TR);
  servoBR.attach(PIN_SERVO_BR);
  
  // Attach Jaw Servos
  servoJawLeft.attach(PIN_SERVO_JAW_LEFT);
  servoJawRight.attach(PIN_SERVO_JAW_RIGHT);
  
  // Attach Eyebrow Servos
  servoBrowLL.attach(PIN_SERVO_BROW_LL);
  servoBrowLR.attach(PIN_SERVO_BROW_LR);
  servoBrowRR.attach(PIN_SERVO_BROW_RR);
  servoBrowRL.attach(PIN_SERVO_BROW_RL);
  
  // Set Fixed Jaw Position
  servoJawLeft.write(0);
  servoJawRight.write(180);
  
  // Set Eyebrows to Neutral (50%)
  setBrowsNeutral();
  
  Serial.println(F("Eye Mechanism Control - Animation Mode Ready."));
  Serial.println(F("Commands: MODE TALK | MODE WAIT | MODE IDLE | MODE AUTO | JAW <0-100> | PING"));

  // Initial Position (Center)
  centerEyes();
  setMotionMode(MOTION_IDLE);
  delay(500);
}

void applyJawPercent(int percent) {
  percent = constrain(percent, 0, 100);
  talkJawPercent = percent;
  int targetLeft = map(percent, 0, 100, limitJawLeft.minAngle, limitJawLeft.maxAngle);
  servoJawLeft.write(targetLeft);
  servoJawRight.write(180 - targetLeft);
}

void setMotionMode(MotionMode mode) {
  currentMode = mode;
  if (currentMode == MOTION_TALK) {
    lastTalkUpdateMs = millis();
    nextTalkBrowUpdateMs = 0;
  } else {
    applyJawPercent(0);
    if (currentMode == MOTION_IDLE) {
      setBrowsNeutral();
    }
  }
}

void handleSerialCommand(const char* command) {
  if (strcmp(command, "MODE TALK") == 0) {
    setMotionMode(MOTION_TALK);
    Serial.println(F("OK MODE TALK"));
    return;
  }

  if (strcmp(command, "MODE WAIT") == 0) {
    setMotionMode(MOTION_WAIT);
    Serial.println(F("OK MODE WAIT"));
    return;
  }

  if (strcmp(command, "MODE IDLE") == 0) {
    setMotionMode(MOTION_IDLE);
    Serial.println(F("OK MODE IDLE"));
    return;
  }

  if (strcmp(command, "MODE AUTO") == 0) {
    setMotionMode(MOTION_IDLE);
    Serial.println(F("OK MODE AUTO"));
    return;
  }

  if (strcmp(command, "PING") == 0) {
    Serial.println(F("PONG"));
    return;
  }

  if (strncmp(command, "JAW ", 4) == 0) {
    int percent = atoi(command + 4);
    percent = constrain(percent, 0, 100);
    lastTalkUpdateMs = millis();
    if (currentMode == MOTION_TALK) {
      applyJawPercent(percent);
    }
    Serial.print(F("OK JAW "));
    Serial.println(percent);
    return;
  }

  Serial.print(F("ERR "));
  Serial.println(command);
}

void serviceSerial() {
  while (Serial.available() > 0) {
    char c = (char)Serial.read();
    if (c == '\n' || c == '\r') {
      if (serialLineLen > 0) {
        serialLine[serialLineLen] = '\0';
        handleSerialCommand(serialLine);
        serialLineLen = 0;
      }
    } else if (c >= 32 && c <= 126) {
      if (serialLineLen < sizeof(serialLine) - 1) {
        serialLine[serialLineLen++] = c;
      }
    }
  }

  // Safety fallback: if host stops sending jaw updates in talk mode, close jaw.
  if (currentMode == MOTION_TALK && (millis() - lastTalkUpdateMs > TALK_TIMEOUT_MS)) {
    applyJawPercent(0);
  }
}

void smartDelay(unsigned long delayMs) {
  unsigned long start = millis();
  while (millis() - start < delayMs) {
    serviceSerial();
    delay(2);
  }
}

// Set all servos to Center (90)
void centerEyes() {
  servoLR.write(90);
  servoUD.write(90);
  updateLidsForUD(90);
}

float smoothStep(float t) {
  t = constrain(t, 0.0f, 1.0f);
  return t * t * (3.0f - 2.0f * t);
}

int lerpInt(int start, int end, float t) {
  return start + (int)((end - start) * t);
}

void moveLidsTo(int targetTL, int targetTR, int targetBL, int targetBR, int durationMs) {
  int startTL = servoTL.read();
  int startTR = servoTR.read();
  int startBL = servoBL.read();
  int startBR = servoBR.read();

  int maxDelta = max(max(abs(targetTL - startTL), abs(targetTR - startTR)),
                     max(abs(targetBL - startBL), abs(targetBR - startBR)));
  if (maxDelta <= 0) {
    return;
  }

  int steps = max(4, maxDelta);
  int stepDelayMs = max(2, durationMs / steps);

  for (int i = 1; i <= steps; ++i) {
    float t = smoothStep((float)i / (float)steps);
    servoTL.write(lerpInt(startTL, targetTL, t));
    servoTR.write(lerpInt(startTR, targetTR, t));
    servoBL.write(lerpInt(startBL, targetBL, t));
    servoBR.write(lerpInt(startBR, targetBR, t));
    smartDelay(stepDelayMs);
  }
}

void moveEyesNatural(int targetLR, int targetUD, int durationMs) {
  targetLR = constrain(targetLR, limitLR.minAngle, limitLR.maxAngle);
  targetUD = constrain(targetUD, limitUD.minAngle, limitUD.maxAngle);

  int startLR = servoLR.read();
  int startUD = servoUD.read();
  int maxDelta = max(abs(targetLR - startLR), abs(targetUD - startUD));
  if (maxDelta <= 0) {
    return;
  }

  int steps = max(5, maxDelta);
  int stepDelayMs = max(2, durationMs / steps);

  for (int i = 1; i <= steps; ++i) {
    float t = smoothStep((float)i / (float)steps);
    int lr = lerpInt(startLR, targetLR, t);
    int ud = lerpInt(startUD, targetUD, t);

    servoLR.write(lr);
    servoUD.write(ud);
    updateLidsForUD(ud);
    smartDelay(stepDelayMs + random(0, 2));
  }
}

// Dynamic blink with variable closure and timing for less robotic motion.
void blink() {
  bool isPartial = random(0, 100) < 14;
  float closeRatio = isPartial ? (random(45, 71) / 100.0f) : (random(88, 101) / 100.0f);

  int targetTL = limitTL.maxAngle + (int)((limitTL.minAngle - limitTL.maxAngle) * closeRatio);
  int targetTR = limitTR.maxAngle + (int)((limitTR.minAngle - limitTR.maxAngle) * closeRatio);
  int targetBL = limitBL.maxAngle + (int)((limitBL.minAngle - limitBL.maxAngle) * closeRatio);
  int targetBR = limitBR.maxAngle + (int)((limitBR.minAngle - limitBR.maxAngle) * closeRatio);

  moveLidsTo(targetTL, targetTR, targetBL, targetBR, isPartial ? random(24, 42) : random(34, 66));
  smartDelay(isPartial ? random(6, 18) : random(16, 46));
  moveLidsTo(limitTL.maxAngle, limitTR.maxAngle, limitBL.maxAngle, limitBR.maxAngle, random(45, 96));
}
// Move both jaw servos synchronously
void moveJawSync(int targetLeft, int delayMs) {
  int currentLeft = servoJawLeft.read();
  
  if (currentLeft == targetLeft) return;
  
  if (currentLeft < targetLeft) {
    for (int pos = currentLeft; pos <= targetLeft; pos++) {
      servoJawLeft.write(pos);
      servoJawRight.write(180 - pos);
      smartDelay(delayMs);
    }
  } else {
    for (int pos = currentLeft; pos >= targetLeft; pos--) {
      servoJawLeft.write(pos);
      servoJawRight.write(180 - pos);
      smartDelay(delayMs);
    }
  }
}

// Set eyebrows to Neutral position (50% of range)
void setBrowsNeutral() {
  int ll_mid = limitBrowLL.minAngle + (limitBrowLL.maxAngle - limitBrowLL.minAngle) / 2;
  int lr_mid = limitBrowLR.minAngle + (limitBrowLR.maxAngle - limitBrowLR.minAngle) / 2;
  int rr_mid = limitBrowRR.minAngle + (limitBrowRR.maxAngle - limitBrowRR.minAngle) / 2;
  int rl_mid = limitBrowRL.minAngle + (limitBrowRL.maxAngle - limitBrowRL.minAngle) / 2;

  servoBrowLL.write(ll_mid);
  servoBrowLR.write(lr_mid);
  servoBrowRR.write(rr_mid);
  servoBrowRL.write(rl_mid);
}

// Set eyebrows to specific percentage (0=Bottom, 100=Top)
void setBrows(int percent) {
  int ll_target = map(percent, 0, 100, limitBrowLL.minAngle, limitBrowLL.maxAngle);
  int lr_target = map(percent, 0, 100, limitBrowLR.minAngle, limitBrowLR.maxAngle);
  int rr_target = map(percent, 0, 100, limitBrowRR.minAngle, limitBrowRR.maxAngle);
  int rl_target = map(percent, 0, 100, limitBrowRL.minAngle, limitBrowRL.maxAngle);

  servoBrowLL.write(ll_target);
  servoBrowLR.write(lr_target);
  servoBrowRR.write(rr_target);
  servoBrowRL.write(rl_target);
}

// Set eyebrows with independent left/right percentages.
void setBrowsSplit(int leftPercent, int rightPercent) {
  leftPercent = constrain(leftPercent, 0, 100);
  rightPercent = constrain(rightPercent, 0, 100);

  servoBrowLL.write(map(leftPercent, 0, 100, limitBrowLL.minAngle, limitBrowLL.maxAngle));
  servoBrowLR.write(map(leftPercent, 0, 100, limitBrowLR.minAngle, limitBrowLR.maxAngle));
  servoBrowRR.write(map(rightPercent, 0, 100, limitBrowRR.minAngle, limitBrowRR.maxAngle));
  servoBrowRL.write(map(rightPercent, 0, 100, limitBrowRL.minAngle, limitBrowRL.maxAngle));
}

void moveBrowsSplitSmooth(int leftPercent, int rightPercent, int durationMs) {
  leftPercent = constrain(leftPercent, 0, 100);
  rightPercent = constrain(rightPercent, 0, 100);

  int targetLL = map(leftPercent, 0, 100, limitBrowLL.minAngle, limitBrowLL.maxAngle);
  int targetLR = map(leftPercent, 0, 100, limitBrowLR.minAngle, limitBrowLR.maxAngle);
  int targetRR = map(rightPercent, 0, 100, limitBrowRR.minAngle, limitBrowRR.maxAngle);
  int targetRL = map(rightPercent, 0, 100, limitBrowRL.minAngle, limitBrowRL.maxAngle);

  int startLL = servoBrowLL.read();
  int startLR = servoBrowLR.read();
  int startRR = servoBrowRR.read();
  int startRL = servoBrowRL.read();

  int maxDelta = max(max(abs(targetLL - startLL), abs(targetLR - startLR)),
                     max(abs(targetRR - startRR), abs(targetRL - startRL)));
  if (maxDelta <= 0) {
    return;
  }

  int steps = max(5, maxDelta);
  int stepDelayMs = max(2, durationMs / steps);

  for (int i = 1; i <= steps; ++i) {
    float t = smoothStep((float)i / (float)steps);
    servoBrowLL.write(lerpInt(startLL, targetLL, t));
    servoBrowLR.write(lerpInt(startLR, targetLR, t));
    servoBrowRR.write(lerpInt(startRR, targetRR, t));
    servoBrowRL.write(lerpInt(startRL, targetRL, t));
    smartDelay(stepDelayMs);
  }
}

void updateLidsForUD(int udAngle) {
  float ud_progress = (float)(udAngle - limitUD.minAngle) / (limitUD.maxAngle - limitUD.minAngle);
  ud_progress = constrain(ud_progress, 0.0f, 1.0f);

  float tl_target = limitTL.maxAngle - ((limitTL.maxAngle - limitTL.minAngle) * (0.8 * (1.0 - ud_progress)));
  float tr_target = limitTR.maxAngle + ((limitTR.minAngle - limitTR.maxAngle) * (0.8 * (1.0 - ud_progress)));
  float bl_target = limitBL.maxAngle + ((limitBL.minAngle - limitBL.maxAngle) * (0.4 * ud_progress));
  float br_target = limitBR.maxAngle - ((limitBR.maxAngle - limitBR.minAngle) * (0.4 * ud_progress));

  servoTL.write((int)tl_target);
  servoTR.write((int)tr_target);
  servoBL.write((int)bl_target);
  servoBR.write((int)br_target);
}

// Animate Eyebrows Randomly
void animateBrows() {
  // Random Expression
  int expression = random(0, 5);
  
  switch (expression) {
    case 0: // Neutral
      setBrowsNeutral();
      break;
    case 1: // Surprise (High)
      setBrows(random(80, 100));
      break;
    case 2: // Suspicious/Low
      setBrows(random(0, 30));
      break;
    case 3: // One Up (Left)
      servoBrowLL.write(limitBrowLL.maxAngle);
      servoBrowLR.write(limitBrowLR.maxAngle);
      servoBrowRR.write(limitBrowRR.minAngle);
      servoBrowRL.write(limitBrowRL.minAngle);
      break;
    case 4: // One Up (Right)
      servoBrowLL.write(limitBrowLL.minAngle);
      servoBrowLR.write(limitBrowLR.minAngle);
      servoBrowRR.write(limitBrowRR.maxAngle);
      servoBrowRL.write(limitBrowRL.maxAngle);
      break;
  }
}

void runIdleAnimationStep() {
  // Idle: natural fixation -> saccade -> microsaccade, with human-like blink timing.
  applyJawPercent(0);

  static int fixationLR = 90;
  static int fixationUD = 90;

  if (random(0, 100) < 75) {
    int centerBiasedLR = 90 + random(-18, 19) + random(-8, 9);
    int centerBiasedUD = 90 + random(-14, 11) + random(-6, 7);
    fixationLR = constrain(centerBiasedLR, limitLR.minAngle + 4, limitLR.maxAngle - 4);
    fixationUD = constrain(centerBiasedUD, limitUD.minAngle + 6, limitUD.maxAngle - 4);
    moveEyesNatural(fixationLR, fixationUD, random(120, 250));
  }

  if (random(0, 100) < 70) {
    int microCount = random(1, 4);
    for (int i = 0; i < microCount; ++i) {
      int microLR = constrain(fixationLR + random(-3, 4), limitLR.minAngle, limitLR.maxAngle);
      int microUD = constrain(fixationUD + random(-2, 3), limitUD.minAngle, limitUD.maxAngle);
      moveEyesNatural(microLR, microUD, random(20, 55));
      smartDelay(random(18, 45));
    }
    moveEyesNatural(fixationLR, fixationUD, random(30, 80));
  }

  if (random(0, 100) < 30) {
    blink();
    if (random(0, 100) < 18) {
      smartDelay(random(70, 170));
      blink();
    }
  }

  // Brows are intentionally prominent in idle so expressions read clearly at distance.
  if (random(0, 100) < 82) {
    int leftPercent = constrain(random(18, 93) + random(-8, 9), 0, 100);
    int rightPercent = constrain(leftPercent + random(-18, 19), 0, 100);
    moveBrowsSplitSmooth(leftPercent, rightPercent, random(90, 220));
    smartDelay(random(60, 140));
    if (random(0, 100) < 70) {
      moveBrowsSplitSmooth(50, 50, random(90, 220));
    }
  }

  smartDelay(random(90, 230));
}

void runWaitAnimationStep() {
  // Waiting/listening: brows stay active, gaze remains centered and attentive.
  applyJawPercent(0);

  int leftPercent = random(8, 99);
  int rightPercent = random(8, 99);
  if (random(0, 100) < 72) {
    rightPercent = constrain(leftPercent + random(-16, 17), 0, 100);
  }
  moveBrowsSplitSmooth(leftPercent, rightPercent, random(70, 170));

  if (random(0, 100) < 22) {
    blink();
  }

  if (random(0, 100) < 45) {
    int targetLR = random(85, 98);
    int targetUD = random(83, 99);
    moveEyesNatural(targetLR, targetUD, random(90, 180));
    if (random(0, 100) < 60) {
      int settleLR = constrain(targetLR + random(-2, 3), limitLR.minAngle, limitLR.maxAngle);
      int settleUD = constrain(targetUD + random(-2, 3), limitUD.minAngle, limitUD.maxAngle);
      moveEyesNatural(settleLR, settleUD, random(25, 60));
    }
  }

  smartDelay(random(70, 150));

  if (random(0, 100) < 55) {
    moveBrowsSplitSmooth(50, 50, random(80, 160));
  }

  if (random(0, 100) < 35) {
    int accentLeft = constrain(50 + random(-35, 36), 0, 100);
    int accentRight = constrain(50 + random(-35, 36), 0, 100);
    moveBrowsSplitSmooth(accentLeft, accentRight, random(55, 120));
  }
  smartDelay(random(45, 110));
}

void runTalkAnimationStep() {
  // Talking: jaw comes from host; eyebrows stay expressive and energetic.
  unsigned long now = millis();
  if (nextTalkBrowUpdateMs == 0 || now >= nextTalkBrowUpdateMs) {
    int energyBase = map(constrain(talkJawPercent, 0, 100), 0, 100, 45, 78);
    int swing = map(constrain(talkJawPercent, 0, 100), 0, 100, 14, 26);
    int leftPercent = constrain(energyBase + random(-swing, swing + 1), 0, 100);
    int rightPercent = constrain(energyBase + random(-swing, swing + 1), 0, 100);

    if (random(0, 100) < 70) {
      rightPercent = constrain(leftPercent + random(-20, 21), 0, 100);
    }

    moveBrowsSplitSmooth(leftPercent, rightPercent, random(55, 120));

    if (random(0, 100) < 24) {
      if (random(0, 100) < 50) {
        moveBrowsSplitSmooth(constrain(leftPercent + random(14, 26), 0, 100), rightPercent, random(35, 70));
      } else {
        moveBrowsSplitSmooth(leftPercent, constrain(rightPercent + random(14, 26), 0, 100), random(35, 70));
      }
    }

    nextTalkBrowUpdateMs = now + (unsigned long)random(85, 165);
  }

  if (random(0, 100) < 8) {
    int targetLR = random(84, 99);
    int targetUD = random(82, 100);
    moveEyesNatural(targetLR, targetUD, random(45, 100));
  }

  if (random(0, 100) < 5) {
    blink();
  }

  smartDelay(8);
}

void loop() {
  serviceSerial();

  if (currentMode == MOTION_TALK) {
    runTalkAnimationStep();
  } else if (currentMode == MOTION_WAIT) {
    runWaitAnimationStep();
  } else {
    runIdleAnimationStep();
  }

  serviceSerial();
}
