"""CameraX Arduino box rotator controller.

Usage:
python rotator.py
  --ch <channel number of rotator, by default is 1>
  --dir <Rotate direction(ON: rotate, OFF: return to 0)>
  --debug
"""
import argparse
import logging
import time
import pyudev
import serial

ARDUINO_ANGLE_MAX = 180  # degrees
ARDUINO_BAUD_RATE = 9600
CMD_TEST = ['\x01', '\x02', '\x03']  # test hex command to send to Arduino
CMD_WORD_LENGTH = 3  # bytes
HS755HB_ANGLE_MAX = 202.0  # degrees
NUM_TRYS = 3  # number of tries to get serial port to communicate properly
START_BYTE_SERVO = 255


def get_cmdline_args():
  """Get command line arguments."""
  parser = argparse.ArgumentParser()
  parser.add_argument('--ch', help='--ch [1:6]', default=1, required=True,
                      type=int, choices=[1, 2, 3, 4, 5, 6])
  parser.add_argument('--dir', help='--dir OFF|ON', required=True, type=str,
                      choices=['ON', 'OFF'])
  parser.add_argument('--offset', help='--offset #', default=0, type=int)
  parser.add_argument(
      '--debug', help='--debug', default=False, action='store_true')
  args = parser.parse_args()

  # determine angle for rotation from ON|OFF direction
  angle = 0  # default OFF
  if args.dir == 'ON':
    angle = 90
  return args.ch, angle, args.offset, args.debug


def serial_port_def(name):
  """Determine the serial port and open.

  Args:
    name: Device to locate (ie. 'Arduino')

  Returns:
    Serial port object
  """
  devices = pyudev.Context()
  for device in devices.list_devices(subsystem='tty', ID_BUS='usb'):
    if name in device['ID_VENDOR']:
      arduino_port = device['DEVNAME']
      continue
  logging.info('Using port %s', arduino_port)
  return serial.Serial(arduino_port, ARDUINO_BAUD_RATE, timeout=1)


def read_command(port):
  """Read back command from serial port."""
  cmd = []
  for _ in range(CMD_WORD_LENGTH):
    cmd.append(port.read())
  return cmd


def send_command(port, cmd):
  """Send command to serial port."""
  for i in range(CMD_WORD_LENGTH):
    port.write(cmd[i])
  time.sleep(2.0 * CMD_WORD_LENGTH / ARDUINO_BAUD_RATE)  # round trip
  return read_command(port)


def establish_serial_comm(port):
  """Establish connection with serial port."""
  logging.info('Establishing communication with %s', port.name)
  attempts = 0
  hex_test = convert_to_hex(CMD_TEST)
  logging.debug(' test tx: %s %s %s', hex_test[0], hex_test[1], hex_test[2])
  while attempts < NUM_TRYS:
    cmd_read = send_command(port, CMD_TEST)
    hex_read = convert_to_hex(cmd_read)
    logging.debug(' test rx: %s %s %s', hex_read[0], hex_read[1], hex_read[2])
    if cmd_read != CMD_TEST:
      attempts += 1
    else:
      logging.debug('Arduino comm established after %d attempt(s)', attempts)
      break


def convert_to_hex(cmd):
  return [x.encode('hex') for x in cmd]


def rotate_servo(servo, angle, port):
  """Rotate servo to the specified angle.

  Args:
    servo: int; servo to rotate [1,2,3,4,5,6]
    angle: int; servo angle to move to
    port: object; serial port

  Returns:
    None
  """

  err_msg = 'Servo angle %.1f must be between 0 and %d.' % (
      angle, ARDUINO_ANGLE_MAX)
  if angle < 0:
    logging.error('%s', err_msg)
    angle = 0
  elif angle > ARDUINO_ANGLE_MAX:
    logging.error('%s', err_msg)
    angle = ARDUINO_ANGLE_MAX
  cmd = [chr(x) for x in [START_BYTE_SERVO, servo, angle]]
  cmd_read = send_command(port, cmd)
  hex_read = convert_to_hex(cmd_read)
  logging.debug(' rx: %s %s %s', hex_read[0], hex_read[1], hex_read[2])


def main():
  # get channel, rotate, offset, debug from command line
  servo, angle, offset, debug = get_cmdline_args()

  # set logging level
  if debug:
    logging.basicConfig(level=logging.DEBUG)
  else:
    logging.basicConfig(level=logging.INFO)

  # initialize port
  serial_port = serial_port_def('Arduino')

  # send test commands to Arduino until command returns properly
  establish_serial_comm(serial_port)

  # rotate servo to angle
  angle_norm = int(
      round((angle - offset) * ARDUINO_ANGLE_MAX / HS755HB_ANGLE_MAX, 0))
  logging.info('Moving servo %d to position %d', servo, angle)
  rotate_servo(servo, angle_norm, serial_port)


if __name__ == '__main__':
  main()