#!/usr/bin/env python

import roslib; roslib.load_manifest('maiss_decision_making')
import rospy
import math
import smach
import smach_ros
import std_srvs.srv as std_srvs
import std_msgs
import signal
import sys
from geometry_msgs.msg import *
from move_base_msgs.msg import *
from maiss_msg.msg import *
from maiss_hri.msg import *
from maiss_nav.msg import *
from smach_ros import SimpleActionState

import threading

# define state Monitor Emergencies
class MonitorActiveEmergencies(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['cleared_to_act','preempted','terminated'],
                                   output_keys=['emergency_goal_x',
                                                'emergency_goal_y'])
        s = rospy.Service('act', std_srvs.Empty, self.server_callback)
        self.wakeup = threading.Event()
        self.ongoing_emergency = False
        self.cleared_to_act = False
                
    def emergency_callback(self, msg, userdata):
        if msg.event.type == MaissControlEvent.FIRE:
            goal_position = MoveToEventPositionActionGoal()
            userdata.emergency_goal_x = msg.event_position.x;
            userdata.emergency_goal_y = msg.event_position.y;
            self.ongoing_emergency = True;
        elif msg.event.type == MaissControlEvent.FIRE_RESOLVED:
            self.ongoing_emergency = False;

    def server_callback(self, req):
        self.cleared_to_act = True
        self.wakeup.set()
        return std_srvs.EmptyResponse()
        
    def execute(self, userdata):
        self.cleared_to_act = False
        self.ongoing_emergency = False

        rospy.Subscriber("/maiss_control_events", MaissControlEvent, self.emergency_callback, userdata)
        
        while not (self.ongoing_emergency and self.cleared_to_act):
            self.wakeup.wait()
            self.wakeup.clear()
            #Blocking until callback triggers
            if self._preempt_requested:
                userdata.emergency_goal_x = 0
                userdata.emergency_goal_y = 0
                return 'preempted'
    
        #cancelling any active HRI tasks
        agent_id = rospy.get_param('agent_id',0)
        hripub = rospy.Publisher('/agent/'+str(agent_id)+'/HRI_request', HRITaskRequest)
        emergencyhri = HRITaskRequest()
        emergencyhri.request = HRITaskRequest.EMERGENCY_HRI
        hripub.publish(emergencyhri)
        
        return 'cleared_to_act'

    def request_preempt(self):
        self._preempt_requested = True
        self.wakeup.set()

class SignalCompletion(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['task_completed'])
        self.pub = rospy.Publisher('/maiss_control_events', MaissControlEvent)
        
    def execute(self, userdata):
        agent_id = rospy.get_param('agent_id',0)
        d = MaissControlEvent()
        d.event.type = MaissControlEvent.FIRE_RESOLVED
        d.event.id = agent_id
        d.event.timestamp = rospy.Time.now()
        self.pub.publish(d)
        rospy.sleep(0.1)
        return 'task_completed'

class WaitForPartner(smach.State):
    def __init__(self):
        smach.State.__init__(self, outcomes=['task_completed'])
        self.partner_close = False
        self.time_over = False
        self.partner_range = 2.0 #minimum meters to partner
        self.my_id = rospy.get_param('agent_id',0)
        self.time_to_wait = 120
        self.wakeup = threading.Event()
        self.ongoing_emergency = False        

    def posesCallback(self, data):
        if not self.ongoing_emergency:
            return
        my_ix = -1
        for i in range(len(data.id)):
            if data.id[i] == self.my_id:
                my_ix = i
        
        if my_ix < 0 or len(data.data) < my_ix or len(data.id) != len(data.data):
            rospy.logwarn("Inconsistent team pose data. Skipping.")
            return
        
        my_pose = data.data[my_ix]

        any_dangerous_robots = False
        for i in range(len(data.data)):
            if data.data[i].header.seq == 0:
                continue #disregard uninitialized data
            if data.id[i] == self.my_id:
                continue
            delta = math.hypot(data.data[i].pose.pose.position.y - my_pose.pose.pose.position.y,
                               data.data[i].pose.pose.position.x - my_pose.pose.pose.position.x)
            if delta < self.partner_range:
               self.partner_close = True
               self.wakeup.set()
               rospy.logwarn("Emergency Response:: Partner is Close. Exiting.")
    
    def timerCallback(self, event):
        self.time_over = True
        rospy.logwarn("Emergency Response:: Time is Over. Exiting.")
        self.wakeup.set()
    
    def execute(self, userdata):
        self.ongoing_emergency = True
        t = rospy.Timer(rospy.Duration(self.time_to_wait), self.timerCallback, True) #oneshot
        self.partner_close = False
        self.time_over = False
        while not (self.partner_close or self.time_over):
            self.wakeup.wait()
            self.wakeup.clear()
        
        t.shutdown()
        self.ongoing_emergency = False
        return 'task_completed'
        
def signal_handler(signal, frame):
    sys.exit(0)
        
def main():
    rospy.init_node('emergency_response_smach')
    signal.signal(signal.SIGINT, signal_handler)

    ##sm_main: Encapsulates the whole task and keeps looping (no I/O except SIGINT)
    sm_main = smach.StateMachine(outcomes=['terminated']) 
    
    with sm_main:
        smach.StateMachine.add('Monitor Active Emergencies', 
                               MonitorActiveEmergencies(),
                               transitions={'cleared_to_act':'Move to Emergency',
                                            'preempted':'Monitor Active Emergencies',
                                            'terminated':'terminated'})
        smach.StateMachine.add('Move to Emergency',
                               SimpleActionState('move_to_event_position',
                               MoveToEventPositionAction,
                               goal_slots=['x','y']),
                               transitions={'succeeded':'Wait for Partner',
                                            'preempted':'Monitor Active Emergencies',
                                            'aborted':'Monitor Active Emergencies'},
                               remapping={'x':'emergency_goal_x',
                                          'y':'emergency_goal_y'})
        smach.StateMachine.add('Wait for Partner',
                               WaitForPartner(),
                               transitions={'task_completed':'Signal Completion'})
        smach.StateMachine.add('Signal Completion',
                               SignalCompletion(),
                               transitions={'task_completed':'Monitor Active Emergencies'})

    # Create and start the introspection server
    agent_id = rospy.get_param('agent_id',0)
    sis = smach_ros.IntrospectionServer('emergency_smach_viewer_'+`agent_id`, sm_main, '/SM_EMERGENCY_'+`agent_id`)
    sis.start()
    
    outcome = sm_main.execute()

    rospy.spin()
    sis.stop()

if __name__ == '__main__':
    main()