#!/usr/bin/env python3

from abc import ABC, abstractmethod
from time import sleep

import rospy
import smach
import smach_ros
from larcc_demos.srv import ActivateStateOutcome, ActivateStateOutcomeRequest, ActivateStateOutcomeResponse


class ServiceTriggeredState(ABC):
    """A general class that implements a service server that receives the outcome with which a state should end.
    Useful for manual transitions and tests.
    """

    def __init__(self, name, outcomes):
        self.server = rospy.Service(name + '/activate_state_outcome', ActivateStateOutcome, self.outcomeRequestCallback)
        self.outcome = None
        self.outcomes = outcomes
        self.name = name
 
    def outcomeRequestCallback(self, request):

        response = ActivateStateOutcomeResponse()

        if request.outcome in self.outcomes:
            self.outcome = request.outcome
            response.success = True
            rospy.loginfo('State: ' + self.name + ' received request to produce outcome ' + self.outcome)
        else:
            self.outcome = None
            response.success = False
            rospy.logwarn('State ' + self.name + ' requested invalid outcome ' + self.outcome)

        return response

    def execute(self, userdata):
        """"default execute method that transits with a service call.
         To be overridable by specific implementations for states.
         """

        r = rospy.Rate(1) # 10hz 
        while not rospy.is_shutdown():

            rospy.loginfo('Executing state ' + self.name)
            if not self.outcome is None:
                outcome = self.outcome
                self.outcome = None
                return outcome

            r.sleep()


# Define state S1
class S1_FastObjectManipulation(smach.State, ServiceTriggeredState):
    def __init__(self):
        outcomes = ['e1_operator_detected']
        self.name = self.__class__.__name__
        ServiceTriggeredState.__init__(self, name=self.name, outcomes=outcomes)
        smach.State.__init__(self, outcomes=outcomes)

    #TODO implement specific state execute
    def execute(self, userdata):
        return ServiceTriggeredState.execute(self, userdata)

# Define state S2
class S2_SafeObjectManipulation(smach.State, ServiceTriggeredState):
    def __init__(self):
        outcomes = ['e2_operator_not_detected', 'e3_gesture1_detected']
        self.name = self.__class__.__name__
        ServiceTriggeredState.__init__(self, name=self.name, outcomes=outcomes)
        smach.State.__init__(self, outcomes=outcomes)

    #TODO implement specific state execute
    def execute(self, userdata):
        return ServiceTriggeredState.execute(self, userdata)

# Define state S3
class S3_GiveObject(smach.State, ServiceTriggeredState):
    def __init__(self):
        outcomes = ['e9_object_available']
        self.name = self.__class__.__name__
        ServiceTriggeredState.__init__(self, name=self.name, outcomes=outcomes)
        smach.State.__init__(self, outcomes=outcomes)

    #TODO implement specific state execute
    def execute(self, userdata):
        return ServiceTriggeredState.execute(self, userdata)

# Define state S4
class S4_WaitForInteraction(smach.State, ServiceTriggeredState):
    def __init__(self):
        outcomes = ['e5_interaction_pull_detected', 'e6_interaction_push_detected',
                    'e7_interaction_twist_detected']
        self.name = self.__class__.__name__
        ServiceTriggeredState.__init__(self, name=self.name, outcomes=outcomes)
        smach.State.__init__(self, outcomes=outcomes)

    #TODO implement specific state execute
    def execute(self, userdata):
        return ServiceTriggeredState.execute(self, userdata)

# Define state S5
class S5_WaitToRecoverObject(smach.State, ServiceTriggeredState):
    def __init__(self):
        outcomes = ['e8_gesture2_detected']
        self.name = self.__class__.__name__
        ServiceTriggeredState.__init__(self, name=self.name, outcomes=outcomes)
        smach.State.__init__(self, outcomes=outcomes)

    #TODO implement specific state execute
    def execute(self, userdata):
        return ServiceTriggeredState.execute(self, userdata)

# Define state S6
class S6_RecoverObject(smach.State, ServiceTriggeredState):
    def __init__(self):
        outcomes = ['e10_object_recovered']
        self.name = self.__class__.__name__
        ServiceTriggeredState.__init__(self, name=self.name, outcomes=outcomes)
        smach.State.__init__(self, outcomes=outcomes)

    #TODO implement specific state execute
    def execute(self, userdata):
        return ServiceTriggeredState.execute(self, userdata)


    
# main
def main():
    rospy.init_node('larcc_demo_sm')

    # Create a SMACH state machine
    sm = smach.StateMachine(outcomes=['e99_finish_demo'])

    # Open the container
    with sm:
        # Add states to the container
        smach.StateMachine.add('S1_FastObjectManipulation', S1_FastObjectManipulation(), 
                               transitions={'e1_operator_detected':'S2_SafeObjectManipulation'})

        smach.StateMachine.add('S2_SafeObjectManipulation', S2_SafeObjectManipulation(), 
                               transitions={'e2_operator_not_detected':'S1_FastObjectManipulation', 
                                            'e3_gesture1_detected':'S3_GiveObject'})

        smach.StateMachine.add('S3_GiveObject', S3_GiveObject(), 
                               transitions={'e9_object_available':'S4_WaitForInteraction'})

        smach.StateMachine.add('S4_WaitForInteraction', S4_WaitForInteraction(), 
                               transitions={'e5_interaction_pull_detected': 'S5_WaitToRecoverObject',
                                            'e6_interaction_push_detected': 'S2_SafeObjectManipulation',
                                            'e7_interaction_twist_detected': 'S3_GiveObject'})

        smach.StateMachine.add('S5_WaitToRecoverObject', S5_WaitToRecoverObject(), 
                               transitions={'e8_gesture2_detected': 'S6_RecoverObject'})

        smach.StateMachine.add('S6_RecoverObject', S6_RecoverObject(), 
                               transitions={'e10_object_recovered': 'S2_SafeObjectManipulation'})




    # Create and start the introspection server
    sis = smach_ros.IntrospectionServer('server_name', sm, '/S0_Start')
    sis.start()


    # Execute SMACH plan
    outcome = sm.execute()

    sis.stop()

if __name__ == '__main__':
    main()