#!/usr/bin/env python3

import serial, time, sys, re, os, argparse, logging
import config

logging.basicConfig(level=logging.INFO)
log = logging.getLogger(__name__)

f = serial.Serial(config.USB_DEVICE, 115200)
banner = f.readline()
log.info("Banner is '%s'" % (banner))

def command(cmd):
    i=1
    f.write(("#%d %s\n" % (i, cmd)).encode("ascii") )
    response = f.readline().decode('ascii').rstrip()
    m = re.match("\$(?P<token>\d+) (?P<result_code>OK|E\d+) ?(?P<value>.*)", response).groupdict()
    log.debug("Command '%s', response token '%s' result_code '%s' value '%s'" % (cmd, m['token'], m['result_code'], m.get('value', '*NONE*')))
    return m


# Reference: https://cdn.sparkfun.com/datasheets/Robotics/uArm_Vacuum_System_User_Guide.pdf
def pump(state):
    command("M231 V%d" % (state))

def valve(state):
    command("M240 N5 V%d" % (state))

def grab():
    valve(0)
    pump(1)

def release():
    pump(0)
    valve(1)


def getdigital(pin):
    m = command("P240 N%d" % (pin))
    if m['value'] == 'V0':
        return True
    else:
        return False

def limitsw():
    return getdigital(2)

def armpos(x,y,z,v=10):
    command("G0 X%.1f Y%.1f Z%.1f F%d" % (x,y,z,v))
    wait()

def armposrel(dx,dy,dz,v=0):
    command("G204 X%.1f Y%.1f Z%.1f F%d" % (dx,dy,dz,v))
    wait()

def device_name():
    command("P201")
    command("P202")
    command("P203")
    command("P204")
    command("P205")

def arm_reset():
    armposrel(0, 0, 100)
    armpos(0, 150, 100)

def wait():
    log.info("Waiting for move to end")
    while True:
        moving = command("M200")
        if moving['value'] == "V0":
            break
        time.sleep(0.1)

def grab_cd():
    log.info("Grabbing CD")

    z = 125
    while True:
        armposrel(0, 0, -0.5)

        z = z - 0.5
        if z < -20:
            arm_reset()
            log.fatal("Something is very wrong")

        if limitsw() is False:
            continue

        log.info("We got something")
        grab()
        time.sleep(1)

        armposrel(0,0,100)

        break

def pickup_new_cd():
    log.info("Moving to source tray")
    armpos(*config.SRC_TRAY_POS)
    grab_cd()

def return_cd():
    log.info("Returning CD to dump tray")
    armpos(*config.DUMP_TRAY_POS)

    release()
    time.sleep(2)

def drop_cd_in_drive():
    armpos(*config.TRAY_CD_POS)
    release();
    time.sleep(2)

def pickup_cd_from_drive():
    armpos(*config.TRAY_CD_POS)
    grab_cd()
    time.sleep(1)

if __name__ == "__main__":

    device_name()

    for i in range(int( sys.argv[1] )):

        os.system("eject %s" % (config.CD_DEVICE))

        pickup_new_cd()
        drop_cd_in_drive()

        tries = config.TRIES
        while tries > 0:
            if os.system("eject -t %s" % (config.CD_DEVICE)) > 0:
                log.warn("Tray close not successful, trying again")
                os.system("eject %s" % (config.CD_DEVICE))
                tries = tries - 1

                if tries == 0:
                    armpos(*tray_cd_pos)
                    grab_cd()

                    # Drop cd into *SOURCE* tray to pick it up again
                    log.info("Moving to source tray")
                    armpos(*config.SRC_TRAY_POS)
                    release()

                    arm_reset()

                    pickup_new_cd()
                    drop_cd_in_drive()

                    tries = config.TRIES

                    continue

            else:

                log.info("Now we are reading the CD")
                # os.system("cdrdao --device=%s disk-info")
                # os.system("dd if=%s of=dump.bin bs=10000000 count=1" % (config.CD_DEVICE))
                time.sleep(3)

                log.info("Finished reading CD")
                break

        os.system("eject %s" % (config.CD_DEVICE))

        pickup_cd_from_drive()
        return_cd()

    arm_reset()
    os.system("eject -t %s" % (config.CD_DEVICE))
    os.system("beep -f 1000 -n -f 2000 -n -f 1500")