#include <Servo.h>
#define POTPIN  A0
#define BUTTONPIN  8
#define LEDPIN  13
Servo myservo;
int currentAngle = 0;
int desiredAngle = 0;
bool isEPSEnabled = false;
int lastButtonState = HIGH;
int buttonState = HIGH;
unsigned long lastDebounceTime = 0;
unsigned long debounceDelay = 50;

void setup() {
  Serial.begin(9600);
  pinMode(POTPIN, INPUT);
  pinMode(BUTTONPIN, INPUT_PULLUP);
  pinMode(LEDPIN, OUTPUT);
  myservo.attach(9);
  Serial.println("EPS System Initialized");
  Serial.println("Enter a desired steering angle (45° to 135°) via Serial Monitor.");
}

void loop() {
  if (Serial.available() > 1) {
    desiredAngle = Serial.parseInt();
    Serial.print("input Angle: ");
    Serial.println(desiredAngle);
    if(!isEPSEnabled){
      Serial.println("Enable EPS");
    }
    if (desiredAngle < 45 || desiredAngle > 135) {
      Serial.println("Error: Desired angle out of range");
      isEPSEnabled = false;
      Serial.println("EPS Disabled");
      digitalWrite(LEDPIN, LOW);
    } else {
      if (isEPSEnabled) {
        moveMotorToAngle(desiredAngle);
      }
    }
  }

  int reading = digitalRead(BUTTONPIN);
  if (reading != lastButtonState) {
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    if (reading != buttonState) {
      buttonState = reading;
      if (buttonState == LOW) {
        if (isEPSEnabled) {
          isEPSEnabled = false;
          Serial.println("EPS Disabled");
          digitalWrite(LEDPIN, LOW);
        } else {
          isEPSEnabled = true;
          Serial.println("EPS Enabled");
          digitalWrite(LEDPIN, HIGH);
          Serial.println("EPS System Initialized");
          Serial.println("Enter a desired steering angle (45° to 135°) via Serial Monitor.");
        }
      }
    }
  }

  lastButtonState = reading;
}


void moveMotorToAngle(int targetAngle) {
  currentAngle = readPotentiometer();

  while (currentAngle != targetAngle) {
    currentAngle = readPotentiometer();
	  displayCurrentStatus(currentAngle);
    myservo.write(currentAngle);
    int a=digitalRead(BUTTONPIN);
    
    if(a!=buttonState){
      if(isEPSEnabled){
      isEPSEnabled = false;
      Serial.println("EPS Disabled");
      digitalWrite(LEDPIN, LOW);
      }
      lastButtonState=a;
      //buttonState=HIGH;
      //lastButtonState=HIGH;
      break;
    }
    delay(500);
    if (currentAngle == targetAngle) {
      Serial.println(" Steering Angle Reached");
      Serial.println("Enter a desired steering angle (45° to 135°) via Serial Monitor.");
    }
  }
}

int readPotentiometer() {
  int potentiometerValue = analogRead(POTPIN);
  int angle = map(potentiometerValue, 0, 1023, 45, 135);
  return angle;
}

void displayCurrentStatus(int targetAngle) {
  Serial.print(" Current Angle: ");
  Serial.println(currentAngle);
}