#!/usr/bin/env python3

import serial
import time
from termcolor import colored
from typing import Union
try:
    import gnureadline as readline
except ImportError:
    import readline

readline.parse_and_bind('tab: complete')
baud=9600 # We override Arduino/libraries/grbl/config.h to change to 9600
# because that's the default of the bluetooth module

try:
    s = serial.Serial('/dev/ttyUSB0',baud)
    print("Connected to /dev/ttyUSB0")
except:
    s = serial.Serial('/dev/ttyUSB1',baud)
    print("Connected to /dev/ttyUSB1")

# Wake up grbl
s.write(b"\r\n\r\n")
time.sleep(2)   # Wait for grbl to initialize
s.flushInput()  # Flush startup text in serial input

def readLineFromSerial():
    grbl_out: bytes = s.readline() # Wait for grbl response with carriage return
    print(colored(grbl_out.strip().decode('latin1'), 'green'))

def readAtLeastOneLineFromSerial():
    readLineFromSerial()
    while (s.inWaiting() > 0):
        readLineFromSerial()

def runCommand(cmd: Union[str, bytes]):
    if isinstance(cmd, str):
        cmd = cmd.encode('latin1')
    cmd = cmd.strip() # Strip all EOL characters for consistency
    print('>', cmd.decode('latin1'))
    s.write(cmd + b'\n') # Send g-code block to grbl
    readAtLeastOneLineFromSerial()

motor_angle: float = 0.0
MICROSTEPS: int = 16
YSCALE: float = 1000.0

def sign(x: float):
    return 1 if x >= 0 else -1

def motorYDeltaAngleToValue(delta_angle: float):
    return delta_angle / YSCALE

def motorXLengthToValue(delta_x: float):
    return delta_x

def rotateMotorY_noFeed(new_angle: float):
    global motor_angle
    delta_angle = new_angle - motor_angle
    runCommand(f"G1 Y{motorYDeltaAngleToValue(delta_angle):.3f}")
    motor_angle = new_angle

def rotateMotorY_feed(new_angle: float):
    global motor_angle
    delta_angle = new_angle - motor_angle
    motor_angle = new_angle
    Y = motorYDeltaAngleToValue(delta_angle)

    wire_bend_angle = 30 # fixme
    bend_radius = 3
    wire_length_needed = 3.1415 * bend_radius * bend_radius * wire_bend_angle / 360
    X = motorXLengthToValue(wire_length_needed)
    runCommand(f"G1 X{X:.3f} Y{Y:.3f}")

def rotateMotorY(new_angle: float):
    print(colored(f'{motor_angle}°→{new_angle}°', 'cyan'))
    if new_angle == motor_angle:
        return

    if sign(new_angle) != sign(motor_angle):
        # We are switching from one side to the other side.
        if abs(motor_angle) > 45:
            # First step is to move to 45 on the initial side, feeding the wire
            rotateMotorY_feed(sign(motor_angle) * 45)
        if abs(new_angle) > 45:
            rotateMotorY_noFeed(sign(new_angle) * 45)
            rotateMotorY_feed(new_angle)
        else:
            rotateMotorY_noFeed(new_angle)
    else:
        if abs(motor_angle) < 45 and abs(new_angle) < 45:
            # both start and end are less than 45, so no feeding needed
            rotateMotorY_noFeed(new_angle)
        elif abs(motor_angle) < 45:
            rotateMotorY_noFeed(sign(motor_angle) * 45)
            rotateMotorY_feed(new_angle)
        elif abs(new_angle) < 45:
            rotateMotorY_feed(sign(motor_angle) * 45)
            rotateMotorY_noFeed(new_angle)
        else: # both new and old angle are >45, so feed
            rotateMotorY_feed(new_angle)

def feed(delta_x: float):
    X = motorXLengthToValue(delta_x)
    runCommand(f"G1 X{X:.3f}")

def zigzag():
    for i in range(3):
        rotateMotorY(130)
        rotateMotorY(60)
        feed(5)
        rotateMotorY(0)
        feed(5)
        rotateMotorY(-130)
        rotateMotorY(-60)
        feed(5)
        rotateMotorY(0)
        feed(5)

def s_shape():
    for i in range(6):
        rotateMotorY(120)
        rotateMotorY(45)
    rotateMotorY(-130)
    for i in range(6):
        rotateMotorY(-120)
        rotateMotorY(-45)
    rotateMotorY(0)
    feed(20)

def paperclip():
    rotateMotorY(120)
    feed(1)
    rotateMotorY(130)
    rotateMotorY(140)

    rotateMotorY(30)
    feed(3)
    rotateMotorY(140)
    rotateMotorY(45)
    feed(4)
    feed(10)
    rotateMotorY(140)
    rotateMotorY(45)
    feed(3)
    rotateMotorY(140)
    rotateMotorY(50)
    rotateMotorY(150)
    rotateMotorY(45)
    feed(5)
    rotateMotorY(0)

runCommand('F32000') # Feed rate - affects X and Y
runCommand('G91')
runCommand('G21')  # millimeters
runCommand(f'$100={6.4375 * MICROSTEPS}') # Number of steps per mm for X
runCommand(f'$101={YSCALE * 0.5555 * MICROSTEPS}') # Number of steps per YSCALE degrees for Y
runCommand('?')
#rotateMotorY(-90)
#paperclip()
while True:
    line = input('> ("stop" to quit): ').upper()
    if line == 'STOP':
        break
    if len(line) == 0:
        continue
    cmd = line[0]
    if cmd == 'R':
        val = int(line[1:])
        rotateMotorY(val)
    elif cmd == 'F':
        val = int(line[1:])
        feed(val)
    else:
        runCommand(line)

runCommand('G4P0') # Wait for pending commands to finish
runCommand('?')

s.close()