import math
import serial
import RPi.GPIO as gpio
import time

ser = serial.Serial('/dev/ttyUSB0', 115200)
gpio.setmode(gpio.BCM)
gpio.setup(13, gpio.OUT)
pwm = gpio.PWM(13, 100)
pwm.start(13)
verbose = True

print("[ INFO ] Initializing Grbl...")
ser.write(b'\r\n\r\n')
time.sleep(2)
ser.write(b'$RST=#\n')
time.sleep(1)
ser.flushInput()
time.sleep(0.25)
ser.write(b'$X\n')
print("[ INFO ] Grbl is ready.")

def goToBin(bin):
    print("[ INFO ] Delivering item to bin: " + str(bin))
    adjustedBin = math.floor(bin / 2)
    if adjustedBin > 11:
        print("[ INFO ] All bins full! Using overflow bin.")
        bin = 0;
        adjustedBin = 0;
    distance = adjustedBin * 18
    delay = 0.5 + 0.93 * adjustedBin
    command = '$J=X-'
    command += str(distance)
    command += ' F2000'
    print("[ INFO ] Sending command to Grbl: " + command)
    command += '\n'
    time.sleep(0.25)
    ser.write(command.encode('utf-8'))
    # s.write("$C\n")
    while True:
        grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
        print(grbl_out)
        if int(grbl_out.find('error')) >= 0 :
            print("[ EXIT ] Grbl reported an error.")
            quit()
        elif int(grbl_out.find('ok')) >= 0 :
            if verbose: print('[ INFO ] Grbl message: ',grbl_out)
            break
    print("[ INFO ] Waiting for " + str(delay) + " seconds.")
    time.sleep(delay)
    if bin % 2 == 0: # tilt to left
        print("[ INFO ] Titling motor to left side.")
        pwm.ChangeDutyCycle(5)
        time.sleep(1)
        pwm.ChangeDutyCycle(14)
    else:
        print("[ INFO ] Titling motor to right side.")
        pwm.ChangeDutyCycle(25)
        time.sleep(1)
        pwm.ChangeDutyCycle(14)
    time.sleep(1)
    print("[ INFO ] Sending command to Grbl: G0 X0")
    ser.write(b'$j=X0 F2000\n')
    while True:
        grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
        if int(grbl_out.find('error')) >= 0 :
            print("[ EXIT ] Grbl reported an error.")
            quit()
        elif int(grbl_out.find('ok')) >= 0 :
            if verbose: print('[ INFO ] Grbl message: ',grbl_out)
            break
    print("[ INFO ] Waiting for " + str(delay) + " seconds.")
    time.sleep(delay)
def stopInput():
    command = '!'
    command += '\n'
    ser.write(command.encode('utf-8'))
    print(command)
    #command2 = str(0x85)
    #ser.write(command2.encode('utf-8'))
    #while True:
    #    grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
    #    if int(grbl_out.find('error')) >= 0 :
    #        print("[ EXIT ] Grbl reported an error.")
    #        quit()
    #    elif int(grbl_out.find('ok')) >= 0 :
    #        if verbose: print('[ INFO ] Grbl message: ',grbl_out)
    #        break
def startInput():
    ser.write(b'$J=G91 Y-5000 F400\n')
    print("intake")
    #x = 0
    #while True and x < 10:
        #x += 1
        #grbl_out = str(ser.readline().strip()) # Wait for grbl response with carriage return
        #if int(grbl_out.find('error')) >= 0 :
        #    print("[ EXIT ] Grbl reported an error.")
        #    quit()
        #elif int(grbl_out.find('ok')) >= 0 :
        #    if verbose: print('[ INFO ] Grbl message: ',grbl_out)
        #    break