#include <Arduino.h>

// ----- Pedal Pins -----
#define ACC_PIN 32     // Accelerator potentiometer
#define BRAKE_PIN 33   // Brake potentiometer

// ----- Pedal Config -----
const float BRAKE_THRESHOLD = 20.0;  // Brake threshold (percent)

float accPercent = 0.0;
float brakePercent = 0.0;
float netAccel = 0.0;

// ----- Functions -----
float readPedal(int pin) {
  int raw = analogRead(pin);            // ESP32: 0–4095 (12-bit ADC)
  float percent = (raw / 4095.0) * 100; // Convert to 0–100%
  return percent;
}

void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println("Drive-by-Wire Pedal System (Potentiometer Based)");
}

void loop() {
  // --- Read Pedal Inputs ---
  accPercent = readPedal(ACC_PIN);
  brakePercent = readPedal(BRAKE_PIN);

  // --- Control Logic ---
  if (brakePercent >= BRAKE_THRESHOLD) {
    netAccel = 0; // Brake overrides accelerator
  } else {
    netAccel = accPercent - brakePercent;
    if (netAccel < 0) netAccel = 0; // Clamp to 0
  }

  // --- Display Results ---
  Serial.print("Accelerator: ");
  Serial.print(accPercent, 1);
  Serial.print("% | Brake: ");
  Serial.print(brakePercent, 1);
  Serial.print("% | Net Acceleration: ");
  Serial.print(netAccel, 1);
  Serial.println("%");

  delay(1000);
}