#pragma once

#include <string>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include "../utils/gnss.h"

namespace juefx
{

#define VOXEL_RESOLUTION_1X   0
#define VOXEL_RESOLUTION_2X   1
#define VOXEL_RESOLUTION_4X   2
#define VOXEL_RESOLUTION_8X   3
#define VOXEL_RESOLUTION_16X  4
#define VOXEL_RESOLUTION_NUM  5

#define GRID_MOVE_LEFT      1
#define GRID_MOVE_RIGHT     2
#define GRID_MOVE_UP        4
#define GRID_MOVE_DOWN      8

struct GridIndex
{
    struct SByteId
    {
        int8_t x = 0;
        int8_t y = 0;
    };
    struct ByteId
    {
        uint8_t x = 0;
        uint8_t y = 0;
    };
    struct ShortId {
        int16_t x = 0;
        int16_t y = 0;
    };
    SByteId meshIndex;
    ByteId blockIndex;
    ShortId gridIndex;

    GridIndex() { meshIndex.x = -127; }

    inline GridIndex Move(int move_type) const
    {
        GridIndex ret = *this;
        if ((move_type & GRID_MOVE_LEFT) != 0)
            ret.gridIndex.x--;
        if ((move_type & GRID_MOVE_RIGHT) != 0)
            ret.gridIndex.x++;
        if ((move_type & GRID_MOVE_UP) != 0)
            ret.gridIndex.y++;
        if ((move_type & GRID_MOVE_DOWN) != 0)
            ret.gridIndex.y--;
        return ret;
    }

    inline GridIndex Move(int16_t x, int16_t y) const
    {
        GridIndex ret = *this;
        ret.gridIndex.x += x;
        ret.gridIndex.y += y;
        return ret;
    }

    bool IsValid() { return meshIndex.x != -127; }
};

struct Distribution
{
    int32_t floor;
    uint32_t height : 24;
    uint32_t intensity : 8;
};

typedef std::vector<Distribution> GridData;

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

public:
    inline float GetResolution(uint8_t type) const { return 0; }
    inline const GridIndex GetCellIndex(uint8_t type, float x, float y) const { return GridIndex(); };
    inline GridData GetCellValue(uint8_t type, GridIndex index) const { return GridData(); }
    inline Eigen::Vector2f GetCenterOfCell(uint8_t type, float x, float y) const { return Eigen::Vector2f::Zero(); }
    inline Eigen::Vector2f GetCenterOfCell(uint8_t type, GridIndex index) const { return Eigen::Vector2f::Zero(); }
};

}