/* les1.cpp
 * Assignment done in the first lesson explaining
 * the basics of ROS2
 *
 * Reviewed by: <x>
 * Changelog:
 * [04-09-2025] Wessel T:
 *   - Implement template
 */

#include <cstdlib>

#include "rclcpp/rclcpp.hpp"

#include "geometry_msgs/msg/point.hpp"  // Point for x, y, z coordinates
#include "les_interface/msg/hardware_status.hpp"

using namespace std::placeholders;

class NodeLes1Subscriber : public rclcpp::Node {

public:
  NodeLes1Subscriber()
    : Node("node_les1_subscriber")
    {
        subscriber_location_ =
            this->create_subscription<geometry_msgs::msg::Point>(
                "location", 10,
                std::bind(&NodeLes1Subscriber::sub_callback_location, this, _1)
            );

        subscriber_hw_status_ =
            this->create_subscription<les_interface::msg::HardwareStatus>(
                "hardware_status", 10,
                std::bind(&NodeLes1Subscriber::sub_callback_hw_status, this, _1)
            );
    }

    void sub_callback_location(const geometry_msgs::msg::Point::SharedPtr msg) {
        RCLCPP_INFO(this->get_logger(),
            "Received: x=%.2f, y=%.2f, z=%.2f", msg->x, msg->y, msg->z
        );
    }

    void sub_callback_hw_status(const les_interface::msg::HardwareStatus::SharedPtr msg) {
        RCLCPP_INFO(this->get_logger(),
            "Received HW status: version=%ld, temp=%.2f, motors_ready=%s, msg=%s",
            msg->version, msg->temperature,
            msg->are_motors_ready ? "true" : "false",
            msg->debug_message.c_str()
        );
    }

private:
    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr subscriber_location_;
    rclcpp::Subscription<les_interface::msg::HardwareStatus>::SharedPtr subscriber_hw_status_;
};

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

    auto node = std::make_shared<NodeLes1Subscriber>();

    rclcpp::spin(node);
    rclcpp::shutdown();

    return 0;
}
