#define xAxis 35
#define yAxis 34

// Motor control pins
const int LEFT = 5;
const int RIGHT = 2;
const int UP = 17;
const int DOWN = 18;
// const int LED_BUILTIN = 12;

// Button pins
const int StopLEFT = 15;
const int StopRIGHT = 16;
const int StopUP = 13;
const int StopDOWN = 14;

// Shared Run-once button
const int runOncePin = 4;

// Duration arrays (ms)
unsigned int durations1[] = {1000, 2000, 3000, 4000};
unsigned int durations2[] = {1500, 2500, 3500, 4500};

// Motor state tracking
bool motor1Running = false;
bool motor2Running = false;
bool motor1Direction = false;
bool motor2Direction = false;
unsigned long motor1EndTime = 0;
unsigned long motor2EndTime = 0;

// Button press tracking
bool runOncePressed = false;

bool operationLocked = false;


// System state
enum SystemState { IDLE, RUNNING_MOTORS, WAITING_COMPLETION, EXECUTING_OPERATION, MANUAL_CONTROL };
SystemState currentState = IDLE;

#define buttonAPin 21
#define buttonBPin 22
#define buttonCPin 23
#define led5Pin 0
#define led6Pin 19

bool led5Off = true;
bool led6Off = true;

// Joystick control tracking
bool manualControlActive = false;
unsigned long lastJoystickRead = 0;
const int JOYSTICK_READ_INTERVAL = 50;

void setup() {
  Serial.begin(115200);
  
  // Pushball buttons and LEDs
  pinMode(buttonAPin, INPUT);
  pinMode(buttonBPin, INPUT);
  pinMode(buttonCPin, INPUT);
  pinMode(led5Pin, OUTPUT);
  pinMode(led6Pin, OUTPUT);

  // Motor pins
  pinMode(LEFT, OUTPUT);
  pinMode(RIGHT, OUTPUT);
  pinMode(UP, OUTPUT);
  pinMode(DOWN, OUTPUT);
  // pinMode(LED_BUILTIN, OUTPUT);
  
  // Stop buttons - USE PULLUP FOR CONSISTENCY
  pinMode(StopLEFT, INPUT);
  pinMode(StopRIGHT, INPUT);
  pinMode(StopUP, INPUT);
  pinMode(StopDOWN, INPUT);

  // Analog pins
  pinMode(xAxis, INPUT);
  pinMode(yAxis, INPUT);

  // Run-once button - USE PULLUP
  pinMode(runOncePin, INPUT);

  // Start with motors off
  stopMotor1();
  stopMotor2();
  
  Serial.println("System initialized. Press run button to start sequence.");
}

void loop() {

int ButtonBState = digitalRead(buttonBPin);

  // Read joystick only in manual mode or when not in automatic sequence
  if(ButtonBState == HIGH){
  if (currentState == MANUAL_CONTROL || currentState == IDLE ) {
    readJoystick();
  }
}

  // Read pushball buttons
  PushBall();

  // Check for run button press (LOW because of PULLUP)
  if(ButtonBState == HIGH){
  if (digitalRead(runOncePin) == HIGH && !runOncePressed && currentState == IDLE) {
    runOncePressed = true;
    startMotorSequence();
    Serial.println("Run button pressed - starting sequence");
  }
  }
  // Reset trigger when button is released
  if (digitalRead(runOncePin) == LOW) {
    runOncePressed = false;
  }
  
  // Run the current state machine
  runMotorSequence();
  
}

void readJoystick() {
  if (millis() - lastJoystickRead >= JOYSTICK_READ_INTERVAL) {
    lastJoystickRead = millis();
    
    int xValue = analogRead(xAxis);
    int yValue = analogRead(yAxis);

    // X-axis control (LEFT/RIGHT)
    if (xValue < 1000) {
      if (digitalRead(StopLEFT) == LOW) { // HIGH = not pressed (PULLUP)
        digitalWrite(LEFT, HIGH);
        digitalWrite(RIGHT, LOW);
        Serial.println("LEFT");
      } else {
        digitalWrite(LEFT, LOW);
        Serial.println("StopLEFT triggered");
      }
    } 
    else if (xValue > 3000) {
      if (digitalRead(StopRIGHT) == LOW) {
        digitalWrite(RIGHT, HIGH);
        digitalWrite(LEFT, LOW);
        Serial.println("RIGHT");
      } else {
        digitalWrite(RIGHT, LOW);
        Serial.println("StopRIGHT triggered");
      }
    }
    else {
      digitalWrite(LEFT, LOW);
      digitalWrite(RIGHT, LOW);
    }

    // Y-axis control (UP/DOWN)
    if (yValue < 1000) {
      if (digitalRead(StopUP) == LOW) {
        digitalWrite(UP, HIGH);
        digitalWrite(DOWN, LOW);
        Serial.println("UP");
      } else {
        digitalWrite(UP, LOW);
        Serial.println("StopUP triggered");
      }
    } 
    else if (yValue > 3000) {
      if (digitalRead(StopDOWN) == LOW) {
        digitalWrite(DOWN, HIGH);
        digitalWrite(UP, LOW);
        Serial.println("DOWN");
      } else {
        digitalWrite(DOWN, LOW);
        Serial.println("StopDOWN triggered");
      }
    }
    else {
      digitalWrite(UP, LOW);
      digitalWrite(DOWN, LOW);
    }
  }
}

void PushBall() {
 int ButtonAState = digitalRead(buttonAPin);
  int ButtonBState = digitalRead(buttonBPin);
  int ButtonCState = digitalRead(buttonCPin);

if(ButtonAState == HIGH && (ButtonBState == HIGH||ButtonBState == LOW) && ButtonCState == LOW && led5Off && led6Off)
{
 digitalWrite(led5Pin, HIGH);
 digitalWrite(led6Pin, LOW);
 led5Off = false;
 led6Off = true;
 Serial.println("SB Start");
}

if(ButtonAState == LOW && ButtonBState == LOW && ButtonCState == LOW && !led5Off && led6Off)
{
  Serial.println("SB");
 digitalWrite(led5Pin, HIGH);
 digitalWrite(led6Pin, LOW);
 led5Off = false;
 led6Off = true;
}
 
if(ButtonAState == LOW && ButtonBState == LOW && ButtonCState == HIGH && !led5Off && led6Off)
{
 digitalWrite(led5Pin, LOW);
 digitalWrite(led6Pin, HIGH);
 led5Off = true;
 led6Off = false;
 Serial.println("Stop");
}
if(ButtonAState == LOW && ButtonBState == LOW && ButtonCState == LOW && led5Off && !led6Off)
{
 digitalWrite(led5Pin, LOW);
 digitalWrite(led6Pin, HIGH);
 led5Off = true;
 led6Off = false;
 Serial.println("Back");
}
if(ButtonAState == LOW && ButtonBState == HIGH && ButtonCState == LOW && led5Off && !led6Off)
{
  Serial.println("FullStop");
 digitalWrite(led5Pin, LOW);
 digitalWrite(led6Pin, LOW);
 led5Off = true;
 led6Off = true;
}
  delay(10); // this speeds up the simulation
}


// MAIN FUNCTION: Runs the complete motor sequence
void runMotorSequence() {
  switch(currentState) {
    case IDLE:
      // Allow manual control
      manualControlActive = true;
      break;
      
    case RUNNING_MOTORS:
      manualControlActive = false;
      monitorMotors();
      
      if (!motor1Running && !motor2Running) {
        currentState = WAITING_COMPLETION;
        Serial.println("Both motors completed! Waiting 2 seconds...");
      }
      break;
      
    case WAITING_COMPLETION:
      if (millis() - motor1EndTime >= 2000) {

        currentState = EXECUTING_OPERATION;
      }
      break;
      
    case EXECUTING_OPERATION:
    
      executeManualOperation();
      currentState = IDLE;
      break;
      
    case MANUAL_CONTROL:
      // Manual control handled in readJoystick()
      break;
  }
}

void startMotorSequence() {
  Serial.println("=== STARTING AUTOMATIC SEQUENCE ===");
  currentState = RUNNING_MOTORS;
  startMotor1();
  startMotor2();
}

void monitorMotors() {
  // Stop logic for Motor 1 (LOW = pressed with PULLUP)
  if (motor1Running) {
    if (digitalRead(StopLEFT) == HIGH && motor1Direction == false) {
      stopMotor1();
      Serial.println("Motor 1 stopped by Left Button");
    }
    if (digitalRead(StopRIGHT) == HIGH && motor1Direction == true) {
      stopMotor1();
      Serial.println("Motor 1 stopped by Right Button");
    }
  }

  // Stop logic for Motor 2
  if (motor2Running) {
    if (digitalRead(StopUP) == HIGH && motor2Direction == false) {
      stopMotor2();
      Serial.println("Motor 2 stopped by Up Button");
    }
    if (digitalRead(StopDOWN) == HIGH && motor2Direction == true) {
      stopMotor2();
      Serial.println("Motor 2 stopped by Down Button");
    }
  }

  // Auto-stop
  if (motor1Running && millis() >= motor1EndTime) {
    stopMotor1();
    Serial.println("Motor 1 stopped (Time Expired)");
  }
  if (motor2Running && millis() >= motor2EndTime) {
    stopMotor2();
    Serial.println("Motor 2 stopped (Time Expired)");
  }
}

void executeManualOperation() {
  Serial.println("=== STARTING MANUAL OPERATION ===");
  
  int ButtonBState = digitalRead(buttonBPin);
  int ButtonCState = digitalRead(buttonCPin);


// if(ButtonAState == HIGH && (ButtonBState == HIGH||ButtonBState == LOW) && ButtonCState == LOW && led5Off && led6Off)
if(ButtonBState == HIGH && ButtonCState == LOW && led5Off && led6Off)
{
 digitalWrite(led5Pin, HIGH);
 digitalWrite(led6Pin, LOW);
 led5Off = false;
 led6Off = true;
 Serial.println("SB Start");
}

// if(ButtonAState == LOW && ButtonBState == LOW && ButtonCState == LOW && !led5Off && led6Off)
if(ButtonBState == LOW && ButtonCState == LOW && !led5Off && led6Off)
{
  Serial.println("SB");
 digitalWrite(led5Pin, HIGH);
 digitalWrite(led6Pin, LOW);
 led5Off = false;
 led6Off = true;
}

// if(ButtonAState == LOW && ButtonBState == LOW && ButtonCState == HIGH && !led5Off && led6Off)
if(ButtonBState == LOW && ButtonCState == HIGH && !led5Off && led6Off)
{
 digitalWrite(led5Pin, LOW);
 digitalWrite(led6Pin, HIGH);
 led5Off = true;
 led6Off = false;
 Serial.println("Stop");
}
// if(ButtonAState == LOW && ButtonBState == LOW && ButtonCState == LOW && led5Off && !led6Off)
if(ButtonBState == LOW && ButtonCState == LOW && led5Off && !led6Off)
{
 digitalWrite(led5Pin, LOW);
 digitalWrite(led6Pin, HIGH);
 led5Off = true;
 led6Off = false;
 Serial.println("Back");
}


  
  Serial.println("=== MANUAL OPERATION COMPLETED ===");
}

// Motor control functions (keep as is)
void startMotor1() {
  motor1Direction = random(0, 2);
  unsigned int duration = durations1[random(0, 4)];

  digitalWrite(LEFT, motor1Direction ? LOW : HIGH);
  digitalWrite(RIGHT, motor1Direction ? HIGH : LOW);

  Serial.print("Motor 1 moving ");
  Serial.print(motor1Direction ? "BACKWARD" : "FORWARD");
  Serial.print(" for ");
  Serial.print(duration);
  Serial.println(" ms");

  motor1Running = true;
  motor1EndTime = millis() + duration;
}

void startMotor2() {
  motor2Direction = random(0, 2);
  unsigned int duration = durations2[random(0, 4)];

  digitalWrite(UP, motor2Direction ? LOW : HIGH);
  digitalWrite(DOWN, motor2Direction ? HIGH : LOW);

  Serial.print("Motor 2 moving ");
  Serial.print(motor2Direction ? "BACKWARD" : "FORWARD");
  Serial.print(" for ");
  Serial.print(duration);
  Serial.println(" ms");

  motor2Running = true;
  motor2EndTime = millis() + duration;
}

void stopMotor1() {
  digitalWrite(LEFT, LOW);
  digitalWrite(RIGHT, LOW);
  motor1Running = false;
}

void stopMotor2() {
  digitalWrite(UP, LOW);
  digitalWrite(DOWN, LOW);
  motor2Running = false;
}