#include "ceres_matcher/ceres_scan_matcher_2d.h"

namespace hlzn_slam {
namespace ceres_matching {

CeresScanMatcher2D::CeresScanMatcher2D(const double& occupied_space_weight, const double& translation_weight,
                                        const double& rotation_weight, const double& self_stab_weight)
{
    occupied_space_weight_ = occupied_space_weight;
    translation_weight_ = translation_weight;
    rotation_weight_ = rotation_weight;
    self_stab_weight_ = self_stab_weight;
    
    ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
    ceres_solver_options_.use_nonmonotonic_steps = true;
    ceres_solver_options_.max_num_iterations = 20;
    ceres_solver_options_.num_threads = 1;
    ceres_solver_options_.minimizer_progress_to_stdout = false;
}

CeresScanMatcher2D::~CeresScanMatcher2D() {}

void CeresScanMatcher2D::Match(const common::Rigid2f& target_pose,
                               const common::Rigid2f& initial_pose_estimate,
                               const common::PointCloud& point_cloud,
                               Map& map,
                               common::Rigid2f* const pose_estimate,
                               ceres::Solver::Summary* const summary) const {
    double ceres_pose_estimate[3] = {initial_pose_estimate.translation()(0),
                                    initial_pose_estimate.translation()(1),
                                    initial_pose_estimate.rotation().angle()};
    Eigen::Vector2d target_translation = Eigen::Vector2d(target_pose.translation()(0), target_pose.translation()(1));
    ceres::Problem problem;
    
    problem.AddResidualBlock(
        CreateOccupiedSpaceCostFunction2D(
            occupied_space_weight_ / std::sqrt(static_cast<double>(point_cloud.data.size())),
            point_cloud, map),
        nullptr /* loss function */, ceres_pose_estimate);

    problem.AddResidualBlock(
        TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
                translation_weight_, target_translation), //10.0
        nullptr /* loss function */, ceres_pose_estimate);

    problem.AddResidualBlock(
        RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
            rotation_weight_, ceres_pose_estimate[2]), // 40.0
        nullptr /* loss function */, ceres_pose_estimate);
    
    // double angle = target_pose.rotation().angle();
    
    // if ((ceres_pose_estimate[2] - angle) > M_PI) {
    //     angle += M_PI * 2.0;
    // }
    // else if ((ceres_pose_estimate[2] - angle) < -M_PI){
    //     angle -= M_PI * 2.0;
    // }
    // problem.AddResidualBlock(
    //     RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
    //         self_stab_weight_, target_pose.rotation().angle()), // 6.0
    //     nullptr /* loss function */, ceres_pose_estimate);

    ceres::Solve(ceres_solver_options_, &problem, summary);

    *pose_estimate = common::Rigid2f(
        {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);
}

}  // namespace scan_matching
}  // namespace cartographer