import sys, getopt
import RPi.GPIO as GPIO
import time
import threading
import math

t1_m1 = 0.00005
t2_m1 = 0.0004
te_m1 = 0.0003
t1_m2 = 0.0003
t2_m2 = 0.001
te_m2 = 0.0005
t1_m3 = 0.0003
t2_m3 = 0.001
te_m3 = 0.0006

delay = 0.004
delay_y = 0.012
delay_z = 0.004
freq = 800
freq_halten = 30000
cyc = 12
pos_m1 = 0
pos_m2 = 0
pos_m3 = 0
ausgleich_m3 = 1

schritte_m1 = 180
schritte_m2 = 90
schritte_m3 = 40
#schritte = int(sys.argv[1])
#zoom = int(sys.argv[2])
#print('Schritte: ', schritte)
#print('Zoom: ', zoom)

GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)

m1_coil_A_1_pin = 17 # gelb
m1_coil_B_1_pin = 18 # schwarz
m1_coil_A_2_pin = 27 # orange
m1_coil_B_2_pin = 22 # braun

m2_coil_A_1_pin = 19 # schwarz
m2_coil_B_1_pin = 13 # gelb
m2_coil_A_2_pin = 6 # orange
m2_coil_B_2_pin = 5 # rot

m3_coil_A_1_pin = 16 # schwarz
m3_coil_B_1_pin = 12 # gelb
m3_coil_A_2_pin = 21 # braun
m3_coil_B_2_pin = 20 # orange

RELAIS_1_GPIO = 2
RELAIS_2_GPIO = 3

Endschalter_1 = 23
Endschalter_2 = 26
Endschalter_3 = 25

# anpassen, falls andere Sequenz
#StepCount = 8
#Seq = list(range(0, StepCount))
#Seq[0] = [1,0,0,0]
#Seq[1] = [1,1,0,0]
#Seq[2] = [0,1,0,0]
#Seq[3] = [0,1,1,0]
#Seq[4] = [0,0,1,0]
#Seq[5] = [0,0,1,1]
#Seq[6] = [0,0,0,1]
#Seq[7] = [1,0,0,1]

StepCount = 4
Seq = list(range(0, StepCount))
Seq[0] = [1,0,0,0]
Seq[1] = [0,1,0,0]
Seq[2] = [0,0,1,0]
Seq[3] = [0,0,0,1]

#GPIO.setup(enable_pin, GPIO.OUT)
GPIO.setup(m1_coil_A_1_pin, GPIO.OUT)
GPIO.setup(m1_coil_A_2_pin, GPIO.OUT)
GPIO.setup(m1_coil_B_1_pin, GPIO.OUT)
GPIO.setup(m1_coil_B_2_pin, GPIO.OUT)

GPIO.setup(m2_coil_A_1_pin, GPIO.OUT)
GPIO.setup(m2_coil_A_2_pin, GPIO.OUT)
GPIO.setup(m2_coil_B_1_pin, GPIO.OUT)
GPIO.setup(m2_coil_B_2_pin, GPIO.OUT)

GPIO.setup(m3_coil_A_1_pin, GPIO.OUT)
GPIO.setup(m3_coil_A_2_pin, GPIO.OUT)
GPIO.setup(m3_coil_B_1_pin, GPIO.OUT)
GPIO.setup(m3_coil_B_2_pin, GPIO.OUT)

GPIO.setup(Endschalter_1, GPIO.IN)
GPIO.setup(Endschalter_2, GPIO.IN)
GPIO.setup(Endschalter_3, GPIO.IN)

# pwm Setup fuer Haltestrom
pwm1_m1 = GPIO.PWM(m1_coil_A_1_pin, freq)
pwm2_m1 = GPIO.PWM(m1_coil_B_1_pin, freq)
pwm3_m1 = GPIO.PWM(m1_coil_A_2_pin, freq)
pwm4_m1 = GPIO.PWM(m1_coil_B_2_pin, freq)

pwm1_m2 = GPIO.PWM(m2_coil_A_1_pin, freq)
pwm2_m2 = GPIO.PWM(m2_coil_B_1_pin, freq)
pwm3_m2 = GPIO.PWM(m2_coil_A_2_pin, freq)
pwm4_m2 = GPIO.PWM(m2_coil_B_2_pin, freq)

pwm1_m3 = GPIO.PWM(m3_coil_A_1_pin, freq)
pwm2_m3 = GPIO.PWM(m3_coil_B_1_pin, freq)
pwm3_m3 = GPIO.PWM(m3_coil_A_2_pin, freq)
pwm4_m3 = GPIO.PWM(m3_coil_B_2_pin, freq)

pwm1_m1.start(0)
pwm1_m1.ChangeFrequency(freq)
pwm1_m1.ChangeDutyCycle(0)
pwm2_m1.start(0)
pwm2_m1.ChangeFrequency(freq)
pwm2_m1.ChangeDutyCycle(0)
pwm3_m1.start(0)
pwm3_m1.ChangeFrequency(freq)
pwm3_m1.ChangeDutyCycle(0)
pwm4_m1.start(0)
pwm4_m1.ChangeFrequency(freq_halten)
pwm4_m1.ChangeDutyCycle(cyc)

pwm1_m2.start(0)
pwm1_m2.ChangeFrequency(freq)
pwm1_m2.ChangeDutyCycle(0)
pwm2_m2.start(0)
pwm2_m2.ChangeFrequency(freq)
pwm2_m2.ChangeDutyCycle(0)
pwm3_m2.start(0)
pwm3_m2.ChangeFrequency(freq)
pwm3_m2.ChangeDutyCycle(0)
pwm4_m2.start(0)
pwm4_m2.ChangeFrequency(freq_halten)
pwm4_m2.ChangeDutyCycle(cyc)

pwm1_m3.start(0)
pwm1_m3.ChangeFrequency(freq)
pwm1_m3.ChangeDutyCycle(0)
pwm2_m3.start(0)
pwm2_m3.ChangeFrequency(freq)
pwm2_m3.ChangeDutyCycle(0)
pwm3_m3.start(0)
pwm3_m3.ChangeFrequency(freq)
pwm3_m3.ChangeDutyCycle(0)
pwm4_m3.start(0)
pwm4_m3.ChangeFrequency(freq)
pwm4_m3.ChangeDutyCycle(0)

GPIO.setup(RELAIS_1_GPIO, GPIO.OUT) # GPIO Modus zuweisen
GPIO.setup(RELAIS_2_GPIO, GPIO.OUT) # GPIO Modus zuweisen

GPIO.output(RELAIS_1_GPIO, GPIO.LOW) # an
GPIO.output(RELAIS_2_GPIO, GPIO.HIGH) # aus
time.sleep(3)
GPIO.output(RELAIS_2_GPIO, GPIO.LOW) # an

def schritt(spule, status, ges_schritte, schritt, t1, t2):
    w = (schritt * 100 / (ges_schritte - 1)) * 1.8
    s = round(math.sin(math.radians(w)), 2)
    t = t2 - (s * (t2 - t1))
    #print(str(t))

    if status == 1:
        for i in range(0, 91, 5):
            dc = round(math.sin(math.radians(i)) * 100, 0)
            spule.ChangeDutyCycle(dc)
            time.sleep(t)
    else:
        for i in range(0, 91, 5):
            dc = round(math.cos(math.radians(i)) * 100, 0)
            spule.ChangeDutyCycle(dc)
            time.sleep(t)

def vorwaerts(motor, sp1, sp2, sp3, sp4, schritte, t1, t2, halten):
    global pos_m1, pos_m2, pos_m3
    ges_schritte = schritte * 8

    if halten == 1:
        sp4.ChangeFrequency(freq)

    for i in range(int(schritte)):
        if motor == 'm1':
            if pos_m1 >= schritte_m1:
                print('Motor1 hat positives Ende erreicht')
                break
            else:
                pos_m1 = pos_m1 + 1

        if motor == 'm2':
            if pos_m2 >= schritte_m2:
                print('Motor2 hat positives Ende erreicht')
                break
            else:
                pos_m2 = pos_m2 + 1
                #print('Motor2-Pos: ' + str(pos_m2))

        if motor == 'm3':
            if pos_m3 >= schritte_m3:
                print('Motor3 hat positives Ende erreicht')
                break
            else:
                pos_m3 = pos_m3 + 1

        pos = i * 8
        schritt(sp1, 1, ges_schritte, pos, t1, t2)
        schritt(sp4, 0, ges_schritte, pos + 1, t1, t2)
        schritt(sp2, 1, ges_schritte, pos + 2, t1, t2)
        schritt(sp1, 0, ges_schritte, pos + 3, t1, t2)
        schritt(sp3, 1, ges_schritte, pos + 4, t1, t2)
        schritt(sp2, 0, ges_schritte, pos + 5, t1, t2)
        schritt(sp4, 1, ges_schritte, pos + 6, t1, t2)
        schritt(sp3, 0, ges_schritte, pos + 7, t1, t2)

    if halten == 1:
        sp4.ChangeFrequency(freq_halten)
        sp4.ChangeDutyCycle(cyc)
    else:
        sp4.ChangeDutyCycle(0)

def rueckwaerts(motor, sp1, sp2, sp3, sp4, schritte, t1, t2, halten):
    global pos_m1, pos_m2, pos_m3
    ges_schritte = schritte * 8

    if halten == 1:
        sp4.ChangeFrequency(freq)

    for i in range(int(schritte)):
        if motor == 'm1':
            if GPIO.input(Endschalter_1) == GPIO.LOW:
                print("Motor1 Endschalter")
                pos_m1 = 0
                break
            else:
                pos_m1 = pos_m1 - 1

        if motor == 'm2':
            if GPIO.input(Endschalter_2) == GPIO.LOW:
                print("Motor2 Endschalter")
                pos_m2 = 0
                break
            else:
                pos_m2 = pos_m2 - 1

        if motor == 'm3':
            if GPIO.input(Endschalter_3) == GPIO.LOW:
                print("Motor3 Endschalter")
                pos_m3 = 0
                break
            else:
                pos_m3 = pos_m3 - 1

        pos = i * 8
        schritt(sp3, 1, ges_schritte, pos, t1, t2)
        schritt(sp4, 0, ges_schritte, pos + 1, t1, t2)
        schritt(sp2, 1, ges_schritte, pos + 2, t1, t2)
        schritt(sp3, 0, ges_schritte, pos + 3, t1, t2)
        schritt(sp1, 1, ges_schritte, pos + 4, t1, t2)
        schritt(sp2, 0, ges_schritte, pos + 5, t1, t2)
        schritt(sp4, 1, ges_schritte, pos + 6, t1, t2)
        schritt(sp1, 0, ges_schritte, pos + 7, t1, t2)

    if halten == 1:
        sp4.ChangeFrequency(freq_halten)
        sp4.ChangeDutyCycle(cyc)
    else:
        sp4.ChangeDutyCycle(0)

def initMotor(motor):
    global pos_m1, pos_m2, pos_m3

    if motor == 'm1':
        print('Motor1 eichen')
        if GPIO.input(Endschalter_1) == GPIO.LOW:
            vorwaerts('m1', pwm1_m1, pwm2_m1, pwm3_m1, pwm4_m1, 10, te_m1, te_m1, 1)

        rueckwaerts('m1', pwm1_m1, pwm2_m1, pwm3_m1, pwm4_m1, schritte_m1, te_m1, te_m1, 1)
        pos_m1 = 0

    if motor == 'm2':
        print('Motor2 eichen')
        if GPIO.input(Endschalter_2) == GPIO.LOW:
            vorwaerts('m2', pwm1_m2, pwm2_m2, pwm3_m2, pwm4_m2, 10, te_m2, te_m2, 1)

        rueckwaerts('m2', pwm1_m2, pwm2_m2, pwm3_m2, pwm4_m2, schritte_m2, te_m2, te_m2, 1)
        pos_m2 = 0

    if motor == 'm3':
        print('Motor3 eichen')
        if GPIO.input(Endschalter_3) == GPIO.LOW:
            vorwaerts('m3', pwm1_m3, pwm2_m3, pwm3_m3, pwm4_m3, 10, te_m3, te_m3, 0)

        rueckwaerts('m3', pwm1_m3, pwm2_m3, pwm3_m3, pwm4_m3, schritte_m3, te_m3, te_m3, 0)
        pos_m3 = 0

class MoveV(threading.Thread):
  def __init__(self, motor, sp1, sp2, sp3, sp4, schritte, delay1, delay2, halten):
    threading.Thread.__init__(self)
    self.motor = motor
    self.sp1 = sp1
    self.sp2 = sp2
    self.sp3 = sp3
    self.sp4 = sp4
    self.schritte = schritte
    self.delay1 = delay1
    self.delay2 = delay2
    self.halten = halten
    self.daemon = True
    self.start()

  def run(self):
    vorwaerts(self.motor, self.sp1, self.sp2, self.sp3, self.sp4, self.schritte, self.delay1, self.delay2, self.halten)

class MoveR(threading.Thread):
  def __init__(self, motor, sp1, sp2, sp3, sp4, schritte, delay1, delay2, halten):
    threading.Thread.__init__(self)
    self.motor = motor
    self.sp1 = sp1
    self.sp2 = sp2
    self.sp3 = sp3
    self.sp4 = sp4
    self.schritte = schritte
    self.delay1 = delay1
    self.delay2 = delay2
    self.halten = halten
    self.daemon = True
    self.start()

  def run(self):
    rueckwaerts(self.motor, self.sp1, self.sp2, self.sp3, self.sp4, abs(self.schritte), self.delay1, self.delay2, self.halten)

#initMotor('m1')
print('start')
initMotor('m1')
initMotor('m2')
initMotor('m3')

try:
  while True:
    drehen = input("Drehen: ")
    hoehe = input("Hoehe: ")
    zoom = input("Zoom: ")
    #speed = raw_input("Speed: ")
    #f = raw_input("Frequenz: ")
    #c = raw_input("Zykluslaenge: ")

    #delay = int(speed)
    #freq = int(f)
    #cyc = int(c)

    steps_m1 = int(drehen) - pos_m1
    #print('Drehen: ' + str(steps_m1))

    steps_m2 = int(hoehe) - pos_m2
    #print('Hoehe: ' + str(steps_m2))

    steps_m3 = int(zoom) - pos_m3
    #print('Zoom: ' + str(steps_m3))

    if int(steps_m1) > 0:
      t1 = MoveV('m1', pwm1_m1, pwm2_m1, pwm3_m1, pwm4_m1, int(steps_m1), t1_m1, t2_m1, 1)
    else:
      t1 = MoveR('m1', pwm1_m1, pwm2_m1, pwm3_m1, pwm4_m1, int(steps_m1), t1_m1, t2_m1, 1)

    if int(steps_m2) > 0:
      t2 = MoveV('m2', pwm1_m2, pwm2_m2, pwm3_m2, pwm4_m2, int(steps_m2), t1_m2, t2_m2, 1)
    else:
      t2 = MoveR('m2', pwm1_m2, pwm2_m2, pwm3_m2, pwm4_m2, int(steps_m2), t1_m2, t2_m2, 1)

    if int(steps_m3) > 0:
      t3 = MoveV('m3', pwm1_m3, pwm2_m3, pwm3_m3, pwm4_m3, int(steps_m3), t1_m3, t2_m3, 0)
    else:
      t3 = MoveR('m3', pwm1_m3, pwm2_m3, pwm3_m3, pwm4_m3, int(steps_m3), t1_m3, t2_m3, 0)

    t1.join()
    t2.join()
    t3.join()

except KeyboardInterrupt:
  print("Ctl C pressed - ending program")

#pwm.stop()
GPIO.cleanup()


#fertig

#if __name__ == '__main__':
#  while True:
#    test = raw_input('Test')