# LEGO type:standard slot:6 autostart

import math

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

HELLO = "HELLO IQ"
BRICKIES_BOT = "brickies"
BRICKIES_BOT_2 = "brickies_2"
BACKSTEIN_BOT = "backstein"

'''
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, typ: str):
        
        self.hub: PrimeHub = hub
        self.typ=typ

        if self.typ==BACKSTEIN_BOT:
            # Radantrieb
            LEFT_MOTOR_PORT = 'F'
            RIGHT_MOTOR_PORT = 'B'
            # Motoren für Aufsätze
            FRONT_MOTOR_RIGHT_PORT = "E"
            self.frontMotorRight: Motor = Motor(FRONT_MOTOR_RIGHT_PORT)

        elif self.typ==BRICKIES_BOT:
            # Radantrieb
            LEFT_MOTOR_PORT = 'E'
            RIGHT_MOTOR_PORT = 'F'
            # Motoren für Aufsätze
            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)
            # Radius der Antriebsräder
            self.rad_radius = 2.1
            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
            self.abstand_rad_front = 5.55
            
        elif self.typ==BRICKIES_BOT_2:    
            # Radantrieb
            LEFT_MOTOR_PORT = 'E'
            RIGHT_MOTOR_PORT = 'F'
            # Radius der Antriebsräder
            self.rad_radius = 2.9
            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
            self.abstand_rad_front = 8.5
            
        ## Allgemein ##        
        self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
        # Radumfang neu berechnen und Motor konfigurieren
        rad_umfang = 2 * math.pi * self.rad_radius
        self.movementMotors.set_motor_rotation(rad_umfang)
        self.leftMotor: Motor = Motor(LEFT_MOTOR_PORT)
        self.rightMotor: Motor = Motor(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 grad == 0 or grad == 360 :
            print("nichts zu tun")
            return
        if with_reset:
            self.motionSensor.reset_yaw_angle()
        #steering = 100 if grad > 0 else -100
        toleranz = 0
        aktuell = self.motionSensor.get_yaw_angle()
        ziel = grad
        steering = 100 if ziel > aktuell else -100
        self.movementMotors.start(steering=steering, speed=10)
        differenz = ziel - aktuell
        print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
        while abs(differenz) > toleranz :
            aktuell = self.motionSensor.get_yaw_angle()
            differenz = ziel - aktuell
            pass
        self.movementMotors.stop()
        print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))


    def fahre_gerade(self, cm):
        if self.typ == "brickies":
            cm = -cm
        self.motionSensor.reset_yaw_angle()
        self.movementMotors.start_tank(10, 10)
        self.movementMotors.set_default_speed(10)
        self.movementMotors.move(cm)
        versatz = self.motionSensor.get_yaw_angle()
        self.drehe(grad=-versatz)
    

    def fahre_gerade_geregelt(self, cm):
        if self.typ == "brickies":
            cm = -cm
        self.motionSensor.reset_yaw_angle()
        self.movementMotors.start_tank(10, 10)
        self.movementMotors.set_default_speed(10)

        sum_cm = 0
        while sum_cm < cm:
            self.movementMotors.move(1)
            versatz = self.motionSensor.get_yaw_angle()
            self.drehe(grad=-versatz)
            self.motionSensor.reset_yaw_angle()
            sum_cm = sum_cm + 1

        self.movementMotors.move(cm - sum_cm)

    def fahre_mit_drehung(self, strecke1, grad, strecke2):
        self.fahre_gerade_geregelt(strecke1 + self.abstand_rad_front)
        self.drehe(grad)
        self.fahre_gerade_geregelt(strecke2 - self.abstand_rad_front)
    
    def fahre_gerade_aus(self, cm,speed):
    
        self.motionSensor.reset_yaw_angle()
    
        self.movementMotors.move_tank(amount=cm,left_speed=speed, right_speed=speed)
        drehung = self.motionSensor.get_yaw_angle()
        print(drehung)
        if drehung > 0:
            richtung = -1
        else:
            richtung = 1
        while abs(drehung) > 2:
            self.movementMotors.move(amount=richtung * 0.1, steering=100)
            drehung = self.motionSensor.get_yaw_angle()
            print(drehung)

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