#include <memory>
#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"

class LaneFollowerNode : public rclcpp::Node
{
public:
  LaneFollowerNode()
  : Node("lane_follower_node")
  {
    this->declare_parameter("follow_linear_speed", 0.3);
    this->declare_parameter("follow_angular_ratio", 1.0);
    this->declare_parameter("image_width", 640.0);

    follow_linear_speed_ = this->get_parameter("follow_linear_speed").as_double();
    follow_angular_ratio_ = this->get_parameter("follow_angular_ratio").as_double();
    image_width_ = this->get_parameter("image_width").as_double();

    // Subscriber: racing track center detection
    // Expects Float32MultiArray with [center_x] in image coordinates,
    // or a custom format. We assume center_x relative to image center.
    track_sub_ = this->create_subscription<std_msgs::msg::Float32MultiArray>(
      "racing_track_center_detection", 10,
      std::bind(&LaneFollowerNode::trackCallback, this, std::placeholders::_1));

    // Publisher: lane cmd_vel (NOT /cmd_vel directly)
    cmd_vel_pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
      "/lane_cmd_vel", 10);

    RCLCPP_INFO(this->get_logger(),
      "LaneFollowerNode started. linear=%.2f, angular_ratio=%.2f",
      follow_linear_speed_, follow_angular_ratio_);
  }

private:
  void trackCallback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
  {
    if (msg->data.empty()) {
      // No track detected — stop
      geometry_msgs::msg::Twist cmd;
      cmd.linear.x = 0.0;
      cmd.angular.z = 0.0;
      cmd_vel_pub_->publish(cmd);
      return;
    }

    // center_x from detection: pixel offset from image center
    // range: [-image_width/2, image_width/2]
    double center_offset = msg->data[0];  // in pixels

    // Normalize to [-1, 1]
    double normalized_error = center_offset / (image_width_ / 2.0);
    normalized_error = std::max(-1.0, std::min(1.0, normalized_error));

    // Angular velocity: steer toward center line
    // Positive error = line is to the right → positive angular to turn right
    double angular_z = follow_angular_ratio_ * normalized_error;

    geometry_msgs::msg::Twist cmd;
    cmd.linear.x = follow_linear_speed_;
    cmd.angular.z = angular_z;
    cmd_vel_pub_->publish(cmd);
  }

  rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr track_sub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;

  double follow_linear_speed_;
  double follow_angular_ratio_;
  double image_width_;
};

int main(int argc, char* argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<LaneFollowerNode>());
  rclcpp::shutdown();
  return 0;
}
