/*
  L298N Custom Chip Demo
  Based on the amazing work by DarwinWasWrong
  https://github.com/DarwinWasWrong/wokwi-l298module-chip/tree/main

  The custom chip is apparently not happy with 0 or 255 for PWM,
  limit the range to 1 - 254.
  Bump 9/14/26
*/

// pin constants
const int SPEED_PIN = A1;
const int STEER_PIN = A0;
// Motor A
const int ENA = 11;
const int IN1 = 10;
const int IN2 = 9;
// Motor B
const int ENB = 3;
const int IN3 = 5;
const int IN4 = 4;

const int DEADBAND = 5;
const int JITTER = 2;

//int speed = 0;
int oldSpeed = 0;
//int steer = 0;
int oldSteer = 0;

void goForward(int speed)  {
  //Serial.println("FWD");
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENA, speed);
  analogWrite(ENB, speed);
}

void goReverse(int speed)  {
  //Serial.println("REV");
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, speed);
  analogWrite(ENB, speed);
}

void goLeft(int lSpeed, int rSpeed) {
  //Serial.println("LEFT");
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, lSpeed);
  analogWrite(ENB, rSpeed);
}

void goRight(int lSpeed, int rSpeed)  {
  //Serial.println("RIGHT");
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
  analogWrite(ENA, lSpeed);
  analogWrite(ENB, rSpeed);
}

void allStop()  {
  Serial.println("STOP");
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
}

void setup () {
  Serial.begin(115200);
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  Serial.println("Ready!");
}

void loop() {

  int rawSpeed = analogRead(SPEED_PIN);
  int rawSteer = analogRead(STEER_PIN);
  int speed = 0;
  int steer = 0;

  if (rawSpeed - JITTER > oldSpeed || rawSpeed + JITTER < oldSpeed) {
    int mapSpeed = map(rawSpeed, 0, 1023, 254, 1);
    if (mapSpeed >= 128 + DEADBAND)  {
      speed = map(mapSpeed, 128, 254, 1, 254);
      Serial.println("FWD");
      goForward(speed);
    } else if (mapSpeed < 128 - DEADBAND) {
      speed = map(mapSpeed, 127, 0, 1, 254);
      Serial.println("REV");
      goReverse(speed);
    } else {
      allStop();
    }
    oldSpeed = rawSpeed;
  }

  if (rawSteer - JITTER > oldSteer || rawSteer + JITTER < oldSteer) {
    int mapSteer = map(rawSteer, 0, 1023, 254, 1);
    if (mapSteer >= 128 + DEADBAND)  {
      steer = map(mapSteer, 128, 254, 1, 254);
      Serial.println("Left");
      goLeft(steer - 254, steer);
    } else if (mapSteer < 128 - DEADBAND) {
      steer = map(mapSteer, 127, 0, 1, 254);
      Serial.println("Right");
      goRight(steer, steer);
    } else {
      allStop();
    }
    oldSteer = rawSteer;
  }

}