from src.einstellungen import *
import time
import pigpio
import threading
import math

pi = pigpio.pi()

class motor():

    def __init__(self, name, sp1, sp2, sp3, sp4, t1, t2, te, es, halten, schritte):
        self.name = name
        self.sp1 = sp1
        self.sp2 = sp2
        self.sp3 = sp3
        self.sp4 = sp4
        self.t1 = t1
        self.t2 = t2
        self.te = te
        self.endschalter = es
        self.halten = halten
        self.schritte_max = schritte
        self.pos = 0
        self.freq = freq
        self.freq_halten = freq_halten
        self.cyc_halten = cyc_halten
        self.aktiv = 0
    
        pi.set_mode(self.sp1, pigpio.OUTPUT)
        pi.set_mode(self.sp2, pigpio.OUTPUT)
        pi.set_mode(self.sp3, pigpio.OUTPUT)
        pi.set_mode(self.sp4, pigpio.OUTPUT)

        pi.set_PWM_range(self.sp1, 100)
        pi.set_PWM_range(self.sp2, 100)
        pi.set_PWM_range(self.sp3, 100)
        pi.set_PWM_range(self.sp4, 100)

        pi.set_PWM_frequency(self.sp1, self.freq)
        pi.set_PWM_dutycycle(self.sp1, 0)

        pi.set_PWM_frequency(self.sp2, self.freq)
        pi.set_PWM_dutycycle(self.sp2, 0)
        
        pi.set_PWM_frequency(self.sp3, self.freq)
        pi.set_PWM_dutycycle(self.sp3, 0)

        if self.halten == 1:
            pi.set_PWM_frequency(self.sp4, self.freq_halten)
            pi.set_PWM_dutycycle(self.sp4, cyc_halten)
        else:
            pi.set_PWM_frequency(self.sp4, self.freq)
            pi.set_PWM_dutycycle(self.sp4, 0)

    def __vorwaerts(self, schritte, eichen=0):
        self.aktiv = 1
        ges_schritte = schritte * 8

        if eichen == 1:
            temp_t1 = self.te
            temp_t2 = self.te
        else:
            temp_t1 = self.t1
            temp_t2 = self.t2
       
        if self.halten == 1:
            pi.set_PWM_frequency(self.sp4, self.freq)
            
        for i in range(int(schritte)):
            if self.pos >= self.schritte_max:
                self.aktiv = 0
                break
            else:
                self.pos += 1

            temp_pos = i * 8
            einzelschritt(self.sp1, 1, ges_schritte, temp_pos, temp_t1, temp_t2)
            einzelschritt(self.sp4, 0, ges_schritte, temp_pos + 1, temp_t1, temp_t2)
            einzelschritt(self.sp2, 1, ges_schritte, temp_pos + 2, temp_t1, temp_t2)
            einzelschritt(self.sp1, 0, ges_schritte, temp_pos + 3, temp_t1, temp_t2)
            einzelschritt(self.sp3, 1, ges_schritte, temp_pos + 4, temp_t1, temp_t2)
            einzelschritt(self.sp2, 0, ges_schritte, temp_pos + 5, temp_t1, temp_t2)
            einzelschritt(self.sp4, 1, ges_schritte, temp_pos + 6, temp_t1, temp_t2)
            einzelschritt(self.sp3, 0, ges_schritte, temp_pos + 7, temp_t1, temp_t2)

        if self.halten == 1:
            pi.set_PWM_frequency(self.sp4, self.freq_halten)
            pi.set_PWM_dutycycle(self.sp4, cyc_halten)
        else:
            pi.set_PWM_dutycycle(self.sp4, 0)

        self.aktiv = 0

    def __rueckwaerts(self, schritte, eichen=0):
        self.aktiv = 1
        ges_schritte = schritte * 8

        if eichen == 1:
            temp_t1 = self.te
            temp_t2 = self.te
        else:
            temp_t1 = self.t1
            temp_t2 = self.t2

        if self.halten == 1:
            pi.set_PWM_frequency(self.sp4, self.freq)
            
        for i in range(int(schritte)):
            if pi.read(self.endschalter) == 0:
                print('Endschalter ' + self.name)
                self.pos = 0
                self.aktiv = 0
                break
            else:
                self.pos -= 1

            temp_pos = i * 8
            einzelschritt(self.sp3, 1, ges_schritte, temp_pos, temp_t1, temp_t2)
            einzelschritt(self.sp4, 0, ges_schritte, temp_pos + 1, temp_t1, temp_t2)
            einzelschritt(self.sp2, 1, ges_schritte, temp_pos + 2, temp_t1, temp_t2)
            einzelschritt(self.sp3, 0, ges_schritte, temp_pos + 3, temp_t1, temp_t2)
            einzelschritt(self.sp1, 1, ges_schritte, temp_pos + 4, temp_t1, temp_t2)
            einzelschritt(self.sp2, 0, ges_schritte, temp_pos + 5, temp_t1, temp_t2)
            einzelschritt(self.sp4, 1, ges_schritte, temp_pos + 6, temp_t1, temp_t2)
            einzelschritt(self.sp1, 0, ges_schritte, temp_pos + 7, temp_t1, temp_t2)

        if self.halten == 1:
            pi.set_PWM_frequency(self.sp4, self.freq_halten)
            pi.set_PWM_dutycycle(self.sp4, cyc_halten)
        else:
            pi.set_PWM_dutycycle(self.sp4, 0)
            
        self.aktiv = 0

    def eichen(self):
        if self.aktiv == 0:
            if pi.read(self.endschalter) == 0:
                self.vorwaerts(10, 1)

            self.rueckwaerts(self.schritte_max, 1)
            self.pos = 0

    def gehe_zu(self, position):
        if self.aktiv == 0:
            temp_schritte = int(position) - self.pos
            
            if temp_schritte > 0:
                self.vorwaerts(temp_schritte)
            else:
                self.rueckwaerts(abs(temp_schritte))

class motor_bewegen(threading.Thread):
  def __init__(self, motor, position):
    threading.Thread.__init__(self)
    self.motor = motor
    self.position = position
    self.start()

  def run(self):
    self.motor.gehe_zu(self.position)

def einzelschritt(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))

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