#include <Arduino.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>

#define TRIG_PIN_1 35
#define ECHO_PIN_1 34
#define TRIG_PIN_2 33
#define ECHO_PIN_2 23
#define SERVO_PIN 25

Servo myServo;
LiquidCrystal_I2C lcd(0x27, 16, 2);
long distance1[5];
long distance2[5];
unsigned long gateOpenTime = 0;
int state = 0;

void setGatePosition(bool isGateOpen)
{
    int position = isGateOpen ? 0 : 90;
    myServo.write(position);
}

long readUltrasonicDistance(int trigPin, int echoPin)
{
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);

    long duration = pulseIn(echoPin, HIGH, 30000);
    if (duration == 0)
    {  
        return -1; 
    }

    return duration / 29 / 2;
}

long getMedianDistance(long *distances)
{
    long localCopy[5];
    for (int i = 0; i < 5; i++)
    {
        localCopy[i] = distances[i];
    }

    for (int i = 0; i < 4; i++)
    {
        for (int j = 0; j < 4 - i; j++)
        {
            if (localCopy[j] > localCopy[j + 1])
            {
                long temp = localCopy[j];
                localCopy[j] = localCopy[j + 1];
                localCopy[j + 1] = temp;
            }
        }
    }

    return localCopy[2];
}

void updateDistance()
{
    static unsigned long lastUpdate = 0;
    static int index = 0;

    if (millis() - lastUpdate < 1000)
    {
        return;
    }

    long val1 = readUltrasonicDistance(TRIG_PIN_1, ECHO_PIN_1);
    long val2 = readUltrasonicDistance(TRIG_PIN_2, ECHO_PIN_2);

    if (val1 == -1 || val2 == -1)
    {
        return;
    }

    distance1[index] = val1;
    distance2[index] = val2;
    index = (index + 1) % 5;

    lastUpdate = millis();
}

void setup()
{
    pinMode(TRIG_PIN_1, OUTPUT);
    pinMode(ECHO_PIN_1, INPUT);
    pinMode(TRIG_PIN_2, OUTPUT);
    pinMode(ECHO_PIN_2, INPUT);
    digitalWrite(TRIG_PIN_1, LOW);
    digitalWrite(TRIG_PIN_2, LOW);

    lcd.init();
    lcd.backlight();
    lcd.clear();
    lcd.print("Czekam na pojazd");

    myServo.attach(SERVO_PIN);
    setGatePosition(false);

    long val1 = readUltrasonicDistance(TRIG_PIN_1, ECHO_PIN_1);
    long val2 = readUltrasonicDistance(TRIG_PIN_2, ECHO_PIN_2);

    for (int i = 0; i < 5; i++)
    {
        distance1[i] = val1;
        distance2[i] = val2;
    }
}

void loop()
{
    updateDistance();

    long medianDistance1 = getMedianDistance(distance1);
    long medianDistance2 = getMedianDistance(distance2);
    bool isCarDetected = (medianDistance1 >= 10 && medianDistance1 <= 50) || (medianDistance2 >= 10 && medianDistance2 <= 50);
    bool isCarClose = false;

    for (int i = 0; i < 5; i++)
    {
        if ((distance1[i] >= 10 && distance1[i] <= 50) || (distance2[i] >= 10 && distance2[i] <= 50))
        {
            isCarClose = true;
            break;
        }
    }

    if (state != 0 && !isCarClose && !isCarDetected)
    {
        state = 0;
        lcd.clear();
        lcd.print("Czekam na pojazd");
    }
    else if (state != 1 && isCarClose && !isCarDetected)
    {
        state = 1;
        lcd.clear();
        lcd.print("Pojazd wykryty");
    }
    else if (state != 2 && gateOpenTime == 0 && isCarDetected)
    {
        state = 2;
        setGatePosition(true);
        gateOpenTime = millis();

        lcd.clear();
        lcd.print("Brama otwarta");
    }
    else if (state != 3 && gateOpenTime != 0 && (millis() - gateOpenTime >= 5000))
    {
        state = 0;
        setGatePosition(false);
        gateOpenTime = 0;

        lcd.clear();
        lcd.print("Czekam na pojazd");
    }
}