#!/usr/bin/env python
"""
Copyright (c) 2014, Innok Robotics GmbH
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer. 
2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
"""

import rospy
#import tf
#import PyKDL as kdl

from geometry_msgs.msg import Twist
#from geometry_msgs.msg import Point, Quaternion
#from nav_msgs.msg import Odometry


from system_defines import *
from rmp_interface import RMP
from user_event_handlers import RMPEventHandlers
import sys,time,threading,Queue


  
RMP_SET_TRACTOR = [RMP_CFG_CMD_ID,RMP_CMD_SET_OPERATIONAL_MODE,TRACTOR_REQUEST]
RMP_SET_STANDBY = [RMP_CFG_CMD_ID,RMP_CMD_SET_OPERATIONAL_MODE,STANDBY_REQUEST]
"""
Define the update delay or update period in seconds. Must be greater
than the minimum of 0.01s
"""
UPDATE_DELAY_SEC = 0.05

"""
Create and response and command queue. The responses will be in the form of 
a dictionary containing the vaiable name as the key and a converted value
the names are defined in the feedback_X_bitmap_menu_items dictionaries if a particular
variable is of interest
"""
rsp_queue = Queue.Queue()
cmd_queue = Queue.Queue()
in_flags  = Queue.Queue()
out_flags = Queue.Queue()

def cmd_callback(msg):
	mode = 1
	speed = msg.linear.x * 1000
	yaw_rate = msg.angular.z * 1000
	
	cmd_queue.put([RMP_MOTION_CMD_ID,msg.linear.x,-msg.angular.z])

def listener():
    rospy.init_node('segwayrmp_cc', anonymous=True)
    rospy.Subscriber("/cmd_vel", Twist, cmd_callback)
    
    host = rospy.get_param('host', '192.168.0.40')
    port = rospy.get_param('port', 8080)
    
    rmp_addr = ("192.168.0.40",8080) 
    
    """
    Create the thread to run RMP
    """
    rmp_thread = threading.Thread(target=RMP, args=(rmp_addr,rsp_queue,cmd_queue,in_flags,out_flags,UPDATE_DELAY_SEC,False))
    rmp_thread.daemon = True
    rmp_thread.start()
    
    """
    Generate a goto tractor event
    """
    cmd_queue.put(RMP_SET_TRACTOR)
    
    req_exit = False

    while not (rospy.is_shutdown() or req_exit):
        while not out_flags.empty():
		event = out_flags.get()
		if event == RMP_RSP_DATA_RDY:
			fb_dict = rsp_queue.get()
			my_data = [['operational_time         : ',fb_dict["operational_time"]],
				   ['right_front_pos_m        : ',fb_dict["right_front_pos_m"]],
				   ['left_front_pos_m         : ',fb_dict["left_front_pos_m"]],
				   ['fram_wheel_track_width_m : ',fb_dict["fram_wheel_track_width_m"]]]
			
			temp = ""
			for i in range(0,len(my_data)):
			    temp += my_data[i][0]+str(my_data[i][1])+"\n"
			    
			# TODO : calculate and publish odometry
			
		elif event == RMP_INIT_FAILED:
			print "RMP initialization failed...." 
			print "exiting....."
			req_exit = True


if __name__ == '__main__':

        try:
                listener()

        except rospy.ROSInterruptException:
                pass

