#!/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 *
import PyKDL as kdl

DT = 1.0 / 100.0  # 100 HZ

class Example(object):

    def __init__(self):

        # Initialize general class attributes
        self.q = np.array([0.0] * 7)
        self.eff = kdl.Frame()

        self.target_marker = TargetMarker(pose=[0.6, 0, 0.8])

        # Setup EXOtica
        self.solver = exo.Setup.load_solver(
            '{exotica_examples}/resources/configs/example_eff_axis_alignment.xml')
        self.problem = self.solver.get_problem()

    def update(self, event):
        eff = self.target_marker.position
        direction = list((eff.M * kdl.Vector(1, 0, 0)))

        # Setup problem
        self.problem.get_scene().attach_object_local("Target", "", self.target_marker.position_exo)
        self.problem.get_task_maps()['Direction'].set_direction(
            'lwr_arm_7_link', direction)
        self.problem.start_state = self.q

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


if __name__ == '__main__':
    rospy.init_node('example_ik_eff_axis_alignment_node')
    exo.Setup.init_ros()
    rospy.Timer(rospy.Duration(DT), Example().update)
    signal.signal(signal.SIGINT, sig_int_handler)
    rospy.spin()
