//arduino5

// Arduino Motor Shield Pins
const int DIR_A = 12;
const int PWM_A = 3;
const int DIR_B = 13;
const int PWM_B = 11;

// Input Pins
const int PIN_45 = 45; // Start Stage 1
const int PIN_43 = 43; // Start Stage 2 (Exit 1)
const int PIN_30 = 30; // Start Stage 3 (Exit 2)
const int PIN_25 = 25; // Forward
const int PIN_27 = 27; // Right Fwd / Left Rev
const int PIN_23 = 23; // Left Fwd / Right Rev
const int PIN_24 = 24; // Stage 2 Forward
const int PIN_26 = 26; // Stage 2 Right Fwd / Left Rev
const int PIN_22 = 22; // Stage 2 Left Fwd / Right Rev

// Output Pins
const int PIN_41 = 41;

// Motor Speed (80% of 255)
const int MOTOR_SPEED = 204;

enum State { WAITING, STAGE_1, STAGE_2, STAGE_3 };
State currentState = WAITING;

void setup() {
  pinMode(DIR_A, OUTPUT); pinMode(PWM_A, OUTPUT);
  pinMode(DIR_B, OUTPUT); pinMode(PWM_B, OUTPUT);
  pinMode(PIN_41, OUTPUT);
  
  // Set all defined inputs to INPUT
  int inputs[] = {48, 49, 30, 26, 24, 22, 46, 47, 27, 25, 23, 45, 43};
  for(int p : inputs) pinMode(p, INPUT);
  
  stopMotors();
}

void loop() {
  switch (currentState) {
    case WAITING:
      if (digitalRead(PIN_45) == HIGH) currentState = STAGE_1;
      break;

    case STAGE_1:
      handleMovement(PIN_25, PIN_27, PIN_23);
      if (digitalRead(PIN_43) == HIGH) currentState = STAGE_2;
      break;

    case STAGE_2:
      // Fixed sequence
      stopMotors(); delay(5000);
      moveReverse(); delay(5000);
      moveTurn();    delay(6000);//manually changed for more time
      stopMotors();
      
      // Reactive loop for Stage 2
      while (digitalRead(PIN_30) == LOW) {
        handleMovement(PIN_24, PIN_26, PIN_22);
      }
      currentState = STAGE_3;
      break;

    case STAGE_3:
      stopMotors();
      digitalWrite(PIN_41, HIGH); delay(1000);
      digitalWrite(PIN_41, LOW);  delay(23000);
      moveReverse(); delay(5000);
      moveTurn();    delay(5000);
      stopMotors();
      currentState = WAITING;
      break;
  }
}

void handleMovement(int fwd, int right, int left) {
  if (digitalRead(fwd) == HIGH) {
    moveForward();
  } else if (digitalRead(right) == HIGH) {
    moveTurn();
  } else if (digitalRead(left) == HIGH) {
    moveCounterTurn();
  } else {
    stopMotors();
  }
}

void moveForward() {
  digitalWrite(DIR_A, HIGH); analogWrite(PWM_A, MOTOR_SPEED);
  digitalWrite(DIR_B, HIGH); analogWrite(PWM_B, MOTOR_SPEED);
}

void moveReverse() {
  digitalWrite(DIR_A, LOW); analogWrite(PWM_A, MOTOR_SPEED);
  digitalWrite(DIR_B, LOW); analogWrite(PWM_B, MOTOR_SPEED);
}

void moveTurn() { // Right Fwd, Left Rev
  digitalWrite(DIR_A, HIGH); analogWrite(PWM_A, MOTOR_SPEED);
  digitalWrite(DIR_B, LOW);  analogWrite(PWM_B, MOTOR_SPEED);
}

void moveCounterTurn() { // Left Fwd, Right Rev
  digitalWrite(DIR_A, LOW);  analogWrite(PWM_A, MOTOR_SPEED);
  digitalWrite(DIR_B, HIGH); analogWrite(PWM_B, MOTOR_SPEED);
}

void stopMotors() {
  analogWrite(PWM_A, 0);
  analogWrite(PWM_B, 0);
}