// Made by EasyCore
// student at TATC (computer tachnical 67219090038)

#include <Servo.h>

Servo servo;

#define CM          (58)

#define LED_RED     (12)
#define LED_YELLOW  (8)
#define LED_GREEN   (4)
#define UTS_TRIG    (3)
#define UTS_ECHO    (2)
#define BUTTON      (7)

int s_pos = 0;
bool toggle_btn = false;

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

  pinMode(LED_RED,    OUTPUT);
  pinMode(LED_YELLOW, OUTPUT);
  pinMode(LED_GREEN,  OUTPUT);
  pinMode(BUTTON,     INPUT_PULLUP);
  pinMode(UTS_TRIG,   OUTPUT);
  pinMode(UTS_ECHO,   INPUT);

  servo.attach(11);
}

bool IsOnButton(int slot) {
  return !digitalRead(slot);
}

void ResetServo() {
  if (s_pos > 0) {
    for (s_pos = 90; s_pos >= 0; s_pos--) {
      if (s_pos <= 20) {
        digitalWrite(LED_RED,     HIGH);
        digitalWrite(LED_YELLOW,  LOW);
        digitalWrite(LED_GREEN,   LOW);
      } else if (s_pos <= 60) {
        digitalWrite(LED_RED,     LOW);
        digitalWrite(LED_YELLOW,  HIGH);
        digitalWrite(LED_GREEN,   LOW);
      } else {
        digitalWrite(LED_RED,     LOW);
        digitalWrite(LED_YELLOW,  LOW);
        digitalWrite(LED_GREEN,   HIGH);
      }
      servo.write(s_pos);
      Serial.print("Servo: ");
      Serial.println(s_pos);
      delay(15);
    }
  }
}

void ActiveServo() {
  for (s_pos = 0; s_pos <= 90; s_pos++) {
    if (s_pos <= 20) {
      digitalWrite(LED_RED,     HIGH);
      digitalWrite(LED_YELLOW,  LOW);
      digitalWrite(LED_GREEN,   LOW);
    } else if (s_pos <= 60) {
      digitalWrite(LED_RED,     LOW);
      digitalWrite(LED_YELLOW,  HIGH);
      digitalWrite(LED_GREEN,   LOW);
    } else {
      digitalWrite(LED_RED,     LOW);
      digitalWrite(LED_YELLOW,  LOW);
      digitalWrite(LED_GREEN,   HIGH);
    }
    servo.write(s_pos);
    Serial.print("Servo: ");
    Serial.println(s_pos);
    delay(15);
  }
}

void loop() {
  digitalWrite(UTS_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(UTS_TRIG, LOW);

  if (IsOnButton(BUTTON)) {
    if (toggle_btn != true) {
      ResetServo();
      toggle_btn = true;
    }
  } else {
    if (toggle_btn != false) {
      toggle_btn = false;
    }
  }

  int distance = pulseIn(UTS_ECHO, HIGH) / CM;
  if (distance <= 20 && toggle_btn == false) {
    if (s_pos != 90 && s_pos < 90) {
      ActiveServo();
    }
  } else {
    if (s_pos != 0 && s_pos > 0) {
      ResetServo();
    }
  }

  delay(700);
}