class Controller():
    def __init__(self, port="/dev/ttyACM1"):
        self.robot_api = RobotAPI(port,115200)


    def update(self, command):
        if command is None:
            return

        speed = command["speed"]
        direction = command["direction"]
        kick = command["kick"]

        log_time('dict_control')
        log('speed', speed, 'dict_control')
        log('direction', direction, 'dict_control')
        log('kick', kick, 'dict_control')

        if direction == "Forward":
            log_time('dict_control', 'time go forward called')
            if 'speedr' in command:
                speed_right = command["speedr"]
                self.robot_api.go_forward_asym(speed, speed_right)
            else:
                self.robot_api.go_forward(speed)

        elif (direction == "Backward"):
            self.robot_api.go_backward(speed)
        elif (direction == "Right"):
            self.robot_api.turn_right(speed)
        elif (direction == "Left"):
            self.robot_api.turn_left(speed)
        elif (direction == "None"):
            for i in range(0, 10):
                self.robot_api.stop()
                time.sleep(.001)

        if (kick == "Kick"):
            for i in range(0, 10):
                self.robot_api.kick(100)
                time.sleep(.001)
        elif (kick == "Prepare"):
            log_time("dict_control", "prepare")
            for i in range(0, 10):
                self.robot_api.prepare_catch()
                time.sleep(.001)
        elif (kick == "Catch"):
            for i in range (0, 10):
                self.robot_api.catch(100)
                time.sleep(.001)
def main():
     motor_speed = 100
     
     # Prompt for the path to the RF dongle
     serial_port = get_param("Enter path to serial port")

     robot = RobotAPI(serial_port, 115200)
     robot.enabled = True
     x = 0

     while x != ord('q'):
          screen.clear()
          screen.border(0)
          screen.addstr(1, 2, "SDP Group 6 - Robot Control System")
          screen.addstr(2, 2, "----------------------------------")
          screen.addstr(4, 4, "1 - Go Forward")
          screen.addstr(5, 4, "2 - Go Backward")
          screen.addstr(6, 4, "3 - Turn Right")
          screen.addstr(7, 4, "4 - Turn Left")
          screen.addstr(8, 4, "5 - Stop")
          screen.addstr(9, 4, "6 - Prepare for Catch")
          screen.addstr(10, 4, "7 - Catch")
          screen.addstr(11, 4, "8 - Kick")
          screen.addstr(12, 4, "9 - Go forward n centimeters")
          screen.addstr(13, 4, "0 - Go forward for n seconds")
          screen.addstr(14, 4, "a - Go backward n centimeters")
          screen.addstr(15, 4, "b - Set both motor speeds")

          screen.addstr(17, 4, "s - Set motor speed")
          screen.addstr(19, 4, "q - Quit")
          screen.addstr(21, 2, "Please pick an option...")
          screen.addstr(22, 2, "System Information")
          screen.addstr(23, 2, "------------------")
          screen.addstr(24, 2, "Serial Port: {0}".format(serial_port))
          screen.addstr(25, 2, "Motor Speed: {0}".format(motor_speed))
          screen.refresh()

          x = screen.getch()

          if x == ord('1'):
               robot.go_forward(speed=motor_speed)
          if x == ord('2'):
               robot.go_backward(speed=motor_speed)
          if x == ord('3'):
               robot.turn_right(speed=motor_speed)
          if x == ord('4'):
               robot.turn_left(speed=motor_speed)
          if x == ord('5'):
               robot.stop()
          if x == ord('6'):
               robot.prepare_catch()
          if x == ord('7'):
               robot.catch(speed=motor_speed)
          if x == ord('8'):
               robot.kick(speed=motor_speed)
          if x == ord('9'):
               num_cm = get_param("Enter distance in centimeters")
               robot.go_forward_for(int(num_cm))
          if x == ord('a'):
               num_cm = get_param("Enter distance in centimeters")
               robot.go_backward_for(int(num_cm))
          if x == ord('b'):
               left_speed = int(get_param("Enter speed for left motor"))
               right_speed = int(get_param("Enter speed for right motor"))

               robot.set_both_wheels(left_speed, right_speed)

          if x == ord('0'):
               num_seconds = get_param("Enter number of seconds to drive for")
               robot.forward_n_seconds(int(num_seconds))
          if x == ord('s'):
               motor_speed = int(get_param("Enter speed for motor from 1 to 100"))

     curses.endwin()