Pythonの作例

"""
EXAMPLE 01: GET TRAIN INFO
---------------------------
TOPIC: Connecting to a train

SETUP: no track needed

NOTES: This simple example shows how to connect to your train and print available information.
"""
from intelino.trainlib import TrainScanner


def main():
    with TrainScanner() as train:
        print("Name:", train.name)
        print("ID:", train.id)
        print("Is connected:", train.is_connected)
        print("Current driving direction:", train.direction)
        print("Current speed [cm/s]:", train.speed_cmps)
        print("Next planned turn:", train.next_split_decision)
        print("Distance driven since connection [cm]:", train.distance_cm)


if __name__ == "__main__":
    main()

"""
EXAMPLE 02-A: BLINK TOP LED
---------------------------
TOPIC: LED lights

SETUP: no track needed

NOTES: Blink the top LED on the intelino train 5 times using random colors.
"""
import random
import time
from intelino.trainlib import TrainScanner


def main():
    with TrainScanner() as train:
        blink_delay = 0.5  # in seconds

        for _ in range(5):
            red = random.randint(0, 1) * 255
            green = random.randint(0, 1) * 255
            blue = random.randint(0, 1) * 255

            # turn LED on
            train.set_top_led_color(red, green, blue)
            time.sleep(blink_delay)

            # turn LED off
            train.set_top_led_color(0, 0, 0)
            time.sleep(blink_delay)


if __name__ == "__main__":
    main()

"""
EXAMPLE 02-B: LIGHTS
---------------------------
TOPIC: LED lights

SETUP: no track needed

NOTES: Sequentially set train's top, front and rear LED lights to red, green and blue.
"""
import time
from intelino.trainlib import TrainScanner


def main():
    with TrainScanner() as train:
        color_change_delay = 2.0  # in seconds

        rgb_red = (255, 0, 0)
        rgb_green = (0, 255, 0)
        rgb_blue = (0, 0, 255)
        rgb_black = (0, 0, 0)

        # red
        train.set_top_led_color(*rgb_red)
        train.set_headlight_color(rgb_red, rgb_red)
        time.sleep(color_change_delay)

        # green
        train.set_top_led_color(*rgb_green)
        train.set_headlight_color(rgb_green, rgb_green)
        time.sleep(color_change_delay)

        # blue
        train.set_top_led_color(*rgb_blue)
        train.set_headlight_color(rgb_blue, rgb_blue)
        time.sleep(color_change_delay)

        # turn top LED off
        train.set_top_led_color(*rgb_black)
        # reset headlights and taillights
        train.set_headlight_color()


if __name__ == "__main__":
    main()

"""
EXAMPLE 03-A: DRIVING SPEEDS
------------------------------
TOPIC: Driving, Speed Control, Distance

SETUP: build a circle layout using 8 curve tracks and place the train anywhere on it

NOTES: Control the train's driving speed. First, using predefined speed levels,
then with manual speeds in cm/s. Use the circumference of the circle track to
change speed level after each lap, then briefly stop and gradually accelerate
from 20 to 75 cm/s over 3 laps.
"""

# Matrain adaptation: keep speed within the documented 15-75 cm/s range.
import time

from intelino.trainlib import TrainScanner
from intelino.trainlib.enums import (
    MovementDirection,
    SpeedLevel,
    StopDrivingFeedbackType,
)

# Constant: 1 circle track length is approx. 127 cm (PI * 40.5 cm)
CIRCLE_LENGTH_CM = 127.0


def main():
    with TrainScanner() as train:
        #
        # Run 3 loops to test out the 3 predefined speed levels.
        #
        train.drive_at_speed_level(
            # start slow
            speed_level=SpeedLevel.LEVEL1,
            # optional: direction defaults to forward, if not specified
            direction=MovementDirection.FORWARD,
            # optional: whether to blink the top LED, defaults to True
            play_feedback=True,
        )
        # wait until the train drives 1 circle length
        while train.distance_cm < 1 * CIRCLE_LENGTH_CM:
            time.sleep(0.001)  # 1ms sleep

        # medium speed
        train.drive_at_speed_level(SpeedLevel.LEVEL2)
        while train.distance_cm < 2 * CIRCLE_LENGTH_CM:
            time.sleep(0.001)

        # fast
        train.drive_at_speed_level(SpeedLevel.LEVEL3)
        while train.distance_cm < 3 * CIRCLE_LENGTH_CM:
            time.sleep(0.001)

        # stop and rest for a moment
        train.stop_driving(StopDrivingFeedbackType.END_ROUTE)
        time.sleep(3)

        # reset train distance measurement
        train.distance_cm = 0

        #
        # Run 3 loops on a circle while smoothly increasing train speed.
        #
        slow_cmps = 20
        fast_cmps = 75
        step_count = fast_cmps - slow_cmps
        # calculate how long (in cm) we drive at every speed
        step_length_cm = 3 * CIRCLE_LENGTH_CM / step_count

        # start slow
        current_speed_cmps = slow_cmps
        while train.distance_cm < 3 * CIRCLE_LENGTH_CM:
            start_cm = train.distance_cm
            train.drive_at_speed(current_speed_cmps)
            while (train.distance_cm - start_cm) < step_length_cm:
                time.sleep(0.001)  # 1ms sleep

            # increase the speed for the next iteration
            current_speed_cmps = min(current_speed_cmps + 1, fast_cmps)

        train.stop_driving(StopDrivingFeedbackType.END_ROUTE)


if __name__ == "__main__":
    main()

"""
EXAMPLE 03-B: RAINBOW DRIVE
------------------------------
TOPIC: Driving, LED Lights, Distance

SETUP: build any track layout

NOTES: Gradually change the train's top LED color hue based on the distance travelled.
Use the 'colorsys.hsv_to_rgb' function to convert the distance-based color hue to RGB
color space and then scale the values to byte values.
"""
import colorsys
import time
from intelino.trainlib import TrainScanner
from intelino.trainlib.enums import SpeedLevel


def main():
    with TrainScanner() as train:
        # let 1 rainbow be 100 cm
        rainbow_length = 100

        # make sure our rainbow colors are not interrupted by other animations
        train.set_snap_command_feedback(sound=True, lights=False)

        train.drive_at_speed_level(SpeedLevel.LEVEL2)

        # Matrain adaptation: stop explicitly before disconnecting on Ctrl+C.
        try:
            # infinite loop, so exit with Ctrl + C
            while True:
                rainbow_position = train.distance_cm % rainbow_length

                # HSV rainbow color
                hsv_coordinate = (rainbow_position / rainbow_length, 1, 1)
                # convert HSV to RGB
                rgb_coordinate = colorsys.hsv_to_rgb(*hsv_coordinate)
                # scale RGB coordinate values (0-1) to bytes (0-255)
                rgb_bytes = tuple(int(255 * val) for val in rgb_coordinate)

                # set LED color
                train.set_top_led_color(*rgb_bytes)

                time.sleep(0.1)
        finally:
            train.stop_driving()
            train.set_snap_command_feedback(sound=True, lights=True)
            train.set_top_led_color(0, 0, 0)


if __name__ == "__main__":
    main()

"""
EXAMPLE 04-A: ALTERNATING ROUTES
----------------------------------
TOPIC: Driving, Steering

SETUP: build a nested loop layout

NOTES: This program shows how to alternate steering directions between left
and right. It makes the train alternately go straight and turn on the nested
loop track. This example uses busy-waiting to set the next steering decision
after the previous decision value is used and cleared.
"""
import time

from intelino.trainlib import TrainScanner
from intelino.trainlib.enums import SteeringDecision


def main():
    with TrainScanner() as train:
        start_driving = True
        split_counter = 0

        # Matrain adaptation: stop explicitly before disconnecting on Ctrl+C.
        try:
            # infinite loop, so exit with Ctrl + C
            while True:
                # pick a side based on the counter
                if split_counter % 2 == 0:
                    side = SteeringDecision.RIGHT
                    color = (0, 0, 255)  # blue
                else:
                    side = SteeringDecision.LEFT
                    color = (255, 0, 0)  # red

                print(f"Next {repr(side)}, splits seen: {split_counter}")
                train.set_next_split_steering_decision(side)
                train.set_headlight_color(color, color)

                if start_driving:
                    # start driving after the next decision is set
                    train.drive_at_speed(40)
                    start_driving = False

                # wait until the `next_split_decision` is cleared
                while train.next_split_decision != SteeringDecision.NONE:
                    # check every 10ms
                    time.sleep(0.010)

                # a cleared `next_split_decision` means it was used on a split-track
                split_counter += 1
        finally:
            train.stop_driving()
            train.set_headlight_color()


if __name__ == "__main__":
    main()

"""
EXAMPLE 04-B: ALTERNATING ROUTES
----------------------------------
TOPICS: Driving, Steering, Events

SETUP: build a nested loop layout

NOTES: This example shows another approach to accomplish
alternating steering using the split decision event. Whenever the
split decision event notification is received from the train,
the next steering decision is set based on the value of the
`split_counter` variable being even or odd. The `input` function is
used for convenient program exiting.
"""
from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.enums import SteeringDecision
from intelino.trainlib.messages import TrainMsgEventSplitDecision


def main():
    with TrainScanner() as train:
        split_counter = 0

        def set_decision_and_headlights():
            # pick a side based on the counter
            if split_counter % 2 == 0:
                side = SteeringDecision.RIGHT
                color = (0, 0, 255)  # blue
            else:
                side = SteeringDecision.LEFT
                color = (255, 0, 0)  # red

            print(f"Next {repr(side)}, splits seen: {split_counter}")
            train.set_next_split_steering_decision(side)
            train.set_headlight_color(color, color)

        def handle_splits(train: Train, msg: TrainMsgEventSplitDecision):
            nonlocal split_counter
            split_counter += 1
            set_decision_and_headlights()

        set_decision_and_headlights()
        train.add_split_decision_listener(handle_splits)
        train.drive_at_speed(40)

        # block until the user wants to exit
        input("Press the Enter key to exit...\n")

        # cleanup
        train.remove_split_decision_listener(handle_splits)
        train.set_headlight_color()
        train.stop_driving()


if __name__ == "__main__":
    main()

"""
EXAMPLE 05-A: ALL EVENTS
---------------------------
TOPIC: Events

SETUP: build any track layout

NOTES: This example gives a basic overview of all events available from
the train. Detected event messages are printed on the screen.
"""
from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.messages import (
    TrainMsgEventBackColorChanged,
    TrainMsgEventButtonPressDetected,
    TrainMsgEventFrontColorChanged,
    TrainMsgEventLowBattery,
    TrainMsgEventMovementDirectionChanged,
    TrainMsgEventSnapCommandDetected,
    TrainMsgEventSnapCommandExecuted,
    TrainMsgEventSplitDecision,
)


def handle_direction_change(train: Train, msg: TrainMsgEventMovementDirectionChanged):
    print("direction changed to:", msg.direction)


def handle_low_battery(train: Train, msg: TrainMsgEventLowBattery):
    print("low battery")


def handle_button_press(train: Train, msg: TrainMsgEventButtonPressDetected):
    print("button press:", msg.button_press_type)


def handle_snap_command_detection(train: Train, msg: TrainMsgEventSnapCommandDetected):
    print("snap command detected:", msg.colors)


def handle_snap_command_execution(train: Train, msg: TrainMsgEventSnapCommandExecuted):
    print("snap command executed:", msg.colors)


def handle_front_color_change(train: Train, msg: TrainMsgEventFrontColorChanged):
    print("front sensor color:", msg.color)


def handle_back_color_change(train: Train, msg: TrainMsgEventBackColorChanged):
    print("back sensor color:", msg.color)


def handle_split_decision(train: Train, msg: TrainMsgEventSplitDecision):
    print("split decision made, the train went:", msg.decision)


def main():
    with TrainScanner() as train:
        train.add_movement_direction_change_listener(handle_direction_change)
        train.add_low_battery_listener(handle_low_battery)
        train.add_button_press_listener(handle_button_press)
        train.add_snap_command_detection_listener(handle_snap_command_detection)
        train.add_snap_command_execution_listener(handle_snap_command_execution)
        train.add_front_color_change_listener(handle_front_color_change)
        train.add_back_color_change_listener(handle_back_color_change)
        train.add_split_decision_listener(handle_split_decision)

        print("Try running the train and observe the event stream.")
        input("When done, press Enter to exit...\n")

        train.remove_movement_direction_change_listener(handle_direction_change)
        train.remove_low_battery_listener(handle_low_battery)
        train.remove_button_press_listener(handle_button_press)
        train.remove_snap_command_detection_listener(handle_snap_command_detection)
        train.remove_snap_command_execution_listener(handle_snap_command_execution)
        train.remove_front_color_change_listener(handle_front_color_change)
        train.remove_back_color_change_listener(handle_back_color_change)
        train.remove_split_decision_listener(handle_split_decision)


if __name__ == "__main__":
    main()

"""
EXAMPLE 05-B: SNAP COMMANDS
-----------------------------
TOPIC: Color Snap Commands, Events

SETUP: build any loop track and add WHITE-RED, WHITE-GREEN and WHITE-BLUE snap commands

NOTES: This example shows how to disable the default snap command actions and 
define custom behaviors. Snap command detection event notifications are used to trigger
the custom actions. In this case, we simply update the top LED color based 
on the second color of each detected snap command.
"""

from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.enums import (
    MovementDirection,
    SnapColorValue as C,
    SpeedLevel,
)
from intelino.trainlib.messages import TrainMsgEventSnapCommandDetected


def handle_snap_commands(train: Train, msg: TrainMsgEventSnapCommandDetected):
    # Note: the `SnapColorValue` enum is imported as `C` for a shorter notation

    if msg.colors == (C.WHITE, C.RED, C.BLACK, C.BLACK):
        # exact match (the missing snaps are detected as black to length 4)
        train.set_top_led_color(r=255, g=0, b=0)

    elif msg.colors == (C.WHITE, C.GREEN):
        # exact match with implicit blacks
        train.set_top_led_color(r=0, g=255, b=0)

    elif msg.colors.start_with(C.WHITE, C.BLUE):
        # partial / prefix match
        train.set_top_led_color(r=0, g=0, b=255)


def main():
    with TrainScanner() as train:
        # disable all snap commands
        # - built in snaps including split track snaps (so no random steering)
        # - custom snaps (starting with white-magenta)
        train.set_snap_command_execution(False)
        train.add_snap_command_detection_listener(handle_snap_commands)

        train.drive_at_speed_level(
            SpeedLevel.LEVEL2, MovementDirection.FORWARD, play_feedback=False
        )

        input("When done, press Enter to exit...\n")

        # cleanup
        train.stop_driving()
        train.remove_snap_command_detection_listener(handle_snap_commands)
        train.set_snap_command_execution(True)


if __name__ == "__main__":
    main()

"""
EXAMPLE 05-C: COLOR DETECTION
-------------------------------
TOPIC: Color Detection, Events

SETUP: build any loop track and add GREEN, YELLOW and MAGENTA individual snaps
around the track

NOTES: In this example, the top LED color is set based on GREEN, YELLOW and MAGENTA
snap detection. We use the color change event notifications to update the LED color.
"""
from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.enums import (
    MovementDirection,
    SnapColorValue as C,
    SpeedLevel,
)
from intelino.trainlib.messages import TrainMsgEventSensorColorChanged


def handle_color_change(train: Train, msg: TrainMsgEventSensorColorChanged):
    # Note: the `SnapColorValue` enum is imported as `C` for a shorter notation

    # Note: we picked green, yellow, and magenta, because these colors are not
    # used on split tracks. Split tracks have embedded cyan, red, and blue
    # colors.

    if msg.color == C.GREEN:
        train.set_top_led_color(r=0, g=255, b=0)

    elif msg.color == C.YELLOW:
        train.set_top_led_color(r=255, g=255, b=0)

    elif msg.color == C.MAGENTA:
        train.set_top_led_color(r=255, g=0, b=255)


def main():
    with TrainScanner() as train:
        # snap command execution can stay on, we only disable LED feedback
        train.set_snap_command_feedback(sound=True, lights=False)

        train.add_front_color_change_listener(handle_color_change)

        train.drive_at_speed_level(
            SpeedLevel.LEVEL2, MovementDirection.FORWARD, play_feedback=False
        )

        input("When done, press Enter to exit...\n")

        # cleanup
        train.remove_front_color_change_listener(handle_color_change)
        train.set_snap_command_feedback(sound=True, lights=True)
        train.stop_driving()


if __name__ == "__main__":
    main()

"""
EXAMPLE 05-D: COLOR DETECTION IN BOTH DIRECTIONS
---------------------------------------------------
TOPIC: Color Detection, Movement Direction, Events

SETUP: build any loop track and add a WHITE-BLUE snap command as well as individual 
snaps in colors GREEN, YELLOW and MAGENTA around the track

NOTES: Similarly to example 05-C, the top LED color is set based on GREEN, YELLOW and 
MAGENTA snap detection. But now the train executes this task in both driving directions.
The leading color sensor is set to front or back based on train's movement direction.  
"""
from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.enums import (
    ColorSensor,
    MovementDirection,
    SnapColorValue as C,
    SpeedLevel,
)
from intelino.trainlib.messages import TrainMsgEventSensorColorChanged


def is_relative_front_sensor(direction: MovementDirection, sensor: ColorSensor) -> bool:
    """Check if the given sensor is a front sensor relative to the movement direction.

    Use back sensor only if really going backwards, use front sensor in all
    other cases (e.g. going forward or stopped - moved by hand)
    """
    accept_sensor = ColorSensor.FRONT
    if direction == MovementDirection.BACKWARD:
        accept_sensor = ColorSensor.BACK

    return sensor == accept_sensor


def handle_color_change(train: Train, msg: TrainMsgEventSensorColorChanged):
    # Note, that we use the `TrainMsgEventSensorColorChanged` union type, so we
    # can assign this callback to both color sensor events.

    if not is_relative_front_sensor(train.direction, msg.sensor):
        # don't handle this sensor event (return early)
        return

    if msg.color == C.GREEN:
        train.set_top_led_color(r=0, g=255, b=0)

    elif msg.color == C.YELLOW:
        train.set_top_led_color(r=255, g=255, b=0)

    elif msg.color == C.MAGENTA:
        train.set_top_led_color(r=255, g=0, b=255)


def main():
    with TrainScanner() as train:
        # snap command execution can stay on, we only disable LED feedback
        train.set_snap_command_feedback(sound=True, lights=False)

        # listen to events on both color sensors
        train.add_front_color_change_listener(handle_color_change)
        train.add_back_color_change_listener(handle_color_change)

        train.drive_at_speed_level(
            SpeedLevel.LEVEL2, MovementDirection.FORWARD, play_feedback=False
        )

        input("When done, press Enter to exit...\n")

        # cleanup
        train.remove_front_color_change_listener(handle_color_change)
        train.remove_back_color_change_listener(handle_color_change)
        train.set_snap_command_feedback(sound=True, lights=True)
        train.stop_driving()


if __name__ == "__main__":
    main()

"""
EXAMPLE 05-E: SNAP COMMAND EXECUTION TOGGLING
------------------------------------------------
TOPIC: Snap Command Detection, Color Detection, Lights, Events

SETUP: build any loop track and add two separate WHITE-GREEN snap commands and a YELLOW
individual snap around the track

NOTES: In this example, the train toggles between the default and custom behaviors
when it detects the WHITE-GREEN snap command. When the default behavior execution is
disabled, the train stops for 2 seconds while all LEDs turn RED. When the default behavior
is enabled, the WHITE-GREEN snap command is executed with the normal behavior AND with
all LEDs set to GREEN. The YELLOW snap command detection is used for updating the LEDs
to YELLOW and disabling the snap command execution.
"""
import time
from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.enums import StopDrivingFeedbackType
from intelino.trainlib.enums import SnapColorValue as C
from intelino.trainlib.messages import (
    TrainMsgEventFrontColorChanged,
    TrainMsgEventSnapCommandDetected,
)


def main():
    with TrainScanner() as train:

        # disable default snap commands and feedback effects
        train.set_snap_command_execution(False)
        train.set_snap_command_feedback(sound=True, lights=False)
        color_commands = False

        # define snap command actions
        def handle_snap_commands(train: Train, msg: TrainMsgEventSnapCommandDetected):
            nonlocal color_commands

            # define actions for WHITE GREEN snap sequence
            if msg.colors == (C.WHITE, C.GREEN, C.BLACK, C.BLACK):

                # custom behavoir when default commands are disabled
                if color_commands is False:
                    train.set_top_led_color(255, 0, 0)
                    train.set_headlight_color(front=(255, 0, 0), back=(255, 0, 0))
                    train.stop_driving(play_feedback_type=StopDrivingFeedbackType.NONE)
                    time.sleep(2.0)
                    train.set_snap_command_execution(True)
                    train.set_top_led_color(0, 255, 255)
                    train.set_headlight_color()
                    train.drive_at_speed(40)
                    color_commands = True

                # only turn on LEDs to green when default commands are enabled
                else:
                    train.set_top_led_color(0, 255, 0)
                    train.set_headlight_color(front=(0, 255, 0), back=(0, 255, 0))

        # define actions when color yellow is detected
        def handle_colors(train: Train, msg: TrainMsgEventFrontColorChanged):
            nonlocal color_commands
            if msg.color == C.YELLOW:
                train.set_top_led_color(255, 255, 0)
                train.set_headlight_color(front=(255, 255, 0), back=(255, 255, 0))
                train.set_snap_command_execution(False)
                train.drive_at_speed(40)
                color_commands = False

        # setup event listeners
        train.add_snap_command_detection_listener(handle_snap_commands)
        train.add_front_color_change_listener(handle_colors)

        # set top LED to cyan and start driving forward at 40 cm/s
        train.set_top_led_color(0, 255, 255)
        print("Start driving forward")
        train.drive_at_speed(40)

        input("When done, press Enter to exit...\n")

        # cleanup
        train.stop_driving()
        train.set_top_led_color(0, 0, 0)
        train.set_headlight_color()
        train.remove_snap_command_detection_listener(handle_snap_commands)
        train.remove_front_color_change_listener(handle_colors)


if __name__ == "__main__":
    main()

"""
EXAMPLE 06: MULTIPLE TRAINS
-----------------------------
TOPIC: Connecting to Multiple Trains, Lights

SETUP: no track needed

NOTES: Easily connect to multiple trains and set their LEDs to the same randomly-picked color.
You can set the `train_count` variable to the number of trains you have availble.
"""
import random
import time
from intelino.trainlib import TrainScanner


def random_rgb_color():
    red = random.randint(0, 1) * 255
    green = random.randint(0, 1) * 255
    blue = random.randint(0, 1) * 255

    return (red, green, blue)


def main():
    train_count = 2
    blink_delay = 0.5  # in seconds

    print("scanning and connecting...")

    trains = TrainScanner(timeout=3.0).get_trains(train_count)

    print("connected train count:", len(trains))

    # set the same random color on all trains
    for _ in range(10):
        color = random_rgb_color()
        for train in trains:
            train.set_top_led_color(*color)
            train.set_headlight_color(front=color, back=color)
        time.sleep(blink_delay)

    print("disconnecting...")

    # cleanup
    for train in trains:
        train.set_top_led_color(0, 0, 0)
        train.set_headlight_color()
        train.disconnect()


if __name__ == "__main__":
    main()

"""
EXAMPLE 07: SIMPLE COLLISION AVOIDANCE
-----------------------------------------
TOPIC: Multiple Trains, Driving, Steering, Snap Command and Color Events

SETUP: build a nested loop track with a shared track section

NOTES: This example demonstrates a simple solution for a 2-train merge or head-on collision
avoidance on a shared track section (referred to as 'pass' in this program). For head-on 
collsion avoidance, one of the trains needs to move clockwise and the other counter-clockwise.
Also, you can use default steering snap commands to make the trains run separate routes and only
share the 'pass' section.

Based on the movement direction of each train, place WHITE-MAGENTA snap
commands prior to the entrance to the 'pass' on both sides of the shared section. 
Similarly, place YELLOW snaps after the 'pass' section. 

The WHITE-MAGENTA snap command's handling function acts as a "traffic light", which
either lets a train pass or stops it until the section is 'freed' by the other train 
when it passes over the YELLOW snap.
"""
import threading
from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.enums import SnapColorValue as C
from intelino.trainlib.messages import (
    TrainMsgEventFrontColorChanged,
    TrainMsgEventSnapCommandDetected,
)


def main():
    trains = TrainScanner().get_trains(count=2)
    # shared variable that represents the state of the pass
    pass_used_by = None
    # Matrain adaptation: two trains, synchronized reservation, cancellable wait.
    pass_condition = threading.Condition()
    shutting_down = False

    def enter_pass(train: Train):
        nonlocal pass_used_by
        with pass_condition:
            if shutting_down:
                return
            if pass_used_by is not None:
                train.stop_driving()
                print(train.alias, "waits for the pass to be free")
                pass_condition.wait_for(lambda: pass_used_by is None or shutting_down)
                if shutting_down:
                    return
                pass_used_by = train.id
                train.drive_at_speed(40)
            else:
                pass_used_by = train.id
            print(train.alias, "entered the pass")

    def leave_pass(train: Train):
        nonlocal pass_used_by
        with pass_condition:
            if pass_used_by == train.id:
                pass_used_by = None
                pass_condition.notify_all()
                print(train.alias, "left the pass")

    def handle_snap_commands(train: Train, msg: TrainMsgEventSnapCommandDetected):
        if msg.colors == (C.WHITE, C.MAGENTA, C.BLACK, C.BLACK):
            enter_pass(train)

    def handle_colors(train: Train, msg: TrainMsgEventFrontColorChanged):
        if msg.color == C.YELLOW:
            leave_pass(train)

    # identify our trains
    trains[0].alias = "Red Train"
    trains[0].set_headlight_color(front=(255, 0, 0), back=(255, 0, 0))
    trains[1].alias = "Blue Train"
    trains[1].set_headlight_color(front=(0, 0, 255), back=(0, 0, 255))

    # setup event listeners
    for train in trains:
        train.clear_custom_snap_commands()
        train.add_snap_command_detection_listener(handle_snap_commands)
        train.add_front_color_change_listener(handle_colors)

    try:
        # start driving
        for train in trains:
            train.drive_at_speed(40)
        input("When done, press Enter to exit...\n")
    finally:
        with pass_condition:
            shutting_down = True
            pass_condition.notify_all()
        for train in trains:
            try:
                train.stop_driving()
                train.set_headlight_color()
                train.remove_snap_command_detection_listener(handle_snap_commands)
                train.remove_front_color_change_listener(handle_colors)
            finally:
                train.disconnect()



if __name__ == "__main__":
    main()

"""
EXAMPLE 08: WAGON EXCHANGE
---------------------------
TOPIC: Multiple Trains, Driving, Steering, Wagon Decoupling, Snap Command Events

SETUP: build a track with nested loops on both sides (using 4x splits, 2x straights, 12x curves).
Add snap commands as follows:
    - WHITE-MAGENTA-WHITE: double sided snap command for wagon drop off and pickup
      on both inner loop sides
    - WHITE-MAGENTA-RED: waiting state on the outer loops on both sides
    - WHITE-MAGENTA-BLUE: reversing maneuver for wagon pickup
      between the split tracks (on a straight segment)
The trains should start on the outer loops opposite from each other

NOTES: This example enables wagon exchange between two trains. The WHITE-MAGENTA-WHITE
snap commands are used to 'mark' the wagon drop off areas on the track. Each train
delivers the wagon and moves to the waiting area of the track marked with WHITE-MAGENTA-RED.
The moment the drop-off occurs, the other train receives a notification to proceed to the
wagon pick-up.
"""
import collections
from enum import Enum
import random
from typing import Callable, NamedTuple
from intelino.trainlib import TrainScanner, Train
from intelino.trainlib.enums import (
    SnapColorValue as Color,
    MovementDirection,
    SteeringDecision,
)
from intelino.trainlib.messages import TrainMsgEventSnapCommandDetected

#
# Data types
#

WagonState = Enum("WagonState", "UNKNOWN CARRIED_BY READY_FOR")


class WagonInfo(NamedTuple):
    state: WagonState
    train_alias: str


class TrainState:
    def __init__(self):
        # waiting for the wagon to be ready for us
        self.waiting = True
        # when carrying the wagon, we need to pass a checkpoint before drop off
        # is allowed
        self.checkpoint_passed = False


class SharedState:
    def __init__(self):
        self.wagon = WagonInfo(WagonState.UNKNOWN, "")
        # train state indexed by the train's alias
        self.trains: dict[str, TrainState] = collections.defaultdict(TrainState)


#
# Functions
#


def train_setup_function(
    train: Train, state: SharedState, wagon_ready_notifier: Callable
):
    # snap command handler callback
    def milestone(train: Train, msg: TrainMsgEventSnapCommandDetected):
        # for easier readability, saving our alias as "me"
        me = train.alias

        if msg.colors == (Color.WHITE, Color.MAGENTA, Color.BLACK, Color.BLACK):
            # inner loop - wagon exchange place
            if (state.wagon == (WagonState.CARRIED_BY, me)) and state.trains[
                me
            ].checkpoint_passed:
                # drop off (and make sure to update the state asap)
                state.wagon = WagonInfo(WagonState.READY_FOR, "")
                train.decouple_wagon()
                print(me, "wagon dropped off")
                # navigate home
                train.set_next_split_steering_decision(SteeringDecision.STRAIGHT)
                # pick a next train to have the wagon
                train_aliases = list(state.trains.keys())
                train_aliases.remove(me)
                next_train = random.choice(train_aliases)
                state.wagon = WagonInfo(WagonState.READY_FOR, next_train)
                # inform the other train of the wagon being ready
                wagon_ready_notifier()

            elif state.wagon == (WagonState.READY_FOR, me):
                # pickup
                # "hope" that we attached the wagon
                state.trains[me].checkpoint_passed = False
                train.drive_at_speed(40, MovementDirection.FORWARD)
                train.set_next_split_steering_decision(SteeringDecision.LEFT)
                state.wagon = WagonInfo(WagonState.CARRIED_BY, me)
                print(me, "wagon picked up")

        elif msg.colors.start_with(Color.WHITE, Color.MAGENTA, Color.RED):
            # outer loop - resting place
            if state.wagon.train_alias != me:
                # wagon is not ours (nor carried by nor ready for us)
                state.trains[me].waiting = True
                train.stop_driving()
                print(me, "waiting")

        elif msg.colors.start_with(Color.WHITE, Color.MAGENTA, Color.BLUE):
            # middle section - checkpoint
            if state.wagon == (WagonState.CARRIED_BY, me):
                state.trains[me].checkpoint_passed = True
                train.set_next_split_steering_decision(SteeringDecision.LEFT)
                print(me, "reached checkpoint with a wagon")

            elif state.wagon == (WagonState.READY_FOR, me):
                # reverse for pickup
                train.drive_at_speed(30, MovementDirection.BACKWARD)
                train.set_next_split_steering_decision(SteeringDecision.RIGHT)

    # clear all custom snap commands stored on the train (just to be sure)
    train.clear_custom_snap_commands()
    # disable built-in snap commands (just to be sure)
    train.set_snap_command_execution(False)
    # register the "milestone" listener
    train.add_snap_command_detection_listener(milestone)


def main():
    print("Connecting...")
    trains = TrainScanner().get_trains(count=2)

    # identify our trains
    trains[0].alias = "Red Train"
    trains[0].set_headlight_color(front=(255, 0, 0), back=(255, 0, 0))
    trains[1].alias = "Blue Train"
    trains[1].set_headlight_color(front=(0, 0, 255), back=(0, 0, 255))

    # the "first" train to have the wagon should glow green
    trains[0].set_top_led_color(0, 255, 0)
    input("Attach the wagon to the train with a green top LED and press Enter...\n")

    # initialize the state
    shared_state = SharedState()
    shared_state.wagon = WagonInfo(WagonState.CARRIED_BY, trains[0].alias)
    for train in trains:
        # all trains are waiting
        shared_state.trains[train.alias].waiting = True
    # except the first one
    shared_state.trains[trains[0].alias].waiting = False

    def handle_wagon_ready():
        if shared_state.wagon.state == WagonState.READY_FOR:
            alias = shared_state.wagon.train_alias
            if shared_state.trains[alias].waiting:
                # search for the train and "wake up"
                for train in trains:
                    if train.alias == alias:
                        train.drive_at_speed(35)
                        print(alias, "going for the wagon")

    # initialize event listeners
    for train in trains:
        train_setup_function(train, shared_state, handle_wagon_ready)

    # start driving
    for train in trains:
        if not shared_state.trains[train.alias].waiting:
            train.drive_at_speed(35)

    input("When done, press Enter to exit...\n")

    # cleanup
    for train in trains:
        train.stop_driving()
        train.set_snap_command_execution(True)
        train.set_headlight_color()
        train.disconnect()


if __name__ == "__main__":
    main()