#include <ESP32Servo.h>
#include <Wire.h>
#include <LiquidCrystal_I2C.h>

#define MIN_ANGLE_POT A0
#define MAX_ANGLE_POT A3
#define SPEED_POT A6
#define BUTTON_PIN 15
#define SERVO_PIN 18

#define LCD_ADDRESS 0x27
#define LCD_COLUMNS 20
#define LCD_ROWS 4

Servo servo;
LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);

int minAngle = 60;   // Default minimum angle
int maxAngle = 120;  // Default maximum angle
int currentAngle = 0;
int stepDelay = 15;  // Default step delay in milliseconds
int LL = 60;
int LH = 85;
int RL = 90;
int RH = 120;
int SL = 250;
int SH = 15;
int SpeedPercentage;

void setup() {
  servo.attach(SERVO_PIN);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  servo.write(90);
  delay(2000);

  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Fox Winding Traverse");
  delay(3000);
  lcd.clear();
}

void loop() {
  // Read potentiometer values and map them to appropriate ranges
  int minAnglePotValue = map(analogRead(MIN_ANGLE_POT), 0, 4095, LL, LH);
  int maxAnglePotValue = map(analogRead(MAX_ANGLE_POT), 0, 4095, RL, RH);
  stepDelay = map(analogRead(SPEED_POT), 0, 4095, SL, SH);
  SpeedPercentage = map(stepDelay, SL, SH, 0, 100);

  // Check button state
  bool buttonPressed = digitalRead(BUTTON_PIN) == LOW;

  // Print potentiometer values on LCD
  lcd.setCursor(0, 0);
  lcd.print("Min Angle: ");
  lcd.print(minAnglePotValue);
  lcd.print(" ");
  lcd.setCursor(0, 1);
  lcd.print("Max Angle: ");
  lcd.print(maxAnglePotValue);
  lcd.print(" ");
  lcd.setCursor(0, 2);
  lcd.print("Speed: ");
  lcd.print(SpeedPercentage);
  lcd.print(" ");

  // Task 1: Set left angle limit
  if (currentAngle == 0) {
    lcd.setCursor(0, 3);
    lcd.print("Set Left Limit");
    servo.write(minAnglePotValue);
    if (buttonPressed) {
      minAngle = minAnglePotValue;
      currentAngle = 1;  // Move to next task
      lcd.clear();
      delay(500);  // Button debounce delay
    }
  }

  // Task 2: Set right angle limit
  else if (currentAngle == 1) {
    lcd.setCursor(0, 3);
    lcd.print("Set Right Limit");
    servo.write(maxAnglePotValue);
    if (buttonPressed) {
      maxAngle = maxAnglePotValue;
      currentAngle = 2;  // Move to next task
      lcd.clear();
      delay(500);  // Button debounce delay
    }
  }

  // Task 3: Sweep between left and right angle limits
  else if (currentAngle == 2) {
    for (int angle = minAngle; angle <= maxAngle; angle++) {
      stepDelay = map(analogRead(SPEED_POT), 0, 4095, SL, SH);
      SpeedPercentage = map(stepDelay, SL, SH, 0, 100);
      lcd.setCursor(0, 2);
      lcd.print("Speed: ");
      lcd.print(SpeedPercentage);
      lcd.print(" ");
      servo.write(angle);
      delay(stepDelay);
    }
    for (int angle = maxAngle; angle >= minAngle; angle--) {
      stepDelay = map(analogRead(SPEED_POT), 0, 4095, SL, SH);
      lcd.setCursor(0, 2);
      lcd.print("Speed: ");
      lcd.print(SpeedPercentage);
      SpeedPercentage = map(stepDelay, SL, SH, 0, 100);
      lcd.print(" ");
      servo.write(angle);
      delay(stepDelay);
    }
  }
}