#include <Servo.h>

#define servo_pin 9

#define sensor_id_number   2

#define led_status_pin 13
#define sensor_trig_pin 3
#define sensor_echo_pin 2
#define led_green_pin 7
#define led_red_pin 8

Servo servo;

void setup() {
    pinMode(sensor_trig_pin, OUTPUT);
    pinMode(sensor_echo_pin, INPUT);
    pinMode(led_green_pin, OUTPUT);
    pinMode(led_red_pin, OUTPUT);
    pinMode(led_status_pin, OUTPUT);

    servo.attach(servo_pin);

    Serial.begin (9600);
}

void loop() {
    long duration, distance;

    digitalWrite(led_status_pin, HIGH);
    //servo.write(1);

    digitalWrite(sensor_trig_pin, LOW);
    delayMicroseconds(2);
    digitalWrite(sensor_trig_pin, HIGH);
    delayMicroseconds(10);
    digitalWrite(sensor_trig_pin, LOW);
    duration = pulseIn(sensor_echo_pin, HIGH);
    distance = (duration/2) / 29.1;

    check_distance(distance);

    Serial.print("Sensor ");
    Serial.print(distance);
    Serial.println(" cm");

    digitalWrite(led_status_pin, LOW);
    //servo.write(90);
    delay(1000);
}

long ls_distances[3] = {};
int ls_distances_p = 0;
int ls_distances_max = 3;

void check_distance(long distance) {

    if (ls_distances_p >= ls_distances_max)
        ls_distances_p = 0;
    ls_distances[ls_distances_p++] = distance;

    long avg = 0;
    for(int x = 0; x < ls_distances_max; x++) {
        avg += ls_distances[x];
    }
    avg /= ls_distances_max;

    Serial.print("Avg ");
    Serial.print(avg);
    Serial.println(" cm");


    if (avg == 0) {
        set_status(0);
    } else if (avg >= 50) {
        set_status(2);
    } else {
        set_status(1);
    }

}

int last_status = 0;
int current_status = 0;
// status == 1 => OPEN
// status == 2 => CLOSE

void set_status(int sta)
{
    if (sta == 1) {
        digitalWrite(led_green_pin, HIGH);
        digitalWrite(led_red_pin, LOW);

        current_status = 1;

    } else if (sta == 2) {
        digitalWrite(led_green_pin, LOW);
        digitalWrite(led_red_pin, HIGH);

        current_status = 2;

    } else {
        digitalWrite(led_green_pin, LOW);
        digitalWrite(led_red_pin, LOW);

        current_status = 1;
    }


    if (last_status != current_status) {

      Serial.print("Status ");
    Serial.print(current_status);
    Serial.println("");


        if (current_status == 2) {
            servo.write(60);
            delay(3000);

        } else if (current_status == 1) {
            servo.write(160);
            delay(3000);
        }

        last_status = current_status;

    }


}