#include <Servo.h>

#define STEP1 2
#define DIR1  3
#define EN1   4
#define STEP2 5
#define DIR2  6
#define EN2   7

#define TRIG_F 8
#define ECHO_F 9
#define TRIG_L 10
#define ECHO_L 11
#define TRIG_R 12
#define ECHO_R 13
#define TRIG_B A0
#define ECHO_B A1

Servo servoPan;
Servo servoTilt;
#define PAN_PIN A2
#define TILT_PIN A3

#define LED_RED A4
#define LED_GREEN A5
#define LED_BLUE 0
#define BUTTON 1

long duration;
int distF, distL, distR, distB;

void setup() {
  pinMode(STEP1, OUTPUT); pinMode(DIR1, OUTPUT); pinMode(EN1, OUTPUT);
  pinMode(STEP2, OUTPUT); pinMode(DIR2, OUTPUT); pinMode(EN2, OUTPUT);
  digitalWrite(EN1, LOW); digitalWrite(EN2, LOW);

  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
  pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
  pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);
  pinMode(TRIG_B, OUTPUT); pinMode(ECHO_B, INPUT);

  servoPan.attach(PAN_PIN); servoTilt.attach(TILT_PIN);
  servoPan.write(90); servoTilt.write(90);

  pinMode(LED_RED, OUTPUT); pinMode(LED_GREEN, OUTPUT); pinMode(LED_BLUE, OUTPUT);
  digitalWrite(LED_RED, LOW); digitalWrite(LED_GREEN, LOW); digitalWrite(LED_BLUE, LOW);

  pinMode(BUTTON, INPUT_PULLUP);
}

void loop() {
  if (digitalRead(BUTTON) == LOW) {
    digitalWrite(LED_GREEN, HIGH);
    digitalWrite(LED_RED, LOW);
    digitalWrite(LED_BLUE, LOW);
    runDemo();
    digitalWrite(LED_GREEN, LOW);
  } else {
    readSensors();
    if (distF < 30 || distL < 30 || distR < 30 || distB < 30) {
      digitalWrite(LED_RED, HIGH);
      digitalWrite(LED_BLUE, LOW);
    } else {
      digitalWrite(LED_RED, LOW);
      digitalWrite(LED_BLUE, HIGH);
    }
  }
  delay(100);
}

void readSensors() {
  distF = getDistance(TRIG_F, ECHO_F);
  distL = getDistance(TRIG_L, ECHO_L);
  distR = getDistance(TRIG_R, ECHO_R);
  distB = getDistance(TRIG_B, ECHO_B);
}

int getDistance(int trig, int echo) {
  digitalWrite(trig, LOW); delayMicroseconds(2);
  digitalWrite(trig, HIGH); delayMicroseconds(10);
  digitalWrite(trig, LOW);
  duration = pulseIn(echo, HIGH);
  return duration * 0.034 / 2;
}

void runDemo() {
  rotateStepper(STEP1, DIR1, 200, 1);
  delay(500);

  for (int a = 0; a <= 180; a += 10) {
    servoPan.write(a);
    servoTilt.write(90 - a/2);
    delay(50);
  }
  for (int a = 180; a >= 0; a -= 10) {
    servoPan.write(a);
    servoTilt.write(90 - a/2);
    delay(50);
  }
  servoPan.write(90); servoTilt.write(90);

  rotateStepper(STEP2, DIR2, 400, 1);
  delay(300);
  rotateStepper(STEP2, DIR2, 400, -1);
  delay(300);

  readSensors();
  if (distF < 20) {
    digitalWrite(LED_RED, HIGH); delay(200);
    digitalWrite(LED_RED, LOW);  delay(200);
    digitalWrite(LED_RED, HIGH);
  } else {
    digitalWrite(LED_GREEN, HIGH); delay(200);
    digitalWrite(LED_GREEN, LOW);  delay(200);
    digitalWrite(LED_GREEN, HIGH);
  }
  delay(1000);
}

void rotateStepper(int stepPin, int dirPin, int steps, int dir) {
  digitalWrite(dirPin, dir > 0 ? HIGH : LOW);
  for (int i = 0; i < steps; i++) {
    digitalWrite(stepPin, HIGH);
    delayMicroseconds(1000);
    digitalWrite(stepPin, LOW);
    delayMicroseconds(1000);
  }
}