/* 云風的求解最短路徑代碼 (Cloud Wu's Pathfinding code)
 *                          1999 年 1月 8 日 (1999, Jan 8)
 * 這段代碼沒有進行任何優(yōu)化(包括算法上的), 但不意味我不知道該怎樣優(yōu)化它，
 * 它是為教學目的而做,旨在用易于理解和簡潔的代碼描述出 A* 算法在求最段路
 * 徑中的運用. 由于很久沒有摸算法書, 本程序不能保證是純正的 A* 算法 ;-)
 * 你可以在理解了這段程序的基礎上,按自己的理解寫出類似的代碼. 但是簡單的
 * 復制它到你的程序中是不允許的,如果你真要這樣干,請在直接使用它的軟件的
 * 文檔中,寫上我的名字 ;-)
 * 有任何的問題,或建議請 E-mail 到 cloudwu@263.net 
 * 歡迎參觀我的主頁 http://www.netease.com/~cloudwu (云風工作室)
 * (你可以在上面找到一些有關這個問題的討論,和有關游戲設計的其它大量資料)
 *
 * 本程序附帶有一個數(shù)據(jù)文件 map.dat, 保存有地圖的數(shù)據(jù)
 */

// #define NDEBUG
#include <stdio.h>
#include <conio.h>
#include <assert.h>
#include <stdlib.h>
#define MAPMAXSIZE 100  //地圖面積最大為 100x100
#define MAXINT 8192     //定義一個最大整數(shù), 地圖上任意兩點距離不會超過它
#define STACKSIZE 65536 //保存搜索節(jié)點的堆棧大小

#define tile_num(x,y) ((y)*map_w+(x))  //將 x,y 坐標轉換為地圖上塊的編號
#define tile_x(n) ((n)%map_w)          //由塊編號得出 x,y 坐標
#define tile_y(n) ((n)/map_w)

// 樹結構, 比較特殊, 是從葉節(jié)點向根節(jié)點反向鏈接
typedef struct node *TREE;

struct node {
	int h;
        int tile;
	TREE father;
	} ;

typedef struct node2 *LINK;

struct node2 {
       TREE node;
       int f;
       LINK next;
       };

LINK queue;               // 保存沒有處理的行走方法的節(jié)點
TREE stack[STACKSIZE];    // 保存已經處理過的節(jié)點 (搜索完后釋放)
int stacktop;
unsigned char map[MAPMAXSIZE][MAPMAXSIZE];   //地圖數(shù)據(jù)
int dis_map[MAPMAXSIZE][MAPMAXSIZE];         //保存搜索路徑時,中間目標地最優(yōu)解

int map_w,map_h;                             //地圖寬和高
int start_x,start_y,end_x,end_y;             //地點,終點坐標

// 初始化隊列
void init_queue()
{
 queue=(LINK)malloc(sizeof(*queue));
 queue->node=NULL;
 queue->f=-1;
 queue->next=(LINK)malloc(sizeof(*queue));
 queue->next->f=MAXINT;
 queue->next->node=NULL;
 queue->next->next=NULL;
}

// 待處理節(jié)點入隊列, 依靠對目的地估價距離插入排序
void enter_queue(TREE node,int f)
{
 LINK p=queue,father,q;
 while(f>p->f) {
   father=p;
   p=p->next;
   assert(p);
   }
 q=(LINK)malloc(sizeof(*q));
 assert(queue);
 q->f=f,q->node=node,q->next=p;
 father->next=q;
}

// 將離目的地估計最近的方案出隊列
TREE get_from_queue()
{
 TREE bestchoice=queue->next->node;
 LINK next=queue->next->next;
 free(queue->next);
 queue->next=next;
 stack[stacktop++]=bestchoice;
 assert(stacktop<STACKSIZE);
 return bestchoice;
}

// 釋放棧頂節(jié)點
void pop_stack()
{
 free(stack[--stacktop]);
}

// 釋放申請過的所有節(jié)點
void freetree()
{
 int i;
 LINK p;
 for (i=0;i<stacktop;i++)
    free(stack[i]);
 while (queue) {
   p=queue;
   free(p->node);
   queue=queue->next;
   free(p);
   }
}

// 估價函數(shù),估價 x,y 到目的地的距離,估計值必須保證比實際值小
int judge(int x,int y)
{
 int distance;
 distance=abs(end_x-x)+abs(end_y-y);
 return distance;
}

// 嘗試下一步移動到 x,y 可行否
int trytile(int x,int y,TREE father)
{
 TREE p=father;
 int h;
 if (map[y][x]!=' ') return 1; // 如果 (x,y) 處是障礙,失敗
 while (p) {
   if (x==tile_x(p->tile) && y==tile_y(p->tile)) return 1; //如果 (x,y) 曾經經過,失敗
   p=p->father;
   }
 h=father->h+1;
 if (h>=dis_map[y][x]) return 1; // 如果曾經有更好的方案移動到 (x,y) 失敗
 dis_map[y][x]=h; // 記錄這次到 (x,y) 的距離為歷史最佳距離

// 將這步方案記入待處理隊列
 p=(TREE)malloc(sizeof(*p));
 p->father=father;
 p->h=father->h+1;
 p->tile=tile_num(x,y);
 enter_queue(p,p->h+judge(x,y));
 return 0;
}

// 路徑尋找主函數(shù)
void findpath(int *path)
{
 TREE root;
 int i,j;
 stacktop=0;
 for (i=0;i<map_h;i++)
     for (j=0;j<map_w;j++)
         dis_map[i][j]=MAXINT;
 init_queue();
 root=(TREE)malloc(sizeof(*root));
 root->tile=tile_num(start_x,start_y);
 root->h=0;
 root->father=NULL;
 enter_queue(root,judge(start_x,start_y));
 for (;;) {
    int x,y,child;
    TREE p;
    root=get_from_queue();
    if (root==NULL) {
      *path=-1;
      return;
    }
    x=tile_x(root->tile);
    y=tile_y(root->tile);
    if (x==end_x && y==end_y) break; // 達到目的地成功返回

    child=trytile(x,y-1,root);  //嘗試向上移動
    child&=trytile(x,y+1,root); //嘗試向下移動
    child&=trytile(x-1,y,root); //嘗試向左移動
    child&=trytile(x+1,y,root); //嘗試向右移動
    if (child!=0)
       pop_stack();  // 如果四個方向均不能移動,釋放這個死節(jié)點
    }

// 回溯樹，將求出的最佳路徑保存在 path[] 中
 for (i=0;root;i++) {
    path[i]=root->tile;
    root=root->father;
    }
 path[i]=-1;
 freetree();
}

void printpath(int *path)
{
 int i;
 for (i=0;path[i]>=0;i++) {
    gotoxy(tile_x(path[i])+1,tile_y(path[i])+1);
    cprintf("\xfe");
    }
}

int readmap()
{
 FILE *f;
 int i,j;
 f=fopen("map.dat","r");
 assert(f);
 fscanf(f,"%d,%d\n",&map_w,&map_h);
 for (i=0;i<map_h;i++)
	fgets(&map[i][0],map_w+1,f);
 fclose(f);
 start_x=-1,end_x=-1;
 for (i=0;i<map_h;i++)
     for (j=0;j<map_w;j++) {
         if (map[i][j]=='s') map[i][j]=' ',start_x=j,start_y=i;
         if (map[i][j]=='e') map[i][j]=' ',end_x=j,end_y=i;
         }
 assert(start_x>=0 && end_x>=0);
 return 0;
}

void showmap()
{
 int i,j;
 clrscr();
 for (i=0;i<map_h;i++) {
    gotoxy(1,i+1);
    for (j=0;j<map_w;j++)
       if (map[i][j]!=' ') cprintf("\xdb");
       else cprintf(" ");
    }
 gotoxy(start_x+1,start_y+1);
 cprintf("s");
 gotoxy(end_x+1,end_y+1);
 cprintf("e");
}

int main()
{
 int path[MAXINT];
 readmap();
 showmap();
 getch();
 findpath(path);
 printpath(path);
 getch();
 return 0;
}