#include <Servo.h>

#define JOY_X A0
#define JOY_Y A1
#define POT_PIN A2

// Servo
#define SERVO_PIN 9
Servo steeringServo;

// Stepper Motor Left
#define DIR_L 2
#define STEP_L 3

// Stepper Motor Right
#define DIR_R 4
#define STEP_R 5

// Indicators
#define LED_LEFT 10
#define LED_RIGHT 11

// Buzzer and Reverse Cue
#define BUZZER 12
#define REV_LED 13

// Movement threshold
const int threshold = 100;

void setup() {
  // Servo setup
  steeringServo.attach(SERVO_PIN);
  steeringServo.write(90); // Centered

  // Stepper motor pins
  pinMode(DIR_L, OUTPUT);
  pinMode(STEP_L, OUTPUT);
  pinMode(DIR_R, OUTPUT);
  pinMode(STEP_R, OUTPUT);

  // Indicator LEDs and buzzer
  pinMode(LED_LEFT, OUTPUT);
  pinMode(LED_RIGHT, OUTPUT);
  pinMode(BUZZER, OUTPUT);
  pinMode(REV_LED, OUTPUT);

  // Start neutral
  digitalWrite(LED_LEFT, LOW);
  digitalWrite(LED_RIGHT, LOW);
  digitalWrite(BUZZER, LOW);
  digitalWrite(REV_LED, LOW);
}

void loop() {
  int xVal = analogRead(JOY_X);
  int yVal = analogRead(JOY_Y);
  int speedVal = analogRead(POT_PIN);

  // Speed mapping: 0 to 1023 → 50ms to 1ms
  int stepDelay = map(speedVal, 0, 1023, 50, 1);

  // Direction detection
  if (yVal > 600) {
    moveForward(3, stepDelay);
  } else if (yVal < 400) {
    moveBackward(3, stepDelay);
    // Activate reverse indicators
    tone(BUZZER, 1000);
    digitalWrite(REV_LED, HIGH);
    delay(200);
    digitalWrite(REV_LED, LOW);
    noTone(BUZZER);
  }

  // Steering
  if (xVal > 600) {
    steeringServo.write(0); // Right
    digitalWrite(LED_RIGHT, HIGH);
    delay(200);
    digitalWrite(LED_RIGHT, LOW);
  } else if (xVal < 400) {
    steeringServo.write(180); // Left
    digitalWrite(LED_LEFT, HIGH);
    delay(200);
    digitalWrite(LED_LEFT, LOW);
  } else {
    steeringServo.write(90); // Center
  }

  delay(100); // Loop delay
}

void moveForward(int steps, int stepDelay) {
  digitalWrite(DIR_L, HIGH);
  digitalWrite(DIR_R, HIGH);
  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_L, HIGH);
    digitalWrite(STEP_R, HIGH);
    delay(stepDelay);
    digitalWrite(STEP_L, LOW);
    digitalWrite(STEP_R, LOW);
    delay(stepDelay);
  }
}

void moveBackward(int steps, int stepDelay) {
  digitalWrite(DIR_L, LOW);
  digitalWrite(DIR_R, LOW);
  for (int i = 0; i < steps; i++) {
    digitalWrite(STEP_L, HIGH);
    digitalWrite(STEP_R, HIGH);
    delay(stepDelay);
    digitalWrite(STEP_L, LOW);
    digitalWrite(STEP_R, LOW);
    delay(stepDelay);
  }
}