#!/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()