#include <ESP32Servo.h>

#define POT_PIN 34          // Vibration sensor input
#define BUZZER_PIN 15       // Alarm buzzer
#define SERVO_PIN 13        // Servo control
#define ALARM_THRESHOLD 70  // Buzzer on ≥70%
#define FULL_STOP 100  // Servo stops at 100%

Servo myServo;
unsigned long lastMoveTime = 0;
unsigned long lastVibTime = 0;

// Natural movement parameters
float baseAngle = 0;
bool movingForward = true;
const float baseSpeed = 0.5; // Degrees per millisecond (adjust for speed)

// Vibration parameters
float vibAmplitude = 0;
float vibFrequency = 0;

void setup() {
  Serial.begin(115200);
  pinMode(BUZZER_PIN, OUTPUT);
  myServo.attach(SERVO_PIN, 500, 2500);
  Serial.println("Natural Servo Movement with Vibration");
  Serial.println("------------------------------------");
}

void loop() {
  int vibrationPercent = map(analogRead(POT_PIN), 0, 4095, 0, 100);
  
  // Calculate vibration parameters
  vibFrequency = map(vibrationPercent, 0, 100, 1, 10); // 1-10 Hz
  vibAmplitude = vibrationPercent * 0.18; // 0-18° vibration range
  
  // Alarm system
  if (vibrationPercent >= ALARM_THRESHOLD) {
    tone(BUZZER_PIN, 1000 + vibrationPercent * 10); // Pitch increases with vibration
    Serial.print("ALARM | ");
  } else {
    noTone(BUZZER_PIN);
    Serial.print("OK    | ");
  }

  // Handle emergency stop
  if (vibrationPercent >= FULL_STOP) {
    myServo.write(90);
    Serial.println("SERVO STOPPED");
    delay(100);
    return;
  }

  // Update natural movement
  if (millis() - lastMoveTime > 20) { // Base movement update rate
    if (movingForward) {
      baseAngle += baseSpeed;
      if (baseAngle >= 180) {
        baseAngle = 180;
        movingForward = false;
      }
    } else {
      baseAngle -= baseSpeed;
      if (baseAngle <= 0) {
        baseAngle = 0;
        movingForward = true;
      }
    }
    lastMoveTime = millis();
  }

  // Calculate vibration effect (sine wave)
  float vibrationEffect = vibAmplitude * sin(millis() * 0.001 * vibFrequency * TWO_PI);
  
  // Combine movements
  float finalAngle = baseAngle + vibrationEffect;
  finalAngle = constrain(finalAngle, 0, 180); // Keep within servo limits
  
  myServo.write(finalAngle);

  // Serial output
  Serial.printf("Base: %.1f° | Vib: ±%.1f° @ %.1fHz | Final: %.1f°\n",
               baseAngle, vibAmplitude, vibFrequency, finalAngle);
  
  delay(10); // Main loop delay
}