#!/usr/bin/env python
from __future__ import print_function, division

import pyexotica as exo
from numpy import array
from numpy import matrix
import math
from pyexotica.publish_trajectory import *
from time import time, sleep
import signal


def figureEight(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])


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

dt = 0.002
t = 0.0
q = array([0.0] * 7)
print('Publishing IK')
signal.signal(signal.SIGINT, sig_int_handler)
planning_times = []
while True:
    try:
        s = time()
        problem.set_goal('Position', figureEight(t))
        problem.start_state = q
        q = solver.solve()[0]
        planning_time = solver.get_planning_time()
        planning_times.append(planning_time)
        publish_pose(q, problem)
        e = time()
        time_taken = e - s
        if time_taken < dt:
            sleep(dt - time_taken)
        t = t + dt
    except KeyboardInterrupt:
        del solver, problem
        print("Number of samples", len(planning_times), "Mean", sum(planning_times)/len(planning_times))
        break
