'''
Pinout:
A4988-1:
Step = 17
Dir = 16

A4988-2:
Step = 5
Dir = 16

Rotary Encoder:
DT = 18
CLK = 19
SW = 21

E LCD2 = 15
'''

from machine import Pin
import time

class Port:
    def __init__(self, pin_nums):
        self.pins = [Pin(num, Pin.OUT) for num in pin_nums]

    def write(self, value):
        for i, p in enumerate(self.pins):
            p.value((value >> i) & 1)

E2 = Pin(15,Pin.OUT,value=0)

def LCD_cmd(num,RS,E,LCD_Port,cmd):
    RS.value(0)
    MSN = (cmd & 0xF0) >> 4
    LSN = cmd & 0x0F
    LCD_Port.write(MSN)
    if num==1:
        E.value(1)
        time.sleep(0.005)
        E.value(0)
    else:
        E2.value(1)
        time.sleep(0.005)
        E2.value(0)
    time.sleep(0.005)
    LCD_Port.write(LSN)
    if num==1:
        E.value(1)
        time.sleep(0.005)
        E.value(0)
    else:
        E2.value(1)
        time.sleep(0.005)
        E2.value(0)
    time.sleep(0.005)

def LCD_data(num,RS,E,LCD_Port,data):
    RS.value(1)
    MSN = (data & 0xF0) >> 4
    LSN = data & 0x0F
    LCD_Port.write(MSN)
    if num==1:
        E.value(1)
        time.sleep(0.005)
        E.value(0)
    else:
        E2.value(1)
        time.sleep(0.005)
        E2.value(0)
    time.sleep(0.005)
    LCD_Port.write(LSN)
    if num==1:
        E.value(1)
        time.sleep(0.005)
        E.value(0)
    else:
        E2.value(1)
        time.sleep(0.005)
        E2.value(0)
    time.sleep(0.005)

def LCD_init(num,RS,E,LCD_Port):
    LCD_cmd(num,RS,E,LCD_Port,0x32)
    LCD_cmd(num,RS,E,LCD_Port,0x0C)
    LCD_cmd(num,RS,E,LCD_Port,0x01)
    time.sleep(0.002)               # זמן מחיקה ארוך יותר
    LCD_cmd(num,RS,E,LCD_Port,0x06)

def LCD_Print(num,RS,E,LCD_Port,string):
    for val in string:
        LCD_data(num,RS,E,LCD_Port,ord(val))


def LCD_setCursor(num,RS,E,LCD_Port,x,y):
    if(y==1):
        LCD_cmd(num,RS,E,LCD_Port,0x80 | x)

    elif(y==2):
        LCD_cmd(num,RS,E,LCD_Port,0xC0 | x)

step1 = Pin(17,Pin.OUT,value=0)
step2 = Pin(5,Pin.OUT,value=0)
direction = Pin(16,Pin.OUT,value=0)

LCD_Port = Port([16, 4, 0, 2])  # D4, D5, D6, D7

RS = Pin(23,Pin.OUT, value=0)
E = Pin(22,Pin.OUT, value=0)

DT = Pin(18,Pin.IN)
CLK = Pin(19,Pin.IN)
SW = Pin(21,Pin.IN)

LCD_init(1,RS,E,LCD_Port)
LCD_setCursor(1,RS,E,LCD_Port,0,1)
LCD_Print(1,RS,E,LCD_Port,f'Count: 0')

LCD_init(2,RS,E,LCD_Port)
LCD_setCursor(2,RS,E,LCD_Port,0,1)
LCD_Print(2,RS,E,LCD_Port,f'Count: 0')

last_clk_val = 1

motor = 0
x = 0
y = 0

LCD_setCursor(1,RS,E,LCD_Port,0,2)
LCD_Print(1,RS,E,LCD_Port,f'angle = 0')
LCD_setCursor(2,RS,E,LCD_Port,0,2)
LCD_Print(2,RS,E,LCD_Port,f'angle = 0')

while True:
    if(SW.value() == 0):
        motor = not motor
        while(SW.value() == 0):
            time.sleep_ms(1)
    new_clk_val = CLK.value()
    if(not(new_clk_val == last_clk_val)):
        last_clk_val = new_clk_val
        dt_val = DT.value()
        if(new_clk_val == 0 and dt_val == 1):
            print('CW')
            direction.value(1)
            if(motor == 0):
                step1.value(1)
                time.sleep_ms(5)
                step1.value(0)
                x=x+1
            else:
                step2.value(1)
                time.sleep_ms(5)
                step2.value(0)
                y=y+1
        if(new_clk_val == 0 and dt_val == 0):
            print('CCW')
            direction.value(0)
            if(motor == 0):
                step1.value(1)
                time.sleep_ms(5)
                step1.value(0)
                x=x-1
            else:
                step2.value(1)
                time.sleep_ms(5)
                step2.value(0)
                y=y-1

        LCD_setCursor(1,RS,E,LCD_Port,0,1)
        LCD_Print(1,RS,E,LCD_Port,f'Count: {x}')
        LCD_setCursor(1,RS,E,LCD_Port,0,2)
        LCD_Print(1,RS,E,LCD_Port,f'angle = {x*1.8:.1f}')

        LCD_setCursor(2,RS,E,LCD_Port,0,1)
        LCD_Print(2,RS,E,LCD_Port,f'Count: {y}')
        LCD_setCursor(2,RS,E,LCD_Port,0,2)
        LCD_Print(2,RS,E,LCD_Port,f'angle = {y*1.8:.1f}')