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: 50})
    dxl_io.set_goal_position({3: 103.5})
    dxl_io.set_goal_position({4: 120})
    dxl_io.set_goal_position({5: 104})
    dxl_io.set_goal_position({6: 47.5})
    dxl_io.set_goal_position({7: -21.5})
    dxl_io.set_goal_position({8: -63.5})
    dxl_io.set_goal_position({9: -92})
    dxl_io.set_goal_position({10: -113.75})
    dxl_io.set_goal_position({11: -121.5})
    time.sleep(5)

    # move all the motors to it's contrary position
    dxl_io.set_goal_position({2: -121.5})
    dxl_io.set_goal_position({3: -113.75})
    dxl_io.set_goal_position({4: -92})
    dxl_io.set_goal_position({5: -63.5})
    dxl_io.set_goal_position({6: -21.5})
    dxl_io.set_goal_position({7: 67.5})
    dxl_io.set_goal_position({8: 104})
    dxl_io.set_goal_position({9: 120})
    dxl_io.set_goal_position({10: 103.5})
    dxl_io.set_goal_position({11: 50})
    time.sleep(5)

    # move all the motors back to 0 position
    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)