#include <Servo.h>
int inputs[] = {8, 9, 10, 11}; 
int steeringWheel = A1; 
Servo myservo; // Servo object
int gear = A0; 
int button = 5;
int enA = A2; 
int enB = A3; 

// Forward and backward function
void forward(int speed);
void backward(int speed);
float calcSpeed(int rpm);

void setup() {
  // Attach the servo to pin 3
  myservo.attach(3);
  // Configure input pins for the car
  for (int i = 0; i < 4; i++) {
    pinMode(inputs[i], OUTPUT); // Set all inputs as output
  }

  pinMode(gear, INPUT); 
  pinMode(button, INPUT); 
  pinMode(steeringWheel, INPUT); 
  pinMode(enA, OUTPUT); 
  pinMode(enB, OUTPUT); 
  Serial.begin(9600);
}

void loop() {
  // Read and map the steering wheel value to servo angle
  int angle = analogRead(steeringWheel);
  angle = map(angle, 0, 1023, 0, 180);
  myservo.write(angle);

  // Read and map the gear value to PWM speed
  int speed = analogRead(gear);
  analogWrite(enA, speed);
  analogWrite(enB, speed);

  // Read the direction button and call forward or backward
  int direction = digitalRead(button);
  direction ? forward(speed) : backward(speed);
  //calcate the speed
  Serial.print(calcSpeed(speed));
  Serial.println("Cm/min");
  delay(500);
}

void forward(int speed) {
    digitalWrite(inputs[1], LOW);
    digitalWrite(inputs[3], LOW);

  if (speed < 3){
    digitalWrite(inputs[0], HIGH);
    digitalWrite(inputs[2], HIGH);
    Serial.println("Moving forward");
  }else{
    digitalWrite(inputs[0], LOW);
    digitalWrite(inputs[2], LOW);

  }
  }


void backward(int speed) {
    digitalWrite(inputs[2], LOW);
    digitalWrite(inputs[4], LOW);

  if (speed < 3){
    digitalWrite(inputs[1], HIGH);
    digitalWrite(inputs[3], HIGH);
    Serial.println("Moving backward");
  }else{
    digitalWrite(inputs[1], LOW);
    digitalWrite(inputs[3], LOW);

  }
  }



float calcSpeed(int rpm) {
  // Assuming max RPM is 600 and wheel radius is 10 cm
  int radius = 10; // Radius in cm
  rpm = map(rpm, 1023, 0, 0, 600); // Corrected map function usage/
  float speed = (2 * 3.14 * radius) * rpm; // Circumference of the wheel * RPM
  return speed;
}