// Servo Sweep example for the ESP32
// https://wokwi.com/arduino/projects/323706614646309460

#include <ESP32Servo.h>

// Constants
const int servoPin = 18;
const int steeringPin = 13;
const int accelerationPin = 12;
const int maxAccelVal = 4096;
const int midAccelVal = maxAccelVal/2;
const int minAccelVal = 0;
const int midAccelOffset = 200;
const int lightPWM = 200;
const int brakePWM = 255;

// LED Pins
const int brakeLedPin =  26;
const int headlightLedPin = 25;
const int reverselightLedPin = 19;

// Buttons


// otjer
bool bool_hl = false;
int directionVal = 0;
int steeringVal = 0;
int glob_headlight_pwm = 0;
int servoVal = 0;

Servo servo;

void setup() {
  servo.attach(servoPin, 500, 2400);
  Serial.begin(9600);
  pinMode(brakeLedPin, OUTPUT); 
 
}

int pos = 0;

void funcbrakelight(int LedPWM) {
  analogWrite(brakeLedPin, LedPWM);
}


void funcreverselight(int LedPWM){
  analogWrite(reverselightLedPin, LedPWM);

}

void loop() {

  directionVal = analogRead(accelerationPin);  // Read the value from the analog pin
  steeringVal = analogRead(steeringPin);
  servoVal = map(steeringVal, 0, 4095, 0, 180);
  servo.write(servoVal);
  
  if (directionVal >= midAccelVal + midAccelOffset && directionVal < maxAccelVal) {
    Serial.println("fwd: " + String(directionVal));
    funcbrakelight(glob_headlight_pwm);  // Turn off brake light
    funcreverselight(0);
  }
  else if (minAccelVal <= directionVal && directionVal <= midAccelVal - midAccelOffset) {
    Serial.println("bck: " + String(directionVal));
    funcbrakelight(glob_headlight_pwm);  // Turn off brake light
    funcreverselight(255);
  }
  else {
    Serial.println("stp: " + String(directionVal) + " > " + String(glob_headlight_pwm) );
    funcbrakelight(255);  // Turn on brake light at full brightness
    funcreverselight(0);
  }

  delay(10);  // Small delay to avoid flooding the serial output
}