#include "voxel_map.h"

#include "stream_info/streammanager.h"
#include "area/maparea.h"
boost::shared_ptr<localize::MapArea> mapArea_;

namespace juefx {

VoxelMap::VoxelMap()
{
    mapArea_.reset(new localize::MapArea);
}

bool VoxelMap::SetMapPath(
        std::string path_name){
    return mapArea_->SetMapPath(path_name);
}

bool VoxelMap::LoadMap()
{
    return mapArea_->LoadMap();
}

void VoxelMap::loadArea(const Vector3d pose){
    mapArea_->loadArea(pose);
}

bool VoxelMap::IsMapLoaded(
        const GnssPoint blh_pose){
    return mapArea_->IsMapLoaded(blh_pose);
}

bool VoxelMap::IsMapLoaded(
        const Vector3d pose){
    return mapArea_->IsMapLoaded(pose);
}

bool VoxelMap::GuessPosition(
        const GnssPoint &init_pose,
        Isometry3d &pose_in_map) {
    return mapArea_->GuessPosition(init_pose, pose_in_map);
}

void VoxelMap::ConfirmPosition(
        const Vector3d &pose_in_map,
        GnssPoint &final_pose) {
    mapArea_->ConfirmPosition(pose_in_map, final_pose);
}

float VoxelMap::GetResolution(uint8_t type) const
{
    float resolution = mapArea_->getResolution();
    return resolution * (1 << type);
}

BlockGridIndex VoxelMap::GetCellIndex(float x, float y) const
{
    return mapArea_->GetCellIndex(x, y);
}

Distribution* VoxelMap::GetCellValue(
        uint8_t type, const BlockGridIndex &index, uint8_t &length) const
{
    return mapArea_->GetCellValue(type, index, length);
}

Vector2f VoxelMap::GetCenterOfCell(
        uint8_t type, float x, float y) const
{
    return mapArea_->GetCenterOfCell(type, x, y);
}

}