'''
Matúš Kočka, 230269
Bakalárska Práca
Elektronické šahcy
'''

from machine import Pin
from utime import sleep_ms



'''________FOR MOTOR 1________'''
MRen = Pin(13, Pin.OUT,value=1)
MRstep = Pin(12,Pin.OUT)
MRdir = Pin(14, Pin.OUT)

'''________FOR MOTOR 2________'''
MLen = Pin(27, Pin.OUT,value=1)
MLstep = Pin(26,Pin.OUT)
MLdir = Pin(25, Pin.OUT)

'''________ELECTROMAGNET________'''
magnetPin = Pin(32,Pin.OUT)

STEPS_PER_REV = 200#400 # steps per one revolution (ms1 = High, ms2 = Low, ms3 = Low) -> Half steps
SPEED = 500 # [Hz] (steps per second)
T = int(1000 / (2 * SPEED)) # [ms] period to wait between each steps
PULLEY = 38.39 # [mm] Circumference
DISTANCE_PER_STEP = PULLEY/STEPS_PER_REV # distance per one half-step
SQUARE_DISTANCE = 38.3 / DISTANCE_PER_STEP # number of steps needed to go from one square to another horizontaly/verticaly
SQUARE_DISTANCE_DIAG = 77 / DISTANCE_PER_STEP # number of steps needed to go from one square to another diagonaly

class Mechanics:
    
    def __init__(self):
        self.fil = [0 for x in range(2)] 	#fil[0] = initial value of file; fil[1] = final value of file
        self.rank = [0 for x in range(2)]	#rank[0] = initial value of rank; rank[1] = final value of rank
        self.HomePos = [1 for x in range(2)] # constant home position
        self.ActualPos = self.HomePos #ActualPos[0] = actual position on x axis; ActualPos[1] = actual position on y axis
        self.TargetPos = [0 for x in range(2)] # TargetPos[0] = target position on x axis; TargetPos[1] = target position on y axis 
        self.IsMoving = False # True if one or both motors are moving
    
    # process info about a square from sunfishChessEngine search() method-
    # INPUT: sqr: move generated by search() method
    def render(self, sqr):
        y0,x0 = divmod(28 - sqr[0], 10)
        self.fil[0], self.rank[0] = x0+1, -y0+1
        y1,x1 = divmod(28 - sqr[1], 10)
        self.fil[1], self.rank[1] = x1+1, -y1+1
        
    # set next target position
    def setTarget(self, x, y):
        self.TargetPos[0] = x
        self.TargetPos[1] = y
        
    # set actual position
    def setActual(self, x,y):
        self.ActualPos[0] = x
        self.ActualPos[1] = y
    
    # move number of squares verticaly ^
    def MoveVertical(self, squares):
        steps = abs(squares) * SQUARE_DISTANCE
        if squares > 0 :
            MLdir.on()
            MRdir.off()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        elif squares < 0:
            MLdir.off()
            MRdir.on()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        else:
            pass
        self.IsMoving = False
        
        
    # move number of squares horizontaly <-->    
    def MoveHorizont(self, squares):
        steps = abs(squares) * SQUARE_DISTANCE
        if squares > 0 :
            MLdir.on()
            MRdir.on()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        elif squares < 0:
            MLdir.off()
            MRdir.off()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        else:
            pass
        self.IsMoving = False
    
    
    # move number of squares diagonally (4 possible ways)
    # Input : diff_x = delta in x axis (in squares)
    #         diff_y = delta in y axis (in squares)
    def MoveDiagonal(self, diff_x, diff_y):
        MRen.off()
        MLen.off()
        if (abs(diff_x) != abs(diff_y)):
            pass
        steps = abs(diff_x) * SQUARE_DISTANCE_DIAG
        
        if (diff_x < 0 and diff_y < 0):
            MLdir.off()
            self.IsMoving = True
            for i in range(0,steps):
                MLstep.on()
                sleep_ms(T)
                MLstep.off()
                sleep_ms(T)
        elif(diff_x > 0 and diff_y > 0):
            MLdir.on()
            self.IsMoving = True
            for i in range(0,steps):
                MLstep.on()
                sleep_ms(T)
                MLstep.off()
                sleep_ms(T)
        elif(diff_x > 0 and diff_y < 0):
            MRdir.on()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                sleep_ms(T)
                MRstep.off()
                sleep_ms(T)
        elif(diff_x < 0 and diff_y > 0):
            MRdir.off()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                sleep_ms(T)
                MRstep.off()
                sleep_ms(T)
        else:
            pass
        self.IsMoving = False
        MRen.on()
        MLen.on()
    
    
    # transfer inactive magnet to specified position
    def Transfer(self, toPos):
        MRen.off()
        MLen.off()
        diff_file = (toPos[0] - self.ActualPos[0])
        diff_rank = (toPos[1] - self.ActualPos[1])
        if (abs(diff_file) == abs(diff_rank)):
            self.MoveDiagonal(diff_file, diff_rank)
        else:
            self.MoveVertical(diff_rank)
            self.MoveHorizont(diff_file)
        self.setActual(toPos[0], toPos[1])
        MRen.on()
        MLen.on()
    
    # returns back to HomePosition
    def GoHome(self):
        MRen.off()
        MLen.off()
        diff_x = -(self.ActualPos[0]-1)
        diff_y = -(self.ActualPos[1]-1)
        self.MoveHorizont(diff_x)
        self.MoveVertical(diff_y)
        #self.ActualPos = self.HomePos
        self.setActual(1,1)
        MRen.on()
        MLen.on()
        
    def OffsetVertical(self, mmDist):
        steps = abs(mmDist) / DISTANCE_PER_STEP
        MRen.off()
        MLen.off()
        if mmDist > 0 :
            MLdir.on()
            MRdir.off()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        elif mmDist < 0:
            MLdir.off()
            MRdir.on()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        else:
            pass
        MRen.on()
        MLen.on()
        self.IsMoving = False
    
    
    def OffsetHorizontal(self, mmDist):
        steps = abs(mmDist) / DISTANCE_PER_STEP
        MRen.off()
        MLen.off()
        if mmDist > 0 :
            MLdir.on()
            MRdir.on()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        elif mmDist < 0:
            MLdir.off()
            MRdir.off()
            self.IsMoving = True
            for i in range(0,steps):
                MRstep.on()
                MLstep.on()
                sleep_ms(T)
                MRstep.off()
                MLstep.off()
                sleep_ms(T)
        else:
            pass
        MRen.on()
        MLen.on()
        self.IsMoving = False
    
    # transfers data from move generated by engine and tells-
    # motors what to do ...
    # Inputs: move - move generated by search() method
    #         capture - True if engine is capturing a piece, False if regular move
    #         castling - if True move is castling
    #         KnightMove - if True move is a KnightMove and need to prevent collision with other pieces
    def EngineMove(self, move, capture = False, KingMove = False, KnightMove = False):
        self.render(move)
        wait = 300
        offset = 20
        #TAKING A PIECE
        if(capture):
            self.setTarget(self.fil[1], self.rank[1])
            self.Transfer(self.TargetPos)
            sleep_ms(wait)
            magnetPin.on()
            if(self.rank[1] >= 5):
                self.OffsetVertical(-offset)
            else:
                self.OffsetVertical(offset)
                
            sleep_ms(wait)
            self.setTarget( 1, self.rank[1])
            self.Transfer(self.TargetPos)
            self.OffsetHorizontal(-2*offset)
            magnetPin.off()
            sleep_ms(wait)
            self.OffsetHorizontal(2*offset)
            if(self.rank[1] >= 5):
                self.OffsetVertical(offset)
            else:
                self.OffsetVertical(-offset)
            self.setActual(1 , self.rank[1])
        # NORMAL MOVE
        if (not KnightMove and not (KingMove and (abs(self.fil[1]-self.fil[0]) == 2 ))):
            self.setTarget(self.fil[0], self.rank[0])
            self.Transfer(self.TargetPos)
            magnetPin.on()
            sleep_ms(700)
            self.setTarget(self.fil[1], self.rank[1])
            self.Transfer(self.TargetPos)
            magnetPin.off()
            sleep_ms(500)
            self.GoHome()
        # KNIGHT IS MOVING (needs to go around other pieces)
        elif(KnightMove):
            self.setTarget(self.fil[0], self.rank[0])
            self.Transfer(self.TargetPos)
            sleep_ms(wait)
            magnetPin.on()
            #if knight moving two to the side and one up
            if ((abs(self.fil[1] - self.fil[0]) > abs(self.rank[1] - self.rank[0])) and self.rank[1]>self.rank[0]):
                self.OffsetVertical(offset)
                sleep_ms(wait)
                self.setTarget(self.fil[1], self.rank[0])
                self.Transfer(self.TargetPos)
                sleep_ms(wait)
                self.OffsetVertical(offset)
                sleep_ms(wait)
                self.setActual(self.fil[1], self.rank[1])
            #if knight moving two to the side and one down
            elif ((abs(self.fil[1] - self.fil[0]) > abs(self.rank[1] - self.rank[0])) and self.rank[1]<self.rank[0]):
                self.OffsetVertical(-offset)
                sleep_ms(wait)
                self.setTarget(self.fil[1], self.rank[0])
                self.Transfer(self.TargetPos)
                sleep_ms(wait)
                self.OffsetVertical(-offset)
                sleep_ms(wait)
                self.setActual(self.fil[1], self.rank[1])
            #if knight moving two vertically and one to the right
            elif ((abs(self.fil[1] - self.fil[0]) < abs(self.rank[1] - self.rank[0])) and self.fil[1]>self.fil[0]):
                self.OffsetHorizontal(offset)
                sleep_ms(wait)
                self.setTarget(self.fil[0], self.rank[1])
                self.Transfer(self.TargetPos)
                sleep_ms(wait)
                self.OffsetHorizontal(offset)
                sleep_ms(wait)
                self.setActual(self.fil[1], self.rank[1])
            #if knight moving two vertically and one to the left
            elif ((abs(self.fil[1] - self.fil[0]) < abs(self.rank[1] - self.rank[0])) and self.fil[1]<self.fil[0]):
                self.OffsetHorizontal(-offset)
                sleep_ms(wait)
                self.setTarget(self.fil[0], self.rank[1])
                self.Transfer(self.TargetPos)
                sleep_ms(wait)
                self.OffsetHorizontal(-offset)
                sleep_ms(wait)
                self.setActual(self.fil[1], self.rank[1])
            else: pass
            magnetPin.off()
            self.GoHome()
        #IF CASTLING    
        elif (KingMove and (abs(self.fil[1]-self.fil[0]) == 2 )):
            if (self.fil[1] > self.fil[0]):
                self.setTarget(self.fil[0], self.rank[0])
                self.Transfer(self.TargetPos)
                magnetPin.on()
                sleep_ms(wait)
                self.setTarget(self.fil[1], self.rank[1])
                self.Transfer(self.TargetPos)
                sleep_ms(wait)
                magnetPin.off()
                self.setTarget(8, 8)
                self.Transfer(self.TargetPos)
                magnetPin.on()
                sleep_ms(wait)
                self.OffsetVertical(-offset)
                self.setTarget(6, 8)
                self.Transfer(self.TargetPos)
                self.OffsetVertical(offset)
                sleep_ms(wait)
                magnetPin.off()
            elif (self.fil[1] < self.fil[0]):
                self.setTarget(self.fil[0], self.rank[0])
                self.Transfer(self.TargetPos)
                magnetPin.on()
                sleep_ms(wait)
                self.setTarget(self.fil[1], self.rank[1])
                self.Transfer(self.TargetPos)
                sleep_ms(wait)
                magnetPin.off()
                self.setTarget(1, 8)
                self.Transfer(self.TargetPos)
                magnetPin.on()
                sleep_ms(wait)
                self.OffsetVertical(-offset)
                self.setTarget(4, 8)
                self.Transfer(self.TargetPos)
                self.OffsetVertical(offset)
                sleep_ms(wait)
                magnetPin.off()
            else: pass
            self.GoHome()
                
                
                
            
'''SKUSKA

m = Mechanics()
while True:
    tmp = [0 for x in range(2)]
    tmp[0] = int(input("Zadaj file: "))
    tmp[1] = int(input("Zadaj rank: "))
    
    #m.EngineMove(tmp,KnightMove = False, capture = False, KingMove = False)
    m.Transfer(tmp)
    
    '''


