# 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() < 8000: #set threshold for battery level
    print("Spannung der Batterie zu niedrig: " + str(battery.voltage()) + " \n"
        + "--------------------------------------- \n "
        + "#### UNBEDINGT ROBOTER AUFLADEN !!! #### \n"
        + "#### SONST KOMMT MARKUS OPTITZ !!!! #### \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=False, module_name="iqrobot")
import iqrobot as iq
print(iq.HELLO) 

# Definiere an welchen Ports die Sensoren angeschlossen sind 

COLOR_SENSOR_PORT = 'D'
LEFT_MOTOR_PORT = 'B'
RIGHT_MOTOR_PORT = 'F'
STICK_MOTOR_PORT = ''
GABEL_MOTOR_PORT = 'E'
DISTANCE_SENSOR_PORT = ''
#penStatus = False
# Initialisieren des Hubs, der Aktoren und Sensoren
hub = PrimeHub()

# Initialisiere Robot Klasse mit unseren Funktionen
iqRobot: iq.IQRobot = iq.IQRobot(hub, LEFT_MOTOR_PORT, RIGHT_MOTOR_PORT, COLOR_SENSOR_PORT, STICK_MOTOR_PORT, GABEL_MOTOR_PORT, DISTANCE_SENSOR_PORT)

# Das Programm
 
# Variablen
penStatus = False # Oben

# Methoden
def togglePen():
    global penStatus
    if(penStatus == True):
        iqRobot.moveGabel(20)
        penStatus = False
    if(penStatus == False):
       iqRobot.moveGabel(-20)
       penStatus = True

def printChar(c):
    global penStatus
    if(c == "L"):
        print("Printing the character L")
        if not(penStatus == "False"):
            print("Toggling pen")
            togglePen()
        iqRobot.driveForward_for_sec(2.0)
        # iqRobot.leftMotor.run_for_degrees(360)
        # iqRobot.rotateRobot(degrees=100, speed=100)
        iqRobot.gyroRotation(angle=90)
        iqRobot.driveForward_for_sec(1.0)
    if(c == "E"):
        print("Printing the character E")
        if(penStatus == True):
            togglePen()
        iqRobot.driveForward_for_sec(0.1)
        togglePen()
        iqRobot.driveForward_for_sec(1.0)
        togglePen()

    if(c == "G"):
        print("Printing the character G")
    if(c == "O"):
        print("Printing the character O")

wait_for_seconds(5)

printChar("L")
printChar("E")
printChar("G")
printChar("O")