#!/usr/bin/env python
import rospy
import pyexotica as exo
import numpy as np
import signal
from pyexotica.publish_trajectory import publish_pose, sig_int_handler
from exotica_examples_py import TargetMarker
import exotica_scipy_solver
from visualization_msgs.msg import Marker

class Example(object):

    def __init__(self):

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

        self.target_marker = TargetMarker(pose=[-0.8, 0, 0.8], marker_shape=2, marker_size=[0.1, 0.1, 0.1])

        # Setup exotica
        self.problem = exo.Setup.load_problem('{exotica_examples}/resources/configs/example_ik_gaze_at_constraint.xml')
        self.solver = exotica_scipy_solver.SciPyEndPoseSolver(self.problem, method='SLSQP')

        # Setup ros publisher
        self.pub = rospy.Publisher('cone', Marker, queue_size=1)

    def update(self, event):
        # Set new look at target into world frame
        self.problem.get_scene().attach_object_local('GazeAtTarget', '', self.target_marker.position_exo)

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

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

    def update_cone(self, event):
        """
        Publishes a cone marker to display in RViz

        Note
        ====

          The cone is not accurate and thus it may be the case that the 
          marker that defines the GazeTarget appears outside the cone. 
        """
        marker = Marker()
        marker.header.frame_id = 'exotica/lwr_arm_6_link'
        marker.header.stamp = rospy.Time.now()
        marker.action = 0
        marker.pose.orientation.w = 1
        marker.type = 10
        marker.scale.x = marker.scale.y = marker.scale.z = 1
        marker.color.a = 0.5
        marker.color.r = 1
        marker.mesh_resource = "package://exotica_examples/resources/cone.stl"
        self.pub.publish(marker)

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