/* les2service_server.cpp
 * Basic example of a service server 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 <cstdlib>
#include "rclcpp/rclcpp.hpp"

#include "les_interface/srv/service_vec.hpp"

using les_interface::srv::ServiceVec;


namespace lessons::two::service {

class NodeServer : public rclcpp::Node {
public:
    NodeServer()
    : Node("node_les2_service_server")
    {
        vec_service_server_ =
          this->create_service<ServiceVec>(
                "vec_service_server",
                std::bind(
                    &NodeServer::callback_service,
                    this,
                    std::placeholders::_1,
                    std::placeholders::_2
                )
        );

        RCLCPP_INFO(this->get_logger(), "Service Server started");
    }

    void callback_service(
        const ServiceVec::Request::SharedPtr request,
        ServiceVec::Response::SharedPtr response
    ) {
        response->vec_len = sqrt(
          pow(request->x, 2) + pow(request->y, 2) + pow(request->z, 2)
        );

        RCLCPP_INFO(this->get_logger(),
          "sqrt(pow(%.6lf), pow(%.6lf), pow(%.6lf))=%.6lf",
          request->x, request->y, request->z, response->vec_len
        );
    }

private:
    rclcpp::Service<ServiceVec>::SharedPtr vec_service_server_;
};

} // namespace lessons::two::service

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);

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

    return 0;
}
