# 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):
        
        self.hub: PrimeHub = hub

        # 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.bothFrontMotors: MotorPair = MotorPair(FRONT_MOTOR_LEFT_PORT, FRONT_MOTOR_RIGHT_PORT)

        self.antrieb: MotorPair = MotorPair(LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT)
        # Radumfang neu berechnen und Motor konfigurieren
        # Radius der Antriebsräder
        self.rad_radius = 2.1
        # Abstand zwischen Rädern (Mitte) und Vorderseite des Roboters
        self.abstand_rad_front = 5.55            
        rad_umfang = 2 * math.pi * self.rad_radius
        self.antrieb.set_motor_rotation(rad_umfang)

        self.bewegungsSensor: 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 drehe(self, grad=90, with_reset=True):
        """
        Funktion um den Roboter auf der Stelle zu drehen

        :param int grad: Grad um die der Roboter gedreht werden soll
        mittels Vorzeichen +/- kann links oder rechts herum gedreht werden
        """
        if grad == 0 or grad == 360 :
            print("nichts zu tun")
            return
        if with_reset:
            self.bewegungsSensor.reset_yaw_angle()
        #steering = 100 if grad > 0 else -100
        toleranz = 0
        aktuell = self.bewegungsSensor.get_yaw_angle()
        ziel = grad
        steering = 100 if ziel > aktuell else -100
        self.antrieb.start(steering=steering, speed=10)
        differenz = ziel - aktuell
        print ("Start Ziel: {}, Aktuell: {}".format(ziel, aktuell))
        while abs(differenz) > toleranz :
            aktuell = self.bewegungsSensor.get_yaw_angle()
            differenz = ziel - aktuell
            pass
        self.antrieb.stop()
        print ("Final Ziel: {}, Aktuell: {}".format(ziel, aktuell))
  

    def fahre_gerade_geregelt(self, cm):
        """
        WIP by Lars & Klaus
        """
        self.bewegungsSensor.reset_yaw_angle()
        self.antrieb.start_tank(10, 10)
        self.antrieb.set_default_speed(10)

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

        self.antrieb.move(cm - sum_cm)


    def fahre_mit_drehung(self, strecke1, grad, strecke2):
        """
        Funktion für eine Fahrt mit 1. Strecke, dann Drehung in der Mitte, dann 2. Strecke
        Vereinfacht die Logik, da der Roboter durch die Drehung einen Versatz hat gegenüber einer
        Strecke die mit dem Lineal ausgemessen wurde
        """
        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: float, speed: int):
        """
        Funktion zum gerade aus fahren mit Korrektur am Ende

        :param int cm: Zentimeter die gerade aus gefahren werden soll
        :param speed: Geschwindigkeit mit der gefahren wird
        """
    
        self.bewegungsSensor.reset_yaw_angle()
    
        self.antrieb.move_tank(amount=cm,left_speed=speed, right_speed=speed)
        drehung = self.bewegungsSensor.get_yaw_angle()
        print(drehung)
        if drehung > 0:
            richtung = -1
        else:
            richtung = 1
        while abs(drehung) > 2:
            self.antrieb.move(amount=richtung * 0.1, steering=100)
            drehung = self.bewegungsSensor.get_yaw_angle()
            print(drehung)

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