#pragma once

#include "../carto/transform/transform.h"

#include "../utils/rigid3.h"
#include "../utils/vector_point_type.h"
#include "interpolated_voxel_map.h"
#include "options.h"

#define CERES_MODE_BASE 0
#define CERES_MODE_DEBUG 1 
#define CERES_MODE_LOSS 2

namespace juefx {

class CeresVoxelMatcher {
public:
    explicit CeresVoxelMatcher(const CeresVoxelMatcherOption &options);

    CeresVoxelMatcher(const CeresVoxelMatcher &) = delete;
    CeresVoxelMatcher &operator=(const CeresVoxelMatcher &) = delete;

    // Ceres uses double as default
    Eigen::Vector4d Match(const Rigid3d &init_pose,
                          const VectorCloud &high_cloud,
                          const VectorCloud &low_cloud,
                          const VoxelMap &voxel_map,
                          Rigid3d *final_pose,
                          ceres::Solver::Summary *summary,
                          int mode = CERES_MODE_BASE);

private:
    void SingleMatch(const Rigid3d &init_pose,
                     const VectorCloud &clouds,
                     const InterpolatedVoxelMap &voxel_map,
                     const float space_weight,
                     const float intensity_weight,
                     const int max_iter_num,
                     std::array<double, 3> &translation,
                     std::array<double, 4> &rotation,
                     ceres::Solver::Summary *summary);

    double CountCloudResidual(const VectorCloud &clouds,
                              const InterpolatedVoxelMap &voxel_map,
                              const Rigid3d &trans,
                              const float space_weight);

    const CeresVoxelMatcherOption options_;
};

}