#!/usr/bin/env python

import pyexotica as exo
from numpy import array
import math

def figure_eight(t):
    return array([0.0, math.sin(t * 2.0 * math.pi * 0.5) * 0.1, math.sin(t * math.pi * 0.5) * 0.2, 0.0, 0.0, 0.0])

solver = exo.Setup.load_solver(
    '{exotica_examples}/resources/configs/example_aico_eight.xml')
problem = solver.get_problem()

for t in range(0, problem.T):
    if t < problem.T / 5:
        problem.set_rho('Frame', 0.0, t)
    else:
        problem.set_rho('Frame', 1e5, t)
        problem.set_goal('Frame', figure_eight(t*problem.tau), t)

solution = solver.solve()

vis = exo.VisualizationMeshcat(problem.get_scene(), 'tcp://127.0.0.1:6000')
print('URL: ' + vis.get_web_url())

# Delete existing scene
vis.delete()
# Display this scene
vis.display_scene()
# Enable shadows and the spotlight
vis.set_property('/Lights/DirectionalLight','visible', False)
vis.set_property('/Lights/FillLight','visible', False)
vis.set_property('/Lights/SpotLight','visible', True)
vis.set_property('/Lights/SpotLight/<object>','castShadow', True)
# Publish trajectory
vis.display_trajectory(solution, 0.1)
