import rclpy
from rclpy.node import Node

import pubsub.pubsub_library_v3 as lib

# Service Type Files
from custom_interfaces.srv import StateMachineSrv


def main(args=None):
    rclpy.init(args=args)
    
    client_cmd_srv = lib.MinimalServiceClientAsync(NODE_NAME="client_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)

    # Create request
    srv_request = StateMachineSrv.Request()
    srv_request.command = "c"
    srv_request.value = 42.0
    print("Sending Service Request...")
    client_cmd_srv.send_request(srv_request)

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

            # Check request
            if client_cmd_srv.done_was_true == False:
                done, response = client_cmd_srv.check_if_service_complete()
                if done:
                    client_cmd_srv.done_was_true = True
                    print("'done': %s"%(done))
                    print("'resp.success': %s"%(response.success))
                else:
                    print("Service not complete")
            
            rclpy.spin_once(client_cmd_srv, timeout_sec=0.1)
            
            srv_request.command = "a"
            client_cmd_srv.send_request(srv_request)

        except (KeyboardInterrupt, SystemExit):
            print("\n\nShutting down...")
            client_cmd_srv  .destroy_node()
            rclpy.shutdown()


if __name__ == '__main__':
    main()