#!/usr/bin/env python3

import sys
import serial
import time
import logging
import readline

from ugv import UGVComms
import messages_pb2 as messages

log = logging.getLogger("ugv_cmd")

class UGV_CLI:
    def __init__(self):
        self.is_running = False
        self.last_state = messages.STATE_IDLE
        self.commands = {
            'help': self.help_msg,
            'h': self.help_msg,
            '?': self.help_msg,
            'exit': self.exit,
            'q': self.exit
        }
        pass


    def help_msg(self):
        print("""Commands:
help, h, ?:         Print this help message
exit, q, C-c, C-d:  Quit the program
""")

    def exit(self):
        self.is_running = False

    def disable(self):
        self.ugv.write_command(messages.CMD_DISABLE)

    def set_target(self):
        # TODO: parse arguments somehow
        cmd = messages.GroundCommand()
        cmd.type = messages.CMD_SET_TARGET
        cmd.target_location.latitude = 34.068415
        cmd.target_location.longitude = -118.443217
        self.ugv.write_command(cmd)

    def set_config(self):
        # TODO: read from config.yml
        cmd = messages.GroundCommand()
        cmd.type = messages.CMD_SET_CONFIG
        cmd.config.angle_pid.kp = 0.10
        cmd.config.angle_pid.ki = 0  # .00005
        cmd.config.angle_pid.kd = 0.4
        cmd.config.angle_pid.max_output = 0.5
        cmd.config.angle_pid.max_i_error = 15.0
        cmd.config.min_target_dist = 10.0
        cmd.config.min_flip_pitch = 90.0
        self.ugv.write_command(cmd)

    def drive_heading(self):
        # TODO: parse arguments somehow
        cmd = messages.GroundCommand()
        cmd.type = messages.CMD_DRIVE_HEADING
        cmd.drive_heading.heading = -115.0 - 180
        cmd.drive_heading.power = 0.3
        self.ugv.write_command(cmd)
        time.sleep(2.0)

    def start(self):
        self.is_running = True
        readline.parse_and_bind('tab: complete')
        #readline.parse_and_bind('set editing-mode vi')

        if len(sys.argv) >= 2:
            ser_url = sys.argv[1]
        else:
            ser_url = "loop://"
        ser = serial.serial_for_url(ser_url, baudrate=9600, parity=serial.PARITY_NONE,
                                    stopbits=serial.STOPBITS_ONE, bytesize=serial.EIGHTBITS,
                                    timeout=0.5)
        self.ugv = UGVComms(ser)
        self.ugv.start()
        time.sleep(0.2)
        try:
            while self.is_running:
                line = input("UGV> ")
                line_parts = line.split(' ')
                if len(line_parts) is 0:
                    continue
                try:
                    cmd = self.commands[line_parts[0]]
                except KeyError:
                    print("Unknown command: '%s'" % line_parts[0])
                    continue
                cmd()
            # TODO: continuously write state
            # while True:
            #    if self.ugv.last_status is None or self.ugv.last_status.state is not messages.STATE_DRIVE_HEADING:
        except (KeyboardInterrupt, EOFError):
            self.exit()
        finally:
            log.info("disabling UGV...")
            try:
                self.ugv.write_command(messages.CMD_DISABLE)
                log.info("done. exiting")
            except KeyboardInterrupt:
                log.info("force exiting...")
            self.ugv.stop()


if __name__ == "__main__":
    logging.basicConfig(format='%(asctime)s [%(name)s] %(levelname)s: %(message)s', datefmt='%Y-%b-%d %H:%M:%S')
    logging.getLogger().setLevel(logging.DEBUG)
    UGV_CLI().start()