# LEGO type:standard slot:5 autostart

import os, sys

from spike import PrimeHub, LightMatrix, Button, StatusLight, ForceSensor, MotionSensor, Speaker, ColorSensor, App, DistanceSensor, Motor, MotorPair
from spike.control import wait_for_seconds, wait_until, Timer
from hub import battery
from math import *

############## Allgemein: Prüfe Batteriezustand ###############################
if battery.voltage() < 7000: #set threshold for battery level
    print("Spannung der Batterie zu niedrig: " + str(battery.voltage()) + " \n"
        + "--------------------------------------- \n "
        + "#### UNBEDINGT ROBOTER AUFLADEN !!! #### \n"
        + "---------------------------------------- \n")
else:
    print("Spannung der Batterie " + str(battery.voltage()) + "\n")
################################################################################

############################## NICHT ÄNDERN ###############################
def importFile(slotid=0, precompiled=False, module_name='importFile'):
    print("##### START # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
    import os, sys
    suffix = ".py"
    if precompiled:
        suffix = ".mpy"
    with open("/projects/.slots","rt") as f:
        slots = eval(str(f.read()))
        print(slots)
    print(os.listdir("/projects/"+str(slots[slotid]["id"])))
    with open("/projects/"+str(slots[slotid]["id"])+"/__init__"+suffix,"rb") as f:
        print("trying to read import program")
        program = f.read()
        print(program)
    try:
        os.remove("/"+module_name+suffix)
    except:
        pass
    with open("/"+module_name+suffix,"w+") as f:
        print("trying to write import program")
        f.write(program)
    if (module_name in sys.modules):
        del sys.modules[module_name]
    #exec("from " + module_name + " import *")
    print("##### END # IMPORTING CODE FROM SLOT "+str(slotid)+" ##############")
#####################################################################################


################ Importiere Code aus andere Dateien #################################

# Importiere Code aus der Datei "iqrobot.py"
# Dateiname und Modulname sollten gleich sein, dann kann man Code Completion nutzen
importFile(slotid=6, precompiled=True, module_name="iqrobot")
import iqrobot as iq
print(iq.HELLO) 

# Importiere Go Robot Code
#importFile(slotid=3, precompiled=True, module_name="gorobot")
#import gorobot as gr
#gr.exampleFour()
#gr.db.gyroRotation(90, 25, 35, 25)


################### Hauptcode ####################################
'''
Code zum Lösen einer Aufgabe, kann oben importierten Code nutzen
Es können auch pro Aufgabe eigene Funktionen geschrieben werden
Wichtig ist, dass die PORTS der Sensoren überall gleich sind
und auch `hub` als Instanz von PrimeHub
dh auch an die Funktionen im importierten Code übergeben werde
'''

# Initialisieren des Hubs, der Aktoren und Sensoren
hub = PrimeHub()

# Initialisiere Robot Klasse mit unseren Funktionen
iqRobot: iq.IQRobot = iq.IQRobot(hub)

# Führe Funktionen aus unser Robot Klasse aus:
iqRobot.show('HAPPY')

def huenchenaufgabe():
    iqRobot.fahre_gerade_aus(40,60)
    iqRobot.drehe(-40,True)
    iqRobot.fahre_gerade_aus(20,60)
    iqRobot.drehe(-20)
    iqRobot.fahre_gerade_aus(55,60)
    iqRobot.heber(10,30)


def druckmaschine():
    iqRobot.fahre_gerade_aus(18,30)
    iqRobot.drehe(-45)  
    iqRobot.fahre_gerade_aus(20,30)
    iqRobot.fahre_gerade_aus(-15,30)

def hologram():
    iqRobot.drehe(45)
    iqRobot.fahre_gerade_aus(38,30)
    iqRobot.drehe(45)
    iqRobot.fahre_gerade_aus(12.5,40)
    iqRobot.fahre_gerade_aus(-15,30)

def augmented_reality():
    iqRobot.drehe(-135)
    iqRobot.fahre_gerade_aus(41,30)
    iqRobot.drehe(90)
    #iqRobot.fahre_gerade_aus(10,30)
    #while(True):
    #    print(str(iqRobot.get_abstand()))
    iqRobot.fahre_bis_abstand(16)     #hir faren wir mit abstand gegen die wand
    iqRobot.schaufel(-100)
    iqRobot.drehe(15)
    iqRobot.fahre_gerade_aus(-7,30)
    iqRobot.drehe(30)
    iqRobot.fahre_gerade_aus(3,20)
    iqRobot.drehe(20)
    iqRobot.schaufel(100)
    iqRobot.fahre_gerade_aus(10,25)
    iqRobot.drehe(17)
    iqRobot.fahre_gerade_aus(7.5,30)
    iqRobot.drehe(-10)
    iqRobot.fahre_gerade_aus(4,30)
    iqRobot.drehe(-10)
    iqRobot.fahre_gerade_aus(3,30)
    iqRobot.drehe(-10)
    iqRobot.fahre_gerade_aus(5,20) 
    iqRobot.drehe(135)


def druckmaschine2():
    iqRobot.fahre_gerade_aus(17,30)
    iqRobot.drehe(90)
    iqRobot.fahre_gerade_aus(17.5)
    iqRobot.drehe(-138)
    #iqRobot.fahre_gerade_aus(-2)
    iqRobot.fahre_bis_abstand(9.5)
    iqRobot.schaufel(-90, speed=5)
    iqRobot.fahre_gerade_aus(1.4,20)
    iqRobot.schaufel(40,speed=5)
    iqRobot.fahre_gerade_aus(0.6,20)
    iqRobot.schaufel(75,speed=10)


#druckmaschine()
#hologram()
#augmented_reality()
druckmaschine2()