import rclpy
from rclpy.node import Node

from pubsub.pubsub_library_v3 import MinimalServiceProvider

# Service Type Files
from custom_interfaces.srv import StateMachineSrv


# Device Class
class StateMachineCMDServiceProvider(MinimalServiceProvider):
    def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE):
        self.NODE_NAME = NODE_NAME
        self.SRV_NAME = SRV_NAME
        self.SRV_TYPE = SRV_TYPE
        # Init above laying class "Node" 
        super().__init__(self.NODE_NAME, self.SRV_NAME, self.SRV_TYPE)
    
    def user_defined(self, request, response):
        # StateMachine Code
        print("Command: %s"%(request.command))
        print("Value: %f"%(request.value))
        if request.command == "c":
            response.success = False
        else:
            response.success = True
        return response



def main(args=None):
    rclpy.init(args=args)
    
    recv_cmd_srv = StateMachineCMDServiceProvider(NODE_NAME="statemachine_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)

    while rclpy.ok():
        try:
            rclpy.spin_once(recv_cmd_srv, timeout_sec=0.1)

            # Either the scripts checks to see if new srv calls are available to work off,
            # or you implement this work inside the class
            # pro/cons: abstraction is at a maximum, every further level is already too specific for the general task 
            
        except (KeyboardInterrupt, SystemExit):
            print("\n\nShutting down...")
            recv_cmd_srv.destroy_node()
            rclpy.shutdown()


if __name__ == '__main__':
    main()