import argparse
import numpy
import time
import sys

import pypot.dynamixel

N = 1000


def read_register(dxl, register, ids):
    print '\tReading {} times the {}...'.format(N, register)

    t = []
    getter = getattr(dxl, 'get_{}'.format(register))

    for _ in range(N):
        start = time.time()
        getter(ids)
        end = time.time()
        t.append(1000 * (end - start))

    print '\tTook {}ms (STD={}) per read'.format(numpy.mean(t), numpy.std(t))
    print


def full_test(dxl, ids):
    print 'Testing the communication speed with motor{} {}'.format('s' if len(ids) else '',
                                                                   ids)
    read_register(dxl, 'present_position', ids)
    read_register(dxl, 'control_table', ids)


if __name__ == '__main__':
    available_ports = pypot.dynamixel.get_available_ports()

    parser = argparse.ArgumentParser(formatter_class=argparse.ArgumentDefaultsHelpFormatter,
                                     description='Test low-level communication with Dynamixel motors.')

    parser.add_argument('-p', '--ports',
                        type=str, nargs='+',
                        default=available_ports,
                        help='Select which port(s) to test')

    parser.add_argument('-b', '--baudrate',
                        type=int, choices=[57600, 1000000], default=1000000,
                        help='Sets the baudrate')

    args = parser.parse_args()

    for port in args.ports:
        print 'Now testing port {} with {}bds...'.format(port, args.baudrate)

        print 'Opening connection...',
        with pypot.dynamixel.DxlIO(port, baudrate=args.baudrate) as dxl:
            print 'Ok!'
            print 'Scanning the bus...',
            sys.stdout.flush()
            ids = dxl.scan()
            print 'Done!'
            print 'Ids found: {}'.format(ids)

            full_test(dxl, ids[:1])
            full_test(dxl, ids)

        print 'Closing port'
        print