#!/usr/bin/env python
from __future__ import print_function
import rospy
import pyexotica as exo
import math
import numpy as np
import signal
import exotica_core_task_maps_py
from pyexotica.publish_trajectory import publish_pose, sig_int_handler
import exotica_scipy_solver

def figure_eight(t):
    return np.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])

class Example(object):

    def __init__(self):

        # Set init variables
        self.q = np.array([0.0] * 7)

        # Setup exotica
        self.problem = exo.Setup.load_problem('{exotica_examples}/resources/configs/example_ik_eff_box.xml')
        self.solver = exotica_scipy_solver.SciPyEndPoseSolver(self.problem, method='SLSQP')
        self.t0 = rospy.Time.now().to_sec()

        # Extract and report EffBox limits
        lower_limits = self.problem.get_task_maps()['EffBox'].get_lower_limit(0)
        upper_limits = self.problem.get_task_maps()['EffBox'].get_upper_limit(0)
        print("EffBox Limits:")
        for i, d in enumerate(['X', 'Y', 'Z']):
            print("  {}Lim: {}, {}".format(d, lower_limits[i], upper_limits[i]))

    def update(self, event):

        # Set position goal
        self.problem.set_goal('Position', figure_eight(rospy.Time.now().to_sec() - self.t0))

        # Set start state
        self.problem.start_state = self.q

        # Solve and publish
        self.q = self.solver.solve()[0]
        publish_pose(self.q, self.problem)

if __name__=='__main__':
    rospy.init_node('example_ik_eff_box_node')
    exo.Setup.init_ros()
    signal.signal(signal.SIGINT, sig_int_handler)
    rospy.Timer(rospy.Duration(1.0/50.0), Example().update)
    rospy.spin()
