# LEGO type:standard slot:7 autostart

########################################################################
# "ALTE" VERSION MIT DER WIR VERSUCHT HABEN DAS WORT "LEGO" ZU SCHREIBEN
########################################################################

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

HELLO = "HELLO IQ 2"

'''
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, colorSensorPort: str, typ: str):
        self.hub: PrimeHub = hub
        self.typ=typ
        if self.typ=="backstein":
            LEFT_MOTOR_PORT = 'F'
            RIGHT_MOTOR_PORT = 'B'
            FRONT_MOTOR_RIGHT_PORT = "E"
            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
        elif self.typ=="brickies":
            LEFT_MOTOR_PORT = 'E'
            RIGHT_MOTOR_PORT = 'F'
            FRONT_MOTOR_RIGHT_PORT = "B"
            FRONT_MOTOR_LEFT_PORT = "A"
            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)
            self.frontMotorLeft: Motor = Motor(FRONT_MOTOR_LEFT_PORT)
            self.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)

        self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
        self.rightMotor: Motor = Motor(RIGHT_MOTOR_PORT)
        self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT) 
        #self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
        #self.frontMotorLeft: Motor = Motor("C")
        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, with_reset=True):
        if with_reset:
            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 drehe_robot(self, grad=90):
        if self.typ == "backstein":
            radius=9.5
            stift_versatz=2.2
        if self.typ == "brickies":
            radius=17.4
            stift_versatz=0.3
        self.fahre_gerade(-radius - stift_versatz)
        self.drehe(grad)
        self.fahre_gerade(radius - stift_versatz)

    def fahre_gerade(self, cm, zeichne=False):
        if zeichne:
            self.bewege_stift(1) # Stift runter
        self.motionSensor.reset_yaw_angle()
        if self.typ == "brickies":
            cm = -cm
        self.movementMotors.move(cm)
        if zeichne:
            self.bewege_stift(-1) # Stift hoch
        versatz = self.motionSensor.get_yaw_angle()
        self.drehe(grad=-versatz)

    def buchstabe_zu_segmenten(self, buchstabe):
        # Segmente um Buchstaben zu schreiben
        #          4_
        #       5 |__|3
        #       0 |6_|2
        #          1
        #
        buchstabe_zu_segmenten = {"L": [1,1,0,0,0,1,0], "E": [1,1,0,0,1,1,1], "G": [1,1,1,0,1,1,0], "O": [1,1,1,1,1,1,0]}
        return buchstabe_zu_segmenten[buchstabe]


    def bewege_stift(self, richtung):
        if self.typ == "backstein":
            self.frontMotorRight.run_for_rotations(richtung*0.4)
        if self.typ == "brickies":
            #print("bewege stift brickies")
            self.bothFrontMotors.move(-richtung*0.2, unit='rotations', speed=5)

    def schreibe_buchstabe(self, buchstabe):
        print("Schreibe " + buchstabe)
        segmente = self.buchstabe_zu_segmenten(buchstabe)
        grad_drehung=-90
        self.fahre_gerade(2)
        self.drehe_robot(-grad_drehung) # drehe rechts
        for segment_nummer, segment in enumerate(segmente):
            print("Segment: " + str(segment) + " , Segment Nummer: " + str(segment_nummer))
            if segment==1:
                self.fahre_gerade(5, zeichne=True)
            else:
                self.fahre_gerade(5)
            if segment_nummer != 2 and segment_nummer != 6:
                self.drehe_robot(grad_drehung) # drehe links
            

    def schreibeL(self, schreibe=True, zurueck=False):
        if zurueck: 
            step = 5
            faktor = -1
        else:
            step = 1
            faktor = 1
        print("Schreibe L")
        #self.frontMotorRight.run_for_rotations(-0.4)
        radius=9.5
        stift_versatz=2.2
        if schreibe:
            self.frontMotorRight.run_for_rotations(0.4)
        self.movementMotors.set_default_speed(10)

        while (True):
            if step == 0:
                break
            if step == 1:
                self.movementMotors.move(faktor * 5)
                if schreibe:
                    self.frontMotorRight.run_for_rotations(-0.4)
            if step == 2:
                self.movementMotors.move(faktor * (-radius - stift_versatz))
            if step == 3:
                self.drehe(faktor * -90)
            if step == 4:
                self.movementMotors.move(faktor*(radius - stift_versatz))
                if schreibe:
                    self.frontMotorRight.run_for_rotations(0.4)
            if step == 5:
                self.movementMotors.move(faktor * 2)
                if schreibe:
                    self.frontMotorRight.run_for_rotations(-0.4)
            if step == 6:
                break
            step += faktor
        # Fahre 5 cm rückwerts  
        # dann drehe nach rechts 90°
        # und fahre 2cm fohrwärts 
        #stift hoch


    def schreibeLego(self):
        #self.schreibeL()
        #self.schreibeL(schreibe=False, zurueck=True)
        self.movementMotors.set_default_speed(10)
        self.bewege_stift(-1)
        self.fahre_gerade(4, zeichne=True)
        self.drehe_robot()
        self.fahre_gerade(4, zeichne=True)
        #self.schreibe_buchstabe("L")
        #self.schreibe_buchstabe("E")
        #self.schreibe_buchstabe("G")
        #self.schreibe_buchstabe("O")

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