from machine import Pin, SoftI2C, UART
import ssd1306
import time
import bluetooth
from BLE import BLEUART

# ESP32 Pin assignment 
i2c = SoftI2C(scl=Pin(22), sda=Pin(21))
b_up = Pin(18, Pin.IN, Pin.PULL_UP)
b_down = Pin(19, Pin.IN, Pin.PULL_UP)
b_left = Pin(23, Pin.IN, Pin.PULL_UP)
b_right = Pin(14, Pin.IN, Pin.PULL_UP)
b_mid = Pin(27, Pin.IN, Pin.PULL_UP)
b_set = Pin(26, Pin.IN, Pin.PULL_UP)
b_reset = Pin(25, Pin.IN, Pin.PULL_UP)
pin_buzzer = Pin(12, Pin.OUT)
pin_buzzer.value(False)
letra_x = 88
letra_y = 89
letra_z = 90
letra_e = 69
letra_c = 67
letra_space = 32
letra_minus = 45
#TIMELAPS VARIABLES
running_status = 0
COUNT = 0
frame_time = 0
PREV_TICKS = 0
MOV_X = 0.0
MOV_Y = 0.0
MOV_Z = 0.0
INC_X = 0.0
INC_Y = 0.0
INC_Z = 0.0

#VIDEO MODE VARIABLES
X_P = 0.0
Y_P = 0.0
Z_P = 0.0
E_P = 0.0
X_A = 0.0
Y_A = 0.0
Z_A = 0.0
E_A = 0.0
X_B = 0.0
Y_B = 0.0
Z_B = 0.0
E_B = 0.0
X_C = 0
Y_C = 0
Z_C = 0
E_C = 0
S_P = 3000
J_P = 1.0
MI_X = 'G0X'
MI_Y = 'G0Y'
MI_Z = 'G0Z'
MI_E = 'G0E'
MY = 'Y'
MZ = 'Z'
MEX = 'E'
MS =  'F'
ML = '\n'
ME = 'F300\n'
XM = ' X:'
YM = ' Y:'
ZM = ' Z:'
EM = ' E:'
SC = '*'
NSC = ' '
SM = 'SPD:'
JM = 'JOG:'
axis_status = 0
menu_status = 0

# INIT SERIAL TERMINAL NUMBER 2
uart = UART(2, 250000)
uart.init(250000, bits=8, parity=None, stop=1)
uart.read()
uart.write('M121\n')
uart.read()
uart.write('M302\n')
uart.read()
uart.write('M42P8S0\n')
uart.read()

# INIT BLUETOOTH
name = 'EDELCLONE'
ble = bluetooth.BLE()
uartble = BLEUART(ble, name)

UP_aux = False
DOWN_aux = False
RIGHT_aux = False
LEFT_aux = False
MID_aux = False
SET_aux = False
RESET_aux = False
valores = {"SAVE A" : [0],
           "SAVE B" : [1],
           "MOVE A" : [2],
           "MOVE B" : [3],
           "MOVE A-B" : [4],
           "MOVE A-B-A" : [5],
           "RESET ENDS" : [6],
           "PWR OFF MOTORS" : [7],
           "PHOTOS" : [8, 0, 5],
           "INTERVAL" : [9, 0, 1],
           "RUN TIMELAPSE" : [10],
           "BEEP" : [11, 1, 1],
           "PRE-DELAY" : [12, 0, 1],
           "UNITSX" : [13, 26, 1],
           "UNITSY" : [14, 26, 1],
           "UNITSZ" : [15, 6250, 1],
           "UNITSE" : [16, 80, 1],
           "ACCELX" : [17, 10, 1],
           "ACCELY" : [18, 10, 1],
           "ACCELZ" : [19, 1, 1],
           "ACCELE" : [20, 1, 1],
           "UPD UNITS" : [21],
           "UPD ACCEL" : [22],
           "SAVEPR" : [23]
           }

MAX_MENU = len(valores)-1
list_valores = list(valores.keys())
list_valores.sort(key = lambda x: valores[x][0])
last_shown = 3
menu_counter = 0

def on_rx():
    global valores
    global list_valores
    global UP_aux
    global DOWN_aux
    global RIGHT_aux
    global LEFT_aux
    global MID_aux
    global SET_aux
    global RESET_aux
    global menu_status
    global axis_status
    global S_P
    global J_P
    
    rx_buffer = uartble.read().decode().strip()
    
    uartble.write('EDELCLONE: ' + str(rx_buffer) + '\n')
    
    if rx_buffer == 'U':
        menu_status = 0
        axis_status = 0
        UP_aux = True
    elif rx_buffer == 'D':
        menu_status = 0
        axis_status = 0
        DOWN_aux = True
    elif rx_buffer == 'R':
        menu_status = 0
        axis_status = 0
        RIGHT_aux = True
    elif rx_buffer == 'L':
        menu_status = 0
        axis_status = 0
        LEFT_aux = True
    elif rx_buffer == 'SA':
        uartble.write('POS A Saved\n')
        activate_action("SAVE A")
    elif rx_buffer == 'SB':
        uartble.write('POS B Saved\n')
        activate_action("SAVE B")
    elif rx_buffer == 'A':
        uartble.write('MOVE TO A\n')
        activate_action("MOVE A")
    elif rx_buffer == 'B':
        uartble.write('MOVE TO B\n')
        activate_action("MOVE B")
    elif rx_buffer == 'AB':
        uartble.write('MOVE A TO B\n')
        activate_action("MOVE A-B")
    elif rx_buffer == 'ABA':
        uartble.write('MOVE A TO B TO A\n')
        activate_action("MOVE A-B-A")
    elif rx_buffer == '1':
        uartble.write('JOG:0.1\n')
        J_P = 0.1
    elif rx_buffer == '10':
        uartble.write('JOG:1.0\n')
        J_P = 1.0
    elif rx_buffer == '100':
        uartble.write('JOG:10.0\n')
        J_P = 10.0
    elif rx_buffer == 'RE':
        uartble.write('Reset Endstops\n')
        activate_action("RESET ENDS")
    elif rx_buffer == 'MO':
        uartble.write('Motor Power OFF\n')
        activate_action("PWR OFF MOTORS")
    elif rx_buffer == 'ZP':
        menu_status = 0
        axis_status = 1
        RIGHT_aux = True
    elif rx_buffer == 'ZM':
        menu_status = 0
        axis_status = 1
        LEFT_aux = True
    elif rx_buffer == 'FP':
        menu_status = 0
        axis_status = 1
        UP_aux = True
    elif rx_buffer == 'FM':
        menu_status = 0
        axis_status = 1
        DOWN_aux = True
    elif rx_buffer == 'SP':
        S_P = S_P + 100
        uartble.write('SPEED: ' + str(S_P) + '\n')
    elif rx_buffer == 'SM':
        S_P = S_P - 100
        uartble.write('SPEED: ' + str(S_P) + '\n')
    elif rx_buffer == '0':
        menu_status = 0
    updatedisplay()
        
uartble.irq(handler=on_rx)


oled_width = 128
oled_height = 64
oled = ssd1306.SSD1306_I2C(oled_width, oled_height, i2c)
oled.text('EDELCLONE', 0, 0)       
oled.show()
time.sleep(0.5)

def beep_sound():
    for x in range(1,50):
        pin_buzzer.value(True)
        time.sleep(0.001)
        pin_buzzer.value(False)
        time.sleep(0.001)
    time.sleep(0.1)    
    for x in range(1,50):
        pin_buzzer.value(True)
        time.sleep(0.001)
        pin_buzzer.value(False)
        time.sleep(0.001)

def activate_action(action):
    global X_A
    global Y_A
    global Z_A
    global E_A
    global X_B
    global Y_B
    global Z_B
    global E_B
    global X_P
    global Y_P
    global Z_P
    global E_P
    global menu_status
    global running_status
    global valores
    if action == "SAVE A":
        X_A = X_P
        Y_A = Y_P
        Z_A = Z_P
        E_A = E_P
        oled.fill(0)
        oled.show()
        oled.text('POS A SAVED', 0, 30)
        oled.show()
        time.sleep(1)
        menu_status = 0
    elif action == "SAVE B":
        X_B = X_P
        Y_B = Y_P
        Z_B = Z_P
        E_B = E_P
        oled.fill(0)
        oled.show()
        oled.text('POS B SAVED', 0, 30)
        oled.show()
        time.sleep(1)
        menu_status = 0
    elif action == "MOVE A":
        if valores["BEEP"][1] != 0:
            beep_sound()
        if valores["PRE-DELAY"][1] > 0:
            time.sleep(valores["PRE-DELAY"][1])
        ALLMSG = 'G0X' + str(X_A) + 'Y' + str(Y_A) + 'Z' + str(Z_A) + 'E' + str(E_A) + 'F' + str(S_P) + '\n'
        uart.write(ALLMSG)
        uart.read()
        X_P = X_A
        Y_P = Y_A
        Z_P = Z_A
        E_P = E_A
        
    elif action == "MOVE B":
        if valores["BEEP"][1] != 0:
            beep_sound()
        if valores["PRE-DELAY"][1] > 0:
            time.sleep(valores["PRE-DELAY"][1])
        ALLMSG = 'G0X' + str(X_B) + 'Y' + str(Y_B) + 'Z' + str(Z_B) + 'E' + str(E_B) + 'F' + str(S_P) + '\n'
        uart.write(ALLMSG)
        uart.read()
        X_P = X_B
        Y_P = Y_B
        Z_P = Z_B
        E_P = E_B
    
    elif action == "MOVE A-B":
        if valores["BEEP"][1] != 0:
            beep_sound()
        if valores["PRE-DELAY"][1] > 0:
            time.sleep(valores["PRE-DELAY"][1])
        ALLMSG = 'G0X' + str(X_A) + 'Y' + str(Y_A) + 'Z' + str(Z_A) + 'E' + str(E_A) + 'F' + str(S_P) + '\n'
        uart.write(ALLMSG)
        uart.read()
        time.sleep(1)
        ALLMSG = 'G0X' + str(X_B) + 'Y' + str(Y_B) + 'Z' + str(Z_B) + 'E' + str(E_B) + 'F' + str(S_P) + '\n'
        uart.write(ALLMSG)
        uart.read()
        X_P = X_B
        Y_P = Y_B
        Z_P = Z_B
        E_P = E_B
    
    elif action == "MOVE A-B-A":
        if valores["BEEP"][1] != 0:
            beep_sound()
        if valores["PRE-DELAY"][1] > 0:
            time.sleep(valores["PRE-DELAY"][1])
        ALLMSG = 'G0X' + str(X_A) + 'Y' + str(Y_A) + 'Z' + str(Z_A) + 'E' + str(E_A) + 'F' + str(S_P) + '\n'
        uart.write(ALLMSG)
        uart.read()
        time.sleep(1)
        ALLMSG = 'G0X' + str(X_B) + 'Y' + str(Y_B) + 'Z' + str(Z_B) + 'E' + str(E_B) + 'F' + str(S_P) + '\n'
        uart.write(ALLMSG)
        uart.read()
        time.sleep(1)
        ALLMSG = 'G0X' + str(X_A) + 'Y' + str(Y_A) + 'Z' + str(Z_A) + 'E' + str(E_A) + 'F' + str(S_P) + '\n'
        uart.write(ALLMSG)
        uart.read()
        X_P = X_A
        Y_P = Y_A
        Z_P = Z_A
        E_P = E_A
    
    elif action == "RESET ENDS":
        ALLMSG = 'M121\n'
        uart.write(ALLMSG)
        uart.read()
        menu_status = 0
        
    elif action == "PWR OFF MOTORS":
        ALLMSG = 'M18\n'
        uart.write(ALLMSG)
        uart.read()
        menu_status = 0
        
    elif action == "RUN TIMELAPSE":
        if running_status == 0:
            if valores["BEEP"][1] != 0:
                beep_sound()
            if valores["PRE-DELAY"][1] > 0:
                time.sleep(valores["PRE-DELAY"][1])
            running_status = 1
        else:
            running_status = 0
        menu_status = 0
    elif action == "UPD UNITS":
        ALLMSG = 'M92X' + str(valores["UNITSX"][1]) + 'Y' + str(valores["UNITSY"][1]) + 'Z' + str(valores["UNITSZ"][1]) + 'E' + str(valores["UNITSE"][1]) + '\n'
        uart.write(ALLMSG)
        uart.read()
        time.sleep(1)
    elif action == "UPD ACCEL":
        ALLMSG = 'M201X' + str(valores["ACCELX"][1]) + 'Y' + str(valores["ACCELY"][1]) + 'Z' + str(valores["ACCELZ"][1]) + 'E' + str(valores["ACCELE"][1]) + '\n'
        uart.write(ALLMSG)
        uart.read()
        time.sleep(1)
    elif action == "SAVEPR":
        ALLMSG = 'M500\n'
        uart.write(ALLMSG)
        uart.read()
        time.sleep(1)
        
def increment_value(valor):
    global valores
    try:
        valores[list_valores[valor]][1] = valores[list_valores[valor]][1] + valores[list_valores[valor]][2]
    except IndexError:
        pass
    
def decrement_value(valor):
    global valores
    try:
        valores[list_valores[valor]][1] = valores[list_valores[valor]][1] - valores[list_valores[valor]][2]
    except IndexError:
        pass

def convert(seconds):
    seconds = seconds % (24 * 3600)
    hour = seconds // 3600
    seconds %= 3600
    minutes = seconds // 60
    seconds %= 60
      
    return "%d:%02d:%02d" % (hour, minutes, seconds)

def updatedisplay():
    global valores
    global menu_counter
    global last_shown
    global COUNT
    oled.fill(0)
    oled.show()
    if menu_status == 0:
        if axis_status == 0:
            d_string = SC + XM + str(X_P) + YM + str (Y_P)
        else:
            d_string = NSC + XM + str(X_P) + YM + str (Y_P)
        oled.text(d_string, 0, 0)
        if axis_status == 1:
            d_string = SC + ZM + str(Z_P) + EM + str (E_P)
        else:
            d_string = NSC + ZM + str(Z_P) + EM + str (E_P)
        oled.text(d_string, 0, 10)
        if axis_status == 2:
            d_string = SC + SM + str(S_P) + JM + str (J_P)
        else:
            d_string = NSC + SM + str(S_P) + JM + str (J_P)
        oled.text(d_string, 0, 20)
        if running_status == 1 or valores["PHOTOS"][1] != 0.0:
            oled.text("PHOTOS:", 10, 30)
            oled.text(str(COUNT), 90, 30)
            oled.text("TIME:", 10, 40)
            TIME = convert(valores["PHOTOS"][1]*valores["INTERVAL"][1]) 
            oled.text(TIME, 60, 40)
            oled.text("RUN:", 10, 50)
            TIME = convert(COUNT*valores["INTERVAL"][1])
            oled.text(TIME, 60, 50)
        oled.show()
    if menu_status == 1:
        if menu_counter > last_shown:
            last_shown = menu_counter
        if menu_counter < last_shown - 3:
            last_shown = menu_counter + 3
        
        oled.text(list_valores[last_shown], 10, 30)
        try:
            oled.text(str(valores[list_valores[last_shown]][1]), 90, 30)
        except:
            pass
        oled.text(list_valores[last_shown-1], 10, 20)
        try:
            oled.text(str(valores[list_valores[last_shown-1]][1]), 90, 20)
        except:
            pass
        oled.text(list_valores[last_shown-2], 10, 10)
        try:
            oled.text(str(valores[list_valores[last_shown-2]][1]), 90, 10)
        except:
            pass
        oled.text(list_valores[last_shown-3], 10, 0)
        try:
            oled.text(str(valores[list_valores[last_shown-3]][1]), 90, 0)
        except:
            pass
        if menu_counter == last_shown:
            oled.text("*", 0, 30)
        elif menu_counter == last_shown-1:
            oled.text("*", 0, 20)
        elif menu_counter == last_shown-2:
            oled.text("*", 0, 10)
        elif menu_counter == last_shown-3:
            oled.text("*", 0, 0)
        oled.show()
        
        
def getposition():
    uart.write('M114\n')
    time.sleep(0.2)
    posicion = uart.read()
    pos_x = 0
    pos_y = 0
    pos_z = 0
    pos_e = 0
    pos_c = 0
    X_Pa = 0
    Y_Pa = 0
    Z_Pa = 0
    E_Pa = 0
    #print(posicion)
    if posicion is not None:
        for index in range(len(posicion)):
            if posicion[index] == letra_x:
                pos_x = index
            elif posicion[index] == letra_y:
                pos_y = index
            elif posicion[index] == letra_z:
                pos_z = index
            elif posicion[index] == letra_e:
                pos_e = index
            elif posicion[index] == letra_c:
                pos_c = index
                break
        posicion_xstr = ''
        posicion_ystr = ''
        posicion_zstr = ''
        posicion_estr = ''
        
        for index_x in range(pos_x+2,pos_y-1):
            posicion_xstr = posicion_xstr + chr(posicion[index_x])
        X_Pa = float(posicion_xstr)
    
        for index_y in range(pos_y+2,pos_z-1):
            posicion_ystr = posicion_ystr + chr(posicion[index_y])
        Y_Pa = float(posicion_ystr)
        for index_z in range(pos_z+2,pos_e-1):
            posicion_zstr = posicion_zstr + chr(posicion[index_z])
        Z_Pa = float(posicion_zstr)
        for index_e in range(pos_e+2,pos_c-1):
            posicion_estr = posicion_estr + chr(posicion[index_e])
        E_Pa = float(posicion_estr)
    return X_Pa,Y_Pa,Z_Pa,E_Pa
 
def timelapse_run():
    global COUNT
    global frame_time
    global MOV_X
    global MOV_Y
    global MOV_Z
    global INC_X
    global INC_Y
    global INC_Z
    global PREV_TICKS
    global running_status
    global valores
    if frame_time == 0:
        if X_B > X_A:
            MOV_X = X_B - X_A
        else:
            MOV_X = X_A - X_B
        if Y_B > Y_A:
            MOV_Y = Y_B - Y_A
        else:
            MOV_Y = Y_A - Y_B
        if Z_B > Z_A:
            MOV_Z = Z_B - Z_A
        else:
            MOV_Z = Z_A - Z_B
        INC_X = MOV_X/(valores["PHOTOS"][1]-1)
        INC_Y = MOV_Y/(valores["PHOTOS"][1]-1)
        INC_Z = MOV_Z/(valores["PHOTOS"][1]-1)
        #first shot
        COUNT = COUNT + 1
        shutter_release()
        INC_XR = COUNT * INC_X
        X_T = round(X_A + INC_XR,3)
        INC_YR = COUNT * INC_Y
        Y_T = round(Y_A + INC_YR,3)
        INC_ZR = COUNT * INC_Z
        Z_T = round(Z_A + INC_ZR,3)
        ALLMSG = 'G0E' + str(E_A) + 'F' + str(S_P) + 'X' + str(X_T) + 'Y' + str(Y_T) + 'Z' + str(Z_T) + '\n'
        #print(ALLMSG)
        uart.write(ALLMSG)
        uart.read()
        PREV_TICKS = time.ticks_ms()
        frame_time = 1
        
    elif frame_time == 1:
        if (time.ticks_ms() - PREV_TICKS) > (valores["INTERVAL"][1] * 1000):
            #disparar en la cama caliente
            shutter_release()
            COUNT = COUNT + 1
            INC_XR = COUNT * INC_X
            X_T = round(X_A + INC_XR,3)
            INC_YR = COUNT * INC_Y
            Y_T = round(Y_A + INC_YR,3)
            INC_ZR = COUNT * INC_Z
            Z_T = round(Z_A + INC_ZR,3)
            ALLMSG = 'G0E' + str(E_A) + 'F' + str(S_P) + 'X' + str(X_T) + 'Y' + str(Y_T) + 'Z' + str(Z_T) + '\n'
            #print(ALLMSG)
            uart.write(ALLMSG)
            uart.read()
            PREV_TICKS = time.ticks_ms()
            if COUNT > (valores["PHOTOS"][1]-2):
                frame_time = 2
    elif frame_time == 2:
        #timelapse finish
        if (time.ticks_ms() - PREV_TICKS) > (valores["INTERVAL"][1] * 1000):
            #disparar en la cama caliente
            shutter_release()
            frame_time = 0
            running_status = 0
            COUNT = 0
 
def shutter_release():
    shstring = 'M42 P8 S255\n'
    uart.write(shstring)
    uart.read()
    time.sleep(0.2)
    shstring = 'M42 P8 S0\n'
    uart.write(shstring)
    uart.read()
    #print('shoot')
    time.sleep(0.5)
    updatedisplay()
    

X_P,Y_P,Z_P,E_P = getposition()
updatedisplay()
            
while True:
    
    if running_status == 1:
        timelapse_run()
    
    if not b_up.value() or UP_aux == True:
        UP_aux = False
        if menu_status == 0:
            if axis_status == 0:
                Y_P = round(Y_P + J_P,1)
                ALLMSG = MI_Y + str(Y_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 1:
                E_P = round(E_P + J_P,1)
                ALLMSG = MI_E + str(E_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 2:
                if J_P == 1:
                    J_P = 10
                elif J_P == 10:
                    J_P = 0.1
                elif J_P == 0.1:
                    J_P = 1
                
        if menu_status == 1:
            menu_counter = menu_counter - 1
            if menu_counter < 0:
                menu_counter = 0
        
        updatedisplay()
        time.sleep(0.1)
        
    if not b_down.value() or DOWN_aux == True:
        DOWN_aux = False
        if menu_status == 0:
            if axis_status == 0:
                Y_P = round(Y_P - J_P,1)
                ALLMSG = MI_Y + str(Y_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 1:
                E_P = round(E_P - J_P,1)
                ALLMSG = MI_E + str(E_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 2:
                if J_P == 1:
                    J_P = 0.1
                elif J_P == 10:
                    J_P = 1
                elif J_P == 0.1:
                    J_P = 10
        if menu_status == 1:
            menu_counter = menu_counter + 1
            if menu_counter > MAX_MENU:
                menu_counter = MAX_MENU
        updatedisplay()
        time.sleep(0.1)
        
    if not b_right.value() or RIGHT_aux == True:
        RIGHT_aux = False
        if menu_status == 0:
            if axis_status == 0:
                X_P = round(X_P + J_P,1)
                ALLMSG = MI_X + str(X_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 1:
                Z_P = round(Z_P + J_P,1)
                ALLMSG = MI_Z + str(Z_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 2:
                S_P = S_P + 100
        if menu_status == 1:
            increment_value(menu_counter)
        updatedisplay()
        time.sleep(0.1)
        
    if not b_left.value() or LEFT_aux == True:
        LEFT_aux = False
        if menu_status == 0:
            if axis_status == 0:
                X_P = round(X_P - J_P,1)
                ALLMSG = MI_X + str(X_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 1:
                Z_P = round(Z_P - J_P,1)
                ALLMSG = MI_Z + str(Z_P) + ME
                uart.write(ALLMSG)
                uart.read()
            if axis_status == 2:
                S_P = S_P - 100
                if S_P < 0:
                    S_P = 0
        if menu_status == 1:
            decrement_value(menu_counter)
        updatedisplay()
        time.sleep(0.1)
        
    if not b_mid.value() or MID_aux == True:
        MID_aux = False
        if menu_status == 0:
            axis_status = axis_status + 1
            if axis_status > 2:
                axis_status = 0
        elif menu_status == 1:
            activate_action(list_valores[menu_counter])
        
        updatedisplay()
        time.sleep(0.2)
        
    if not b_reset.value() or RESET_aux == True:
        RESET_aux = False
        menu_status = menu_status + 1
        if menu_status > 1:
            menu_status = 1
        updatedisplay()
        time.sleep(0.2)
    
    if not b_set.value() or SET_aux == True:
        SET_aux = False
        menu_status = menu_status - 1
        if menu_status < 0:
            menu_status = 0
        if running_status == 1:
            running_status = 0
            COUNT = 0
            frame_time = 0
            PREV_TICKS = 0
            
        updatedisplay()
        time.sleep(0.2)