#include "PathfindingE.h"

PathCell zeroCell;

PathfindingE::PathfindingE()
{
    ZeroMemory(&zeroCell, sizeof(zeroCell));
}

void PathfindingE::Init(int mapW, int mapH, void* mapLayer, std::vector<LONG>* pxmArr, std::vector<int>* pxaArr, int pxmArr_w, size_t cacheSize)
{
    this->mapW = mapW;
    this->mapH = mapH;
    maxLen = (float)(mapW * mapH);
    this->mapLayer = mapLayer;
    this->pxmArr = pxmArr;
    this->pxaArr = pxaArr;
    this->pxmArr_w = pxmArr_w;
    this->cacheSize = cacheSize;
    UpdateMapData();
}

void PathfindingE::Reset()
{
    gridTable.clear();
    UpdateMapData();
}

void PathfindingE::Step()
{
    size_t gridSize = mapW * mapH * 2 * sizeof(float);
    size_t allSize = gridTable.size() * gridSize;
    //如果超出了设置的内存大小就清空一次
    if (allSize > cacheSize)
        Reset();
}

PathGrid* PathfindingE::GetPath(WORD dstX, WORD dstY)
{
    auto iter = gridTable.find(MAKELONG(dstX, dstY));
    if (iter != gridTable.end())
        return &iter->second;
    //生成一个新目的地网格
    gridTable.insert(std::make_pair(MAKELONG(dstX, dstY), PathGrid()));
    PathGrid* grid = &gridTable.at(MAKELONG(dstX, dstY));
    grid->mapLayer = mapLayer;
    MakePath(grid, dstX, dstY);
    return grid;
}

bool PathfindingE::LineBlock(float srcX, float srcY, float dstX, float dstY, bool(PathfindingE::* CellProc)(int X, int Y, XMVECTOR* v))
{
    //计算直线经过的区域
    //直线从src的中心出发
    auto target = XMVectorSet(dstX - srcX, dstY - srcY, 0, 0);
    bool steep = abs(XMVectorGetY(target)) > abs(XMVectorGetX(target));
    auto x = steep ? XMVectorGetY(target) : XMVectorGetX(target);
    auto y = steep ? XMVectorGetX(target) : XMVectorGetY(target);
    //斜率
    float tangent = y / x;
    float delta = x > 0 ? 0.5f : -0.5f;
    for (int i = 1; i < 2 * abs(x); i++)
    {
        float tempX = i * delta;
        float tempY = tangent * tempX;
        bool isOnEdge = abs(tempY - floorf(tempY)) == 0.5f;
        //偶数 格子内部判断
        if ((i & 1) == 0)
        {
            //在边缘 上下两个格子满足条件
            if (isOnEdge)
            {
                //镜像反转
                if (steep)
                {
                    if ((this->*CellProc)((int)(ceilf(tempY) + srcX), (int)(roundf(tempX) + srcY), &target))
                        return true;
                    if ((this->*CellProc)((int)(floorf(tempY) + srcX), (int)(roundf(tempX) + srcY), &target))
                        return true;
                }
                else
                {
                    if ((this->*CellProc)((int)(roundf(tempX) + srcX), (int)(ceilf(tempY) + srcY), &target))
                        return true;
                    if ((this->*CellProc)((int)(roundf(tempX) + srcX), (int)(floorf(tempY) + srcY), &target))
                        return true;
                }
            }
            //不在边缘 所处格子满足条件
            else
            {
                //镜像反转
                if (steep)
                {
                    if ((this->*CellProc)((int)(roundf(tempY) + srcX), (int)(roundf(tempX) + srcY), &target))
                        return true;
                }
                else
                {
                    if ((this->*CellProc)((int)(roundf(tempX) + srcX), (int)(roundf(tempY) + srcY), &target))
                        return true;
                }
            }
        }
        //奇数 格子边缘判断
        else
        {
            //在格子交点处 不视为阻挡 忽略
            if (isOnEdge)
            {
                continue;
            }
            //否则左右两个格子都满足
            else
            {
                //镜像反转
                if (steep)
                {
                    if ((this->*CellProc)((int)(roundf(tempY) + srcX), (int)(ceilf(tempX) + srcY), &target))
                        return true;
                    if ((this->*CellProc)((int)(roundf(tempY) + srcX), (int)(floorf(tempX) + srcY), &target))
                        return true;
                }
                else
                {
                    if ((this->*CellProc)((int)(ceilf(tempX) + srcX), (int)(roundf(tempY) + srcY), &target))
                        return true;
                    if ((this->*CellProc)((int)(floorf(tempX) + srcX), (int)(roundf(tempY) + srcY), &target))
                        return true;
                }
            }
        }
    }
    return false;
}

bool PathfindingE::CellProc(int X, int Y, XMVECTOR* v)
{
    //检查
    float vecX = XMVectorGetX(*v);
    float vecY = XMVectorGetY(*v);
    int index = Y * mapW + X;
    if (index >= mapData.size())
        return false;
    auto pxa = mapData[index];
    //第一、三象限
    if ((vecX >= 0 && vecY >= 0) || (vecX <= 0 && vecY <= 0))
        return IsLine1ZoneBlock(pxa) || IsLine3ZoneBlock(pxa);
    //第二、四象限
    else if ((vecX <= 0 && vecY >= 0) || (vecX >= 0 && vecY <= 0))
        return IsLine2ZoneBlock(pxa) || IsLine4ZoneBlock(pxa);
    return false;
}

void PathfindingE::UpdateMapData()
{
    mapData.clear();
    mapData.resize((size_t)maxLen);
    auto& pxmData = *pxmArr;
    auto& pxaData = *pxaArr;
    for (int y = 0; y < mapH; y++)
        for (int x = 0; x < mapW; x++)
            mapData[y * mapW + x] = pxaData[pxmData[min(y * pxmArr_w + x, pxmData.size() - 1)]];
}

bool PathfindingE::IsPathBlock(BYTE pxa)
{
    switch (pxa)
    {
    case 0x2:
    case 0x12:
    case 0xf:
    case 0x1f:
    case 0x22:
    case 0x32:
    case 0x41:
    case 0x42:
    case 0x43:
    case 0x51:
    case 0x52:
        return true;
    }
    return false;
}

bool PathfindingE::IsUpBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x3:
        case 0x4:
        case 0x5:
        case 0x6:
        case 0xb:
        case 0xc:
            return true;
        }
    }
    return false;
}

bool PathfindingE::IsDownBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x7:
        case 0x8:
        case 0x9:
        case 0xa:
        case 0xd:
        case 0xe:
            return true;
        }
    }
    return false;
}

bool PathfindingE::IsLeftBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x3:
        case 0x4:
        case 0x7:
        case 0x8:
        case 0xb:
        case 0xd:
            return true;
        }
    }
    return false;
}

bool PathfindingE::IsRightBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x5:
        case 0x6:
        case 0x9:
        case 0xa:
        case 0xc:
        case 0xe:
            return true;
        }
    }
    return false;
}

bool PathfindingE::IsLine1ZoneBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x2:
        case 0x3:
        case 0x4:
        case 0xb:
            return true;
        }
    }
    return false;
}

bool PathfindingE::IsLine2ZoneBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x2:
        case 0x5:
        case 0x6:
        case 0xc:
            return true;
        }
    }
    return false;
}

bool PathfindingE::IsLine3ZoneBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x2:
        case 0x9:
        case 0xa:
        case 0xe:
            return true;
        }
    }
    return false;
}

bool PathfindingE::IsLine4ZoneBlock(BYTE pxa)
{
    if (pxa < 0x40)
    {
        BYTE a = pxa % 16;
        switch (a)
        {
        case 0x2:
        case 0x7:
        case 0x8:
        case 0xd:
            return true;
        }
    }
    return false;
}

void PathfindingE::MakePath(PathGrid* grid, WORD dstX, WORD dstY)
{
    //生成向量图
    auto& vecMap = grid->vecMap;
    vecMap.resize(mapW * mapH);

    //DijkstraGrid网格图
    for (auto& p : vecMap)
        p.dis = D3D11_FLOAT32_MAX;
    int index = dstY * mapW + dstX;
    if (index >= grid->vecMap.size())
        return;
    grid->vecMap.at(index).dis = 0;
    //生成网格
    StepGrid(grid, dstX, dstY);
    //生成向量场
    for (int y = 0; y < mapH - 1; y++)
        for (int x = 0; x < mapW - 1; x++)
            StepVector(grid, x, y);
}

void PathfindingE::StepGrid(PathGrid* grid, int X, int Y)
{
    std::queue<LONG> qe;
    if (!IsPathBlock(mapData[Y * mapW + X]))
        qe.push(MAKELONG(X, Y));
    while (!qe.empty())
    {
        //取栈的顶部
        auto pos = qe.front();
        qe.pop();
        int x = LOWORD(pos);
        int y = HIWORD(pos);
        auto nowVal = grid->vecMap.at(y * mapW + x).dis;
        //中间值
        BYTE pxaC = mapData[y * mapW + x];
        //检查周围
        //上
        if (y > 0 && !IsUpBlock(pxaC))
        {
            BYTE pxaU = mapData[(y - 1) * mapW + x];
            if (!IsPathBlock(pxaU))
            {
                //如果上方能写入
                auto& up = grid->vecMap.at((y - 1) * mapW + x).dis;
                if (up > nowVal + 1)
                {
                    up = nowVal + 1.0f;
                    qe.push(MAKELONG(x, y - 1));
                }
            }
        }
        //下
        if (y < mapH - 1 && !IsDownBlock(pxaC))
        {
            BYTE pxaD = mapData[(y + 1) * mapW + x];
            if (!IsPathBlock(pxaD))
            {
                //如果下方能写入
                auto& down = grid->vecMap.at((y + 1) * mapW + x).dis;
                if (down > nowVal + 1)
                {
                    down = nowVal + 1.0f;
                    qe.push(MAKELONG(x, y + 1));
                }
            }
        }
        //左
        if (x > 0 && !IsLeftBlock(pxaC))
        {
            BYTE pxaL = mapData[y * mapW + x - 1];
            if (!IsPathBlock(pxaL))
            {
                //如果左边能写入
                auto& left = grid->vecMap.at(y * mapW + x - 1).dis;
                if (left > nowVal + 1)
                {
                    left = nowVal + 1.0f;
                    qe.push(MAKELONG(x - 1, y));
                }
            }
        }
        //右
        if (x < mapW - 1 && !IsRightBlock(pxaC))
        {
            BYTE pxaR = mapData[y * mapW + x + 1];
            if (!IsPathBlock(pxaR))
            {
                //如果右边能写入
                auto& right = grid->vecMap.at(y * mapW + x + 1).dis;
                if (right > nowVal + 1)
                {
                    right = nowVal + 1.0f;
                    qe.push(MAKELONG(x + 1, y));
                }
            }
        }
    }
}

void PathfindingE::StepVector(PathGrid* grid, int X, int Y)
{
    auto& vecMap = grid->vecMap;
    //计算向量场
    auto& nowCell = vecMap.at(Y * mapW + X);
    auto nowDis = nowCell.dis;
    float disU = nowDis;
    float disD = nowDis;
    float disL = nowDis;
    float disR = nowDis;
    bool BlockU = false;
    bool BlockD = false;
    bool BlockL = false;
    bool BlockR = false;
    //上
    if (Y > 0)
    {
        float v = vecMap.at((Y - 1) * mapW + X).dis;
        if (v <= maxLen)
            disU = v;
        else
            BlockU = true;
    }
    //下
    if (Y < mapH - 1)
    {
        float v = vecMap.at((Y + 1) * mapW + X).dis;
        if (v <= maxLen)
            disD = v;
        else
            BlockD = true;
    }
    //左
    if (X > 0)
    {
        float v = vecMap.at(Y * mapW + X - 1).dis;
        if (v <= maxLen)
            disL = v;
        else
            BlockL = true;
    }
    //右
    if (X < mapW - 1)
    {
        float v = vecMap.at(Y * mapW + X + 1).dis;
        if (v <= maxLen)
            disR = v;
        else
            BlockR = true;
    }
    //生成向量
    auto vec = XMVector2Normalize(XMVectorSet(disL - disR, disU - disD, 0, 0));
    nowCell.vec1_x = XMVectorGetX(vec);
    nowCell.vec1_y = XMVectorGetY(vec);
    //阻挡
    if ((BlockU && nowCell.vec1_y < 0) ||
        (BlockD && nowCell.vec1_y > 0))
    {
        if (nowCell.vec1_x > 0)
            nowCell.vec1_x = 1;
        else if (nowCell.vec1_x < 0)
            nowCell.vec1_x = -1;
        nowCell.vec1_y = 0;
    }
    if ((BlockL && nowCell.vec1_x < 0) ||
        (BlockR && nowCell.vec1_x > 0))
    {
        if (nowCell.vec1_y > 0)
            nowCell.vec1_y = 1;
        else if (nowCell.vec1_y < 0)
            nowCell.vec1_y = -1;
        nowCell.vec1_x = 0;
    }
    //如果上下距离一样
    if (disU == disD && disU != nowDis)
    {
        if (disL < nowDis)
        {
            nowCell.vec1_x = -1;
            nowCell.vec1_y = 0;
        }
        else if (disR < nowDis)
        {
            nowCell.vec1_x = 1;
            nowCell.vec1_y = 0;
        }
        //左右边是墙
        else if (disL == nowDis || disR == nowDis)
        {
            //往上
            nowCell.vec1_x = 0;
            nowCell.vec1_y = -1;
        }
    }
    //如果左右距离一样
    if (disL == disR && disL != nowDis)
    {
        if (disU < nowDis)
        {
            nowCell.vec1_x = 0;
            nowCell.vec1_y = -1;
        }
        else if (disD < nowDis)
        {
            nowCell.vec1_x = 0;
            nowCell.vec1_y = 1;
        }
        //上下边是墙
        else if (disU == nowDis || disD == nowDis)
        {
            //往左
            nowCell.vec1_x = -1;
            nowCell.vec1_y = 0;
        }
    }

    //上下左右都可走时，往左
    if (disU == disD && disL == disR && disL != nowDis)
    {
        nowCell.vec1_x = -1;
        nowCell.vec1_y = 0;
    }
}
