# LEGO type:standard slot:6 autostart

from spike import PrimeHub, Motor, MotorPair, ColorSensor, MotionSensor
from spike.control import wait_for_seconds

HELLO = "HELLO IQ"

'''
Wir nutzen "Duck typing", dh wir schreiben hinter jede Variabel mit ':' die Klasse, zB `leftMotor: Motor`
damit man dann später auch wieder Code Completion hat bei Nutzung der Variablen
'''
class IQRobot:

    def __init__(self, hub: PrimeHub, leftMotorPort: str, rightMotorPort: str, colorSensorPort: str):
        self.hub: PrimeHub = hub
        self.leftMotor: Motor = Motor(leftMotorPort)
        self.rightMotor: Motor = Motor(rightMotorPort)
        self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) 
        #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
        self.frontMotorLeft: Motor = Motor("C")
        self.frontMotorRight: Motor = Motor("D")
        self.motionSensor: MotionSensor = MotionSensor()


    def show(self, image: str):
        '''
        Zeige Bild auf LED Matrix des Spikes
        image: Bildname wie zB 'HAPPY'
        '''
        self.hub.light_matrix.show_image(image)


    def driveForward_for_sec(self, seconds: float):
        # Fahre die übergebene Anzahl seconds gerade aus
        self.movementMotors.start()
        wait_for_seconds(seconds)
        self.movementMotors.stop()

    def getColorIntensity(self):
        # Ermittele Farbintensität über den Farbsensor
        (red, green, blue, colorIntensity) = self.colorSensor.get_rgb_intensity()
        return colorIntensity

    def drehe(self, grad=90):
        self.motionSensor.reset_yaw_angle()
        steering = 100 if grad > 0 else -100
        self.movementMotors.start(steering=steering, speed=10)
        while abs(self.motionSensor.get_yaw_angle()) < abs(grad):
            pass
        self.movementMotors.stop()


    def schreibeL(self):
        print("Schreibe L")
        self.frontMotorRight.run_for_rotations(-0.1)
        self.movementMotors.set_default_speed(20)
        self.movementMotors.move(5)
        #self.movementMotors.move(7, steering=100)
        self.drehe(-90)
        self.movementMotors.move(2)
        self.frontMotorRight.run_for_rotations(0.1)

        # Fahre 5 cm rückwerts  
        # dann drehe nach rechts 90°
        # und fahre 2cm fohrwärts 
        #stift hoch

    def schreibeE(self):
        print("Schreibe E")

    def schreibeG(self):
        print("Schreibe G")


    def schreibeO(self):
        print("Schreibe O")

    def schreibeLego(self):
        self.schreibeL()
        self.schreibeE()
        self.schreibeG()
        self.schreibeO()

print("successfully loaded the IQ Lego teams code :)")