#pragma once

#include <string>
#include <vector>

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

namespace juefx
{

#define VOXEL_RESOLUTION_1X 0   // default 9cm
#define VOXEL_RESOLUTION_2X 1   // default 18cm
#define VOXEL_RESOLUTION_4X 2   // default 36cm
#define VOXEL_RESOLUTION_8X 3   // default 72cm
#define VOXEL_RESOLUTION_16X 4  // default 144cm
#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

union BlockGridId{
    int16_t id;
    struct{
        uint8_t gridId;
        int8_t blockId;
    };
};

struct BlockGridIndex{
    BlockGridId x;
    BlockGridId y;
    inline BlockGridIndex Step(int resolution, int move_type) const
    {
        BlockGridIndex ret = *this;
        if ((move_type & GRID_MOVE_LEFT) != 0)
            ret.x.id -= (1 << resolution);
        if ((move_type & GRID_MOVE_RIGHT) != 0)
            ret.x.id += (1 << resolution);
        if ((move_type & GRID_MOVE_UP) != 0)
            ret.y.id += (1 << resolution);
        if ((move_type & GRID_MOVE_DOWN) != 0)
            ret.y.id -= (1 << resolution);
        return ret;
    }

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

struct DistributionIndex{
    uint32_t start : 24;
    uint32_t length : 8;
};

struct Distribution
{
    int32_t floor;
    uint32_t height : 24;
    uint32_t intensity : 8;
    int64_t getScore() const{
        return (int64_t)floor << 32 | height << 8 | intensity;
//        int64_t score = 0;
//        score += height * 10000003;
//        score += floor * 10003;
//        score += intensity * 13;
//        return score;
    }
};

struct GridIndex
{
    struct SByteId
    {
        int8_t x = 0;
        int8_t y = 0;
    };
    struct ByteId
    {
        uint8_t x = 0;
        uint8_t y = 0;
    };
    struct ShortId
    {
        uint16_t x = 0;
        uint16_t y = 0;
    };
    struct SShortId
    {
        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; }
};

typedef std::vector<Distribution> GridData;

} // namespace juefx