#include<stdio.h>
#include<stdlib.h>
#define N 32
#define W 10

typedef struct STARNODE{
    int x;//節點的x,y座標
    int y;
    int G;//該節點的G, H值
    int H;
    int is_snakebody;//是否為蛇身，是為1，否則為0；
    int in_open_table;//是否在open_table中，是為1，否則為0；
    int in_close_table;//是否在close_table中，是為1，否則為0；
    struct STARNODE* ParentNode;//該節點的父節點
} starnode, *pstarnode;

starnode mapnode[N/2+2][N+4];

pstarnode opentable[N*N/2];
pstarnode closetable[N*N/2];

int opennode_count=0;
int closenode_count=0;
starnode food;

//根據指標所指向的節點的F值，按大頂堆進行調整
void heapadjust(pstarnode a[], int m, int n)
{
    int i;
    pstarnode temp=a[m];
    for(i=2*m;i<=n;i*=2)
    {
        if(i+1<=n && (a[i+1]->G+a[i+1]->H)>(a[i]->G+a[i]->H) )
        {
            i++;
        }
        if((temp->G+temp->H)>(a[i]->G+a[i]->H))
        {
            break;
        }
        a[m]=a[i];
        m=i;
    }
    a[m]=temp;
}

void swap(pstarnode a[],int m, int n)
{
    pstarnode temp;
    temp=a[m];
    a[m]=a[n];
    a[n]=temp;
}


void crtheap(pstarnode a[], int n)
{
    int i;
    for(i=n/2;i>0;i--)
    {
        heapadjust(a, i, n);
    }
}

void heapsort(pstarnode a[], int n)
{
    int i;
    crtheap(a,n);
    for(i=n;i>1;i--)
    {
        swap(a,1,i);
        heapadjust(a, 1,i-1);
    }
}

//x1, y1是鄰域點座標
//curtnode是當前點座標
void insert_opentable(int x1, int y1, pstarnode pcurtnode)
{
    int i;
    if(!mapnode[x1][y1].is_snakebody && !mapnode[x1][y1].in_close_table)//如果不是蛇身也不在closetable中
    {
        if(mapnode[x1][y1].in_open_table && mapnode[x1][y1].G>pcurtnode->G+W)//如果已經在opentable中,但是不是最優路徑
        {
            mapnode[x1][y1].G=pcurtnode->G+W;//把G值更新
            mapnode[x1][y1].ParentNode=pcurtnode;//把該鄰點的雙親節點更新
            //由於改變了opentable中一個點的F值，需要對opentable中的點的順序進行調整，以滿足有序
            for(i=1;i<=opennode_count;i++)
            {
                if(opentable[i]->x==x1 && opentable[i]->y==y1)
                {
                    break;
                }
                heapsort(opentable, i);
            }
        }
        else//把該點加入opentable中
        {
           opentable[++opennode_count]=&mapnode[x1][y1];

           mapnode[x1][y1].G=pcurtnode->G+W;
           mapnode[x1][y1].H=(abs(food.x-x1)+abs(food.y-y1))*W;
           mapnode[x1][y1].in_open_table=1;
           mapnode[x1][y1].ParentNode=pcurtnode;
           heapsort(opentable, opennode_count);
        }
    }
}

//尋找當前點的四鄰域點，把符合條件的點加入opentable中
void find_neighbor(pstarnode pcurtnode)
{
    int x=pcurtnode->x;
    int y=pcurtnode->y;

    if(x+1<=N/2)
    {
        insert_opentable(x+1, y, pcurtnode);
    }
    if(x-1>=1)
    {
        insert_opentable(x-1, y, pcurtnode);
    }
    if(y+1<=N+1)
    {
        insert_opentable(x,y+1, pcurtnode);
    }
    if(y-1>=2)
    {
        insert_opentable(x,y-1, pcurtnode);
    }
}

int search_road(pstarnode startnode, pstarnode endnode)
{
    int is_search_road=0;
    opennode_count=0;
    closenode_count=0;
    pstarnode pcurtnode;

    opentable[++opennode_count]=startnode;//起始點加入opentable中
    startnode->in_open_table=1;
    startnode->ParentNode=NULL;
    startnode->G=0;
    startnode->H=(abs(endnode->x-startnode->x)+abs(endnode->y-startnode->y))*W;

    if(startnode->x==endnode->x && startnode->y==endnode->y)//如果起點和終點重合
    {
        is_search_road=1;
        return is_search_road;
    }

    while(1)
    {
        //取出opentable中第1個節點加入closetable中
        pcurtnode=opentable[1];
        opentable[1]=opentable[opennode_count--];

        closetable[++closenode_count]=pcurtnode;
        pcurtnode->in_open_table=0;
        pcurtnode->in_close_table=1;

        if(pcurtnode->x==endnode->x && pcurtnode->y==endnode->y)
        {
            is_search_road=1;
            break;
        }

        find_neighbor(pcurtnode);

        if(!opennode_count)//如果opentable已經為空，即沒有找到路徑
        {
            break;
        }
    }

    return is_search_road;
}

int main(void)
{
    int i, j;
    pstarnode startnode;

    for(i=0;i<N/2+2;i++)
        for(j=0;j<N+4;j++)
        {
            mapnode[i][j].G=0;
            mapnode[i][j].H=0;
            mapnode[i][j].in_close_table=0;
            mapnode[i][j].in_open_table=0;
            mapnode[i][j].is_snakebody=0;
            mapnode[i][j].ParentNode=NULL;
            mapnode[i][j].x=i;
            mapnode[i][j].y=j;
        }

    startnode=&mapnode[5][10];
    food.x=5;
    food.y=15;
    mapnode[5][13].is_snakebody=1;
    mapnode[6][13].is_snakebody=1;
    mapnode[4][13].is_snakebody=1;
    mapnode[4][12].is_snakebody=1;
    mapnode[6][12].is_snakebody=1;

    int flag;
    flag=search_road(startnode, &food);
    pstarnode temp=&mapnode[5][15];

    do{
        printf("%d %d\n",temp->x, temp->y);
        temp=temp->ParentNode;
    }while(temp);

    return 0;
}