#include <AccelStepper.h>

// Motor 1 connections (Left Motor - ULN2003)
const int M1_IN1 = 7;
const int M1_IN2 = 6;
const int M1_IN3 = 5;
const int M1_IN4 = 4;

// Motor 2 connections (Right Motor - ULN2003)
const int M2_IN1 = 8;
const int M2_IN2 = 9;
const int M2_IN3 = 10;
const int M2_IN4 = 11;
const int LED = 13;

// Define motors using AccelStepper in HALF4WIRE mode
AccelStepper motorLeft(AccelStepper::HALF4WIRE, M1_IN1, M1_IN3, M1_IN2, M1_IN4);
AccelStepper motorRight(AccelStepper::HALF4WIRE, M2_IN1, M2_IN3, M2_IN2, M2_IN4);

// Movement parameters
const int MAX_SPEED = 250;       // steps per second
const int MAX_ACCEL = 500; 
const int STEP_COUNT_FWD = 100;
const int STEP_COUNT_TURN_FWD = 3490;
const int STEP_COUNT_TURN = 2100;
const int STEP_COUNT_UTURN = 1600;

String input = "";

void setup() 
{
  Serial.begin(115200);
  motorLeft.setAcceleration(MAX_ACCEL);
  motorRight.setAcceleration(MAX_ACCEL);
  motorLeft.setMaxSpeed(MAX_SPEED);
  motorRight.setMaxSpeed(MAX_SPEED);
  pinMode(LED, OUTPUT);
  digitalWrite(LED, LOW);
}

void loop() 
{
  if (Serial.available() > 0) 
  {
    input = Serial.readStringUntil('\n');
    Serial.println(input);
    input.trim();
    if (input.equalsIgnoreCase("f")) 
    {
      digitalWrite(LED, HIGH);
      motorLeft.move(STEP_COUNT_FWD);
      motorRight.move(STEP_COUNT_FWD);
      runUntilDone();
    } 
    else if (input.equalsIgnoreCase("s")) 
    {
      motorLeft.stop();
      motorRight.stop();
    } 
    else if (input.equalsIgnoreCase("l")) 
    {
      motorLeft.move(STEP_COUNT_TURN_FWD);
      motorRight.move(STEP_COUNT_TURN_FWD);
      runUntilDone();
      motorLeft.move(-STEP_COUNT_TURN);
      motorRight.move(STEP_COUNT_TURN);
      runUntilDone();
    } 
    else if (input.equalsIgnoreCase("r")) 
    {
      motorLeft.move(STEP_COUNT_TURN_FWD);
      motorRight.move(STEP_COUNT_TURN_FWD);
      runUntilDone();
      motorLeft.move(STEP_COUNT_TURN);
      motorRight.move(-STEP_COUNT_TURN);
      runUntilDone();
    } 
    else if (input.equalsIgnoreCase("u")) 
    {
      motorLeft.move(STEP_COUNT_TURN_FWD);
      motorRight.move(STEP_COUNT_TURN_FWD);
      runUntilDone();
      motorLeft.move(-STEP_COUNT_TURN);
      motorRight.move(STEP_COUNT_TURN);
      runUntilDone();
      motorLeft.move(-STEP_COUNT_TURN-1100);
      motorRight.move(STEP_COUNT_TURN-1100);
      runUntilDone();
    } 
    else if (input.startsWith("al")) 
    {
      digitalWrite(LED, LOW);
      int val = input.substring(2).toInt();
      motorLeft.move(-val);
      motorRight.move(val);
      runUntilDone();
    } 
    else if (input.startsWith("ar")) 
    {
      int val = input.substring(2).toInt();
      motorLeft.move(val);
      motorRight.move(-val);
      runUntilDone();
    }
  }
  motorLeft.run();
  motorRight.run();
}

void runUntilDone() 
{
  while (motorLeft.distanceToGo() != 0 || motorRight.distanceToGo() != 0) {
    motorLeft.run();
    motorRight.run();
  }
}