// Pin Definitions
#define accelPin A0    // Accelerator potentiometer pin
#define wheelPin 2    // Wheel sensor pin
#define pwmPin 8     // PWM output pin


#define wheelDiameter  0.45  // Wheel diameter in meters
#define pi 3.14159      // Pi value

void setup() {
  // Initialize serial communication
  Serial.begin(9600);

  // Set pin modes
  pinMode(accelPin, INPUT);
  pinMode(wheelPin, INPUT);
  pinMode(pwmPin, OUTPUT);
}

void loop() {
  // Read accelerator potentiometer
  float accelReading = analogRead(accelPin);
  float accelDemand = (accelReading / 1023.0) * 100.0;  // Convert to percentage

  // Calculate PWM signal for engine demand
  int pwmValue = map(accelReading, 0, 1023, 0, 255);
  analogWrite(pwmPin, pwmValue);

  // Calculate speed using wheel sensor
  float speed = calculateSpeed(wheelPin, wheelDiameter);

  // Print speed and demand to Serial
  Serial.print("Speed = ");
  Serial.print(speed, 2);  // Print speed with 2 decimal places
  Serial.print(" Km/H    Demand = ");
  Serial.print(accelDemand, 2);  // Print demand with 2 decimal places
  Serial.println(" %");

  // Delay to update 20 times per second (50 ms)
  delay(50);

}

// Function to calculate speed using pulseIn
float calculateSpeed(int sensorPin, float wheelSize) {
  unsigned long duration = pulseIn(sensorPin, HIGH);

  if (duration == 0) {
    return 0.0;  // No pulse detected
  }

  // Calculate wheel circumference
  float circumference = pi * wheelSize;  // in meters

  // Calculate speed in km/h
  float speedKmh = (circumference * 3600000) / duration;

  return speedKmh;
}