#include <ESP32Servo.h>

Servo head;
Servo tail;
Servo leg1;
Servo leg2;
Servo leg3;
Servo leg4;

//Pin config. for my components
#define leg1_pin 18
#define leg2_pin 19
#define leg3_pin 25
#define leg4_pin 26
#define head_pin 4
#define tail_pin 5
#define pir_pin 15
#define echo_pin 23
#define trig_pin 21
#define speed_sound 0.0343

//duration_buffer's AKA threshold AKA samay samapti ki ghosna
const unsigned long pir_stable = 500;
const unsigned long scan_duration = 20000;
const unsigned long confirmation_time = 3000;
const unsigned long output_interval = 500;
const unsigned long cool_down = 3000;
const int entry_distance = 100;
const int exit_distance = 120;

//timer
unsigned long tail_timer = 0;
unsigned long pir_start = 0;
unsigned long scanStartTime = 0;
unsigned long confirmation_start = 0;
unsigned long sweap_time = 0;
unsigned long start_coolDown = 0;
unsigned long exit_start = 0;

//billi ki pooch
int tail_angle = 40;
int tail_direction = 1;

bool isStanding = false;

//ROBO ke paanch roop
enum robo_state {
  IDLE,
  SCANNING,
  CONFIRMING,
  TARGET_LOCKED,
  COOLDOWN
};
robo_state State = IDLE;

// chaan been kaa angle AKA (Scanning angle)
int angle = 0;
int direction = 1;
int locked_angle = 90;

//robo standing ding
void standingRobo()
{
  for (int pos = 0; pos <= 90; pos += 2)
  {
    leg1.write(pos);
    leg2.write(pos);
    leg3.write(pos);
    leg4.write(pos);
    delay(15);
  }
  isStanding = true;
}

//Robo going for sleeppppp, lala lala lori....
void sleepyRobo()
{
  for (int pos = 90; pos >= 0; pos -= 2)
  {
    leg1.write(pos);
    leg2.write(pos);
    leg3.write(pos);
    leg4.write(pos);
    delay(15);
  }
  isStanding = false;
}


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

  pinMode(trig_pin, OUTPUT);
  pinMode(echo_pin, INPUT);
  pinMode(pir_pin, INPUT);

  //servooooo attachment's
  head.attach(head_pin);
  tail.attach(tail_pin);
  leg1.attach(leg1_pin);
  leg2.attach(leg2_pin);
  leg3.attach(leg3_pin);
  leg4.attach(leg4_pin);

  head.write(90);
  tail.write(120);

  sleepyRobo(); 
  Serial.println("System Initialized. State = IDLE");
}

//dushman kitni door hai????
long readDistance()
{
  digitalWrite(trig_pin, LOW);
  delayMicroseconds(2);

  digitalWrite(trig_pin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trig_pin, LOW);

  long duration = pulseIn(echo_pin, HIGH, 30000);
  long distance = speed_sound * duration / 2;

  if (distance == 0)
    distance = 400;

  return distance;
}


void loop()
{
  long distance = readDistance();
  bool pir = digitalRead(pir_pin);

  switch (State)
  {
    case IDLE:
      tail.write(120);

      if (pir)
      {
        if (pir_start == 0)
          pir_start = millis();

        if (millis() - pir_start >= pir_stable)
        {
          State = SCANNING;
          scanStartTime = millis();
          pir_start = 0;
          Serial.println("State -> SCANNING");
        }
      }
      else
        pir_start = 0;
      break;

    case SCANNING:
      tail.write(30);

      if (millis() - sweap_time > 20)
      {
        sweap_time = millis();
        angle += direction;

        if (angle >= 180 || angle <= 0)
          direction *= -1;

        head.write(angle);
      }

      if (distance < entry_distance)
      {
        locked_angle = angle;
        confirmation_start = millis();
        State = CONFIRMING;

        tail_angle = 40;
        tail_direction = 1;

        Serial.println("State ---> CONFIRMING");
      }

      if (millis() - scanStartTime >= scan_duration)
      {
        State = COOLDOWN;
        start_coolDown = millis();
        Serial.println("State ----> COOLDOWN");
      }
      break;

    case CONFIRMING:
      if (millis() - tail_timer >= 10)
      {
        tail_timer = millis();
        tail_angle += tail_direction * 5;

        if (tail_angle > 110 || tail_angle < 40)
          tail_direction *= -1;

        tail.write(tail_angle);
      }

      head.write(locked_angle);

      if (distance > entry_distance)
      {
        State = SCANNING;
        Serial.println("State ----> SCANNING");
      }
      else if (millis() - confirmation_start >= confirmation_time)
      {
        State = TARGET_LOCKED;
        Serial.println("State ----> TARGET_LOCKED");
      }
      break;

    case TARGET_LOCKED:
      if (!isStanding)
        standingRobo();

      if (millis() - tail_timer >= 10)
      {
        tail_timer = millis();
        tail_angle += tail_direction * 5;

        if (tail_angle > 110 || tail_angle < 40)
          tail_direction *= -1;

        tail.write(tail_angle);
      }

      head.write(locked_angle);

      if (distance > exit_distance)
      {
        if (exit_start == 0)
          exit_start = millis();

        if (millis() - exit_start >= 2000)
        {
          State = SCANNING;
          scanStartTime = millis();
          exit_start = 0;
          Serial.println("Target lost -> SCANNING");
        }
      }
      else
        exit_start = 0;
      break;

    case COOLDOWN:
      tail.write(120);

      if (isStanding)
        sleepyRobo();

      if (millis() - start_coolDown >= cool_down)
      {
        State = IDLE;
        Serial.println("State ---> IDLE");
      }
      break;
  }

//Aaiyeeee output dekte hai
  static unsigned long output_start = 0;

  if (millis() - output_start >= output_interval)
  {
    output_start = millis();

    Serial.print("State: ");
    Serial.print(State);
    Serial.print(" | Distance: ");
    Serial.print(distance);
    Serial.print(" | Standing: ");
    Serial.println(isStanding);
  }
}