#include  "precompile.h"

#include <math.h>
#include <string.h>

#include <f8/udplog.h>

#include <glm/gtx/intersect.hpp>

#include "DetourCommon.h"

#include "mapinstance.h"
#include "mapservice.h"
#include "gridservice.h"
#include "obstacle.h"
#include "loot.h"
#include "mapmgr.h"
#include "room.h"
#include "entityfactory.h"
#include "creature.h"
#include "glmhelper.h"
#include "debugcmd.h"

#include "mt/MetaMgr.h"
#include "mt/Param.h"
#include "mt/Map.h"
#include "mt/MapArea.h"
#include "mt/MapCollider.h"

#include "roommgr.h"

#include "navmesh.pb.h"

static const int NAV_ERROR_NEARESTPOLY = -2;

static const int NAV_ERROR = -1;

static const long RCN_NAVMESH_VERSION = 1;
static const int INVALID_NAVMESH_POLYREF = 0;

const int MAP_GRID_WIDTH = 64;

static const int NAVMESHSET_MAGIC = 'M'<<24 | 'S'<<16 | 'E'<<8 | 'T'; //'MSET';
static const int NAVMESHSET_VERSION = 1;

struct NavMeshSetHeader
{
    int magic;
    int version;
    int numTiles;
    dtNavMeshParams params;
};

struct NavMeshTileHeader
{
    dtTileRef tileRef;
    int dataSize;
};

static float frand()
{
    return (float)rand()/(float)RAND_MAX;
}

static void TraverseMapColliderNode(mc::ColliderNode* node, std::function<void (mc::ColliderNode*)> cb)
{
    for (auto& pair : node->childs) {
        cb(pair.second);
        if (!pair.second->childs.empty()) {
            TraverseMapColliderNode(pair.second, cb);
        }
    }
}

void MapInstance::Init()
{
    map_meta_ = mt::Map::GetById(map_id);
    if (!map_meta_) {
        A8_ABORT();
    }
    if (map_meta_->map_width() < 1) {
        A8_ABORT();
    }
    if (map_meta_->map_height() < 1) {
        A8_ABORT();
    }
    map_service_ = new MapService();
    grid_service_ = new GridService();
    grid_service_->Init(map_meta_->map_width(),
                        map_meta_->map_height(),
                        mt::Param::s().map_cell_width);
    map_service_->Init(map_meta_->map_width() / MAP_GRID_WIDTH,
                       map_meta_->map_height() / MAP_GRID_WIDTH,
                       MAP_GRID_WIDTH);
    f8::UdpLog::Instance()->Info
        ("map_id:%d",
         {
          map_id
         });
    {
        navmesh_ = dtAllocNavMesh();
        FILE *fp = fopen((mt::MetaMgr::Instance()->GetResDir() + "map4.bin").c_str(), "rb");
        if(fp){
            //fseek(fp, 0, SEEK_END);
            //int file_size = ftell(fp);
            int file_size = 1;
            if(file_size){
                NavMeshSetHeader header;
                size_t readLen = fread(&header, sizeof(NavMeshSetHeader), 1, fp);
                if (readLen != 1) {
                    fclose(fp);
                    abort();
                }
                if (header.magic != NAVMESHSET_MAGIC) {
                    fclose(fp);
                    abort();
                }
                if (header.version != NAVMESHSET_VERSION) {
                    fclose(fp);
                    abort();
                }
                dtStatus status = navmesh_->init(&header.params);
                if (dtStatusFailed(status)) {
                    fclose(fp);
                    abort();
                }
#ifdef DEBUG
                a8::XPrintf("orig:%f,%f,%f tileWidth:%f tileHeight:%f maxTiles:%d maxPolys:%d\n",
                            {
                                header.params.orig[0],
                                header.params.orig[1],
                                header.params.orig[2],
                                    header.params.tileWidth,
                                    header.params.tileHeight,
                                    header.params.maxTiles,
                                    header.params.maxPolys,
                            });
#endif
                {
                    // Read tiles.
                    for (int i = 0; i < header.numTiles; ++i) {
                        NavMeshTileHeader tileHeader;
                        readLen = fread(&tileHeader, sizeof(tileHeader), 1, fp);
                        if (readLen != 1) {
                            fclose(fp);
                            abort();
                        }

                        if (!tileHeader.tileRef || !tileHeader.dataSize) {
                            abort();
                            break;
                        }

                        unsigned char* data = (unsigned char*)dtAlloc(tileHeader.dataSize,
                                                                      DT_ALLOC_PERM);
                        if (!data) {
                            abort();
                            break;
                        }
                        memset(data, 0, tileHeader.dataSize);
                        readLen = fread(data, tileHeader.dataSize, 1, fp);
                        if (readLen != 1) {
                            dtFree(data);
                            fclose(fp);
                            abort();
                        }
                        navmesh_->addTile(data,
                                          tileHeader.dataSize,
                                          DT_TILE_FREE_DATA,
                                          tileHeader.tileRef,
                                          0);
                    }
                }
            }
            fclose(fp);
        }
    }
    navmesh_query_ = new dtNavMeshQuery();
    navmesh_query_->init(navmesh_, 1024);
    MarkMapAreaPolys();
    LoadHouse();
}

void MapInstance::UnInit()
{
    delete navmesh_query_;
    navmesh_query_ = nullptr;
    dtFreeNavMesh(navmesh_);
    navmesh_ = nullptr;

    map_service_->UnInit();
    grid_service_->UnInit();
    A8_SAFE_DELETE(map_service_);
    A8_SAFE_DELETE(grid_service_);
}

void MapInstance::AttachRoom(Room* room, RoomInitInfo& init_info)
{
    init_info.map_tpl_name = map_tpl_name_;
    init_info.map_meta = map_meta_;
    init_info.grid_service = grid_service_;
    init_info.map_service = map_service_;
    init_info.map_instance = shared_from_this();
}

int MapInstance::FindStraightPath(
                                  const glm::vec3& start,
                                  const glm::vec3& end,
                                  std::vector<glm::vec3>& paths)
{
    float spos[3];
    spos[0] = start.x;
    spos[1] = start.y;
    spos[2] = start.z;

    float epos[3];
    epos[0] = end.x;
    epos[1] = end.y;
    epos[2] = end.z;

    dtQueryFilter filter;
    filter.setIncludeFlags(0xffff);
    filter.setExcludeFlags(0);

    const float extents[3] = {2.f, 4.f, 2.f};

    dtPolyRef startRef = INVALID_NAVMESH_POLYREF;
    dtPolyRef endRef = INVALID_NAVMESH_POLYREF;

    float startNearestPt[3];
    float endNearestPt[3];
    navmesh_query_->findNearestPoly(spos, extents, &filter, &startRef, startNearestPt);
    navmesh_query_->findNearestPoly(epos, extents, &filter, &endRef, endNearestPt);

    if (!startRef || !endRef) {
        return NAV_ERROR_NEARESTPOLY;
    }

    int npolys;
    float straightPath[MAX_POLYS * 3];
    unsigned char straightPathFlags[MAX_POLYS];
    dtPolyRef straightPathPolys[MAX_POLYS];
    int nstraightPath;
    int pos = 0;

    navmesh_query_->findPath
        (startRef,
         endRef,
         startNearestPt,
         endNearestPt,
         &filter,
         polys_,
         &npolys,
         MAX_POLYS);
    nstraightPath = 0;

    if (npolys) {

        float epos1[3];
        dtVcopy(epos1, endNearestPt);

        if (polys_[npolys-1] != endRef) {
            navmesh_query_->closestPointOnPoly(polys_[npolys-1], endNearestPt, epos1, 0);
        }

        navmesh_query_->findStraightPath(startNearestPt,
                                         endNearestPt,
                                         polys_,
                                         npolys,
                                         straightPath,
                                         straightPathFlags,
                                         straightPathPolys,
                                         &nstraightPath,
                                         MAX_POLYS);
        for(int i = 0; i < nstraightPath * 3; ) {
            glm::vec3 currpos;
            currpos.x = straightPath[i++];
            currpos.y = straightPath[i++];
            currpos.z = straightPath[i++];
            paths.push_back(currpos);
            pos++;
        }
    }

    return pos;
}

int MapInstance::FindRandomPointAroundCircle(const glm::vec3& center_pos,
                                             float max_radius,
                                             glm::vec3& random_pt)
{
    dtQueryFilter filter;
    filter.setIncludeFlags(0xffff);
    filter.setExcludeFlags(0);

    dtPolyRef startRef = INVALID_NAVMESH_POLYREF;

    const float extents[3] = {2.f, 4.f, 2.f};
    float nearestPt[3];

    float center[3];
    center[0] = center_pos.x;
    center[1] = center_pos.y;
    center[2] = center_pos.z;

    navmesh_query_->findNearestPoly(center, extents, &filter, &startRef, nearestPt);
    if (!startRef) {
        return NAV_ERROR_NEARESTPOLY;
    }

    dtPolyRef randomRef = INVALID_NAVMESH_POLYREF;
    float randomPt[3];
    dtStatus status = navmesh_query_->findRandomPointAroundCircle
        (startRef,
         center,
         max_radius,
         &filter,
         frand,
         &randomRef,
         randomPt);

    if (dtStatusSucceed(status)) {
        random_pt.x = randomPt[0];
        random_pt.y = randomPt[1];
        random_pt.z = randomPt[2];
        return 1;
    } else {
        return 0;
    }
}

bool MapInstance::Raycast(const glm::vec3& start, const glm::vec3& end,
                          glm::vec3& hit_point, bool& hit_result)
{
    float spos[3];
    spos[0] = start.x;
    spos[1] = start.y;
    spos[2] = start.z;

    float epos[3];
    epos[0] = end.x;
    epos[1] = end.y;
    epos[2] = end.z;

    dtStatus status = 0;
    dtQueryFilter filter;
    filter.setIncludeFlags(0xffff);
    filter.setExcludeFlags(0);

    dtPolyRef startRef = INVALID_NAVMESH_POLYREF;
    {
        const float extents[3] = {2.f/10, 4.f, 2.f/10};
        float nearestPt[3];
        status = navmesh_query_->findNearestPoly(spos, extents, &filter, &startRef, nearestPt);
        if (startRef == INVALID_NAVMESH_POLYREF) {
            return false;
        }
    }

    float t = 0;
    int npolys;
    memset(hit_normal_, 0, sizeof(hit_normal_));
    status = navmesh_query_->raycast(startRef, spos, epos, &filter, &t, hit_normal_, polys_, &npolys, MAX_POLYS);
    if (!dtStatusSucceed(status)) {
        #ifdef DEBUG
        abort();
        #else
        return false;
        #endif
    }
    if (npolys <= 0) {
        return false;
    }

    if (t > 1) {
        // No Hit
        hit_pos_[0] = epos[0];
        hit_pos_[1] = epos[1];
        hit_pos_[2] = epos[2];
        hit_result = false;
    }  else {
        if (t < 0.00001f) {
            return false;
        }
        //需要处理spos == epos的情况!!!!
        // Hit
        dtVlerp(hit_pos_, spos, epos, t);
        hit_result = true;
    }

    {
        float closest[3];
        bool pos_over_poly = false;
        status = navmesh_query_->closestPointOnPoly(polys_[npolys - 1], hit_pos_, closest, &pos_over_poly);
        if (!dtStatusSucceed(status)) {
            abort();
        }
        hit_pos_[1] = closest[1];
    }

    hit_point.x = hit_pos_[0];
    hit_point.y = hit_pos_[1];
    hit_point.z = hit_pos_[2];

    return true;
}

bool MapInstance::FindNearestPoint(const glm::vec3& center, float radius, glm::vec3& nearest_pt)
{
    dtPolyRef startRef = INVALID_NAVMESH_POLYREF;

    dtQueryFilter filter;
    filter.setIncludeFlags(0xffff);
    filter.setExcludeFlags(0);

    const float extents[3] = {radius, 10.0f, radius};
    float nearestPt[3];

    float pos[3];
    pos[0] = center.x;
    pos[1] = center.y;
    pos[2] = center.z;

    navmesh_query_->findNearestPoly(pos, extents, &filter, &startRef, nearestPt);
    if (!startRef) {
        return false;
    }

    nearest_pt.x = nearestPt[0];
    nearest_pt.y = nearestPt[1];
    nearest_pt.z = nearestPt[2];
    return true;
}

bool MapInstance::GetPosHeight(const Position& pos, float& out_height)
{
    dtPolyRef startRef = INVALID_NAVMESH_POLYREF;

    dtQueryFilter filter;
    filter.setIncludeFlags(0xffff);
    filter.setExcludeFlags(0);

    const float extents[3] = {2.f, 4.f, 2.f};
    float nearestPt[3];

    float center[3];
    center[0] = pos.GetX() * GetMapMeta()->scale();
    center[1] = pos.GetY();
    center[2] = pos.GetZ() * GetMapMeta()->scale();

    bool isOverPoly;
    navmesh_query_->findNearestPoly(center, extents, &filter, &startRef, nearestPt, &isOverPoly);
    if (!startRef) {
        return false;
    }

    auto ret = navmesh_query_->getPolyHeight(startRef, nearestPt, &out_height);
    #if 0
    assert(ret == DT_SUCCESS);
    #endif
    if (ret != DT_SUCCESS) {
        out_height = pos.GetY();
        return false;
    }
    return true;
}

void MapInstance::Scale(glm::vec3& v)
{
    float old_y = v.y;
    v *= GetMapMeta()->scale();
    v.y = old_y;
}

void MapInstance::UnScale(glm::vec3& v)
{
    float old_y = v.y;
    v /= GetMapMeta()->scale();
    v.y = old_y;
}

glm::vec3 MapInstance::UnScaleEx(const glm::vec3& v)
{
    glm::vec3 result = v;
    UnScale(result);
    return result;
}

dtPoly* MapInstance::GetPoly(glm::vec3 pos, int& poly_idx)
{
    dtPolyRef startRef = INVALID_NAVMESH_POLYREF;

    dtQueryFilter filter;
    filter.setIncludeFlags(0xffff);
    filter.setExcludeFlags(0);

    const float extents[3] = {2.f, 4.f, 2.f};
    float nearestPt[3];

    Scale(pos);

    float pos1[3];
    pos1[0] = pos.x;
    pos1[1] = pos.y;
    pos1[2] = pos.z;

    const dtMeshTile* tile = navmesh_->getTileAt(0, 0, 0);
    assert(tile);
    navmesh_query_->findNearestPoly(pos1, extents, &filter, &startRef, nearestPt);
    if (startRef) {
        unsigned int slat = 0;
        unsigned int it = 0;
        unsigned int ip = 0;
        navmesh_->decodePolyId(startRef, slat, it, ip);
        poly_idx = ip;
        return &tile->polys[ip];
    }
    return nullptr;
}

void MapInstance::MarkMapAreaPolys()
{
    return;
    const dtMeshTile* tile = navmesh_->getTileAt(0, 0, 0);
    if (!tile) {
        abort();
    }
    poly_ext_datas_.reserve(tile->header->polyCount);
    for (int i = 0; i < tile->header->polyCount; ++i) {
        int ext_flag = 0;
        dtPoly* poly = &tile->polys[i];
        if ((poly->flags & SAMPLE_POLYFLAGS_SWIM) == SAMPLE_POLYFLAGS_SWIM) {
            const mt::MapArea* last_area_meta = nullptr;
            for (int ii = 0; ii < poly->vertCount; ++ii) {
                const float* vc = &tile->verts[poly->verts[ii]*3];
                const mt::MapArea* area_meta = mt::MapArea::GetAreaByPoint
                    (map_meta_->map_id(),
                     vc[0] / GetMapMeta()->scale(),
                     vc[1],
                     vc[2] / GetMapMeta()->scale());
                if (!area_meta) {
                    abort();
                }
                if (last_area_meta && last_area_meta != area_meta) {
                    abort();
                }
                last_area_meta = area_meta;
            }
            if (!last_area_meta) {
                abort();
            }
            if (last_area_meta->area_type() != 1) {
                abort();
            }
            switch (last_area_meta->area_subtype()) {
            case 1:
                {
                    a8::SetBitFlag(ext_flag, kWater1ExtFlag);
                }
                break;
            case 2:
                {
                    a8::SetBitFlag(ext_flag, kWater2ExtFlag);
                }
                break;
            case 3:
                {
                    a8::SetBitFlag(ext_flag, kWater3ExtFlag);
                }
                break;
            default:
                {
                    abort();
                }
                break;
            }
        }
        poly_ext_datas_.push_back(ext_flag);
    }
}

bool MapInstance::RaycastEx(const glm::vec3& start, const glm::vec3& end,
                            glm::vec3& hit_point, bool& hit_result,
                            int& same_polys_flags, std::vector<dtPolyRef>& spec_polys)
{
    same_polys_flags = 0;

    float spos[3];
    spos[0] = start.x;
    spos[1] = start.y;
    spos[2] = start.z;

    float epos[3];
    epos[0] = end.x;
    epos[1] = end.y;
    epos[2] = end.z;

    dtStatus status = 0;
    dtQueryFilter filter;
    filter.setIncludeFlags(0xffff);
    filter.setExcludeFlags(0);

    dtPolyRef startRef = INVALID_NAVMESH_POLYREF;
    {
        const float extents[3] = {2.f/10, 4.f, 2.f/10};
        float nearestPt[3];
        status = navmesh_query_->findNearestPoly(spos, extents, &filter, &startRef, nearestPt);
        if (startRef == INVALID_NAVMESH_POLYREF) {
            return false;
        }
    }

    float t = 0;
    int npolys;
    memset(hit_normal_, 0, sizeof(hit_normal_));
    status = navmesh_query_->raycast(startRef, spos, epos, &filter, &t, hit_normal_, polys_, &npolys, MAX_POLYS);
    if (!dtStatusSucceed(status)) {
        #ifdef DEBUG
        abort();
        #else
        return false;
        #endif
    }
    if (npolys <= 0) {
        return false;
    }

    if (t > 1) {
        // No Hit
        hit_pos_[0] = epos[0];
        hit_pos_[1] = epos[1];
        hit_pos_[2] = epos[2];
        hit_result = false;
    }  else {
        if (t < 0.00001f) {
            return false;
        }
        //需要处理spos == epos的情况!!!!
        // Hit
        dtVlerp(hit_pos_, spos, epos, t);
        hit_result = true;
    }

    {
        float closest[3];
        bool pos_over_poly = false;
        status = navmesh_query_->closestPointOnPoly(polys_[npolys - 1], hit_pos_, closest, &pos_over_poly);
        if (!dtStatusSucceed(status)) {
            abort();
        }
        hit_pos_[1] = closest[1];
    }
    {
        bool is_same_flags = true;
        unsigned short last_flags = 0;
        spec_polys.reserve(npolys);
        for (int i = 0; i < npolys; ++i) {
            unsigned short flags = 0;
            status = navmesh_->getPolyFlags(polys_[i], &flags);
            if (!dtStatusSucceed(status)) {
                abort();
            }
            if (flags) {
                spec_polys.push_back(polys_[i]);
            }
            if (last_flags && last_flags != flags) {
                is_same_flags = false;
            }
            last_flags = flags;
        }
        if (is_same_flags) {
            same_polys_flags = last_flags;
        }
    }

    hit_point.x = hit_pos_[0];
    hit_point.y = hit_pos_[1];
    hit_point.z = hit_pos_[2];

    return true;
}

void MapInstance::CheckTerrain(Creature* c, int same_poly_flags, const std::vector<dtPolyRef>& spec_polys)
{
    if (same_poly_flags) {
        c->CheckSpecObject(same_poly_flags);
    } else {
        if (spec_polys.empty()) {
            c->CheckSpecObject(0);
        } else {
            float pos[3];
            pos[0] = c->GetPos().GetX() * GetMapMeta()->scale();
            pos[1] = c->GetPos().GetY();
            pos[2] = c->GetPos().GetZ() * GetMapMeta()->scale();
            float closest[3];
            bool found = false;
            for (auto& poly_ref : spec_polys) {
                dtStatus status = navmesh_query_->closestPointOnPolyBoundary(poly_ref, pos, closest);
                if (dtStatusSucceed(status) &&
                    std::fabs(closest[0] - c->GetPos().GetX() * GetMapMeta()->scale()) <= 0.1f &&
                    std::fabs(closest[2] - c->GetPos().GetZ() * GetMapMeta()->scale()) <= 0.1f) {
                    unsigned short flags = 0;
                    if (dtStatusSucceed(navmesh_->getPolyFlags(poly_ref, &flags))) {
                        c->CheckSpecObject(flags);
                        found = true;
                        break;
                    }
                }
            }
            if (!found) {
                c->CheckSpecObject(0);
            }
        }
    }
}

void MapInstance::TraverseHouseList(std::function<void (HouseInfo*)> func)
{

}

bool MapInstance::PtInHouse(const glm::vec3& pt, glm::vec3& nearest_pt)
{
    float fpt[3];
    fpt[0] = pt.x;
    fpt[1] = pt.y;
    fpt[2] = pt.z;
    float closest[3];

    float edged[DT_VERTS_PER_POLYGON];
    float edget[DT_VERTS_PER_POLYGON];
    for (auto& house : houses_) {
        if (house.verts.empty()) {
            continue;
        }
        float* verts = &(house.verts[0]);
        int nv = house.verts.size() / 3;
        if (!dtPointInPolygon(fpt, verts, nv)) {
            continue;
        }
        if (dtDistancePtPolyEdgesSqr(fpt, verts, nv, edged, edget)) {
            float dmin = edged[0];
            int imin = 0;
            for (size_t i = 1; i < nv; ++i) {
                if (edged[i] < dmin) {
                    dmin = edged[i];
                    imin = i;
                }
            }
            const float* va = &verts[imin*3];
            const float* vb = &verts[((imin+1)%nv)*3];
            dtVlerp(closest, va, vb, edget[imin]);
            nearest_pt.x = closest[0];
            nearest_pt.y = closest[1];
            nearest_pt.z = closest[2];
            return true;
        }
    }
    return false;
}

void MapInstance::LoadHouse()
{
    if (GetMapMeta()->collider_info) {
        mc::ColliderNode* building_root = GetMapMeta()->collider_info->GetNode("building");
        if (building_root) {
            for (auto& pair : building_root->childs) {
                auto node = pair.second;
                std::vector<glm::vec3> old_points;
                std::vector<glm::vec3> new_points;
                {
                    mc::RotateBounds(node->bounds.center,
                                     node->bounds.size + glm::vec3(10.0f, 0, 10.f),
                                     glm::quat_identity<float, glm::qualifier::defaultp>(),
                                     old_points
                                     );

                    mc::RotateBounds(node->bounds.center,
                                     node->bounds.size + glm::vec3(10.0f, 0, 10.f),
                                     node->transform.local_rotation,
                                     new_points
                                     );
                }
                #ifdef DEBUG1
                {
                    std::string data = "old: " + node->name + " ";
                    for (auto& point : old_points) {
                        data += a8::Format("%f,%f,%f ", {point.x, point.y, point.z});
                    }
                    data += "\nnew:";
                    for (auto& point : new_points) {
                        data += a8::Format("%f,%f,%f ", {point.x, point.y, point.z});
                    }
                    a8::XPrintf("%s\n", {data});
                }
                #endif

                HouseInfo p;
                p.node = node;
                {
                    for (size_t i = 0; i < new_points.size(); ++i) {
                        const auto& point = new_points[i];
                        if (i >= 4) {
                            p.verts.push_back(point.x);
                            p.verts.push_back(point.y);
                            p.verts.push_back(point.z);
                        }
                    }
                    if (p.verts.size() != 3 * 4) {
                        abort();
                    }
                }
                houses_.push_back(p);
            }
        }
        {
            things_.reserve(1024 * 10);
            auto cb =
                [this] (mc::ColliderNode* node)
                {
                    if (!node->colliders.empty()) {
                        for (auto c : node->colliders) {
                            if (c->type == mc::kMeshCollider) {
                                mc::MeshCollider* mesh_collider = (mc::MeshCollider*)c;
                                mesh_collider->mesh.triangles.reserve(mesh_collider->mesh.raw_triangles.size() / 3);
                                for (int i = 0; i < mesh_collider->mesh.raw_triangles.size(); i += 3) {
                                    auto& tri = a8::FastAppend(mesh_collider->mesh.triangles);
                                    tri.node = node;
                                    int v0 = mesh_collider->mesh.raw_triangles[i + 0];
                                    int v1 = mesh_collider->mesh.raw_triangles[i + 1];
                                    int v2 = mesh_collider->mesh.raw_triangles[i + 2];
                                    tri.vert0 = mesh_collider->mesh.vertices[v0];
                                    tri.vert1 = mesh_collider->mesh.vertices[v1];
                                    tri.vert2 = mesh_collider->mesh.vertices[v2];
                                    tri.min_y = tri.vert0.y;
                                    tri.max_y = tri.vert0.y;
                                    for (int ii = 1; ii < 3; ++ii) {
                                        float y = mesh_collider->mesh.vertices[i + ii].y;
                                        if (y < tri.min_y) {
                                            tri.min_y = y;
                                        }
                                        if (y > tri.max_y) {
                                            tri.max_y = y;
                                        }
                                    }//end for ii
                                    tri.min_y -= 1.0f;
                                    tri.max_y += 1.0f;
                                    map_service_->AddTriangle(&tri);
                                }//end for i
                            }//end if
                        }//end forc
                        things_.push_back(node);
                    }
                };
            for (auto& pair : GetMapMeta()->collider_info->GetNodes()) {
                TraverseMapColliderNode(pair.second, cb);
            }
        }
    }
}

bool MapInstance::SceneRaycast(const glm::vec3& orig,
                               const glm::vec3& dir,
                               float max_distance,
                               glm::vec3& hit_pos,
                               float& ray_length)
{
    if (max_distance <= 0.001f) {
        return false;
    }
    float nearest_distance = 0.0f;
    CellNode* nearest_node = nullptr;
    glm::vec3 nearest_pt(0.0f, 0.0f, 0.0f);
    int raycast_index = MapMgr::Instance()->IncCurrRaycastIndex();
    float len = 0.0f;
    bool result = false;
    bool end = false;
    int tri_count = 0;
    do {
        glm::vec3 curr_pos = orig + dir * len;
        if (len > max_distance) {
            len = max_distance;
            curr_pos = orig + dir * len;
            end = true;
        }
        int grid_id = map_service_->GetGridId(curr_pos.x, curr_pos.z);
        list_head* grid_list[9];
        int grid_count = 0;
        map_service_->GetGridList(grid_id, grid_list, grid_count);
        for (int i = 0; i < grid_count; ++i){
            list_head* head = grid_list[i];
            if (list_empty(head)) {
                continue;
            }

            CellNode *node, *tmp;
            list_for_each_entry_safe(node, tmp, head, entry) {
                if (node->tri->check_flag != raycast_index) {
                    glm::vec2 bary_position;
                    float distance;
                    bool hit = glm::intersectRayTriangle
                        (orig,
                         dir,
                         node->tri->vert0,
                         node->tri->vert1,
                         node->tri->vert2,
                         bary_position,
                         distance
                         );
                    node->tri->check_flag = raycast_index;
                    ++tri_count;
                    if (hit && distance > 0.00001f) {
                        if (nearest_node) {
                            if (distance < nearest_distance) {
                                nearest_node = node;
                                nearest_distance = distance;
                                nearest_pt = orig + dir * distance;
                            }
                        } else if (distance <= max_distance) {
                            nearest_node = node;
                            nearest_distance = distance;
                            nearest_pt = orig + dir * distance;
                        }
                    }
                }
            }
        }
        if (nearest_node && nearest_distance <= len) {
            result = true;
            end = true;
        } else {
            len += 5.0f;
        }
    } while (!end);

    if (result) {
#ifdef DEBUG
        if (DebugCmd::Enable()) {
            a8::XPrintf("nearest_node:%s tri_count:%d nearest_distance:%f\n",
                        {nearest_node->tri->node->name,
                                tri_count,
                                nearest_distance});
        }
#endif
        hit_pos = nearest_pt;
        ray_length = nearest_distance;
    }
    return result;
}