#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <boost/shared_ptr.hpp>

#include "voxel_map_define.h"

class StreamManager;

namespace juefx {

class VoxelMap {
public:
    VoxelMap();
    bool SetMapPath(std::string path_name);
    bool LoadMap();
    void loadArea(const Eigen::Vector3d pose);
    bool IsMapLoaded(const GnssPoint blh_pose);
    bool IsMapLoaded(const Eigen::Vector3d pose);
    bool GuessPosition(const GnssPoint &init_pose, Eigen::Isometry3d &pose_in_map);
    void ConfirmPosition(const Eigen::Vector3d &pose_in_map, GnssPoint &final_pose);

public:
    float GetResolution(uint8_t type) const;

    BlockGridIndex GetCellIndex(float x, float y) const;
    BlockGridIndex GetPymdCellIndex(float x, float y) const {
        return GetCellIndex(x, y);
    }

    Distribution* GetCellValue(uint8_t type, const BlockGridIndex &index, uint8_t &length) const;
    Distribution* GetPymdCellValue(uint8_t type, BlockGridIndex index, uint8_t &length) const
    {
        index.x.gridId &= mask[type];
        index.y.gridId &= mask[type];
        return GetCellValue(type, index, length);
    }

    Eigen::Vector2f GetCenterOfCell(uint8_t type, float x, float y) const;

    Eigen::Vector3f GetOffset() const;
private:
    const uint8_t mask[8] = {0xff, 0xfe, 0xfc, 0xf8, 0xf0, 0, 0, 0};
};

}