/* les2/service_client.cpp
 * Basic example of a service client node.
 *
 * Reviewed by: <x>
 * Changelog:
 * [04-09-2025] Wessel T: Implement template
 * [11-09-2025] Wessel T:
 *  - (BASE) Working service client
 *  - Wrap into namespace
 */

#include <chrono>
#include "rclcpp/rclcpp.hpp"

#include "les_interface/srv/service_vec.hpp"

using namespace std::chrono;
using les_interface::srv::ServiceVec;

namespace lessons::two::service {

class NodeClient : public rclcpp::Node {
public:
    NodeClient()
    : Node("node_les2_service_client")
    {
        client_ = this->create_client<ServiceVec>("vec_service_server");
        timer_ = this->create_wall_timer(
            seconds(3),
            std::bind(&NodeClient::send_request, this)
        );
    }

private:
    void send_request() {
        if (!client_->wait_for_service(seconds(1))) {
            RCLCPP_WARN(this->get_logger(), "Service not available");
            return;
        }

        auto request = std::make_shared<ServiceVec::Request>();
        request->x = rand() % 100;
        request->y = rand() % 100;
        request->z = rand() % 100;

        auto future = client_->async_send_request(request);
        auto result = rclcpp::spin_until_future_complete(this->shared_from_this(), future, std::chrono::seconds(2));

        if (result == rclcpp::FutureReturnCode::SUCCESS) {
            auto response = future.get();
            RCLCPP_INFO(this->get_logger(), "Received vec len: %.6lf", response->vec_len);
        } else if (result == rclcpp::FutureReturnCode::TIMEOUT) {
            RCLCPP_WARN(this->get_logger(), "Service call timed out");
        } else {
            RCLCPP_ERROR(this->get_logger(), "Service call failed");
        }
    }

    void handle_response(rclcpp::Client<ServiceVec>::SharedFuture future) {
        auto response = future.get();
        RCLCPP_INFO(this->get_logger(), "Received vec len: %.6lf", response->vec_len);
    }

    rclcpp::Client<ServiceVec>::SharedPtr client_;
    rclcpp::TimerBase::SharedPtr timer_;
};

} // namespace lessons::two::service

int main(int argc, char **argv) {
    srand(time(NULL));
    rclcpp::init(argc, argv);

    auto node = std::make_shared<lessons::two::service::NodeClient>();
    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}
