#include <ESP32Servo.h>

// ----- Sound trigger config -----
#define SOUND_SENSOR_PIN 18        // digital OUT from sound module (active LOW)
unsigned long lastEventMs = 0;
const unsigned long refractoryMs = 300; // ignore retriggers within this window

// ----- Servo objects -----
Servo panServo;    // Pan (GPIO12)
Servo tiltServo;   // Tilt (GPIO13)
Servo servoA;      // Extra servo A (GPIO32)
Servo servoB;      // Extra servo B (GPIO33)

// ----- GPIO assignments -----
const int panPin   = 12;  // Pan
const int tiltPin  = 13;  // Tilt
const int servoAPin = 32; // extra 1
const int servoBPin = 33; // extra 2

// ----- Center positions -----
const int panCenter  = 130;
const int tiltCenter = 80;

// ----- Position tracking -----
int currentPanAngle  = 90;
int currentTiltAngle = 90;

// ----- Function declarations -----
void runDemo();
void centerServos();
void processCommand(String command);
void showCommands();
void smoothMove(Servo &servo, int startAngle, int endAngle, int &currentPos, int delayMs = 15);

void setup() {
  Serial.begin(115200);
  Serial.println("ESP32 4-Servo + Sound Controller Ready");

  // Sound input
  pinMode(SOUND_SENSOR_PIN, INPUT_PULLUP);

  // Allocate timers and set servo frequency
  ESP32PWM::allocateTimer(0);
  ESP32PWM::allocateTimer(1);
  ESP32PWM::allocateTimer(2);
  ESP32PWM::allocateTimer(3);

  panServo.setPeriodHertz(50);
  tiltServo.setPeriodHertz(50);
  servoA.setPeriodHertz(50);
  servoB.setPeriodHertz(50);

  // Attach with explicit pulse widths
  panServo.attach(panPin,   500, 2400);
  tiltServo.attach(tiltPin, 500, 2400);
  servoA.attach(servoAPin,  500, 2400);
  servoB.attach(servoBPin,  500, 2400);

  // Initialize at tracked positions
  panServo.write(currentPanAngle);
  tiltServo.write(currentTiltAngle);
  servoA.write(0);
  servoB.write(0);

  Serial.println("Pins: Pan=GPIO12, Tilt=GPIO13, A=GPIO32, B=GPIO33; Sound=GPIO18 (LOW=event)");
  Serial.println("Enter 'help' for commands; sound will randomize A/B servos 090");
}

void loop() {
  // ----- Sound-triggered random moves for servoA/servoB -----
  int soundState = digitalRead(SOUND_SENSOR_PIN); // LOW = sound detected
  unsigned long now = millis();
  if (soundState == LOW && (now - lastEventMs) > refractoryMs) {
    int a = random(0, 91); // 0..90 inclusive
    int b = random(0, 91);

    servoA.write(a);
    servoB.write(b);

    Serial.print(" Sound! A=");
    Serial.print(a);
    Serial.print(", B=");
    Serial.print(b);
    Serial.println("");

    lastEventMs = now;
  }

  // ----- Serial command handling for pan/tilt -----
  if (Serial.available()) {
    String command = Serial.readStringUntil('\n');
    command.trim();
    command.toLowerCase();
    if (command.length() > 0) {
      processCommand(command);
    }
  }

  delay(10);
}

// Smooth movement with position tracking
void smoothMove(Servo &servo, int startAngle, int endAngle, int &currentPos, int delayMs) {
  if (startAngle == endAngle) return;
  int step = (startAngle < endAngle) ? 1 : -1;
  Serial.print("Moving: " + String(startAngle) + "  " + String(endAngle) + " ");
  for (int angle = startAngle; angle != endAngle + step; angle += step) {
    servo.write(angle);
    currentPos = angle;
    delay(delayMs);
    if (abs(angle - startAngle) % 10 == 0) Serial.print(".");
  }
  Serial.println(" ");
}

void runDemo() {
  Serial.println(" [DEMO] Starting smooth 180x180 demonstration...");
  Serial.println(" Moving to start position (0, 0)");
  smoothMove(panServo,  currentPanAngle,  0, currentPanAngle, 10);
  smoothMove(tiltServo, currentTiltAngle, 0, currentTiltAngle, 10);
  delay(1000);
  Serial.println("  Pan sweep: 0  180");
  smoothMove(panServo, currentPanAngle, 180, currentPanAngle, 12);
  delay(500);
  Serial.println("  Tilt sweep: 0  180");
  smoothMove(tiltServo, currentTiltAngle, 180, currentTiltAngle, 12);
  delay(500);
  Serial.println("  Pan return: 180  0");
  smoothMove(panServo, currentPanAngle, 0, currentPanAngle, 12);
  delay(500);
  Serial.println("  Tilt return: 180  0");
  smoothMove(tiltServo, currentTiltAngle, 0, currentTiltAngle, 12);
  delay(500);
  Serial.println(" [DEMO] Complete! Current position: Pan=" + String(currentPanAngle) + ", Tilt=" + String(currentTiltAngle) + "");
  Serial.println();
}

void centerServos() {
  Serial.println(" [CENTER] Moving to center positions...");
  Serial.println("   Pan: " + String(currentPanAngle) + "  " + String(panCenter) + "");
  Serial.println("   Tilt: " + String(currentTiltAngle) + "  " + String(tiltCenter) + "");
  smoothMove(panServo,  currentPanAngle,  panCenter,  currentPanAngle, 15);
  smoothMove(tiltServo, currentTiltAngle, tiltCenter, currentTiltAngle, 15);
  Serial.println(" [CENTER] Complete! Servos centered at Pan=" + String(panCenter) + ", Tilt=" + String(tiltCenter) + "");
  Serial.println();
}

void processCommand(String command) {
  Serial.println(" Command received: '" + command + "'");
  if (command == "center") {
    centerServos();
  } else if (command == "demo") {
    runDemo();
  } else if (command.startsWith("pan ")) {
    int angle = command.substring(4).toInt();
    if (angle >= 0 && angle <= 180) {
      Serial.println("  [PAN] " + String(currentPanAngle) + "  " + String(angle) + "");
      smoothMove(panServo, currentPanAngle, angle, currentPanAngle, 15);
      Serial.println(" [PAN] Complete! Pan servo at " + String(angle) + "");
    } else {
      Serial.println(" ERROR: Pan angle must be 0-180");
    }
  } else if (command.startsWith("tilt ")) {
    int angle = command.substring(5).toInt();
    if (angle >= 0 && angle <= 180) {
      Serial.println("  [TILT] " + String(currentTiltAngle) + "  " + String(angle) + "");
      smoothMove(tiltServo, currentTiltAngle, angle, currentTiltAngle, 15);
      Serial.println(" [TILT] Complete! Tilt servo at " + String(angle) + "");
    } else {
      Serial.println(" ERROR: Tilt angle must be 0-180");
    }
  } else if (command.startsWith("both ")) {
    int spaceIndex = command.indexOf(' ', 5);
    if (spaceIndex > 0) {
      int panAngle  = command.substring(5, spaceIndex).toInt();
      int tiltAngle = command.substring(spaceIndex + 1).toInt();
      if (panAngle >= 0 && panAngle <= 180 && tiltAngle >= 0 && tiltAngle <= 180) {
        Serial.println(" [BOTH] Moving simultaneously:");
        Serial.println("   Pan: " + String(currentPanAngle) + "  " + String(panAngle) + "");
        Serial.println("   Tilt: " + String(currentTiltAngle) + "  " + String(tiltAngle) + "");
        int panSteps  = abs(panAngle  - currentPanAngle);
        int tiltSteps = abs(tiltAngle - currentTiltAngle);
        int maxSteps  = max(panSteps, tiltSteps);
        if (maxSteps > 0) {
          Serial.print("Moving both servos ");
          for (int step = 0; step <= maxSteps; step++) {
            int newPanAngle  = map(step, 0, maxSteps, currentPanAngle,  panAngle);
            int newTiltAngle = map(step, 0, maxSteps, currentTiltAngle, tiltAngle);
            panServo.write(newPanAngle);
            tiltServo.write(newTiltAngle);
            delay(15);
            if (step % 10 == 0) Serial.print(".");
          }
          Serial.println(" ");
          currentPanAngle  = panAngle;
          currentTiltAngle = tiltAngle;
        }
        Serial.println(" [BOTH] Complete! Pan=" + String(panAngle) + ", Tilt=" + String(tiltAngle) + "");
      } else {
        Serial.println(" ERROR: Both angles must be 0-180");
      }
    } else {
      Serial.println(" ERROR: Usage: both <pan_angle> <tilt_angle>");
    }
  } else if (command.startsWith("speed ")) {
    int speed = command.substring(6).toInt();
    if (speed >= 5 && speed <= 50) {
      Serial.println(" [SPEED TEST] Testing with " + String(speed) + "ms delay");
      Serial.println("   Pan: 0  90  180  center");
      smoothMove(panServo, currentPanAngle, 0,   currentPanAngle, speed);
      smoothMove(panServo, currentPanAngle, 90,  currentPanAngle, speed);
      smoothMove(panServo, currentPanAngle, 180, currentPanAngle, speed);
      smoothMove(panServo, currentPanAngle, panCenter, currentPanAngle, speed);
      Serial.println(" [SPEED TEST] Complete!");
    } else {
      Serial.println(" ERROR: Speed must be 5-50ms delay");
    }
  } else if (command == "status") {
    Serial.println(" [STATUS] Current servo positions:");
    Serial.println("   Pan (GPIO12):  " + String(currentPanAngle)  + "");
    Serial.println("   Tilt (GPIO13): " + String(currentTiltAngle) + "");
    Serial.println("   Pan center:    " + String(panCenter)  + "");
    Serial.println("   Tilt center:   " + String(tiltCenter) + "");
  } else if (command == "help" || command == "commands") {
    showCommands();
  } else {
    Serial.println(" Unknown command: '" + command + "'");
    Serial.println(" Type 'help' for available commands");
  }
  Serial.println();
}

void showCommands() {
  Serial.println("");
  Serial.println("           AVAILABLE COMMANDS         ");
  Serial.println("");
  Serial.println(" center            - Move to center   ");
  Serial.println(" demo              - Run 180x180 demo ");
  Serial.println(" pan <angle>       - Set pan (0-180)  ");
  Serial.println(" tilt <angle>      - Set tilt (0-180) ");
  Serial.println(" both <pan> <tilt> - Set both angles  ");
  Serial.println(" speed <5-50>      - Test speed       ");
  Serial.println(" status            - Show positions   ");
  Serial.println(" help              - Show this help   ");
  Serial.println("");
  Serial.println();
  Serial.println("Examples:");
  Serial.println("  pan 90        Move pan to 90");
  Serial.println("  tilt 45       Move tilt to 45");
  Serial.println("  both 130 80   Move to center positions");
  Serial.println("  speed 10      Test with 10ms delay");
  Serial.println();
}