#define debug
#include <Wire.h>
#include <LiquidCrystal_I2C.h>
//Defines to shorten print statements
#define sp(x) Serial.print(x)
#define spl(x) Serial.println(x)

//This saves dynamic memory by assigning constant strings to Program memory
#define mp(x) Serial.print(F(x))
#define mpl(x) Serial.println(F(x))

//This saves dynamic memory by assigning constant strings to Program memory
#define disp(line,msg) display(line,F(msg))

char bf[25] = "test";
#define qp(...) sprintf(bf, __VA_ARGS__ ); Serial.println(bf)
#define dp(row,...) sprintf(bf, __VA_ARGS__ ); lcd.setCursor(0,row); lcd.print(bf)
// Initialize the LCD library with the I2C address 0x27 (modify if your address is different)
LiquidCrystal_I2C lcd(0x27, 4, 20);
#define pinSpeedIn A0
#define pinTurnIn A1

int steering7; // where steering input values are stored from the receiver
int throttle8; // where throttle input values are stored from the receiver
int stopThreshold = 15; // Speed threshold to stop the motors

int lmf5 = 5;  // PWM control for left motor forward
int rmf6 = 6;  // PWM control for right motor forward
int lmb9 = 9;  // PWM control for left motor backward
int rmb10 = 10; // PWM control for right motor backward



// Function to constrain the motor PWM value within the valid range (-255 to 255)
int constrainMotorPWM(int value) {
  return constrain(value, -255, 255);
}

// Function to control the left motor based on speed amount
void setLeftMotor(int speed) {

  if (speed >= 0) {
    speed = constrainMotorPWM(speed);
    analogWrite(lmf5, speed);
    analogWrite(lmb9, 0);
    dp(3, "MtrLeft  : %d Fwd  ", speed);
    //Serial.print("Left Motor Forward: ");
    //Serial.println(speed);
  } else {
    speed = abs(speed);
    speed = constrainMotorPWM(speed);
    analogWrite(lmf5, 0);
    analogWrite(lmb9, speed);
    dp(3, "MtrLeft  :%d Rev  ", -speed);
    //Serial.print("Left Motor Backward: ");
    //Serial.println(speed);
  }
}

// Function to control the right motor based on speed amount
void setRightMotor(int speed) {

  if (speed >= 0) {
    speed = constrainMotorPWM(speed);
    analogWrite(rmf6, speed);
    analogWrite(rmb10, 0);
    dp(2, "MtrRight : %d Fwd  ", speed);
    //Serial.print("Right Motor Forward: ");
    //Serial.println(speed);
  } else {
    speed = abs(speed);
    speed = constrainMotorPWM(speed);
    analogWrite(rmf6, 0);
    analogWrite(rmb10, speed);
    dp(2, "MtrRight :%d Rev  ", -speed);
    //Serial.print("Right Motor Backward: ");
    //Serial.println(speed);
  }
}

void setup() {

  // Initialize the LCD display
  lcd.begin(20, 4); // Specify the columns and rows of the LCD
  lcd.backlight();

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(7, INPUT); // declare steering pin as an input
  pinMode(8, INPUT); // declare throttle pin an input
  pinMode(lmf5, OUTPUT);  // declare left motor forward an output
  pinMode(rmf6, OUTPUT);  // declare right motor forward an output
  pinMode(lmb9, OUTPUT);  // declare left motor back an output
  pinMode(rmb10, OUTPUT);  // declare right motor back an output

  Serial.begin(115200); // start //Serial print for debugging purposes
  analogWrite(lmf5, 0);  // set all motors to off
  analogWrite(rmf6, 0);
  analogWrite(lmb9, 0);
  analogWrite(rmb10, 0);
}

void loop() {
  int turnAmount, speedAmount;
#ifdef debug
  speedAmount = analogRead(A1);
  speedAmount = map(speedAmount, 0, 1023, -255, 255);
 // mp("A1 Speed:"); spl(speedAmount);
  turnAmount = analogRead(A0);
  turnAmount = map(turnAmount, 0, 1023, -255, 255);
 // mp("A0 Turn:"); spl(turnAmount);
#else
  steering7 = pulseIn(7, HIGH, 25000); // Read the pulse width of the steering channel
  throttle8 = pulseIn(8, HIGH, 25000); // Read the pulse width of the throttle channel
  // Calculate the speed amount (-255 to +255) based on the throttle input
  speedAmount = map(throttle8, 1000, 2000, -255, 255);
  // Calculate the turn amount (-255 to +255) based on the steering input
  turnAmount = map(steering7, 1000, 2000, -255, 255);
#endif
  speedAmount  = constrain(speedAmount, -255, 255);
  turnAmount  = constrain(turnAmount, -255, 255);
  dp(0, "Speed: %d   ", speedAmount );
  dp(1, "Turn : %d   ", turnAmount );

  // Stop both motors if the absolute speed is less than the threshold
  if (abs(speedAmount) < stopThreshold) {
    setLeftMotor(0);
    setRightMotor(0);
    //Serial.println("Motors Stopped");
  } else {

    // Control the motors based on the speed and turn amount
    // When there is a turn - one motor speeds up and the other slows down
    // When the speed is less than the max it is possible for
    // the wheels to go opposite in directions when turning
    if (speedAmount > 0) {
      setLeftMotor(speedAmount + turnAmount);
      setRightMotor(speedAmount - turnAmount);
    } else { //reversing
       //This makes it behave like a vehicle with wheels
      //If you turn the wheel to the right and reverse the 
      //left wheel will travel further then the right
      setLeftMotor(speedAmount - turnAmount);
      setRightMotor(speedAmount + turnAmount);
    }

  }
  delay(10);
}