#! /usr/bin/python

# Test the GoPiGo3 robot
#   --P. Brooks, Stuyvesant High Scbool, May, 2018

import easygopigo3 as easy
gpg = easy.EasyGoPiGo3()
serv = gpg.init_servo()
dist = gpg.init_distance_sensor()

import time, math

Instructions = '''
==========================================
List of commands.  1 or 2 character commands, (plus space and number(s) for some).  Just pressing Enter shows this.
------------------------------------------
h = help (print this list of commands)
q = quit  (stops, closes eyes, resets)
s = Stop  [gpg.stop()]

di nn = drive nn inches  [gpg.drive_inches(distance)]
dd nn = turn nn degrees  [gpg.turn_degrees(angle)]

f = forward     [gpg.forward()]
b = backward    [gpg.backward()]

ss nn = set speed in degrees/sec                 [gpg.set_speed(dps)]
sl nn = set left motor speed in degrees/sec      [gpg.set_motor_dps(gpg.MOTOR_LEFT,dps)]
sr nn = set right motor speed in degrees/sec     [gpg.set_motor_dps(gpg.MOTOR_RIGHT,dps)]
enc = report left and right encoders             [gpg.read_encoders()]
renc = reset encoders to 0                       [gpg.reset_encoders()]

v = print voltage  [gpg.volt()]

dst = report distance to object    [dist.read_inches()]

sv = reset servo                  [serv.reset_servo() ]
svr nn = rotate servo nn degrees  [serv.rotate_servo(dps)]

eyel r g b = set left  eye LED colors   [gpg.set_left_eye_color((r,g,b))]
eyer r g b = set right eye LED colors   [gpg.set_right_eye_color((r,g,b))]
eyelo = open left eye                   [gpg.open_left_eye()]
eyero = open right eye                  [gpg.open_right_eye()]
eyelc = close left eye                  [gpg.close_left_eye()]
eyerc = close right eye                 [gpg.close_right_eye()]
'''

ActualCommands = (('h',0),('q',0),('s',0),('di',1),('dd',1),('f',0),('b',0),\
            ('ss',1),('sl',1),('sr',1),('enc',0),('renc',0),\
            ('v',0),('dst',0),('sv',0),('svr',1),\
            ('eyel',3),('eyer',3),('eyelo',0),('eyero',0),('eyelc',0),('eyerc',0))

            
def main():
    print (Instructions)
    while True:
        try:
            command = raw_input('\nCommand me: ').strip().lower()
            if command == '':
                print (Instructions)
            elif command == 'q':
                break
            else:
                ProcessCommand(command)
        except KeyboardInterrupt:
            break
    ResetAll()

def ResetAll():
    gpg.stop()
    gpg.reset_speed()
    gpg.reset_encoders()
    serv.reset_servo()
    gpg.close_eyes()
    print ('\nVoltage is now: %5.2f\nEnd test.\n' % gpg.volt())

def ProcessCommand(c):
    # test format
    fields = c.split()
    # check that all arguments are integers
    if len(fields) > 1:
        for args in fields[1:]:
            if not args.isdigit() and not (args[0] == '-' and args[1:].isdigit()):
                print ('All arguments for "%s" must be integers.  Command "%s" not understood.' % (fields[0],c))
                return
    
    found = False
    for act in ActualCommands:
        if fields[0] == act[0] and len(fields) == act[1] + 1:
            found = True
            break
    if not found:
        print ('Command: "%s" not recognized.  Use "h" for list of commands.' % c)
        return

    # decode
    if fields[0] == 'h':
        print (Instructions)
    elif fields[0] == 'v':
        print ('%5.2f' % gpg.volt())
    elif fields[0] == 's':
        gpg.stop()
    elif fields[0] == 'f':
        gpg.forward()
    elif fields[0] == 'b':
        gpg.backward()
    elif fields[0] == 'di':
        gpg.drive_inches(int(fields[1]))
    elif fields[0] == 'dd':
        gpg.turn_degrees(int(fields[1]))
    elif fields[0] == 'ss':
        gpg.set_speed(int(fields[1]))
    elif fields[0] == 'sl':
        gpg.set_motor_dps(gpg.MOTOR_LEFT,int(fields[1]))
    elif fields[0] == 'sr':
        gpg.set_motor_dps(gpg.MOTOR_RIGHT,int(fields[1]))
    elif fields[0] == 'enc':
        lt,rt = gpg.read_encoders()
        print('left = %d, right = %d' % (lt,rt))
    elif fields[0] == 'renc':
        gpg.reset_encoders()
    elif fields[0] == 'dst':
        print ('distance: %5.1f' % dist.read_inches())
    elif fields[0] == 'sv':
        serv.reset_servo()
    elif fields[0] == 'svr':
        serv.rotate_servo(int(fields[1]))
    elif fields[0] == 'eyel':
        gpg.set_left_eye_color((int(fields[1]),int(fields[2]),int(fields[3])))
    elif fields[0] == 'eyer':
        gpg.set_right_eye_color((int(fields[1]),int(fields[2]),int(fields[3])))
    elif fields[0] == 'eyelo':
        gpg.open_left_eye()
    elif fields[0] == 'eyero':
        gpg.open_right_eye()
    elif fields[0] == 'eyelc':
        gpg.close_left_eye()
    elif fields[0] == 'eyerc':
        gpg.close_right_eye()

    else:
        print('Command "%s" not recognized.  Use "h" for list of commands.' % c)

main()