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

delay = 0.004
delay_y = 0.012
delay_z = 0.004
freq = 50000
cyc = 4
pos_m1 = 0
pos_m2 = 0
pos_m3 = 0
ausgleich_m3 = 1

schritte_m1 = 180
schritte_m2 = 120
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
pwm1a = GPIO.PWM(m1_coil_A_1_pin, freq)
pwm2a = GPIO.PWM(m2_coil_A_1_pin, freq)
pwm2b = GPIO.PWM(m2_coil_B_1_pin, freq)
#pwm1.start(0) # Initialisierung
#pwm2.start(0) # Initialisierung


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 setStep(motor, w1, w2, w3, w4):
    if motor == 'm1':
      GPIO.output(m1_coil_A_1_pin, w1)
      GPIO.output(m1_coil_B_1_pin, w2)
      GPIO.output(m1_coil_A_2_pin, w3)
      GPIO.output(m1_coil_B_2_pin, w4)
    if motor == 'm2':
      GPIO.output(m2_coil_A_1_pin, w1)
      GPIO.output(m2_coil_B_1_pin, w2)
      GPIO.output(m2_coil_A_2_pin, w3)
      GPIO.output(m2_coil_B_2_pin, w4)

    if motor == 'm3':
      GPIO.output(m3_coil_A_1_pin, w1)
      GPIO.output(m3_coil_B_1_pin, w2)
      GPIO.output(m3_coil_A_2_pin, w3)
      GPIO.output(m3_coil_B_2_pin, w4)


def forward(motor, delay, steps):
    global pos_m1, pos_m2, pos_m3
    global schritte_m1, schritte_m2, schritte_m3
    #d = int(delay) / 1000.0
    d = delay
    s = int(steps)
    for i in range(s):
        if motor == 'm1':
            if pos_m1 >= schritte_m1:
                print('Motor1 hat positives Ende erreicht')
                break
            else:
                pos_m1 = pos_m1 + 1
                #print('Motor1-Pos: ' + str(pos_m1))

        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
                #print('Motor3-Pos: ' + str(pos_m3))

            #time.sleep(0.01)

        for j in range(StepCount):
            setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
            time.sleep(d)

    if motor == 'm3':
      for i in range(int(ausgleich_m3)):
        for j in range(StepCount):
            setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
            time.sleep(d)


def backwards(motor, delay, steps):
    global pos_m1, pos_m2, pos_m3
    global schritte_m1, schritte_m2, schritte_m3
    #d = int(delay) / 1000.0
    d = delay
    s = int(steps)
    for i in range(s):
        if motor == 'm1':
            if GPIO.input(Endschalter_1) == GPIO.LOW:
                print("Motor1 Endschalter")
                pos_m1 = 0
                break
            else:
                pos_m1 = pos_m1 - 1
                #print('Motor1-Pos: ' + str(pos_m1))

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

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

        for j in reversed(range(StepCount)):
            setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
            time.sleep(d)

    if motor == 'm3':
      for i in range(int(ausgleich_m3)):
        for j in reversed(range(StepCount)):
            setStep(motor, Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
            time.sleep(d)

def stopMotor(motor, stop):
    if motor == 'm1':
      if stop == 0:
        pwm1a.stop()
      else:
        pwm1a.start(0)
        pwm1a.ChangeFrequency(freq)
        pwm1a.ChangeDutyCycle(cyc)


    if motor == 'm2':
      if stop == 0:
        pwm2a.stop()
        #pwm2b.stop()
      else:
        pwm2a.start(0)
        pwm2a.ChangeFrequency(freq)
        pwm2a.ChangeDutyCycle(cyc)
        #pwm2b.start(0)
        #pwm2b.ChangeFrequency(freq)
        #pwm2b.ChangeDutyCycle(cyc)

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

    if motor == 'm1':
        print('Motor1 eichen')
        if GPIO.input(Endschalter_1) == GPIO.LOW:
            forward('m1', delay, 10)

        backwards('m1', delay, schritte_m1)
        stopMotor('m1', 1)
        pos_m1 = 0

    if motor == 'm2':
        print('Motor2 eichen')
        if GPIO.input(Endschalter_2) == GPIO.LOW:
            forward('m2', delay_y, 10)

        backwards('m2', delay_y, schritte_m2)
        stopMotor('m2', 1)
        pos_m2 = 0

    if motor == 'm3':
        print('Motor3 eichen')
        if GPIO.input(Endschalter_3) == GPIO.LOW:
            forward('m3', delay_z, 10)

        backwards('m3', delay_z, schritte_m3)
        pos_m3 = 0

class MoveV(threading.Thread):
  def __init__(self, motor, schritte, delay, halten):
    threading.Thread.__init__(self)
    self.motor = motor
    self.schritte = schritte
    self.delay = delay
    self.halten = halten
    self.daemon = True
    self.start()

  def run(self):
    stopMotor(self.motor, 0)
    forward(self.motor, self.delay, self.schritte)
    setStep(self.motor, 0,0,0,0)

    if self.halten == 1:
      stopMotor(self.motor, 1)

class MoveR(threading.Thread):
  def __init__(self, motor, schritte, delay, halten):
    threading.Thread.__init__(self)
    self.motor = motor
    self.schritte = schritte
    self.delay = delay
    self.halten = halten
    self.daemon = True
    self.start()

  def run(self):
    stopMotor(self.motor, 0)
    backwards(self.motor, self.delay, abs(self.schritte))
    setStep(self.motor, 0,0,0,0)

    if self.halten == 1:
      stopMotor(self.motor, 1)

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

try:
  while True:
    drehen = raw_input("Drehen: ")
    hoehe = raw_input("Hoehe: ")
    zoom = raw_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', int(steps_m1), delay, 1)
    else:
      t1 = MoveR('m1', int(steps_m1), delay, 1)

    if int(steps_m2) > 0:
      t2 = MoveV('m2', int(steps_m2), delay_y, 1)
    else:
      t2 = MoveR('m2', int(steps_m2), delay_y, 1)

    if int(steps_m3) > 0:
      t3 = MoveV('m3', int(steps_m3), delay_z, 0)
    else:
      t3 = MoveR('m3', int(steps_m3), delay_z, 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')