#!/usr/bin/env python

import rospy
import actionlib
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from math import radians, degrees
from actionlib_msgs.msg import *
from geometry_msgs.msg import Point
from sound_play.libsoundplay import SoundClient


class map_navigation():

    STATE_NAME = "navigation"
    TIME_TO_SOUND_ALARM = 180
    
    checkpoints = [[1,1], [2,2]]
    current_point = 0
    
    ac
    
    def __init__(self):
        #define a client for to send goal requests to the move_base server through a SimpleActionClient
        ac = actionlib.SimpleActionClient("move_base", MoveBaseAction)


    def callback(change_state_data):
        if (change_state_data == STATE_NAME):
            self.goalReached = self.moveToGoal(checkpoints[current_point][0], checkpoints[current_point][1])
            update_checkpoint(self.goalReached)
            pub.publish(self.goalReached)

    def moveToGoal(self, xGoal, yGoal):
        #wait for the action server to come up
        while(not ac.wait_for_server(rospy.Duration.from_sec(5.0))):
            rospy.loginfo("Waiting for the move_base action server to come up")

        goal = MoveBaseGoal()

        #set up the frame parameters
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.header.stamp = rospy.Time.now()

        # moving towards the goal*/
        goal.target_pose.pose.position =  Point(xGoal,yGoal,0)
        goal.target_pose.pose.orientation.x = 0.0
        goal.target_pose.pose.orientation.y = 0.0
        goal.target_pose.pose.orientation.z = 0.0
        goal.target_pose.pose.orientation.w = 1.0

        rospy.loginfo("Sending goal location ...")
        ac.send_goal(goal)

        ac.wait_for_result(rospy.Duration(TIME_TO_SOUND_ALARM))

        if(ac.get_state() ==  GoalStatus.SUCCEEDED):
            rospy.loginfo("You have reached the destination")
            return True
        else:
            rospy.loginfo("The robot failed to reach the destination")
            return False
            
    def cancel_navigation(self):
        ac.cancelAllGoals()
        update_checkpoint(False)
        
    def update_checkpoint(self, result):
        if result:
            if current_point < checkpoints.length():
                current_point += 1
            else:
                current_point = 0
                

#if __name__ == '__main__':
#    try:
#
#        rospy.loginfo("You have reached the destination")
#        map_navigation()
#        rospy.spin()
#
#    except rospy.ROSInterruptException:
#        rospy.loginfo("map_navigation node terminated.")
               
if __name__ == '__main__':
    rospy.init_node('map_navigation')
    nav = map_navigation()

    rospy.Subscriber('state_machine', String, nav.callback)

    pub = rospy.Publisher('state_navigation', Bool, queue_size=10)
    
    rospy.spin()
    
    
    