#define IN1 12
#define IN2 11
#define IN3 10
#define IN4 9
#define STARTSTOP 8
#define DIRECTION 7
#define enableA 6
#define enableB 5
#define RIGHT 4
#define LEFT 3

int motorSpeedRight = 0;
int motorSpeedLeft = 0;
int HALF_SPEED = 256 / 2;

int gears[5] = {
  20,
  40,
  60,
  80,
  100
};

void rightWheel(bool state, int value)
{
    digitalWrite(IN1, state ? HIGH : LOW);
    digitalWrite(IN2, state ? LOW : HIGH);
    analogWrite(enableA, map(value, 0, 100, 0, 255));
}

void leftWheel(bool state, int value)
{
    digitalWrite(IN3, state ? HIGH : LOW);
    digitalWrite(IN4, state ? LOW : HIGH);
    analogWrite(enableB, map(value, 0, 100, 0, 255));
}

void stopMotors()
{
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    analogWrite(enableA, 0);
    analogWrite(enableB, 0);
}

void setup()
{
    // default 1 -> stop
    pinMode(STARTSTOP, INPUT_PULLUP);
    // default 1 -> forward
    pinMode(DIRECTION, INPUT_PULLUP);

    // default 1 -> not turning
    pinMode(RIGHT, INPUT_PULLUP);
    pinMode(LEFT, INPUT_PULLUP);

    pinMode(IN1, OUTPUT);
    pinMode(IN2, OUTPUT);
    pinMode(IN3, OUTPUT);
    pinMode(IN4, OUTPUT);
    Serial.begin(9600);
}

void loop()
{
    int initialValue = gears[3];
    int valueLeft, valueRight;
    bool start = digitalRead(STARTSTOP) == 0;
    if (start)
    {
      bool direction = digitalRead(DIRECTION);
      valueRight = valueLeft = initialValue;

      bool right = digitalRead(RIGHT) == 0;
      bool left = digitalRead(LEFT) == 0;

      if (right && !left) {
        valueRight *= 0.1;
      } else if (!right && left) {
        valueLeft *= 0.1;
      }

      rightWheel(direction, valueRight);
      leftWheel(direction, valueLeft);
    }
    else
    {
        stopMotors();
    }
    delay(500);
}