Labyrinth

import time
import logging
import json
import random
import threading

from enum import Enum
from agt import AlexaGadget

from ev3dev2.led import Leds
from ev3dev2.sound import Sound
from ev3dev2.motor import OUTPUT_B, OUTPUT_C, MoveTank, SpeedPercent
from ev3dev2.sensor.lego import UltrasonicSensor, ColorSensor

# Set the logging level to INFO to see messages from AlexaGadget
logging.basicConfig(level=logging.INFO)


class Direction(Enum):
    """
    The list of directional commands and their variations.
    These variations correspond to the skill slot values.
    """
    FORWARD = ['forward']
    LEFT = ['left']
    RIGHT = ['right']
    REPEAT = ['repeat']

class MindstormsGadget(AlexaGadget):
    """
    A Mindstorms gadget that moves in a labyrinth.
    """

    def __init__(self):
        """
        Performs Alexa Gadget initialization routines and ev3dev resource allocation.
        """
        super().__init__()

        # Gadget state
        self.patrol_mode = False

        # Ev3dev initialization
        self.leds = Leds()
        self.sound = Sound()
        self.drive = MoveTank(OUTPUT_B, OUTPUT_C)
        self.us = UltrasonicSensor()
        self.us.mode='US-DIST-CM'
        self.cl = ColorSensor()
        self.cl.mode='COL-COLOR'
        self.moves=[]
        self.moves2=[]

    def on_connected(self, device_addr):
        """
        Gadget connected to the paired Echo device.
        :param device_addr: the address of the device we connected to
        """
        self.leds.set_color("LEFT", "GREEN")
        self.leds.set_color("RIGHT", "GREEN")
        print("{} connected to Echo device".format(self.friendly_name))

    def on_disconnected(self, device_addr):
        """
        Gadget disconnected from the paired Echo device.
        :param device_addr: the address of the device we disconnected from
        """
        self.leds.set_color("LEFT", "BLACK")
        self.leds.set_color("RIGHT", "BLACK")
        print("{} disconnected from Echo device".format(self.friendly_name))

    def on_custom_mindstorms_gadget_control(self, directive):
        """
        Handles the Custom.Mindstorms.Gadget control directive.
        :param directive: the custom directive with the matching namespace and name
        """
        try:
            payload = json.loads(directive.payload.decode("utf-8"))
            print("Control payload: {}".format(payload))
            control_type = payload["type"]
            if control_type == "move":

                # Expected params: [direction]
                self._move(payload["direction"])

        except KeyError:
            print("Missing expected parameters: {}".format(directive))

    def _move(self, direction, is_blocking=False):
        """
        Handles move commands from the directive.
        :param direction: the move direction
        :param is_blocking: if set, motor run until duration expired before accepting another command
        """
        if direction in Direction.FORWARD.value:
            if self.us.value()>300:
                self.drive.on_for_seconds(SpeedPercent(30), SpeedPercent(30), 2.87, block=is_blocking)
                self.moves.append('forward')

        if direction in (Direction.RIGHT.value + Direction.LEFT.value):
            self._turn(direction)
        
        if direction in Direction.REPEAT.value:
            #Repeating the previous movments.
            print(self.moves)
            self.moves2=self.moves
            for i in range(0, self.moves2.__len__()):
                self._move(self.moves2[i])
                time.sleep(3)
            self.moves=[]

    def _turn(self, direction):
        """
        Turns based on the specified direction.
        Calibrated for hard smooth surface.
        :param direction: the turn direction
        """
        if direction in Direction.LEFT.value:
            self.drive.on_for_seconds(SpeedPercent(-20), SpeedPercent(20), 0.83)
            self.moves.append('left')

        if direction in Direction.RIGHT.value:
            self.drive.on_for_seconds(SpeedPercent(20), SpeedPercent(-20), 0.83)
            self.moves.append('right')

if __name__ == '__main__':

    # Startup sequence
    gadget = MindstormsGadget()
    gadget.sound.play_song((('C4', 'e'), ('D4', 'e'), ('E5', 'q')))
    gadget.leds.set_color("LEFT", "GREEN")
    gadget.leds.set_color("RIGHT", "GREEN")

    # Gadget main entry point
    gadget.main()

    # Shutdown sequence
    gadget.sound.play_song((('E5', 'e'), ('C4', 'e')))
    gadget.leds.set_color("LEFT", "BLACK")
    gadget.leds.set_color("RIGHT", "BLACK")