#!/usr/bin/env python3

from abc import ABC, abstractmethod
from time import sleep

import rospy
import smach
import smach_ros
from states.service_triggered_state import ServiceTriggeredState
from larcc_demos.srv import ActivateStateOutcome, ActivateStateOutcomeRequest, ActivateStateOutcomeResponse
from hand_gesture_recognition.msg import HandsClassified


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

        # Local variables
        self.detected_gesture = 'None'
        self.detected_gesture_stamp = rospy.Time.now()

        # ROS communications
        rospy.Subscriber("/hgr/classification", HandsClassified, self.gestureClassificationCallback)

    def gestureClassificationCallback(self, msg):
        # print('Received gesture classification callback')
        # print('Right hand gesture: ' + msg.hand_right)
        self.detected_gesture = msg.hand_left
        # self.detected_gesture_stamp = msg.header.stamp
        self.detected_gesture_stamp = rospy.Time.now()



    def execute(self, userdata):
        # return ServiceTriggeredState.execute(self, userdata)

        rate = rospy.Rate(10)
        while True:

            # print('Executing state ' + self.name)

            #TODO do stuff for state s6

            # Transitions
            time_since_gesture_detected = rospy.Time.now()-self.detected_gesture_stamp
            if self.detected_gesture == 'F' and time_since_gesture_detected.to_sec() < 2: # Transition to state S6
                self.detected_gesture = 'None'
                return 'e8_gesture_l_detected'

            rate.sleep()