import math
import threading
import schrittmotor1 as sm1
import schrittmotor2 as sm2
import schrittmotor3 as sm3
import schrittmotor4 as sm4
import camera 

# Festlegung der Vektoren der vier Masten
e = [0, 0, 42]
f = [100, 0, 42]
g = [100, 62, 42]
h = [0, 62, 42]

# Funktion zur Berechnung der Seillängen aus den Vektoren für Start- und Zielpunkt
def berechneSeillaenge(start, ziel): 
    
    # Liste zur Speicherung der Seillängen für die vier Masten
    seillaengen = [0,0,0,0]
    
    # Berechnung der Seillänge für Masten e
    es = [start[0]-e[0], start[1]-e[1], start[2]-e[2]]
    ez = [ziel[0]-e[0] , ziel[1]-e[1] , ziel[2]-e[2]]
    
    ES = math.sqrt(es[0]**2 + es[1]**2 + es[2]**2)
    EZ = math.sqrt(ez[0]**2 + ez[1]**2 + ez[2]**2)
    
    seillaengen[0] = EZ-ES

    # Berechnung der Seillänge für Masten f
    fs = [start[0]-f[0], start[1]-f[1], start[2]-f[2]]
    fz = [ziel[0]-f[0] , ziel[1]-f[1] , ziel[2]-f[2]]
    
    FS =(math.sqrt(fs[0]**2 + fs[1]**2 + fs[2]**2))
    FZ =(math.sqrt(fz[0]**2 + fz[1]**2 + fz[2]**2))
    
    seillaengen[1] = FZ-FS

    # Berechnung der Seillänge für Masten g
    gs = [start[0]-g[0], start[1]-g[1], start[2]-g[2]]
    gz = [ziel[0]-g[0] , ziel[1]-g[1] , ziel[2]-g[2]]
    
    GS =(math.sqrt(gs[0]**2 + gs[1]**2 + gs[2]**2))
    GZ =(math.sqrt(gz[0]**2 + gz[1]**2 + gz[2]**2))
    
    seillaengen[2] = GZ-GS

    # Berechnung der Seillänge für Masten h
    hs = [start[0]-h[0], start[1]-h[1], start[2]-h[2]]
    hz = [ziel[0]-h[0] , ziel[1]-h[1] , ziel[2]-h[2]]
    
    HS =(math.sqrt(hs[0]**2 + hs[1]**2 + hs[2]**2))
    HZ =(math.sqrt(hz[0]**2 + hz[1]**2 + hz[2]**2))
    
    seillaengen[3] = HZ-HS

    # Rückgabe der Liste der Seillängen
    return seillaengen

# Funktion zur Berechnung der Umdrehungen der Motoren für die jeweilige Seillänge der Masten
def berechneMotorUmdrehungen(seillaengen):

    # Liste zur Speicherung der Umdrehungen der vier Motoren
    umdrehungen = [0,0,0,0]

    umfang = 2*math.pi*3.25

    # Berechnung der Umdrehung des Motors an Masten e
    umdrehungen[0] = (seillaengen[0]/umfang)*512
    
    # Berechnung der Umdrehung des Motors an Masten f
    umdrehungen[1] = (seillaengen[1]/umfang)*512

    # Berechnung der Umdrehung des Motors an Masten g
    umdrehungen[2] = (seillaengen[2]/umfang)*512

    # Berechnung der Umdrehung des Motors an Masten h
    umdrehungen[3] = (seillaengen[3]/umfang)*512

    # Rückgabe der Liste der Umdrehungen 
    return umdrehungen

#Funktion zur Steuerung des Motors auf Basis der ermittelten Umdrehungen und Seilängen
def steuereMotor(umdrehungen):

    # Starte die Schrittmotoren
    sm1.setup()
    sm2.setup()
    sm3.setup()
    sm4.setup()

    # Steuerung des Motors an Masten e für die ermittelte Umdrehung
    # (mithilfe von Threads zu gleichzeitigen Ausführung der Umdrehungen)
    if motorUMD[0]>0:
        th1=threading.Thread(target=sm2.forward, args = (int(abs(umdrehungen[0])),))
        th1.start()
    else:
        th1=threading.Thread(target=sm2.backward, args=(int(abs(umdrehungen[0])),))
        th1.start()
        
    # Steuerung des Motors an Masten f für die ermittelte Umdrehung
    if motorUMD[1]>0:
        th2=threading.Thread(target=sm1.forward, args = (int(abs(umdrehungen[1])),))
        th2.start()   
    else:
        th2=threading.Thread(target=sm1.backward, args = (int(abs(umdrehungen[1])),))
        th2.start()
    
    # Steuerung des Motors an Masten g für die ermittelte Umdrehung
    if motorUMD[2]>0:
        th3=threading.Thread(target=sm4.forward, args = (int(abs(umdrehungen[2])),))
        th3.start()      
    else:
        th3=threading.Thread(target=sm4.backward, args = (int(abs(umdrehungen[2])),))
        th3.start()
        
    # Steuerung des Motors an Masten h für die ermittelte Umdrehung
    if motorUMD[3]>0:
        th4=threading.Thread(target=sm3.forward, args = (int(abs(umdrehungen[3])),))
        th4.start()       
    else:
        th4=threading.Thread(target=sm3.backward, args = (int(abs(umdrehungen[3])),))
        th4.start()

#Funktion zur Steuerung des Spidercam mithilfe der oben definierten Funktionen
def bewegeKamera (start, ziel):
    # Erst werden die neuen Seillängen berechnet...
    seillaengen = berechneSeillaenge(start, ziel)
    
    # ... dann die Umdrehungen berechnet ...
    umdrehungen = berechneMotorUMD (seillaengen)
    
    # ... und auf Basis dieser Berechnungen die Motoren gesteuert
    steuereMotor(umdrehungen)

    # Zudem wird die Kamera für die Filmaufname gestartet
    thread5=threading.Thread(target=camera.camera)
    thread5.start()