#!/usr/bin/env python
from __future__ import print_function
import signal
import pyexotica as exo
from pyexotica.publish_trajectory import sig_int_handler
from numpy import array
import numpy as np
import math
from time import sleep


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


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

dt = 1.0/20.0
t = 0.0
q = array([0.0]*7)
print('Publishing IK')
signal.signal(signal.SIGINT, sig_int_handler)
while True:
    try:
        problem.set_goal('Position', figure_eight(t))
        problem.start_state = q
        q = solver.solve()[0]
        print('Solution found in ' + str (solver.get_planning_time()) + 's ' + str(q) + ' in ' + str(len(problem.get_cost_evolution()[0])) + ' iterations')
        sleep(dt)
        t = t + dt
    except KeyboardInterrupt:
        break
