#include <Servo.h>

#define SERVO1_PIN D9
#define SERVO2_PIN D10
#define DIR_PIN D3
#define STEP_PIN D4
#define LED_PIN D8

Servo servo1, servo2;

int angle1 = 30, target1 = 30;
int angle2 = 170, target2 = 170;
bool personDetected = false;

const int centerX = 160;
const int centerY = 120;

long lastStepTime = 0;
unsigned int stepInterval = 300;
unsigned int baseStepInterval = 300;
unsigned int currentStepInterval = 300;
int stepQueue = 0;
int totalSteps = 0;
int stepsExecuted = 0;
bool stepDir = true;

const int ACCEL_STEPS = 10;
const float ACCEL_FACTOR = 3.0;

int stepperPosition = 0;
const int patrolRange = 300;
bool patrolDir = true;
unsigned long lastPatrolTime = 0;
const unsigned long patrolDelay = 500;

unsigned long lastUpdate = 0;

bool commandProcessing = false;
String currentCommand = "";
unsigned long commandStartTime = 0;
const unsigned long MAX_PROCESSING_TIME = 2000;

void setup() {
  Serial1.begin(115200);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  servo1.attach(SERVO1_PIN);
  servo2.attach(SERVO2_PIN);
  servo1.write(angle1);
  servo2.write(angle2);
}

void calculateStepInterval() {
  if (totalSteps <= ACCEL_STEPS * 2) {
    currentStepInterval = baseStepInterval;
    return;
  }
  if (stepsExecuted < ACCEL_STEPS) {
    float progress = (float)stepsExecuted / ACCEL_STEPS;
    currentStepInterval = baseStepInterval + (baseStepInterval * (ACCEL_FACTOR - 1.0) * (1.0 - progress));
  } else if (stepsExecuted >= (totalSteps - ACCEL_STEPS)) {
    int stepsFromEnd = totalSteps - stepsExecuted;
    float progress = (float)stepsFromEnd / ACCEL_STEPS;
    currentStepInterval = baseStepInterval + (baseStepInterval * (ACCEL_FACTOR - 1.0) * (1.0 - progress));
  } else {
    currentStepInterval = baseStepInterval;
  }
}

void sendACK() {
  Serial1.println("OK");
}

void sendNACK() {
  Serial1.println("NACK");
}

bool processCommand(String cmd) {
  if (cmd.startsWith("TRACK:")) {
    digitalWrite(LED_PIN, HIGH);
    personDetected = true;

    int p1 = cmd.indexOf(':');
    int p2 = cmd.indexOf(':', p1 + 1);
    int p3 = cmd.indexOf(':', p2 + 1);

    if (p1 == -1 || p2 == -1 || p3 == -1) return false;

    int x = cmd.substring(p1 + 1, p2).toInt();
    int y = cmd.substring(p2 + 1, p3).toInt();

    int dx = x - centerX;
    if (abs(dx) > 5) {
      stepDir = (dx > 0);
      int steps = map(abs(dx), 0, 160, 0, 50);
      stepQueue = min(steps, 50);
      totalSteps = stepQueue;
      stepsExecuted = 0;
      baseStepInterval = map(abs(dx), 0, 160, 900, 200);
      currentStepInterval = baseStepInterval * ACCEL_FACTOR;
    }

    int desired = map(y, 0, 240, 0, 45);
    target1 = constrain(desired, 0, 45);
    target2 = 140;
    return true;
  }
  else if (cmd == "NO_PERSON") {
    digitalWrite(LED_PIN, LOW);
    personDetected = false;
    target2 = 170;
    return true;
  }
  return false;
}

bool isMovementComplete() {
  return (stepQueue == 0) &&
         (abs(angle1 - target1) <= 1) &&
         (abs(angle2 - target2) <= 1);
}

void loop() {
  if (Serial1.available() && !commandProcessing) {
    String cmd = Serial1.readStringUntil('\n');
    cmd.trim();
    if (cmd.length() > 0) {
      currentCommand = cmd;
      commandProcessing = true;
      commandStartTime = millis();
      if (!processCommand(cmd)) {
        sendNACK();
        commandProcessing = false;
      }
    }
  }

  if (stepQueue > 0 && micros() - lastStepTime > currentStepInterval) {
    digitalWrite(DIR_PIN, stepDir);
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(180);
    digitalWrite(STEP_PIN, LOW);
    lastStepTime = micros();
    stepQueue--;
    stepsExecuted++;
    stepperPosition += (stepDir ? 1 : -1);
    calculateStepInterval();
  }

  if (millis() - lastUpdate > 10) {
    lastUpdate = millis();
    if (abs(angle1 - target1) > 0) {
      angle1 += (target1 > angle1) ? 1 : -1;
      servo1.write(angle1);
    }
    if (abs(angle2 - target2) > 0) {
      angle2 += (target2 > angle2) ? 1 : -1;
      servo2.write(angle2);
    }
  }

  if (commandProcessing) {
    if (isMovementComplete()) {
      sendACK();
      commandProcessing = false;
    } else if (millis() - commandStartTime > MAX_PROCESSING_TIME) {
      sendNACK();
      commandProcessing = false;
    }
  }

  if (!personDetected && !commandProcessing && stepQueue == 0 && millis() - lastPatrolTime > patrolDelay) {
    if (stepperPosition >= patrolRange) patrolDir = false;
    else if (stepperPosition <= -patrolRange) patrolDir = true;

    stepDir = patrolDir;
    stepQueue = 20;
    totalSteps = stepQueue;
    stepsExecuted = 0;
    baseStepInterval = 900;
    currentStepInterval = baseStepInterval * ACCEL_FACTOR;
    lastPatrolTime = millis();
  }
}