#include <Arduino.h>
#include <ESP32Servo.h>

// Define pins for ultrasonic sensor, servos, and button
#define TRIG_PIN 14
#define ECHO_PIN 12
#define ARM_SERVO_PIN 19
#define SPOON_SERVO_PIN 18
#define BUTTON_PIN 2

// Servo objects
Servo armServo;
Servo spoonServo;

// Constants
const int MIN_DISTANCE = 1;
const int THRESHOLD_DISTANCE = 30;
const int INITIAL_POSITION = 90;

// State variables
bool feedingCompleted = false; // Indicates if feeding motion is completed
bool buttonPressed = false;    // Tracks button press state

void setup() {
  Serial.begin(115200);

  // Ultrasonic sensor pins
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  // Button setup
  pinMode(BUTTON_PIN, INPUT_PULLUP); // Use internal pull-up for button

  // Servo setup
  armServo.attach(ARM_SERVO_PIN, 500, 2400);
  spoonServo.attach(SPOON_SERVO_PIN, 500, 2400);

  // Initialize servos to initial position
  armServo.write(INITIAL_POSITION);
  spoonServo.write(INITIAL_POSITION);

  Serial.println("System ready. Waiting for person detection.");
}

float getDistance() {
  // Trigger ultrasonic sensor
  digitalWrite(TRIG_PIN, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG_PIN, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG_PIN, LOW);

  // Read echo duration
  long duration = pulseIn(ECHO_PIN, HIGH);

  // Convert to distance in cm
  return duration * 0.034 / 2;
}

void feedingMotion() {
  // Feeding motion sequence
  armServo.write(70);
  Serial.println("Moving Arm to 45°");
  delay(1000);

  spoonServo.write(45);
  Serial.println("Moving Spoon to 45°");
  delay(1000);

  armServo.write(90);
  Serial.println("Moving Arm to 70°");
  delay(1000);

  spoonServo.write(100);
  Serial.println("Moving Spoon to 100°");
  delay(1000);

  armServo.write(90);
  spoonServo.write(90);
  armServo.write(95);
  armServo.write(100);
  armServo.write(105);
  armServo.write(110);
  armServo.write(170);
  armServo.write(175);
  armServo.write(180);
  delay(200);
  spoonServo.write(35);
  delay(700);

  Serial.println("Feeding motion completed.");
  feedingCompleted = true; // Mark the feeding motion as completed
}

void resetToNormalState() {
  // Return servos to initial position
  armServo.write(INITIAL_POSITION);
  spoonServo.write(INITIAL_POSITION);
  Serial.println("No person detected. Returning to normal state.");
  feedingCompleted = false; // Reset the feeding motion state
}

void loop() {
  float distance = getDistance();

  // If person detected within range and feeding motion not started
  if (distance <= THRESHOLD_DISTANCE && distance >= MIN_DISTANCE && !feedingCompleted) {
    Serial.println("Person detected. Starting feeding motion.");
    feedingMotion();
  }

  // If feeding motion completed and button is pressed, repeat feeding motion
  if (feedingCompleted && digitalRead(BUTTON_PIN) == LOW && !buttonPressed) {
    buttonPressed = true; // Prevent multiple triggers on a single press
    Serial.println("Button pressed. Repeating feeding motion.");
    feedingMotion();
  }

  // Reset button state after release
  if (digitalRead(BUTTON_PIN) == HIGH) {
    buttonPressed = false;
  }

  // Return to normal state if no person is detected after feeding motion
  if (feedingCompleted && distance > THRESHOLD_DISTANCE) {
    resetToNormalState();
  }

  delay(200); // General delay for stability
}