/** ****************************************************************************************
 *  Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach. ICRA'16.
 *  Refactored: Pure C++ algorithm class, NO rclcpp dependencies.
 ******************************************************************************************** */
#ifndef CLaserOdometry2D_H
#define CLaserOdometry2D_H

// ✅ [CLEANED] Removed all rclcpp headers. Only msg types and math libs remain.
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <geometry_msgs/msg/pose.hpp>

#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>
#include <vector>
#include <cmath>

namespace rf2o {

template <typename T> inline T sign(const T x) { return x < T(0) ? -1 : 1; }

template <typename Derived>
inline typename Eigen::MatrixBase<Derived>::Scalar getYaw(const Eigen::MatrixBase<Derived>& r) {
    return std::atan2(r(1, 0), r(0, 0));
}

template<typename T>
inline Eigen::Matrix<T, 3, 3> matrixRollPitchYaw(const T roll, const T pitch, const T yaw) {
    const Eigen::AngleAxis<T> ax = Eigen::AngleAxis<T>(roll, Eigen::Matrix<T, 3, 1>::UnitX());
    const Eigen::AngleAxis<T> ay = Eigen::AngleAxis<T>(pitch, Eigen::Matrix<T, 3, 1>::UnitY());
    const Eigen::AngleAxis<T> az = Eigen::AngleAxis<T>(yaw, Eigen::Matrix<T, 3, 1>::UnitZ());
    return (az * ay * ax).toRotationMatrix().matrix();
}

template<typename T>
inline Eigen::Matrix<T, 3, 3> matrixYaw(const T yaw) {
    return matrixRollPitchYaw<T>(0, 0, yaw);
}

using Scalar = float;
using Pose3d = Eigen::Isometry3d;
using MatrixS31 = Eigen::Matrix<Scalar, 3, 1>;
using IncrementCov = Eigen::Matrix<Scalar, 3, 3>;

// ✅ [FIXED] No longer inherits rclcpp::Node. Pure C++ class.
class CLaserOdometry2D 
{
public:
    CLaserOdometry2D();

    void init(const sensor_msgs::msg::LaserScan& scan,
              const geometry_msgs::msg::Pose& initial_robot_pose);
    
    bool is_initialized();
    
    // ✅ [MODIFIED] Accepts external timestamp instead of using internal clock
    bool odometryCalculation(const sensor_msgs::msg::LaserScan& scan, double current_time_sec);
    
    void setLaserPose(const Pose3d& laser_pose);
    
    const Pose3d& getIncrement() const;
    const IncrementCov& getIncrementCovariance() const;
    Pose3d& getPose();
    const Pose3d& getPose() const;

    bool verbose, module_initialized, first_laser_scan;
    
    // ✅ [MODIFIED] Replaced rclcpp::Time with plain double (seconds)
    double last_odom_time_sec; 
    double current_scan_time_sec;

    // Internal Data Structures (Unchanged)
    std::vector<Eigen::MatrixXf> range, range_old, range_inter, range_warped;
    std::vector<Eigen::MatrixXf> xx, xx_inter, xx_old, xx_warped;
    std::vector<Eigen::MatrixXf> yy, yy_inter, yy_old, yy_warped;
    std::vector<Eigen::MatrixXf> transformations;
    Eigen::MatrixXf range_wf, dtita, dt, rtita, normx, normy, norm_ang, weights;
    Eigen::MatrixXi null;
    Eigen::MatrixXf A, Aw, B, Bw;
    MatrixS31 Var;
    IncrementCov cov_odo;
    float fps, fovh;
    unsigned int cols, cols_i, width, ctf_levels, image_level, level, num_valid_range, iter_irls;
    float g_mask[5];
    double lin_speed, ang_speed;
    MatrixS31 kai_abs_, kai_loc_, kai_loc_old_, kai_loc_level_;
    Pose3d last_increment_, laser_pose_on_robot_, laser_pose_on_robot_inv_;
    Pose3d laser_pose_, laser_oldpose_, robot_pose_, robot_oldpose_;
    bool test;
    std::vector<double> last_m_lin_speeds, last_m_ang_speeds;

private:
    void createImagePyramid();
    void calculateCoord();
    void performWarping();
    void calculaterangeDerivativesSurface();
    void computeNormals();
    void computeWeights();
    void findNullPoints();
    void solveSystemOneLevel();
    void solveSystemNonLinear();
    bool filterLevelSolution();
    void PoseUpdate();
    void Reset(const Pose3d& ini_pose);
};

} /* namespace rf2o */
#endif