# LEGO type:standard slot:6 autostart

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

HELLO = "HELLO IQ"
VERSION = "V1"

'''
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, stickMotorPort: str, gabelMotorPort: str, distanceSensorPort: str):
        self.hub: PrimeHub = hub
        if (leftMotorPort != ''):
            self.leftMotor: Motor = Motor(leftMotorPort)
        if (rightMotorPort != ''):
            self.rightMotor: Motor = Motor(rightMotorPort)
        if (leftMotorPort != '' and rightMotorPort != ''):
            self.movementMotors: MotorPair = MotorPair(leftMotorPort, rightMotorPort) 
        if (colorSensorPort != ''):
            self.colorSensor: ColorSensor = ColorSensor(colorSensorPort)
        if (stickMotorPort != ''):
            self.stickMotor: Motor = Motor(stickMotorPort)
        if (gabelMotorPort != ''):
            self.gabelMotor: Motor = Motor(gabelMotorPort)
        if (distanceSensorPort != ''):
            self.distanceSensor: DistanceSensor = DistanceSensor(distanceSensorPort)

        self.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 driveBackward_for_sec(self, seconds: float):
        # Fahre die übergebene Anzahl seconds gerade aus
        self.movementMotors.set_default_speed(-100)()
        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 main(self):
        
        '''
        self.moveStick(30)
        if self.colorSensor.get_reflected_light() > 1:
            self.show('ANGRY')
        else:
            self.show('SAD')


        colorIntensity = self.getColorIntensity()
        print("Farbintensität: " + str(colorIntensity))
        '''

    def rotateRobot(self,degrees,speed) :
        #Lasse den Roboter bei Bedarf um eine bestimmte Anzahl an Grad sanft und behutsam nach Links oder Rechts drehen,
        #  um den Nutzer zu befriedigen
        self.motionSensor.reset_yaw_angle()
        if (degrees > 0):
            self.leftMotor.start(speed)
            self.rightMotor.start(-speed)
        elif (degrees < 0):
            self.leftMotor.start(-speed)
            self.rightMotor.start(speed)
        while (self.motionSensor.get_yaw_angle() > degrees) :
            self.leftMotor.stop()
            self.rightMotor.stop()
        print("Disabled")  

    def gyroRotation(self, angle, rotate_mode = 0, stop = True):

        global run_generator, runSmall

        run_generator = False

        speed = 30

        #set standard variables
        rotatedDistance = 0
        steering = 1

        #gyro sensor calibration
        angle = angle * (2400/2443) #experimental value based on 20 rotations of the robot

        #Setting variables based on inputs
        loop = True
        gyroStartValue = self.motionSensor.get_yaw_angle() #Yaw angle used due to orientation of the self.hub. This might need to be changed

        #Inversion of steering value for turning counter clockwise
        if angle < 0:
            steering = -1

        #Testing to see if turining is necessary, turns until loop = False

        while loop:
            rotatedDistance = self.motionSensor.get_yaw_angle() #Yaw angle used due to orientation of the self.hub. This might need to be changed
            
            print(abs(rotatedDistance - gyroStartValue))

            #Checking for variants
            #Both Motors turn, robot moves on the spot
            if rotate_mode == 0:
                self.rightMotor.start_at_power(int(speed) * steering)
                self.leftMotor.start_at_power(int(speed) * steering)
            
            elif rotate_mode == 1:

                if angle * speed > 0:
                    self.leftMotor.start_at_power(- int(speed))
                else:
                    self.rightMotor.start_at_power(+ int(speed))

            
            if abs(angle) <= abs(rotatedDistance - gyroStartValue):                   
                    loop = False
                    break



        #Stops movement motors for increased accuracy while stopping
        if stop:
            self.rightMotor.stop()
            self.leftMotor.stop()

        return # End of gyroStraightDrive 
            
    def moveStick(self,degrees) :
        #Bewege sanft und langsam die Schleuderstange am hinteren Rumpf des Geräts
        self.stickMotor.run_for_degrees(degrees)

    def moveGabel(self,degrees) :
        #Bewege sanft und langsam die ungewöhnlich große sich bewegende Stange am vorderen Rumpf des Geräts
        self.gabelMotor.run_for_degrees(degrees)

    def getDistance (self) :
        cmDistance = self.distanceSensor.get_distance_cm(short_range=False)
        return cmDistance
    
    def getShortDistance (self) :
        cmDistance = self.distanceSensor.get_distance_cm(short_range=True)
        return cmDistance
    



print("Programm Library loaded")