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

Servo crossingBarServo;
int servoPin = 9;
const int lamp = 2;

// Parameters for the function
const float T = 5.0;     // Time when the bar reaches near-vertical in seconds
const float A = 10.0;    // Amplitude of the oscillation
const float b = 0.8;     // Damping coefficient
const float c = 10.0;    // Frequency of oscillation

unsigned long lastToggleTime = 0; // To keep track of the last toggle time of the LED
const int ledToggleInterval = 500; // Interval at which the LED should toggle (in milliseconds)

LiquidCrystal_I2C lcd(0x27, 16, 2);

void setup() {
  crossingBarServo.attach(9);
  pinMode(lamp, OUTPUT);

  lcd.init();
  lcd.backlight();
}

void loop() {
    openCrossRoadBar();  // Call function to open the railroad crossing bar
    digitalWrite(lamp, LOW);

    delay(10000);        // Wait 10 seconds before next operation (for demonstration purposes)
}

void openCrossRoadBar() {
    unsigned long startTime = millis();
    float t, angle;
    bool ledState = LOW;

    while(true) {
        t = (millis() - startTime) / 1000.0;  // Current time in seconds since start

        if (t < T) {
            // Initial smooth rise
            angle = 90 * sin(PI / 2 * t / T);
        } else if (t >= T) {
            // Damped oscillation after reaching near-vertical
            angle = 90 - A * exp(-b * (t - T)) * abs(sin(c * (t - T)));
        }

        // Check if the movement should continue
        if (t > T + 2) {
            break;  // End the function after a sufficient time has passed
        }

        // Update the servo position
        crossingBarServo.write((int)angle);
        delay(20);  // Short delay for smoother motion

        // Optional: output current angle to the display
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Time:  ");
        lcd.print(t, 2);
        lcd.setCursor(0, 1);
        lcd.print("Angle: ");
        lcd.print(angle, 1);

        if (millis() - lastToggleTime >= ledToggleInterval) {
            lastToggleTime = millis(); // Update the last toggle time
            ledState = !ledState; // Toggle the LED state
            digitalWrite(lamp, ledState); // Apply the new LED state
        }
    }
}