#include <Servo.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_RESET    -1  // Reset pin not used

// Initialize SSD1306 display with I2C address 0x3C
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define MIN_ANGLE_POT A0
#define MAX_ANGLE_POT A1
#define SPEED_POT A2
#define BUTTON_PIN 2
#define SERVO_PIN 9

Servo servo;

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);

  if(!oled.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    while (1); // Don't proceed, loop forever
  }
  
  oled.clearDisplay();
  oled.setTextSize(1);
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0, 0);
  oled.print("Fox Winding Traverse");
  oled.display();
  delay(3000);
  oled.clearDisplay();
}

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

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

  // Print potentiometer values on OLED
  oled.clearDisplay();
  oled.setCursor(0, 0);
  oled.print("Min Angle: ");
  oled.print(minAnglePotValue);
  oled.setCursor(0, 16);
  oled.print("Max Angle: ");
  oled.print(maxAnglePotValue);
  oled.setCursor(0, 32);
  oled.print("Speed: ");
  oled.print(SpeedPercentage);
  oled.display();

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

  // Task 2: Set right angle limit
  else if (currentAngle == 1) {
    oled.setCursor(0, 48);
    oled.print("Set Right Limit");
    oled.display();
    servo.write(maxAnglePotValue);
    if (buttonPressed) {
      maxAngle = maxAnglePotValue;
      currentAngle = 2;  // Move to next task
      oled.clearDisplay();
      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, 1023, SL, SH);
      SpeedPercentage = map(stepDelay, SL, SH, 1, 100);
      
      oled.setCursor(0, 32);
      oled.print("Speed: ");
      oled.print(SpeedPercentage);
      oled.display();
      
      servo.write(angle);
      delay(stepDelay);
    }
    for (int angle = maxAngle; angle >= minAngle; angle--) {
      stepDelay = map(analogRead(SPEED_POT), 0, 1023, SL, SH);
      
      oled.setCursor(0, 32);
      oled.print("Speed: ");
      oled.print(SpeedPercentage);
      oled.display();
      
      SpeedPercentage = map(stepDelay, SL, SH, 1, 100);
      servo.write(angle);
      delay(stepDelay);
    }
  }
}