#include <Servo.h>
#include <HCSR04.h>

int PIN_TRIGGER = 13;
int PIN_ECHO = 12;
int PIN_MOTOR1 = 9;
int PIN_MOTOR2 = 10;
int PIN_LED = 2;

int TRIGGER_DISTANCE = 10;
int MOTOR_START_DEG = 45;
int MOTOR_END_DEG = 135;
int MOTOR_SPEED = 50; // deg/s

Servo motor1;
Servo motor2;

Servo motors[] = {motor1, motor2};
bool motorDirections[] = {true, true};

UltraSonicDistanceSensor distanceSensor(PIN_TRIGGER, PIN_ECHO);

void motorSetup() {
  motor1.attach(PIN_MOTOR1);
  motor2.attach(PIN_MOTOR2);
  motor1.write(MOTOR_START_DEG);
  motor2.write(MOTOR_START_DEG);
}

void moveMotor(int i) {
  if (motorDirections[i]) { // Clockwise
    motors[i].write(motors[i].read() + 1);
  } else {
    motors[i].write(motors[i].read() - 1);
  }

  if (motors[i].read() <= MOTOR_START_DEG) {
    motorDirections[i] = true;
  }

  if (motors[i].read() >= MOTOR_END_DEG) {
    motorDirections[i] = false;
  }
}

void setup() {
  pinMode(PIN_LED, OUTPUT);

  Serial.begin(9600);
  motorSetup();

  delay(1000);
}

void loop() {
  while (distanceSensor.measureDistanceCm() < TRIGGER_DISTANCE && !(distanceSensor.measureDistanceCm() < 0)) {
    moveMotor(0);
    moveMotor(1);

    digitalWrite(PIN_LED, HIGH);
    delay(1000 / MOTOR_SPEED);
  }
  
  digitalWrite(PIN_LED, LOW);
  // Serial.println("Distance: " + String(distance, 10));
  delay(500);
}