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

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(1)
#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')


#for i in range(int(20)):
#    for j in range(StepCount):
#        setStep('m3', Seq[j][0], Seq[j][1], Seq[j][2], Seq[j][3])
#        time.sleep(0.02)

#setStep('m3', 0,0,0,0)

#time.sleep(2)

schritte = 40
schritte2 = (schritte * 8) - 1
t1 = 0.0003
t2 = 0.001
t_diff = t2 - t1


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(sp1, sp2, sp3, sp4, schritte, t1, t2):
    ges_schritte = schritte * 8
    for i in range(int(schritte)):
        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)

def rueckwaerts(sp1, sp2, sp3, sp4, schritte, t1, t2):
    ges_schritte = schritte * 8
    for i in range(int(schritte)):
        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)

pwm1x = GPIO.PWM(m2_coil_A_1_pin, freq)
pwm2x = GPIO.PWM(m2_coil_B_1_pin, freq)
pwm3x = GPIO.PWM(m2_coil_A_2_pin, freq)
pwm4x = GPIO.PWM(m2_coil_B_2_pin, freq)

f = 800

pwm1x.start(0)
pwm1x.ChangeFrequency(f)
pwm1x.ChangeDutyCycle(0)
pwm2x.start(0)
pwm2x.ChangeFrequency(f)
pwm2x.ChangeDutyCycle(0)
pwm3x.start(0)
pwm3x.ChangeFrequency(f)
pwm3x.ChangeDutyCycle(0)
pwm4x.start(0)
pwm4x.ChangeFrequency(f)
pwm4x.ChangeDutyCycle(100)

time.sleep(4)
vorwaerts(pwm1x, pwm2x, pwm3x, pwm4x, schritte, t1, t1)
time.sleep(2)
rueckwaerts(pwm1x, pwm2x, pwm3x, pwm4x, schritte, t2, t2)

pwm1x.stop()
pwm2x.stop()
pwm3x.stop()
pwm4x.stop()

#pwm.stop()
GPIO.cleanup()