# ============================================================
# RAILWAY LEVEL CROSSING SAFETY SYSTEM
# Raspberry Pi Pico + MicroPython
#
# System:
#   Ultrasonic 1 -> Detect approaching train
#   Servo        -> Close/Open railway gate
#   Red LED      -> Train / Gate closed
#   Green LED    -> Safe / Gate open
#   Ultrasonic 2 -> Detect person/car in danger zone
#   Yellow LED   -> Emergency warning
#   Buzzer       -> Warning / Emergency alarm
#
# ============================================================


from machine import Pin, PWM
import time


# ============================================================
# PIN MAPPING
# ============================================================

# Buzzer
BUZZER_PIN = 13

# Servo Gate
SERVO_PIN = 18

# LEDs
GREEN_LED_PIN = 2
RED_LED_PIN = 12
YELLOW_LED_PIN = 15

# Ultrasonic Sensor 1 - Train Detection
ULTRA1_TRIG_PIN = 14
ULTRA1_ECHO_PIN = 27

# Ultrasonic Sensor 2 - Safety Zone Detection
ULTRA2_TRIG_PIN = 16
ULTRA2_ECHO_PIN = 17


# ============================================================
# COMPONENT SETUP
# ============================================================

# LEDs
green_led = Pin(GREEN_LED_PIN, Pin.OUT)
red_led = Pin(RED_LED_PIN, Pin.OUT)
yellow_led = Pin(YELLOW_LED_PIN, Pin.OUT)


# Ultrasonic 1
trig1 = Pin(ULTRA1_TRIG_PIN, Pin.OUT)
echo1 = Pin(ULTRA1_ECHO_PIN, Pin.IN)


# Ultrasonic 2
trig2 = Pin(ULTRA2_TRIG_PIN, Pin.OUT)
echo2 = Pin(ULTRA2_ECHO_PIN, Pin.IN)


# Servo
servo = PWM(Pin(SERVO_PIN))
servo.freq(50)


# Buzzer
buzzer = PWM(Pin(BUZZER_PIN))
buzzer.freq(2000)
buzzer.duty_u16(0)


# ============================================================
# SYSTEM SETTINGS
# ============================================================

# Distance at which train is detected
TRAIN_DETECT_DISTANCE = 200       # cm

# Distance at which train is considered gone
TRAIN_CLEAR_DISTANCE = 70       # cm

# Safety zone distance
# If person/car is closer than this distance
# while gate is closed -> Emergency
SAFETY_DISTANCE = 30              # cm


# ============================================================
# SERVO SETTINGS
# ============================================================

GATE_OPEN_ANGLE = 0
GATE_CLOSED_ANGLE = 90


# ============================================================
# SYSTEM STATES
# ============================================================

NORMAL = 0
TRAIN_WARNING = 1
EMERGENCY = 2

state = NORMAL


# ============================================================
# SERVO FUNCTION
# ============================================================

def set_servo_angle(angle):

    # Servo PWM range
    min_duty = 1638
    max_duty = 8192

    duty = int(
        min_duty +
        (angle / 180) *
        (max_duty - min_duty)
    )

    servo.duty_u16(duty)


# ============================================================
# ULTRASONIC FUNCTION
# ============================================================

def measure_distance(trig, echo):

    # Make sure trigger is LOW
    trig.value(0)
    time.sleep_us(2)

    # Send trigger pulse
    trig.value(1)
    time.sleep_us(10)
    trig.value(0)

    # Timeout
    timeout = 30000

    # Wait for ECHO HIGH
    start_wait = time.ticks_us()

    while echo.value() == 0:

        if time.ticks_diff(
            time.ticks_us(),
            start_wait
        ) > timeout:

            return 999


    # Echo started
    pulse_start = time.ticks_us()


    # Wait for ECHO LOW
    while echo.value() == 1:

        if time.ticks_diff(
            time.ticks_us(),
            pulse_start
        ) > timeout:

            return 999


    # Echo ended
    pulse_end = time.ticks_us()


    # Calculate pulse duration
    pulse_time = time.ticks_diff(
        pulse_end,
        pulse_start
    )


    # Convert to centimeters
    distance = pulse_time / 58.0

    return distance


# ============================================================
# ULTRASONIC 1
# ============================================================

def get_train_distance():

    return measure_distance(
        trig1,
        echo1
    )


# ============================================================
# ULTRASONIC 2
# ============================================================

def get_safety_distance():

    return measure_distance(
        trig2,
        echo2
    )


# ============================================================
# BUZZER FUNCTIONS
# ============================================================

def buzzer_off():

    buzzer.duty_u16(0)


def buzzer_on():

    buzzer.freq(2000)
    buzzer.duty_u16(30000)


def train_beep():

    # Normal warning beep
    buzzer_on()

    time.sleep(0.25)

    buzzer_off()

    time.sleep(0.25)


def emergency_beep():

    # Fast emergency alarm
    buzzer.freq(3000)

    buzzer.duty_u16(35000)

    time.sleep(0.12)

    buzzer.duty_u16(0)

    time.sleep(0.08)


# ============================================================
# NORMAL MODE
# ============================================================

def normal_mode():

    print("SYSTEM STATUS: NORMAL")

    # Gate OPEN
    set_servo_angle(GATE_OPEN_ANGLE)

    # Green ON
    green_led.value(1)

    # Red OFF
    red_led.value(0)

    # Yellow OFF
    yellow_led.value(0)

    # Buzzer OFF
    buzzer_off()


# ============================================================
# TRAIN WARNING MODE
# ============================================================

def train_warning_mode():

    print("SYSTEM STATUS: TRAIN WARNING")

    # Gate CLOSED
    set_servo_angle(GATE_CLOSED_ANGLE)

    # Green OFF
    green_led.value(0)

    # Red ON
    red_led.value(1)

    # Yellow OFF initially
    yellow_led.value(0)


# ============================================================
# EMERGENCY MODE
# ============================================================

def emergency_mode():

    print("!!! EMERGENCY !!!")

    # Gate MUST remain CLOSED
    set_servo_angle(GATE_CLOSED_ANGLE)

    # Green OFF
    green_led.value(0)

    # Red ON
    red_led.value(1)

    # Yellow warning LED
    yellow_led.value(1)


# ============================================================
# STARTUP
# ============================================================
print("================================================")
print("     RAILWAY LEVEL CROSSING SAFETY SYSTEM")
print("================================================")
# ============================================================
# INITIAL STATE
# ============================================================

normal_mode()


# ============================================================
# MAIN LOOP
# ============================================================

while True:

    # ========================================================
    # STATE 1 - NORMAL
    # ========================================================

    if state == NORMAL:

        # Read train sensor
        train_distance = get_train_distance()

        print(
            "[NORMAL] Train Distance:",
            round(train_distance, 1),
            "cm"
        )


        # Check if train is approaching
        if train_distance <= TRAIN_DETECT_DISTANCE:

            print("================================")
            print("       TRAIN DETECTED!")
            print("================================")

            state = TRAIN_WARNING

            train_warning_mode()

            # Small delay
            time.sleep(0.5)


        else:

            # Keep normal state
            green_led.value(1)
            red_led.value(0)
            yellow_led.value(0)

            buzzer_off()


    # ========================================================
    # STATE 2 - TRAIN WARNING
    # ========================================================

    elif state == TRAIN_WARNING:

        # Gate remains CLOSED
        set_servo_angle(GATE_CLOSED_ANGLE)

        # LEDs
        green_led.value(0)
        red_led.value(1)


        # ----------------------------------------------------
        # Check safety zone first
        # ----------------------------------------------------

        safety_distance = get_safety_distance()

        print(
            "[TRAIN] Safety Zone:",
            round(safety_distance, 1),
            "cm"
        )


        # Someone entered crossing
        if safety_distance <= SAFETY_DISTANCE:

            print()
            print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
            print("       DANGER DETECTED!")
            print("  PERSON / CAR IN CROSSING")
            print("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!")
            print()

            state = EMERGENCY

            emergency_mode()

            time.sleep(0.2)


        else:

            # Normal train warning
            train_beep()


        # ----------------------------------------------------
        # Check if train has passed
        # ----------------------------------------------------

        train_distance = get_train_distance()

        print(
            "[TRAIN] Train Distance:",
            round(train_distance, 1),
            "cm"
        )


        # Only return to normal when:
        # 1. Train is far away
        # 2. Safety zone is clear

        if (
            train_distance >= TRAIN_CLEAR_DISTANCE
            and
            safety_distance > SAFETY_DISTANCE
        ):

            print("================================")
            print("        TRAIN PASSED")
            print("       GATE OPENING")
            print("================================")

            state = NORMAL

            normal_mode()

            time.sleep(0.5)


    # ========================================================
    # STATE 3 - EMERGENCY
    # ========================================================

    elif state == EMERGENCY:

        # Gate ALWAYS CLOSED
        set_servo_angle(GATE_CLOSED_ANGLE)

        # Green OFF
        green_led.value(0)

        # Red ON
        red_led.value(1)


        # ----------------------------------------------------
        # Yellow LED blinking
        # ----------------------------------------------------

        yellow_led.value(1)
        time.sleep(0.5)
        yellow_led.value(0)
        time.sleep(0.5)


        # ----------------------------------------------------
        # Emergency Buzzer
        # ----------------------------------------------------
        emergency_beep()

        # ----------------------------------------------------
        # Check safety zone
        # ----------------------------------------------------

        safety_distance = get_safety_distance()

        print(
            "[EMERGENCY] Safety Distance:",
            round(safety_distance, 1),
            "cm"
        )


        # ----------------------------------------------------
        # If person/car leaves safety zone
        # ----------------------------------------------------

        if safety_distance > SAFETY_DISTANCE:

            print(">>> SAFETY ZONE CLEAR <<<")

            # Check train
            train_distance = get_train_distance()

            print(
                "Train Distance:",
                round(train_distance, 1),
                "cm"
            )


            # If train is still close,
            # go back to TRAIN WARNING
            if train_distance < TRAIN_CLEAR_DISTANCE:

                print("Train still present.")
                print("Returning to TRAIN WARNING.")

                state = TRAIN_WARNING

                train_warning_mode()

            else:

                print()
                print("No train detected.")
                print("Returning to NORMAL.")
                print()

                state = NORMAL

                normal_mode()


        # Small delay
        time.sleep(0.1)