#pragma once

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

#define FAST_MODE_BASE 0
#define FAST_MODE_INIT 1        // init_mode, first match
#define FAST_MODE_GUESS 2       // full_mode, before ceres_match
#define FAST_MODE_GROUND 4      // count ground score
#define FAST_MODE_NOYAW 8       // do not search yaw
#define FAST_MODE_DEBUG 16

namespace juefx {

struct DiscreteScan;
struct Candidate;

class FastVoxelMatcher {
public:
    explicit FastVoxelMatcher(const FastVoxelMatcherOption &options);
    ~FastVoxelMatcher();

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

    struct Result {
        float score;
        float low_resolution_score;
        Rigid3f pose_estimate;
    };

    struct Param {
        int8_t mode;
        uint8_t start_depth;
        uint16_t xy_window_size;
        uint16_t z_window_size;
        uint8_t resolution_cm;
        uint8_t resolution_z_cm;
        uint8_t yaw_window_size;
        float yaw_step;
        float resolution;
        float resolution_z;
    };

    Param GetMatchParam(const VoxelMap &voxel_map,
                        float movement,
                        float rotation,
                        int mode = FAST_MODE_BASE) const;

    std::unique_ptr<Result> Match(const Rigid3f &init_pose,
                                  const VectorCloud &point_cloud,
                                  const VoxelMap &voxel_map,
                                  const Param &param) const;

private:
    std::vector<DiscreteScan> GenerateDiscreteScans(
        const Rigid3f &init_pose,
        const VectorCloud &point_cloud,
        const VoxelMap &voxel_map,
        const Param &param) const;

    DiscreteScan CreateDiscreteScan(const int index, const Rigid3f &trans_pose,
                                    const VectorCloud &point_cloud,
                                    const VoxelMap &voxel_map) const;

    std::vector<Candidate> GenerateLowestResolutionCandidates(
        const VoxelMap &voxel_map,
        const std::vector<DiscreteScan> &discrete_scans,
        const Param &param) const;

    void ScoreCandidates(const VoxelMap &voxel_map,
                         std::vector<Candidate> *const candidates,
                         const FastVoxelMatcher::Param &param,
                         bool low_resolution = false) const;

    Candidate BranchAndBound(const VoxelMap &voxel_map,
                             const std::vector<Candidate> &candidates,
                             const Param &param,
                             float min_score) const;

    const FastVoxelMatcherOption options_;
};

}