#!/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
import exotica_core_task_maps_py
from exotica_examples_py import *

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_interactive_rho_tuning.xml')
        self.problem = self.solver.get_problem()

        # Setup interactive tuning 
        self.tuning = exo.interactive_cost_tuning.InteractiveCostTuning(self.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)

        # 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_node')
    exo.Setup.init_ros()
    signal.signal(signal.SIGINT, sig_int_handler)
    example = Example()
    rospy.Timer(rospy.Duration(DT), example.update)
    example.tuning.mainloop() # use .tuning.mainloop() instead of rospy.spin() when tuning

