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

#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" // Custom HW status message

namespace lessons::one::pubsub {

class NodePublisher : public rclcpp::Node {
public:
  NodePublisher()
	: Node("node_les1_publisher")
  {
    publisher_location_ =
			this->create_publisher<geometry_msgs::msg::Point>("location", 10);

		publisher_hw_status_ =
			this->create_publisher<les_interface::msg::HardwareStatus>("hardware_status", 10);

		timer_location_ = this->create_wall_timer(
			std::chrono::seconds(2),
			std::bind(&NodePublisher::timer_location_function, this)
		);

		timer_hw_status_ = this->create_wall_timer(
			std::chrono::seconds(2),
			std::bind(&NodePublisher::timer_hw_status_function, this)
		);
  }


  void timer_location_function() {
	  geometry_msgs::msg::Point coordinate;
	  coordinate.x = static_cast<double>(std::rand()) / RAND_MAX * 100.0;
	  coordinate.y = static_cast<double>(std::rand()) / RAND_MAX * 100.0;
	  coordinate.z = static_cast<double>(std::rand()) / RAND_MAX * 100.0;

	  publisher_location_->publish(coordinate);
	  RCLCPP_INFO(this->get_logger(),
	    "Published x=%.2f, y=%.2f, z=%.2f", coordinate.x, coordinate.y, coordinate.z
    );
  }

  void timer_hw_status_function() {
	  les_interface::msg::HardwareStatus hw_msg;
	  hw_msg.version = 1;
	  hw_msg.temperature = static_cast<double>(std::rand()) / RAND_MAX * 80.0;
	  hw_msg.are_motors_ready = (std::rand() % 2 == 0);
	  hw_msg.debug_message = "Status OK";

	  publisher_hw_status_->publish(hw_msg);
	  RCLCPP_INFO(this->get_logger(),
		  "Published HW status: version=%ld, temp=%.2f, motors_ready=%s, msg=%s",
		  hw_msg.version, hw_msg.temperature,
		  hw_msg.are_motors_ready ? "true" : "false",
		  hw_msg.debug_message.c_str()
    );
  }

private:
	rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr publisher_location_;
	rclcpp::Publisher<les_interface::msg::HardwareStatus>::SharedPtr publisher_hw_status_;
	rclcpp::TimerBase::SharedPtr timer_location_;
	rclcpp::TimerBase::SharedPtr timer_hw_status_;
};

} // namespace lessons::one::pubsub

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

    auto node = std::make_shared<lessons::one::pubsub::NodePublisher>();

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

    return 0;
}
