#include <AccelStepper.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

// Stepper motor pin definitions for Arduino Mega 2560
const int motor1Pins[2] = {22, 23};
const int motor2Pins[2] = {24, 25};
const int motor3Pins[2] = {26, 27};
const int motor4Pins[2] = {28, 29};

// Pins for potentiometers
const int potPinX = A0; // Potentiometer for acceleration
const int potPinY = A1; // Potentiometer for steering

AccelStepper motor1(AccelStepper::DRIVER, motor1Pins[0], motor1Pins[1]);
AccelStepper motor2(AccelStepper::DRIVER, motor2Pins[0], motor2Pins[1]);
AccelStepper motor3(AccelStepper::DRIVER, motor3Pins[0], motor3Pins[1]);
AccelStepper motor4(AccelStepper::DRIVER, motor4Pins[0], motor4Pins[1]);

LiquidCrystal_I2C lcd(0x27, 20, 4); // I2C address 0x27, 20 column and 4 rows

int xValue = 0;  // Potentiometer X value for acceleration
int yValue = 0;  // Potentiometer Y value for steering

void setup() {
  Serial.begin(9600);
  Wire.begin();

  lcd.init();
  lcd.backlight();
  
  motor1.setMaxSpeed(2000); // Set a high max speed
  motor2.setMaxSpeed(2000);
  motor3.setMaxSpeed(2000);
  motor4.setMaxSpeed(2000);

  motor1.setAcceleration(150);
  motor2.setAcceleration(150);
  motor3.setAcceleration(150);
  motor4.setAcceleration(150);
}

void loop() {
  // Step 1: Read the X and Y values from the potentiometers
  xValue = analogRead(potPinX);
  yValue = analogRead(potPinY);

  // Step 2: Map the X and Y values according to the given conditions
  xValue = map(xValue, 0, 1023, -200, 200);
  yValue = map(yValue, 0, 1023, -500, 500);

  xValue = constrain(xValue, -200, 200);
  yValue = constrain(yValue, -500, 500);

  // Add dead zones
  if (xValue > -20 && xValue < 20) xValue = 0;
  if (yValue > -40 && yValue < 40) yValue = 0;

  // Step 3: Adjust steering effect based on speed
 // float steeringEffect = 1.0 + abs(xValue) / 200.0; 
  //yValue *= steeringEffect;

  // Step 4: Calculate the individual wheel speeds based on the X and Y values
  int leftSpeed = (xValue + yValue) * 10;
  int rightSpeed = (xValue - yValue) * 10;

  // Step 5: Set the motor speeds using the AccelStepper library
  motor1.setSpeed(leftSpeed);
  motor2.setSpeed(rightSpeed);
  motor3.setSpeed(leftSpeed);
  motor4.setSpeed(rightSpeed);

  // Step 6: Move the motors
  motor1.runSpeed();
  motor2.runSpeed();
  motor3.runSpeed();
  motor4.runSpeed();

  // Step 7: Display X, Y, and status on LCD
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("X:");
  lcd.print(xValue);
  lcd.setCursor(0, 1);
  lcd.print("Y:");
  lcd.print(yValue);
  
  lcd.setCursor(0, 2);
  if (xValue == 0 && yValue == 0) {
    lcd.print("X");
  } else if (xValue == 0 && yValue != 0) {
    lcd.print("O");
  } else {
    lcd.print("-");
  }
  
  lcd.setCursor(0, 3);
  lcd.print(motor1.speed());
  lcd.print("|");
  lcd.print(motor2.speed());
}