# 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"

'''
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.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)
            self.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_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.movementMotors: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
            # Radumfang neu berechnen und Motor konfigurieren
            rad_radius = 2.1
            rad_umfang = 2 * math.pi * rad_radius
            self.movementMotors.set_motor_rotation(rad_umfang)
            # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
            self.abstand_rad_front = 5.55

        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 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)


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