/*========================================================================
精简的A*算法 作者：添翼虎
网址：http://tyhweb.163.net Email:tyhweb@163.net
本程序参考了风云的最短路径代码(http://member.nease.com/~cloudwu)，
并加以改进和优化：
1、把原来用于存放已处理节点的堆栈改为(store_queue)队列，这样在从
sort_queue队列出列时可直接放入store_queue中。
2、解除了地图大小的限制(如果有64K内存限制时，地图大小只能是180x180)
3、删除了原程序中的一些冗余，见程序中的注释。
4、程序继续使用dis_map数组保存各点历史历史最佳距离，也包含了某点是否已经
经过的信息，虽然这样做可能会比使用链表多用一些内存，但是在搜索时可以
节省不时间。
5、程序更具有实用性，可直接或修改后运用于你的程序中，但请你使用该代码后
应该返回一些信息给我，如算法的改进或使用于什么程序等。
本程序可以用Borland C++或DJGPP编译，并附带有一个数据文件 map.dat,
保存有地图的数据，（注：该地图文件格式与风云的原代码的地图格式不一样）
-------------------------------------------------------------------------*/

//#define NDEBUG

#include <stdio.h>
#include <conio.h>
#include <assert.h>
#include <stdlib.h>

#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)

#define MAPMAXSIZE 180 //地图面积最大为 180x180，如果没有64K内存限制可以更大

#define MAXINT 32767

//树结构, 比较特殊, 是从叶节点向根节点反向链接，方便从叶节点找到根节点

typedef struct tree_node *TREE;

struct tree_node {
int h; //节点所在的高度，表示从起始点到该节点所有的步数

int tile; //该节点的位置

TREE father; //该节点的上一步

};

//链接结构，用于保存处理过的和没有处理过的结点

typedef struct link_node *LINK;

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

LINK sort_queue; // 保存没有处理的行走方法的节点

LINK store_queue; // 保存已经处理过的节点 (搜索完后释放)


unsigned char * map; //地图数据

unsigned int * dis_map; //保存搜索路径时,中间目标地最优解


int map_w,map_h; //地图宽和高

int start_x,start_y,end_x,end_y; //地点,终点坐标


// 初始化队列

void init_queue()
{
sort_queue=(LINK)malloc(sizeof(*sort_queue));
sort_queue->node=NULL;
sort_queue->f=-1;
sort_queue->next=(LINK)malloc(sizeof(*sort_queue));
sort_queue->next->node=NULL;
sort_queue->next->f=MAXINT;
sort_queue->next->next=NULL;

store_queue=(LINK)malloc(sizeof(*store_queue));
store_queue->node=NULL;
store_queue->f=-1;
store_queue->next=NULL;
}

// 待处理节点入队列, 依靠对目的地估价距离插入排序

void enter_queue(TREE node,int f)
{
LINK p=sort_queue,father,q;
while(f>p->f) {
father=p;
p=p->next;
assert(p);
}
q=(LINK)malloc(sizeof(*q));
assert(sort_queue);
q->f=f,q->node=node,q->next=p;
father->next=q;
}

// 将离目的地估计最近的方案出队列

TREE get_from_queue()
{
LINK bestchoice=sort_queue->next;
LINK next=sort_queue->next->next;
sort_queue->next=next;

bestchoice->next=store_queue->next;
store_queue->next=bestchoice;
return bestchoice->node;
}

// 释放栈顶节点

void pop_stack()
{
LINK s=store_queue->next;
assert(s);
store_queue->next=store_queue->next->next;
free(s->node);
free(s);
}

// 释放申请过的所有节点

void freetree()
{
int i;
LINK p;
while(store_queue){
p=store_queue;
free(p->node);
store_queue=store_queue->next;
free(p);
}
while (sort_queue) {
p=sort_queue;
free(p->node);
sort_queue=sort_queue->next;
free(p);
}
}

// 估价函数,估价 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[tile_num(x,y)]!=' ') return 1; // 如果 (x,y) 处是障碍,失败

//这一步用来判断(x,y)点是否已经加入队列，多余可以删除，因为dis_map已经

//保存该点是否已经保存

//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[tile_num(x,y)]) return 1; // 如果曾经有更好的方案移动到 (x,y) 失败

dis_map[tile_num(x,y)]=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;
}

// 路径寻找主函数

int * findpath(void)
{
TREE root;
int i,j;
int * path;
memset(dis_map,0xff,map_h*map_w*sizeof(*dis_map)); //填充dis_map为0XFF，表示各点未曾经过

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) {
return NULL;
}
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); //尝试向右移动

//child&=trytile(x+1,y-1,root);//尝试向右上移动

//child&=trytile(x+1,y+1,root); //尝试向右下移动

//child&=trytile(x-1,y+1,root); //尝试向左下移动

//child&=trytile(x-1,y-1,root); //尝试向左上移动


if (child!=0)
pop_stack(); // 如果四个方向均不能移动,释放这个死节点

}

// 回溯树，将求出的最佳路径保存在 path[] 中

path=(int*)malloc((root->h+2)*sizeof(int));
assert(path);
for (i=0;root;i++) {
path[i]=root->tile;
root=root->father;
}
path[i]=-1;
freetree();
return path;
}

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

int readmap()
{
FILE *f;
int i,j;
f=fopen("map.dat","r");
assert(f);
fscanf(f,"%d,%d\n",&map_w,&map_h);
map=malloc(map_w*map_h+1);
assert(map);
for(i=0;i<map_h;i++)
fgets(map+tile_num(0,i),map_w+2,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[tile_num(j,i)]=='s') map[tile_num(j,i)]=' ',start_x=j,start_y=i;
if (map[tile_num(j,i)]=='e') map[tile_num(j,i)]=' ',end_x=j,end_y=i;
}
assert(start_x>=0 && end_x>=0);
dis_map=malloc(map_w*map_h*sizeof(*dis_map));
assert(dis_map);
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[tile_num(j,i)]!=' ') cprintf("O");
else cprintf(" ");
}
gotoxy(start_x+1,start_y+1);
cprintf("s");
gotoxy(end_x+1,end_y+1);
cprintf("e");
}

int main()
{
int * path;
readmap();
showmap();
getch();
path=findpath();
printpath(path);
if(dis_map) free(dis_map);
if(path) free(path);
if(map) free(map);
getch();
return 0;
}