#!/usr/bin/env python
import rospy
import tf

from sensor_msgs.msg import JointState

from youbot_arm_kinematics.kinematics import Kinematics
from youbot_arm_kinematics.dh_parameters import YouBotDHParameters


class FKinematics:

    def __init__(self):
        rospy.init_node("fk_test")

        self.subJointState = rospy.Subscriber("joint_states", JointState, self.callbackJointState)

        self.ks = Kinematics(YouBotDHParameters.DH_A, YouBotDHParameters.DH_ALPHA,
                             YouBotDHParameters.DH_D, YouBotDHParameters.DH_THETA)

        rospy.spin()

    def callbackJointState(self, jointState):
        qs = []
        for i, jointPosition in enumerate(jointState.position):
            qs.append(jointPosition)
            # pass griper's fingers frames (and griper base too)
            if i == 4:
                break

        xyz, qtn, rpy, h = self.ks.forward(qs)

        rospy.loginfo(' Qs: ' + str(qs))
        rospy.loginfo('xyz: ' + str(xyz) + ' qtn: ' + str(qtn))
        rospy.loginfo('rpy: ' + str(rpy))

        # set orange brick at the end of griper
        br = tf.TransformBroadcaster()
        br.sendTransform(xyz, qtn, rospy.Time.now(), 'tool_link', "base_link")

if __name__ == '__main__':
    FKinematics()
