#!/usr/bin/env python

import pyexotica as exo
from pyexotica.publish_trajectory import *
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()

plot(solution)
