#!/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 time import sleep
import exotica_core_task_maps_py
from exotica_examples_py import *
import PyKDL as kdl

DT = 1.0 / 100.0 # 100 HZ

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.solver = exo.Setup.load_solver('{exotica_examples}/resources/configs/example_ik_look_at.xml')
        self.problem = self.solver.get_problem()

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

        # 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_lookat_node')
    exo.Setup.init_ros()
    signal.signal(signal.SIGINT, sig_int_handler)
    rospy.Timer(rospy.Duration(DT), Example().update)
    rospy.spin()
