/*
################################################################################
# Aqilah Salsabila                                                              #
# 2206821582                                                                            #
# x = 582/8 = 72,75 = 73
################################################################################
*/

#include <Servo.h>

Servo myservo;
int button1Pin = 11; 
int button2Pin = 10; 
int button3Pin = 9;

int x = 73; // Value of x

int pos1 = constrain(20 + x, 0, 180); // 20 + 73 = 93, constrained to 0-180
int pos2 = constrain(90 - x, 0, 180); // 90 - 73 = 17, constrained to 0-180
int pos3 = constrain(120 + x, 0, 180); // 120 + 73 = 193, constrained to 0-180

int currentPos = 0; 

void setup() {
  myservo.attach(9);  
  pinMode(button1Pin, INPUT_PULLUP); 
  pinMode(button2Pin, INPUT_PULLUP);
  pinMode(button3Pin, INPUT_PULLUP);
  myservo.write(currentPos); 
  Serial.begin(9600); // Start serial communication for debugging
}

void loop() {
  if (digitalRead(button1Pin) == LOW) {
    Serial.println("Button 1 Pressed");
    moveToPosition(pos1);
  } else if (digitalRead(button2Pin) == LOW) {
    Serial.println("Button 2 Pressed");
    moveToPosition(pos2);
  } else if (digitalRead(button3Pin) == LOW) {
    Serial.println("Button 3 Pressed");
    moveToPosition(pos3);
  }
}

void moveToPosition(int targetPos) {
  Serial.print("Moving to position: ");
  Serial.println(targetPos);
  if (currentPos == targetPos) {
    return; 
  }

  int step = (targetPos > currentPos) ? 1 : -1;
  int totalSteps = abs(targetPos - currentPos);
  int delayTime = 2000 / totalSteps; 

  for (int pos = currentPos; pos != targetPos; pos += step) {
    myservo.write(pos);
    delay(delayTime);
  }
  myservo.write(targetPos); 
  currentPos = targetPos;
  Serial.print("Current position: ");
  Serial.println(currentPos);
}