import itertools
import numpy
import time

import pypot.dynamixel


if __name__ == '__main__':
    ports = pypot.dynamixel.get_available_ports()
    print('available ports:', ports)

    if not ports:
        raise IOError('No port available.')

    port = ports[0]
    print('Using the first on the list', port)

    dxl_io = pypot.dynamixel.DxlIO(port)
    print('Connected :)')

    found_ids = dxl_io.scan(range(12))
    print('Found ids:', found_ids)

    ids = found_ids
    dxl_io.enable_torque(ids)

    #move all the motors to 0 position at 50 speed
    dxl_io.set_moving_speed({2: 50})
    dxl_io.set_moving_speed({3: 50})
    dxl_io.set_moving_speed({4: 50})
    dxl_io.set_moving_speed({5: 50})
    dxl_io.set_moving_speed({6: 50})
    dxl_io.set_moving_speed({7: 50})
    dxl_io.set_moving_speed({8: 50})
    dxl_io.set_moving_speed({9: 50})
    dxl_io.set_moving_speed({10: 50})
    dxl_io.set_moving_speed({11: 50})

    dxl_io.set_goal_position({2: 150})
    dxl_io.set_goal_position({3: 150})
    dxl_io.set_goal_position({4: 150})
    dxl_io.set_goal_position({5: 150})
    dxl_io.set_goal_position({6: 150})
    dxl_io.set_goal_position({7: 150})
    dxl_io.set_goal_position({8: 150})
    dxl_io.set_goal_position({9: 150})
    dxl_io.set_goal_position({10: 150})
    dxl_io.set_goal_position({11: 150})
    time.sleep(11)

    # move all the motors to it's function position at 50 speed
    dxl_io.set_moving_speed({2: 50})
    dxl_io.set_moving_speed({3: 50})
    dxl_io.set_moving_speed({4: 50})
    dxl_io.set_moving_speed({5: 50})
    dxl_io.set_moving_speed({6: 50})
    dxl_io.set_moving_speed({7: 50})
    dxl_io.set_moving_speed({8: 50})
    dxl_io.set_moving_speed({9: 50})
    dxl_io.set_moving_speed({10: 50})
    dxl_io.set_moving_speed({11: 50})

    dxl_io.set_goal_position({2: 112.5})
    dxl_io.set_goal_position({3: 111.5})
    dxl_io.set_goal_position({4: 102.5})
    dxl_io.set_goal_position({5: 87})
    dxl_io.set_goal_position({6: 67.5})
    dxl_io.set_goal_position({7: 53.25})
    dxl_io.set_goal_position({8: 47.75})
    dxl_io.set_goal_position({9: 54.50})
    dxl_io.set_goal_position({10: 77})
    dxl_io.set_goal_position({11: 96.5})
    time.sleep(5)

    # move all the motors to next position
    dxl_io.set_goal_position({2: 112.5})
    dxl_io.set_goal_position({3: 111.5})
    dxl_io.set_goal_position({4: 83.25})
    dxl_io.set_goal_position({5: 65})
    dxl_io.set_goal_position({6: 3})
    dxl_io.set_goal_position({7: -65})
    dxl_io.set_goal_position({8: -105})
    dxl_io.set_goal_position({9: -87})
    dxl_io.set_goal_position({10: -19.5})
    dxl_io.set_goal_position({11: 55.5})
    time.sleep(5)

    # move all the motors to next position
    dxl_io.set_goal_position({2: -33.75})
    dxl_io.set_goal_position({3: -21})
    dxl_io.set_goal_position({4: -22})
    dxl_io.set_goal_position({5: -37.75})
    dxl_io.set_goal_position({6: -65.85})
    dxl_io.set_goal_position({7: -90.5})
    dxl_io.set_goal_position({8: -105})
    dxl_io.set_goal_position({9: -92.75})
    dxl_io.set_goal_position({10: -61.75})
    dxl_io.set_goal_position({11: 27})
    time.sleep(5)

    # move all the motors to next position
    dxl_io.set_goal_position({2: 112.5})
    dxl_io.set_goal_position({3: 111.5})
    dxl_io.set_goal_position({4: 102.5})
    dxl_io.set_goal_position({5: 87})
    dxl_io.set_goal_position({6: 67.5})
    dxl_io.set_goal_position({7: 53.25})
    dxl_io.set_goal_position({8: 47.75})
    dxl_io.set_goal_position({9: 54.50})
    dxl_io.set_goal_position({10: 77})
    dxl_io.set_goal_position({11: 96.5})
    time.sleep(5)

    # move all the motors to next position
    dxl_io.set_goal_position({2: 112.5})
    dxl_io.set_goal_position({3: 111.5})
    dxl_io.set_goal_position({4: 83.25})
    dxl_io.set_goal_position({5: 65})
    dxl_io.set_goal_position({6: 3})
    dxl_io.set_goal_position({7: -65})
    dxl_io.set_goal_position({8: -105})
    dxl_io.set_goal_position({9: -87})
    dxl_io.set_goal_position({10: -19.5})
    dxl_io.set_goal_position({11: 55.5})
    time.sleep(5)

    # move all the motors to next position
    dxl_io.set_goal_position({2: -33.75})
    dxl_io.set_goal_position({3: -21})
    dxl_io.set_goal_position({4: -22})
    dxl_io.set_goal_position({5: -37.75})
    dxl_io.set_goal_position({6: -65.85})
    dxl_io.set_goal_position({7: -90.5})
    dxl_io.set_goal_position({8: -105})
    dxl_io.set_goal_position({9: -92.75})
    dxl_io.set_goal_position({10: -61.75})
    dxl_io.set_goal_position({11: 27})
    time.sleep(5)

    # move all the motors back to 0 position
    dxl_io.set_moving_speed({2: 50})
    dxl_io.set_moving_speed({3: 50})
    dxl_io.set_moving_speed({4: 50})
    dxl_io.set_moving_speed({5: 50})
    dxl_io.set_moving_speed({6: 50})
    dxl_io.set_moving_speed({7: 50})
    dxl_io.set_moving_speed({8: 50})
    dxl_io.set_moving_speed({9: 50})
    dxl_io.set_moving_speed({10: 50})
    dxl_io.set_moving_speed({11: 50})

    dxl_io.set_goal_position({2: 150})
    dxl_io.set_goal_position({3: 150})
    dxl_io.set_goal_position({4: 150})
    dxl_io.set_goal_position({5: 150})
    dxl_io.set_goal_position({6: 150})
    dxl_io.set_goal_position({7: 150})
    dxl_io.set_goal_position({8: 150})
    dxl_io.set_goal_position({9: 150})
    dxl_io.set_goal_position({10: 150})
    dxl_io.set_goal_position({11: 150})
    time.sleep(8)