import numpy as np              #biblioteka macierzy
import math
import RPi.GPIO as GPIO
from time import *

def degtorad(stopnie):
    return stopnie*math.pi/180

def radtodeg(stopnie):
    return stopnie*180/math.pi

def degtopercent2(stopnie):
    return stopnie/18+0.025

class NotacjaDH:

    def __init__(self,fi,d,a,alfa):
        self.fi = degtorad(fi)
        self.d = d
        self.a = a
        self.alfa = degtorad(alfa)

        self.rotZ()
        self.tranZ()
        self.tranX()
        self.rotX()
        self.A = (((self.rZ).dot(self.tZ)).dot(self.tX)).dot(self.rX)

    def rotZ(self):
        self.rZ = np.array([[math.cos(self.fi),-math.sin(self.fi),0,0],[math.sin(self.fi), math.cos(self.fi),0,0],[0,0,1,0],[0,0,0,1]])

    def tranZ(self):
        self.tZ = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,self.d],[0,0,0,1]])

    def tranX(self):
        self.tX = np.array([[1,0,0,self.a],[0,1,0,0],[0,0,1,0],[0,0,0,1]])

    def rotX(self):
        self.rX = np.array([[1,0,0,0], [0,math.cos(self.alfa),-math.sin(self.alfa),0], [0,math.sin(self.alfa),math.cos(self.alfa),0], [0,0,0,1]])

class Kinematyka:

    def __init__(self, fi1, fi3, d3):
        self.fi1 = fi1
        self.fi2 = 120
        self.fi3 = fi3
        self.l1 = 40 #mm!
        self.l2 = 80 #mm!
        self.d3 = d3
        self.l4 = 100 #mm!
        self.kinematyka_prosta(self.fi1, self.fi3, self.d3)
        self.kinematyka_odwrotna(self.X, self.Y, self.Z)
        self.sterowanie(0)

    def kinematyka_prosta(self, fi1, fi3, d3):
        self.fi1 = fi1
        self.fi3 = fi3
        self.d3 = d3

        czlon1 = NotacjaDH(self.fi1, self.l1, 0, 90)
        czlon2 = NotacjaDH(self.fi2, 0, self.l2, 0)
        czlon3 = NotacjaDH(-(self.fi2-self.fi3), 0, self.d3+self.l4, 0)
        czlon4 = NotacjaDH(-90, 0, 0, -90)

        self.A = (((czlon1.A).dot(czlon2.A)).dot(czlon3.A)).dot(czlon4.A)
        self.X = self.A[0][3]
        self.Y = self.A[1][3]
        self.Z = self.A[2][3]

        print("X: %.3f" %self.X )
        print("Y: %.3f" %self.Y )
        print("Z: %.3f" %self.Z )

    def kinematyka_odwrotna(self,X,Y,Z):
        self.X = X
        self.Y = Y
        self.Z = Z

        self.fi1_mem = self.fi1     #kopia zapasowa wspolrzednych
        self.fi3_mem = self.fi3
        self.d3_mem = self.d3

        self.fi1 = math.atan(self.Y/self.X)
        a = self.l2*math.cos(degtorad(self.fi2))
        b = self.Z-self.l1-self.l2*math.sin(degtorad(self.fi2));
        self.fi3 = math.atan(self.X/(b*math.cos(self.fi1))-a/b)
        self.d3 = b/math.sin(self.fi3)-self.l4

        self.fi1 = radtodeg(self.fi1)
        self.fi3 = radtodeg(self.fi3)

        print("fi1: %.3f" %self.fi1)
        print("fi3: %.3f" %self.fi3)
        print("d3: %.3f" %self.d3)

    def sterowanie(self, channel):
        if channel != 0:    #przerwanie wywolane przyciskiem
            print("X: %.3f" %self.X )
            print("Y: %.3f" %self.Y )
            print("Z: %.3f" %self.Z )
            self.kinematyka_odwrotna(self.X, self.Y, self.Z)
            print("XDDD")
            self.kinematyka_prosta(self.fi1, self.fi3, self.d3)


        if check_border(self.fi1, self.fi3, self.d3) == -1: #przekroczono wartosci katow
            print("Przekroczono wartosci czlonow na przycisku nr ", channel)
            print("Nie wyslano sygnalow sterujacych")
            self.fi1 = self.fi1_mem         #rozwiazanie: sprawdzic z matlabem algorytm lub zapisywac w pamieci poprawne wspolrzedne XYZ zamiast katow xDD
            self.fi3 = self.fi3_mem
            self.d3 = self.d3_mem
            print("fi1: %.3f" %self.fi1)        #debug
            print("fi3: %.3f" %self.fi3)
            print("d3: %.3f" %self.d3)
            print("BLAD")
            self.kinematyka_prosta(self.fi1, self.fi3, self.d3) #tu jest blad
            self.kinematyka_odwrotna(self.X, self.Y, self.Z)

        print("Wyslano sygnaly sterujace")
        serwo1.ChangeDutyCycle(0)
        serwo2.ChangeDutyCycle(degtopercent2(self.fi3))
        serwo3.ChangeDutyCycle(0)
        #print("Wypelnienie dla fi3: %.4f" %degtopercent2(self.fi3))        #debug

#KONFIGURACJA WEJSC I WYJSC
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(4, GPIO.IN, pull_up_down=GPIO.PUD_UP)        #kierunek X up
GPIO.setup(17, GPIO.IN, pull_up_down=GPIO.PUD_UP)       #kierunek X down
GPIO.setup(27, GPIO.IN, pull_up_down=GPIO.PUD_UP)       #kierunek Y up
GPIO.setup(10, GPIO.IN, pull_up_down=GPIO.PUD_UP)       #kierunek Y down
GPIO.setup(9, GPIO.IN, pull_up_down=GPIO.PUD_UP)        #kierunek Z up
GPIO.setup(11, GPIO.IN, pull_up_down=GPIO.PUD_UP)       #kierunek Z down
GPIO.setup(16, GPIO.OUT)    #serwo1: 360 podstawa
GPIO.setup(20, GPIO.OUT)    #serwo2: ramie
GPIO.setup(21, GPIO.OUT)    #serwo3: 360 teleskop

serwo1 = GPIO.PWM(16, 50)
serwo2 = GPIO.PWM(20, 50)
serwo3 = GPIO.PWM(21, 50)

#PARAMETRY STARTOWE
fi1 = 30    #deg
fi3 = 45    #deg
d3 = 50     #mm
serwo1.start(0)     #domontowac potencjometr jednak
serwo2.start(degtopercent2(fi3))
serwo3.start(0)     #przetwornik polozenia

#WARUNKI KRANCOWE
fi1_min = 5     #deg
fi1_max = 175   #deg
fi3_min = 5     #deg
fi3_max = 175   #deg
d3_min = 40     #mm
d3_max = 90     #mm

def check_border(kat1,kat3,dlugosc3):
    if (kat1 < fi1_min) or (kat1 > fi1_max) or (kat3 < fi3_min) or (kat3 > fi3_max) or (dlugosc3 < d3_min) or (dlugosc3 > d3_max):
        return -1       #przekroczenie wartosci
    else: return 1      #wartosc poprawna


#PARAMETRY NARASTANIA WSPOLRZEDNYCH
dx = 10     #mm
dy = 10     #mm
dz = 5      #mm
dt = 0.2    #s
btime = 200 #bouncetime ms

#OBSLUGA PRZERWAN OD PRZYCISKOW
def callback_upX(channel):
    while GPIO.input(channel) == GPIO.LOW:
        print("\nRuch X+")
        chwytak.X += dx
        chwytak.sterowanie(channel)
        sleep(dt)

def callback_downX(channel):
    while GPIO.input(channel) == GPIO.LOW:
        print("\nRuch X-")
        chwytak.X -= dx
        chwytak.sterowanie(channel)
        sleep(dt)

def callback_upY(channel):
    while GPIO.input(channel) == GPIO.LOW:
        print("\nRuch Y+")
        chwytak.Y += dy
        chwytak.sterowanie(channel)
        sleep(dt)

def callback_downY(channel):
    while GPIO.input(channel) == GPIO.LOW:
        print("\nRuch Y-")
        chwytak.Y -= dy
        chwytak.sterowanie(channel)
        sleep(dt)

def callback_upZ(channel):
    while GPIO.input(channel) == GPIO.LOW:
        print("\nRuch Z+")
        chwytak.Z += dz
        chwytak.sterowanie(channel)
        sleep(dt)

def callback_downZ(channel):
    while GPIO.input(channel) == GPIO.LOW:
        print("\nRuch Z-")
        chwytak.Z += dz
        chwytak.sterowanie(channel)
        sleep(dt)

#KONFIGURACJA PRZERWAN
GPIO.add_event_detect(4, GPIO.FALLING, callback=callback_upX, bouncetime=btime)
GPIO.add_event_detect(17, GPIO.FALLING, callback=callback_downX, bouncetime=btime)
GPIO.add_event_detect(27, GPIO.FALLING, callback=callback_upY, bouncetime=btime)
GPIO.add_event_detect(10, GPIO.FALLING, callback=callback_downY, bouncetime=btime)
GPIO.add_event_detect(9, GPIO.FALLING, callback=callback_upZ, bouncetime=btime)
GPIO.add_event_detect(11, GPIO.FALLING, callback=callback_downZ, bouncetime=btime)


#POCZATEK WYKONYWANEGO PROGRAMU
print("\nPozycja startowa. Na podstawie wymiarow geometrycznych obliczane jest polozenie efektora (kinematyka prosta) a nastepnie sprawdzana jest poprawnosc obliczen  (kinematyka odwrotna). \n")
print("START:")
chwytak = Kinematyka(fi1,fi3,d3)
sleep(1)
print("\nNacisnij przycisk aby poruszac manipulatorem.")

#PETLA GLOWNA PROGRAMU
while True:

    sleep(2)




GPIO.cleanup()