#!/usr/bin/env python

import rospy
import geometry_msgs.msg
import tf
import math
from std_srvs.srv import Trigger
from fulanghua_srvs.srv import Pose

class TsukubaChallengeStrategy:
    def __init__(self):
        rospy.init_node('tsukuba_challenge_strategy')
        self.start_nav_server = rospy.Service('start_nav', Trigger, self.start_nav_callback)
        self.resume_nav_server = rospy.Service('resume_nav', Trigger, self.resume_nav_callback)
        
        suspend_pose_cfg = rospy.get_param('~suspend_pose')
        resume_pose_cfg = rospy.get_param('~resume_pose')
        self.world_frame = rospy.get_param('~world_frame', 'map')
        self.robot_frame = rospy.get_param('~robot_frame', 'base_link')

        self.suspend_pose = geometry_msgs.msg.Pose()
        self.suspend_pose.position.x = suspend_pose_cfg['position']['x']
        self.suspend_pose.position.y = suspend_pose_cfg['position']['y']
        self.suspend_pose.position.z = suspend_pose_cfg['position']['z']
        self.suspend_pose.orientation.x = suspend_pose_cfg['orientation']['x']
        self.suspend_pose.orientation.y = suspend_pose_cfg['orientation']['y']
        self.suspend_pose.orientation.z = suspend_pose_cfg['orientation']['z']
        self.suspend_pose.orientation.w = suspend_pose_cfg['orientation']['w']
        
        self.resume_pose = geometry_msgs.msg.Pose()
        self.resume_pose.position.x = resume_pose_cfg['position']['x']
        self.resume_pose.position.y = resume_pose_cfg['position']['y']
        self.resume_pose.position.z = resume_pose_cfg['position']['z']
        self.resume_pose.orientation.x = resume_pose_cfg['orientation']['x']
        self.resume_pose.orientation.y = resume_pose_cfg['orientation']['y']
        self.resume_pose.orientation.z = resume_pose_cfg['orientation']['z']
        self.resume_pose.orientation.w = resume_pose_cfg['orientation']['w']

        print "suspend_pose = " + str(self.suspend_pose)
        print "resume_pose = " + str(self.resume_pose)

        self.tf_listener = tf.TransformListener()

    def start_nav_callback(self, req):
        try:
            rospy.loginfo("strategy: start_nav_callback")
            rospy.wait_for_service('start_wp_nav')
            start_wp_nav = rospy.ServiceProxy('start_wp_nav', Trigger)
            return start_wp_nav()
            #rospy.loginfo(start_wp_nav())
            #return (True, "successful")
        except rospy.ServiceException, e:
            print "error: %s" % e

    def resume_nav_callback(self, req):
        try:
            rospy.loginfo('strategy: resume_nav_callback')
            rospy.wait_for_service('resume_wp_pose')
            resume_wp_nav = rospy.ServiceProxy('resume_wp_pose', Pose)
            return resume_wp_nav(self.resume_pose)
        except rospy.ServiceException, e:
            print "error: %s" % e

    def spin(self):
        rate = rospy.Rate(10)
        already_send_suspend_pose = False
        while not rospy.is_shutdown():
            rate.sleep()
            
            try:
                (trans, rot) = self.tf_listener.lookupTransform(self.world_frame, self.robot_frame, rospy.Time(0))
                x = trans[0]
                y = trans[1]

                dist = math.sqrt(math.pow(x - self.suspend_pose.position.x, 2) + math.pow(y - self.suspend_pose.position.y, 2))

                print "robot_gl = ("  + str(x) + ", " + str(y) + ")"
                print "dist = " + str(dist)

                if(dist < 1.0) and not already_send_suspend_pose:
                    rospy.loginfo('strategy: send suspending request')
                    rospy.wait_for_service('suspend_wp_pose')
                    suspend_wp_nav = rospy.ServiceProxy('suspend_wp_pose', Pose)
                    print "suspend_wp_nav() = " + str(suspend_wp_nav(self.suspend_pose))
                    already_send_suspend_pose = True

            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
                continue

if __name__ == '__main__':
    strategy = TsukubaChallengeStrategy()
    strategy.spin()

