/*
Servo_Test.ino

Brown Dog Gadgets <https://www.browndoggadgets.com/>



 */

#include <Servo.h>

Servo horizontal;
Servo vertical;

int pos = 0;
int moveDelay = 50;
int potValue;
int mapValue;

unsigned long MOVING_TIME = 3000; // moving time is 3 seconds
unsigned long PAUSE_TIME  = 2000; // 2 second pause
unsigned long moveStartTime;
int startAngle = 0; // 30°
int endAngle  = 180; // 90°

bool movingForward = true; // true = 0→180, false = 180→0
bool paused = false; // true during the 2-second pause
unsigned long pauseStartTime;

void setup() {
  Serial.begin(9600);
  horizontal.attach(5);
  vertical.attach(6);
  horizontal.write(90);
  vertical.write(startAngle);

  delay(500); // Let the servo get to 0°
  moveStartTime = millis(); // start moving
  
}

void loop() {

  potValue = analogRead(A0);
  mapValue = map(potValue, 0, 1023, 0, 180);
  horizontal.write(mapValue);

  //Serial.print("potValue: "); Serial.println(potValue);
  //Serial.print("mapValue: "); Serial.println(mapValue);

  //vertical.write(0);
  //delay(1000);
  //vertical.write(180);
  //delay(1000);

  if (paused) {
    // Handle the 2-second pause
    if (millis() - pauseStartTime >= PAUSE_TIME) {
      paused = false;
      moveStartTime = millis(); // Begin the next sweep
    }
    return;  // Still paused — nothing else to do
  }

// Movement in progress
  unsigned long elapsed = millis() - moveStartTime;

  if (elapsed <= MOVING_TIME) {

    float progress = (float)elapsed / MOVING_TIME;

    int angle;
    if (movingForward) {
      angle = startAngle + (endAngle - startAngle) * progress;   // 0→180
    } else {
      angle = endAngle - (endAngle - startAngle) * progress;     // 180→0
    }

    vertical.write(angle);

  } else {
    // Movement finished — land exactly on final angle
    vertical.write(movingForward ? endAngle : startAngle);

    // Switch direction
    movingForward = !movingForward;

    // Start pause
    paused = true;
    pauseStartTime = millis();
  }

  
/*
  for (pos = 0; pos <= 180; pos++) {
    horizontal.write(pos);
    delay(moveDelay);
  }
  delay(1000);

  for (pos = 180; pos >= 0; pos--) {
    horizontal.write(pos);
    delay(moveDelay);
  }
  delay(1000);


  for (pos = 0; pos <= 180; pos++) {
    vertical.write(pos);
    delay(moveDelay);
  }
  delay(1000);
  
  for (pos = 180; pos >= 0; pos--) {
    vertical.write(pos);
    delay(moveDelay);
  }
  delay(1000);
*/ 

}