/*===============================================================
  通用最短路径A*算法优化求解
  Common shortest path finding in A*

  作者 林伟 于2001年12月19日
  Written by Wei Lin On Dec.19 2001

  电子科技大学通讯工程学院20013080班
  Uestc Dept.1 - 20013080
 */

#ifndef _ASTAR_H_
#define _ASTAR_H_

// 从上面#ifndef开始到#endif结束是头文件内容
typedef unsigned long ADWORD;
class AstarPathFind
{
protected:
    struct TAstarNode
    {
        ADWORD Pos;         // 该点的坐标(16,16)的模式保存(y,x)
        short ActualCost;   // 保存从起点到该节点的实际开销
        short EstimateCost; // 保存此点的估价开销
        short SumCost;      // 上面两者之和
        TAstarNode *Father; // 此点的父节点
        TAstarNode *Prev;   // 在Open或者Next中的上一个节点
        TAstarNode *Next;   // 在Open或者Next链表中的下一个节点
        char Modified;      // 该节点是否被修改过，记录而备清除1空,2 Open,4 Close
        short DirWalked;    // 有字节点的方向(未用)
        short DirFrom;      // 从哪个方向来的(未用)
    };
    TAstarNode **Node;      // 对应地图中每个节点
    TAstarNode *Open;       // 保存没有处理的按估计值排序的节点
    TAstarNode *Close;      // 保存处理过的节点
    TAstarNode **Modify;    // 保存修改过的节点
    int height;             // 地图的高度
    int width;              // 地图的宽度
    ADWORD MaxSize;            // 最大面积即height*width
    ADWORD ModifyPoint;        // Modify数组的指针
    ADWORD OpenCount;          // Open队列中的节点数目
    ADWORD CloseCount;         // Close队列里面的节点数目
    ADWORD OpenLimited;        // Open队列最多节点数目
    ADWORD CloseLimited;       // Close队列最多节点数目
    short DirMask;             // 要搜索方向的标志，0-7位为从上开始顺时针七个方向
    ADWORD MinPos;             // 终点或最接近终点的节点
    short TargetX;             // 终点坐标
    short TargetY;            
    char (*MoveAble)(short x,short y);             // 检查地图是否可以移动函数指针
    short (*JudgeCost)(short,short,short,short);   // 取得估计值的函数指针
    char AddToOpenQueue(TAstarNode *node);         // 将节点排序加入Open队列
    char GetFromOpenQueue();                       // 从Open队列取出最小的并放入Close队列
    char PopCloseStack();                          // 取出Close队列中的节点
    void ClearModified();                          // 清除所有搜索记录
    char TryTile(short x,short y,TAstarNode *Father,char FromDir);
public:
    int Create(int map_w,int map_h,char (*MoveCheck)(short x,short y),short (*JudgeFun)(short x,short y,short dx,short dy));
    int Release();
    virtual int FindPath(short startx,short starty,short endx,short endy);
    int GetPosPath(short *PosXY,int maxpos);
    int GetDirPath(char *Dirs,int maxpos);

    void SetJudgeFun(short (*JudgeFun)(short,short,short,short));
    void SetMapCheck(char (*MapCheck)(short,short));
    void SetOpenLimited(long OpenLimited);
    void SetCloseLimited(long CloseLimited);
    void SetDirMask(long DirMask);
};

#endif


#include <stdio.h>

#define COST_MAX 30000
#define tile_pos(x,y) (((ADWORD)y<<16)|x)
#define tile_x(pos)   (pos&0xffff)
#define tile_y(pos)   (pos>>16)

// 待处理节点入队列, 依靠对目的地估价距离插入排序
char AstarPathFind::AddToOpenQueue(TAstarNode *node)
{
    ADWORD i;
    short f=node->SumCost;
    register TAstarNode *p,*b;
    node->Modified|=2;    // 记录Open标志
    for (b=NULL,p=Open,i=0;p&&i<=OpenCount;b=p,p=p->Next,i++) 
        if (f<=p->SumCost) break;
    if (i>OpenCount) return -2;
    node->Next=p;
    node->Prev=b;
    if (b) b->Next=node;
    else Open=node;
    if (p) p->Prev=node;
    OpenCount++;
    return 0;
}
// 将离目的地估计最近的方案出队列
char AstarPathFind::GetFromOpenQueue()
{
    TAstarNode *BestChoice=Open;
    if (!Open) return -1;
    Open=Open->Next;
    if (Open) Open->Prev=NULL;
    if (BestChoice->Modified&4) return -2; // 已经在Close中了
    BestChoice->Next=Close;
    BestChoice->Prev=NULL;
    BestChoice->Modified&=5;    // 清除Open标志
    BestChoice->Modified|=4;    // 设置Close标志
    if (Close) Close->Prev=BestChoice;
    Close=BestChoice;
    OpenCount--;
    CloseCount++;
    return 0;
}
// 释放栈顶节点
char AstarPathFind::PopCloseStack()
{
    if (Close) { 
        Close->Modified&=3;        // 清除Close标志
        Close=Close->Next;
        if (Close) Close->Prev=NULL;
        CloseCount--; 
        return 0; 
    }
    return -1;
}
// 还原修改过的所有节点
void AstarPathFind::ClearModified()
{
    ADWORD i;
    for (i=0;i<ModifyPoint;i++) {
        Modify[i]->Modified=0;
        Modify[i]->ActualCost=COST_MAX;
    }
    ModifyPoint=0;
    OpenCount=0;
    CloseCount=0;
    Open=NULL;
    Close=NULL;
}
// 尝试下一步移动到 x,y 可行否
char AstarPathFind::TryTile(short x,short y,TAstarNode *Father,char FromDir)
{
     register TAstarNode *node;
    short ActualCost;

    if (!MoveAble(x,y)) return 1;            // 如果地图无法通过则退出
    node=&Node[y][x];
    ActualCost=Father->ActualCost+1;        // 实际值计算
    if (ActualCost>=node->ActualCost) return 1;
    if (node->Modified&2)                    // 在Open表中就清除
    {
        if (node->Next) node->Next->Prev=node->Prev;    
        if (node->Prev) node->Prev->Next=node->Next;
        else Open=node->Next;
        OpenCount--;
        node->Modified=1;
        node->Father=Father;
        node->ActualCost=ActualCost;
        node->SumCost=ActualCost+node->EstimateCost;
        AddToOpenQueue(node);
    }    else if (node->Modified&4) {        // 在Close表中就清除
        if (node->Next) node->Next->Prev=node->Prev;
        if (node->Prev) node->Prev->Next=node->Next;
        else Close=node->Next;
        CloseCount--;
        node->Modified=1;
        node->Father=Father;
        node->ActualCost=ActualCost;
        node->SumCost=ActualCost+node->EstimateCost;
        AddToOpenQueue(node);
    }    else {
        if (!(node->Modified&1)) Modify[ModifyPoint++]=node; // 记录这个修改过的点以还原
        node->Modified=1;
        node->Father=Father;
        node->DirFrom=FromDir;
        node->ActualCost=ActualCost;
        node->EstimateCost=JudgeCost(x,y,TargetX,TargetY);
        node->SumCost=ActualCost+node->EstimateCost;
        AddToOpenQueue(node);            // 将节点加入Open队列
    }
    return 0;
}
// 路径寻找主函数
int AstarPathFind::FindPath(short startx,short starty,short endx,short endy)
{
    TAstarNode *root;
    int j,ok;
    short x,y;
    ADWORD max=0;
    short MinJudge;

    if (!Node||!Modify) return -1;
    ClearModified();
    root=&Node[starty][startx];
    root->ActualCost=0;
    root->EstimateCost=JudgeCost(startx,starty,endx,endy);
    root->SumCost=root->EstimateCost;
    root->Father=NULL; 
    root->Modified=1;
    Modify[ModifyPoint++]=root;
    AddToOpenQueue(root);
    MinPos=tile_pos(startx,starty);
    MinJudge=root->EstimateCost;
    TargetX=endx;
    TargetY=endy;
    for (ok=0;ok==0;)
    {
        ok=GetFromOpenQueue();        // 取出Open队列估价值最小的节点放入Close中
        root=Close;                    // 得到刚才取出的节点
        if (ok||root==NULL) { ok=-1; break; }
        x=(short)tile_x(root->Pos);
        y=(short)tile_y(root->Pos);
        if (root->EstimateCost<MinJudge)    // 找到一个估计离终点最近的点
            MinJudge=root->EstimateCost, MinPos=root->Pos;
        if (CloseCount>max) max=CloseCount;
        if (x==endx&&y==endy) MinPos=root->Pos,ok=1;    // 如果走到终点了
        else {
            j=1;
            if (DirMask&1) j&=TryTile(x,y-1,root,0);
            if (DirMask&2) j&=TryTile(x+1,y-1,root,0);
            if (DirMask&4) j&=TryTile(x+1,y,root,0);
            if (DirMask&8) j&=TryTile(x+1,y+1,root,0);
            if (DirMask&16) j&=TryTile(x,y+1,root,0);
            if (DirMask&32) j&=TryTile(x-1,y+1,root,0);
            if (DirMask&64) j&=TryTile(x-1,y,root,0);
            if (DirMask&128) j&=TryTile(x-1,y-1,root,0);
            if (j) if (PopCloseStack()) { ok=-2; break; }    // 如果不是通路则从Close中取出
        }
        if (OpenCount>=OpenLimited||CloseCount>=CloseLimited) ok=2;
    }
    if (ok<0) return -2;
    return 0;
}
int AstarPathFind::GetPosPath(short *PosXY,int maxpos)
{
    short x,y;
    ADWORD j;
    TAstarNode *p,*s;
    int i;
    if (maxpos>1) maxpos--;
    for (p=&Node[tile_y(MinPos)][tile_x(MinPos)],s=p,j=0;p&&j<MaxSize;p=p->Father,j++)
    {
        x=(short)tile_x(p->Pos);
        y=(short)tile_y(p->Pos);
        i=(p->ActualCost<maxpos)?p->ActualCost:maxpos;
        PosXY[i<<1]=x;
        PosXY[(i<<1)+1]=y;
    }
    i=(s->ActualCost+1<maxpos)?(s->ActualCost+1):maxpos;
    PosXY[i*2]=-1;
    PosXY[i*2+1]=-1;
    return 0;
}
int AstarPathFind::GetDirPath(char *PosDir,int maxpos)
{
    static char inc2r[10]={ 7,0,1,6,8,2,5,4,3,0 }; 
    short i,x,y,nx,ny;
    ADWORD j;
    TAstarNode *p,*s;
    x=(short)tile_x(MinPos);
    y=(short)tile_y(MinPos);
    if (maxpos>1) maxpos--;
    for (p=&Node[y][x],s=p,j=0;p&&j<MaxSize;p=p->Father,j++)
    {
        nx=(short)tile_x(p->Pos);
        ny=(short)tile_y(p->Pos);
        i=(p->ActualCost<maxpos)?(p->ActualCost):maxpos;
        PosDir[i]=inc2r[(y-ny+1)*3+x-nx+1];
        x=nx;
        y=ny;
    }
    i=(s->ActualCost+1<maxpos)?(s->ActualCost+1):maxpos;
    PosDir[i]=8;
    return 0;
}
int AstarPathFind::Create(int mapw,int maph,char (*MoveCheck)(short,short),short (*JudgeFun)(short x,short y,short,short))
{
    int i,j;

    height=maph;    width=mapw;
    MaxSize=maph;    MaxSize*=mapw;
    Modify=new TAstarNode*[MaxSize*2];
    if (!Modify) return -1;
    Node=new TAstarNode*[maph];
    if (!Node) { delete Modify; Modify=NULL; return -1; }
    for (i=0;i<maph;i++) Node[i]=new TAstarNode[mapw];
    for (i=0,j=1;i<maph;i++) if (!Node[i]) j=0;
    if (!j)
    {
        for (i=0;i<maph;i++) if (Node[i]) delete Node[i];
        delete Node;
        delete Modify;
        Node=NULL;
        Modify=NULL;
        return -2;
    }
    for (j=0;j<maph;j++) 
        for (i=0;i<mapw;i++) 
        {
            Node[j][i].Pos=tile_pos(i,j);
            Node[j][i].Modified=0;
            Node[j][i].ActualCost=COST_MAX;
        }
    ModifyPoint=0;
    SetMapCheck(MoveCheck);
    SetJudgeFun(JudgeCost=JudgeFun);
    SetDirMask(0x55);
    SetOpenLimited(200);
    SetCloseLimited(MaxSize/20);
    return 0;
}
int AstarPathFind::Release()
{
    int j;
    if (Node) for (j=0;j<height;j++) if (Node[j]) delete Node[j];
    if (Node) delete Node;
    if (Modify) delete Modify;
    Node=NULL;
    Modify=NULL;
    return 0;
}
void AstarPathFind::SetJudgeFun(short (*JudgeFun)(short,short,short,short)) { JudgeCost=JudgeFun; }
void AstarPathFind::SetMapCheck(char (*MapCheck)(short,short)) { MoveAble=MapCheck; }
void AstarPathFind::SetOpenLimited(long OL) { OpenLimited=OL; }
void AstarPathFind::SetCloseLimited(long CL) { CloseLimited=CL; }
void AstarPathFind::SetDirMask(long DM) { DirMask=(short)DM; }
/////////////////////////////////////////////////////////////////////
#include <conio.h>
#include <stdlib.h>
#include <ctype.h>

char map[100][100];
int maph,mapw;

static short incx[8]={ 0, 1, 1, 1, 0, -1, -1, -1 };
static short incy[8]={ -1, -1, 0, 1, 1, 1, 0, -1 };

AstarPathFind PathFind;
int startx,starty,endx,endy;
short Judge(short x,short y,short endx,short endy) 
{ 
    short dx=abs(x-endx);
    short dy=abs(y-endy);
    return (dx+dy)<<3;
}
char Possible(short x,short y) 
{ 
    if (x<0||y<0||x>=mapw||y>=maph) return 0;
    return (map[y][x]==' ')?1:0; 
}
int readmap()
{
    int i,j;
    FILE *f;
    if ((f=fopen("map.dat","r"))==NULL) return -1;
    fscanf(f,"%d,%d\n",&mapw,&maph);
    for (j=0;j<maph;j++) fgets(map[j],mapw+2,f);
    fclose(f);
    for (j=0;j<maph;j++) for (i=0;i<mapw;i++)
    {
        if (map[j][i]=='s'||map[j][i]=='S') startx=i,starty=j,map[j][i]=' ';
        if (map[j][i]=='e'||map[j][i]=='E') endx=i,endy=j,map[j][i]=' ';
    }
    return 0;
}
#include <time.h>
#define TIMES 512
int main(void)
{
    int i,j;
    int x,y;
    long t;
    double f;
    if (readmap()) { printf("no map file\n"); return -1; } 
    short PosXY[500];
    char  PosR[500];
    for (j=0;j<maph;j++) {
        for (i=0;i<mapw;i++) putchar(map[j][i]);
        putchar('\n');
    }
    i=PathFind.Create(mapw,maph,Possible,Judge);
    if (i) {
        printf("创建失败\n");
        return -1;
    }
    PathFind.SetCloseLimited(79*24L);
    PathFind.SetDirMask(0xff);
    PathFind.FindPath(startx,starty,endx,endy);
    PathFind.GetPosPath(PosXY,250);
    PathFind.GetDirPath(PosR,500);
    getch();
    for (i=0;PosXY[i*2]>=0;i++)
    {
        x=PosXY[i*2];
        y=PosXY[i*2+1];
        map[y][x]='.';
    }
    for (j=0;j<maph;j++) {
        for (i=0;i<mapw;i++) 
        {
            putchar(map[j][i]);
            if (map[j][i]=='.') map[j][i]=' ';
        }
        putchar('\n');
    }
    printf("Any key to Testing speed, finding 512 paths...\n");
    getch();
    for (t=clock();t==clock(););
    for (i=0,t=clock();i<TIMES;i++) 
    {
        PathFind.FindPath(startx,starty,endx,endy);
        PathFind.GetPosPath(PosXY,250);
    }
    t=clock()-t;
    f=TIMES*CLK_TCK;
    f/=t;
    PathFind.Release();
    printf("Time to find %d paths is %ld TCK use! freq=%f   \n",TIMES,t,f);
    getch();
    return 0;
}

/*===============================================================================
原始算法    98.088421次每秒
算法记录一 152.760656次每妙
算法记录二 157.938983次每妙
算法记录三 179.200000次每秒
用VC优化   190.334572次每妙
地图文件:map.dat
79,24
ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo
o                o                                                            o
o ooooooo        o                                                            o
o       o        o           oooooooooooooo                                   o
o    s  o                                 o                                   o
oooooooooooooooooo                        o                                   o
o                oooooooooooooooooooooooooo       oooooooo                    o
o    oooooo      o     oooo                       o      o                    o
o                o        o                       ooo  ooo                    o
o                oooo  oooo                                                   o
o                         o      oooooooooooooooooooooooooooooooooooooooooooooo
o  oooooooooooooooooooooooo                                                   o
o                                                                             o
o                                                                             o
ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo             o
o       o                                                           ooooooooooo
o       o   ooooooo                                             o             o
o       o         o                                             o             o
o       ooooooooooo        oooooooooo                           o             o
o                          o     e  ooo                         o             o
o                          ooooo      o                         o             o
o                              o      oooooooooooooooooo oooooooo             o
o                              o                                              o
ooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooooo
---------------------------------------------------------------------------------*/