# 1. launch single navigation.
# 2. run this test.


from sub_engine_client import IF as Engine
import time
from pyrois.RoIS_Common import Component_Status
from utilities import setup_single_robot

process = setup_single_robot()

engine = Engine('http://127.0.0.1:8010')
engine.connect()
engine.search("engine_profile_test")
destinations = [[5, 0, 0], [10, 0, 0], [25, 0, 0], [25, 10, 0]]
for dest in destinations:
    print(engine.set_parameter('Navigation', dest, "", ""))
    navi_status = Component_Status.BUSY
    while navi_status != Component_Status.READY:
        #  server_status, navi_status = engine.get_navi_status()
        #  print("TEST: status = {}".format(navi_status))
        time.sleep(1)
    #  position = engine.get_position()
    #  print("TEST: position = {}".format(position))
    time.sleep(3)

#print(engine.analysis_c_status())
engine.disconnect()
print("finish")
