#!/usr/bin/env python

# Quick example code to show how to execute a simple trajectory on the PR-2 armin python
# Nick Eckenstein

import roslib; roslib.load_manifest('minipr2')
import rospy
import math
import sys
import string
import actionlib
import pr2_controllers_msgs.msg
pth = roslib.rospack.rospackexec(['find','ckbot'])
sys.path.append(pth)
pth2 = string.replace(pth,'k/ckbot','k')
sys.path.append(pth2)
import logical
import trajectory_msgs.msg

from sensor_msgs.msg import JointState
from actionlib import simple_action_client
from pr2_controllers_msgs.msg import JointTrajectoryAction
from trajectory_msgs.msg import JointTrajectoryPoint
from std_msgs.msg import Int64
from logical import Cluster
from joy import *   

class CKBotReporter(JoyApp):
    def __init__(self,*arg,**kw):
        JoyApp.__init__(self,robot={},*arg,**kw)
        print "ROBOT:",self.robot

    def onStart(self):
        #initializing ROS
        print 'init minipr2'
        rospy.init_node('minipr2')

        # Creates joint trajectory action client and waits for the server
        print 'Waiting for Joint Server'
        l_client = actionlib.SimpleActionClient('l_arm_controller/joint_trajectory_action', JointTrajectoryAction)
        r_client = actionlib.SimpleActionClient('r_arm_controller/joint_trajectory_action', JointTrajectoryAction)
        l_client.wait_for_server()
        r_client.wait_for_server()

        # Creates the goal object to pass to the server
        goal = pr2_controllers_msgs.msg.JointTrajectoryGoal()
        
        # Populates trajectory with joint names.
        goal.trajectory.joint_names.append("r_shoulder_pan_joint")
        goal.trajectory.joint_names.append("r_shoulder_lift_joint")
        goal.trajectory.joint_names.append("r_upper_arm_roll_joint")
        goal.trajectory.joint_names.append("r_elbow_flex_joint")
        goal.trajectory.joint_names.append("r_forearm_roll_joint")
        goal.trajectory.joint_names.append("r_wrist_flex_joint")
        goal.trajectory.joint_names.append("r_wrist_roll_joint")
        
        print 'Populating'
        # Initialization of ckbot
    
        #right arm
        d = Cluster()
        d.populate(None,{0xE4: 'r_shoulder_pan_joint', 0x87: 'r_shoulder_lift_joint', 0xD8: 'r_upper_arm_roll_joint', 0xD7: 'r_elbow_flex_joint', 0x59: 'r_forearm_roll_joint', 0x86: 'r_wrist_flex_joint', 0xAF: 'r_hand_grip_joint'})
    
        #left arm
        #c = Cluster()
        #c.populate(None,{0x04 : 'l_shoulder_pan_joint',0x2A:'l_shoulder_lift_joint',0xE1:'l_upper_arm_roll_joint',0x2C:'l_elbow_flex_joint',0x85:'l_forearm_roll_joint', 0x1E:'l_wrist_flex_joint', 0x07:'l_hand_grip_joint' })

        # Function to get pr2 to current minipr2 position
        self.pr2sync(goal, r_client, d)

        # First trajectory point
        # Positions
        ind = 0
        #print goal.trajectory.points
        point1 = trajectory_msgs.msg.JointTrajectoryPoint()
        point2 = trajectory_msgs.msg.JointTrajectoryPoint()
        goal.trajectory.points = [point1, point2]
        #goal.trajectory.points[ind].positions.resize(7)
        point1.positions = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
        point1.velocities = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
        goal.trajectory.points[ind] = point1
        
        #To be reached 1 second after starting along the trajectory
        goal.trajectory.points[ind].time_from_start = rospy.Duration(1.0)

        # Second trajectory point
        # Positions
        ind += 1
        point1.positions = [-0.3,0.2,-0.1,-1.2,1.5,-0.3,0.5]
        point1.velocities = [0.0,0.0,0.0,0.0,0.0,0.0,0.0]
        goal.trajectory.points[ind] = point1
        goal.trajectory.points[ind].time_from_start = rospy.Duration(2.0)

        goal.trajectory.header.stamp = rospy.Time.now()+rospy.Duration(1.0)        
        r_client.send_goal(goal)
        #rospy.spin()
        print 'Goal run'


        self.goal = goal

        
        #t=Cluster()
        #t.populate()

        #rospy.spin()

    def onEvent( self, evt ):
        

    def pr2sync(goal, r_client, d):


if __name__=='__main__':
    try:
        #Creates ckbot reporter
        rep = CKBotReporter()
        rep.run()
    except rospy.ROSInterruptException: pass