#include <Servo.h>


#define ECHO_PING 0
#define CHECK_LEFT 1
#define CHECK_MID 2
#define CHECK_RIGHT 3
#define LEFT_HIT 4
#define MID_HIT 5
#define RIGHT_HIT 6
#define LAST_WAIT 7

byte outerState = ECHO_PING;
byte innerState = 0;

unsigned long lastDelay = 0;
unsigned int intervalDelay;

Servo rf;
Servo rb;
Servo lf;
Servo lb;
Servo headx;
Servo headz;

//motor drivers connections
byte m1A1 = 37;
byte m1A2 = 39;
byte m2A1 = 41;
byte m2A2 = 43;
byte m3A1 = 26;
byte m3A2 = 28;
byte m4A1 = 29;
byte m4A2 = 31;
byte m5A1 = 33;
byte m5A2 = 35;
byte m6A1 = 34;
byte m6A2 = 36;

//wheel angle manuver
byte servostep = 4;
byte distancechange = 20;
byte timetocheck = 3000;

//wheels initial
byte inrf = 88;
byte inlf = 92;
byte inrb = 94;
byte inlb = 92;

//wheels right
byte rightrf = 101;
byte rightlf = 105;
byte rightrb = 107;
byte rightlb = 105;

///wheels left
byte leftrf = 75;
byte leftlf = 79;
byte leftrb = 81;
byte leftlb = 79;

//front wheels set value
int leftRF = leftrf;
int leftLF = leftlf;
int rightRF = rightrf;
int rightLF = rightlf;

//back wheels set value
int leftRB = leftrb;
int leftLB = leftlb;
int rightRB = rightrb;
int rightLB = rightlb;

//servo pins
byte rfpin = 4;
byte rbpin = 5;
byte lfpin = 2;
byte lbpin = 3;
byte headxpin = 6;
byte headzpin = 9;

// Define ultrasonic sensor pins
byte echoL = 10;
byte triggerL = 11;
byte echoM = 12;
byte triggerM = 13;
byte echoR = 22;
byte triggerR = 24;

// Define servo objects

int headxcurrent = 93;
byte headxin = 93;
byte headzin = 90;
int headzcurrent = 90;
int headxsteps = 10;
int headzsteps = 10;

// Define distance thresholds
byte minDistance = 30;  // Minimum distance in cm
byte maxDistance = 120;  // Maximum distance in cm


// Function to calculate distance
int getDistance(int triggerPin, int echoPin) {
  // Send trigger pulse
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

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

  // Calculate distance in cm
  int distance = duration * 0.034 / 2;

  return distance;
}


void setup() {

  //head code start
  headx.attach(headxpin);
  headz.attach(headzpin);
  headx.write(93);
  headz.write(90);
  //head code end

  // Initialize servo objects
  rf.attach(rfpin);
  rb.attach(rbpin);
  lf.attach(lfpin);
  lb.attach(lbpin);

  // Initialize ultrasonic sensor pins
  pinMode(triggerL, OUTPUT);
  pinMode(echoL, INPUT);
  pinMode(triggerM, OUTPUT);
  pinMode(echoM, INPUT);
  pinMode(triggerR, OUTPUT);
  pinMode(echoR, INPUT);

  // Set initial servo positions
  ServoInit();

  // Wait for servo initialization
  delay(500);

  // Set motor control pins as outputs
  pinMode(m1A1, OUTPUT);
  pinMode(m1A2, OUTPUT);
  pinMode(m2A1, OUTPUT);
  pinMode(m2A2, OUTPUT);
  pinMode(m3A1, OUTPUT);
  pinMode(m3A2, OUTPUT);
  pinMode(m4A1, OUTPUT);
  pinMode(m4A2, OUTPUT);
  pinMode(m5A1, OUTPUT);
  pinMode(m5A2, OUTPUT);
  pinMode(m6A1, OUTPUT);
  pinMode(m6A2, OUTPUT);


}

void loop() {

  int distanceL;
  int distanceM;
  int distanceR;


  switch (outerState) {
    case ECHO_PING:  // Read distances from ultrasonic sensors
      distanceL = getDistance(triggerL, echoL);
      distanceM = getDistance(triggerM, echoM);
      distanceR = getDistance(triggerR, echoR);
      Forward();
      outerState = CHECK_LEFT;
      break;
    case CHECK_LEFT: if (distanceL >= minDistance && distanceL <= maxDistance) {
        outerState = LEFT_HIT;
        innerState = 0;
      }
      else
        outerState = CHECK_MID;
      break;
    case CHECK_MID: if (distanceM >= minDistance && distanceM <= maxDistance) {
        outerState = MID_HIT;
        innerState = 0;
      }
      else
        outerState = CHECK_RIGHT;
      break;
    case CHECK_RIGHT: if (distanceR >= minDistance && distanceR <= maxDistance) {
        outerState = RIGHT_HIT;
        innerState = 0;
      }
      else {
        outerState = LAST_WAIT;
        SetDelay(100);
      }
      break;
    case LEFT_HIT:
      switch (innerState) {
        case 0:  ServoInit();
          AllStop();
          SetDelay(500);
          innerState++;
          break;
        case 1: if (CheckDelay()) innerState++; break;
        case 2: TurnRight();
          SetDelay(700);
          innerState++;
          break;
        case 3: if (CheckDelay()) innerState++; break;
        case 4: Forward();
          SetDelay(3000);
          innerState++;
          break;
        case 5: if (CheckDelay()) innerState++; break;
        case 6: Straight(); outerState = CHECK_MID; break;
      }

      break;
    case MID_HIT:
      switch (innerState) {
        case 0: ServoInit();
          AllStop();
          SetDelay(500);
          innerState++;
          break;
        case 1: if (CheckDelay()) innerState++; break;
        case 2: if (distanceR > distanceL) {
            TurnLeft();
          }
          else if (distanceR < distanceL) {
            TurnLeft();
          }
          SetDelay(700);
          innerState++;
          break;
        case 3: if (CheckDelay()) innerState++; break;
        case 4: Backwards();
          SetDelay(2200);
          innerState++;
          break;
        case 5: if (CheckDelay()) innerState++; break;
        case 6: Straight();
          SetDelay(700);
          innerState++;
          break;
        case 7: if (CheckDelay()) innerState++; break;
        case 8: AllStop();
          SetDelay(500);
          innerState++;
          break;
        case 9: if (CheckDelay()) innerState++; break;
        case 10:  if (distanceR > distanceL) {
            TurnLeft();
          }
          else if (distanceR < distanceL) {
            TurnRight();
          }
          else if (distanceM > distanceL && distanceR) {
            ServoInit();
          }
          SetDelay(700);
          innerState++;
          break;
        case 11: if (CheckDelay()) innerState++; break;
        case 12:  Forward();
          SetDelay(3000);
          innerState++;
          break;
        case 13: if (CheckDelay()) innerState++; break;
        case 14: ServoInit(); outerState = CHECK_RIGHT; break;
      }

      break;
    case RIGHT_HIT:
      switch (innerState) {
        case 0: ServoInit();
          AllStop();
          SetDelay(700);
          innerState++;
          break;
        case 1: if (CheckDelay()) innerState++; break;
        case 2: TurnLeft();
          SetDelay(2000);
          innerState++;
          break;
        case 3: if (CheckDelay()) innerState++; break;
        case 4: Forward();
          SetDelay(3000);
          innerState++;
          break;
        case 5: if (CheckDelay()) innerState++; break;
        case 6: Straight(); outerState = LAST_WAIT; SetDelay(100); break;
      }

      break;
    case LAST_WAIT: if (CheckDelay()) outerState = ECHO_PING; break;

  }

}

void SetDelay(unsigned int mills) {
  lastDelay = millis();
  intervalDelay = mills;
}

bool CheckDelay() {
  if (millis() - lastDelay >= intervalDelay) {
    return true;
  } else return false;
}

void TurnRight() {
  rb.write(rightrf);
  lb.write(rightlf);
}

void TurnLeft() {
  rf.write(leftrf);
  lf.write(leftlf);
}

void Straight() {
  rf.write(inrf);
  lf.write(inlf);
}

void ServoInit() {
  rf.write(inrf);
  lf.write(inlf);
  rb.write(inrb);
  lb.write(inlb);
}

void AllStop() {
  digitalWrite(m1A1, LOW);
  digitalWrite(m1A2, LOW);
  digitalWrite(m2A1, LOW);
  digitalWrite(m2A2, LOW);
  digitalWrite(m3A1, LOW);
  digitalWrite(m3A2, LOW);
  digitalWrite(m4A1, LOW);
  digitalWrite(m4A2, LOW);
  digitalWrite(m5A1, LOW);
  digitalWrite(m5A2, LOW);
  digitalWrite(m6A1, LOW);
  digitalWrite(m6A2, LOW);
}

void Forward() {
  digitalWrite(m1A1, HIGH);
  digitalWrite(m1A2, LOW);
  digitalWrite(m2A1, LOW);
  digitalWrite(m2A2, HIGH);
  digitalWrite(m3A1, LOW);
  digitalWrite(m3A2, HIGH);
  digitalWrite(m4A1, HIGH);
  digitalWrite(m4A2, LOW);
  digitalWrite(m5A1, HIGH);
  digitalWrite(m5A2, LOW);
  digitalWrite(m6A1, HIGH);
  digitalWrite(m6A2, LOW);
}

void Backwards() {
  digitalWrite(m1A1, LOW);
  digitalWrite(m1A2, HIGH);
  digitalWrite(m2A1, HIGH);
  digitalWrite(m2A2, LOW);
  digitalWrite(m3A1, HIGH);
  digitalWrite(m3A2, LOW);
  digitalWrite(m4A1, LOW);
  digitalWrite(m4A2, HIGH);
  digitalWrite(m5A1, LOW);
  digitalWrite(m5A2, HIGH);
  digitalWrite(m6A1, LOW);
  digitalWrite(m6A2, HIGH);
}