//Project       : Actuator (Servo Motor) Tugas
//Nama          : HANNA MALIKA AZ-ZAHRA (2206055914)
//Date created  : 21/05/2024
//Date modified : 23/05/2024  (versi: 1.3)

/* ***************************************************************************** */

#include <ESP32Servo.h>

#define PB1_PIN 14      // Push-button 'biru' dihubungkan ke GPIO 14
#define PB2_PIN 12      // Push-button 'hijau' dihubungkan ke GPIO 12
#define PB3_PIN 13      // Push-button 'abu-abu' dihubungkan ke GPIO 13
#define SERVO_PIN 16    // Servo motor dihubungkan ke GPIO 15

Servo servo1;
void moveServo(float pos, char* button);   // Protipe fungsi moveServo
void handleMove (float pos, char*button);  // Protipe fungsi handleMove

// Angle calculations
float x = 914.0 / 8.0;   // x = 114.25
float pos1 = 20.0 + x;   // Posisi servo untuk push-button 1 = 134.25
float pos2 = 90.0 - x;   // Posisi servo untuk push-button 2 = -24.25
float pos3 = 120.0 + x;  // Posisi servo untuk push-button 3 = 234.25

float currentPos = 90.0;        // Inisialisasi posisi servo
float targetPos = currentPos;

unsigned long moveStartTime = 0;
const unsigned long moveDuration = 2000; // 2 seconds

void setup() {
  Serial.begin(115200);

  //setup servo motor
  servo1.attach(SERVO_PIN);
  servo1.write(currentPos);

  //setup push-button
  pinMode(PB1_PIN, INPUT_PULLUP);
  pinMode(PB2_PIN, INPUT_PULLUP);
  pinMode(PB3_PIN, INPUT_PULLUP);
}

void loop() {
  if (digitalRead(PB1_PIN) == LOW) {         // Cek kondisi terhadap push-button 1
    handleMove(pos1, "1");
  } else if (digitalRead(PB2_PIN) == LOW) {  // Cek kondisi terhadap push-button 2
    handleMove(pos2, "2");
  } else if (digitalRead(PB3_PIN) == LOW) {  // Cek kondisi terhadap push-button 3
    handleMove(pos3, "3");
  }

  if (currentPos != targetPos) {
    unsigned long progress = millis() - moveStartTime;

    if (progress <= moveDuration) {
      float newPos = map(progress, 0, moveDuration, currentPos, targetPos);
      servo1.write(newPos);
      Serial.print("CURRENT POSITION: ");
      Serial.print(newPos);
      Serial.print("°, MOVE TO ");
      Serial.print(targetPos);
      Serial.print("°, time: ");
      Serial.print(progress);
      Serial.println(" ms");
    } else {
      servo1.write(targetPos);
      currentPos = targetPos;
      Serial.print("Reached target position: ");
      Serial.print(targetPos);
      Serial.println("°");
    }
  }
}

//defnisi fungsi moveServo
void moveServo (float pos, char* button) {
  if (pos != targetPos) {
    currentPos = servo1.read();
    targetPos = pos;
    moveStartTime = millis();
    Serial.print(">> Button ");
    Serial.print(button);
    Serial.print(" pressed, SET TO ");
    Serial.print(targetPos);
    Serial.println("°");
    delay(1000);
  }
}

//definisi fungsi handeleMove
void handleMove (float pos, char*button) {
  if(pos != targetPos) {
      moveServo(pos, button);
    } else {
      Serial.print("Position: ");
      Serial.print(targetPos);
      Serial.println("°, already set!");
      delay(1000);
    }
}