/* ■概要
 * 与えた迷路に対する最短ルートをダイクストラ法で探索するプログラム。
 * 
 * 
 * */

/* contact:隣接するノードの座標と個数。距離は常に1なので保持しない。 */
typedef struct
{
    int *index;
    int count;
}
contact;

/* path:スタートからノードまでのルートと距離。 */
typedef struct
{
    int *route;
    int length;
}
path;

 /* 迷路からノードの数を数える */
int dijkstra_count(char *map, int size)
{
    int i;
    int count=0;
    for(i=0;i<size;i++)
    {
        if(map[i]==' ') count++;
    }
    return count+2;
}
 /* リストからノードを削除する */
void dijkstra_remove(int *list, int id, int count)
{
    int i;
    for(i=0;i<count;i++)
    {
        if(list[i]==id)
        {
            list[i]=-1;
            return;
        }
    }
    return;
}
/* リストへノードを追加する */
void dijkstra_add(int *list, int id, int count)
{
    int i;
    for(i=0;i<count;i++)
    {
        if(list[i]==-1)
        {
            list[i]=id;
            return;
        }
    }
    return;
}
/* リストにノードが含まれれば1を、そうでなければ0を返す。 */
int dijkstra_contains(int *list, int id, int count)
{
    int i;
    for(i=0;i<count;i++)
    {
        if(list[i]==id) return 1;
    }
    return 0;
}
/* リストの中で距離が最も短いノードを求める */
int dijkstra_shortestPath(path *all, int *list, int count)
{
    int i, id = 0;
    for(i=0;i<count;i++)
    {
        if(list[i]>=0 && (!id || all[list[i]].length<all[id].length))
        {
            id=list[i];
        }
    }
    return id;
}
/* 与えられた座標が空白,S,Gのどれかであれば隣接するノードとして座標を記録する */
void dijkstra_initContact_add(contact *c, char* map, int index)
{
    if(map[index]==' '||map[index]=='S'||map[index]=='G')
    {
        c->index[c->count++]=index;
    }
    return;
}
/* 隣接するノードの情報を設定する */
void dijkstra_initContact(contact *c, char* map, int index, int col, int size)
{
    if(index-col>=0)     dijkstra_initContact_add(c,map,index-col);
    if(index-1  >=0)     dijkstra_initContact_add(c,map,index-1);
    if(index+1  <size) dijkstra_initContact_add(c,map,index+1);
    if(index+col<size) dijkstra_initContact_add(c,map,index+col);
    return;
}
int dijkstra(int *route, char *map, int col, int size)
{
    int i, j, k, V, W;
    int count;  /* ノードの個数 */
    int *index; /* id→index(座標) */
    int *id;    /* index→id(ノード番号) */
    int *uninvestigation; /* 未調査リスト */
    int *candidate;       /* 候補リスト */
    int *SPT;             /* Shortest Path Tree */
    path    *allPath;     /* ノードごとのパス情報   */
    contact *allContact;  /* ノードごとの隣接ノード */
    /* 初期化 */
    count = dijkstra_count(map, size);
    allPath    = (path*)    calloc(count, sizeof(path));
    allContact = (contact*) calloc(count, sizeof(contact));
    index      = (int*)     calloc(count, sizeof(int));
    id         = (int*)     calloc(size , sizeof(int));
    uninvestigation = (int*)calloc(count, sizeof(int));
    candidate       = (int*)calloc(count, sizeof(int));
    SPT             = (int*)calloc(count, sizeof(int));
    /* ノード初期化 */
    j = 0;
    for(i=0;i<count;i++)
    {
        allPath[i].route = (int*)calloc(count, sizeof(int)); /* ルートは最大でノード数となる */
        allContact[i].index = (int*)calloc(4, sizeof(int));  /* 隣接ノード数は最大4 */
        for(k=0;k<count;k++) allPath[i].route[k] = -1;
        /* id:0,1 はスタート・ゴールを設定 */
        if(i > 1) /* ここではそれ以外のノードを初期化 */
        {
            while(map[j] != ' ') j++;
            index[i] = j;
            id[j] = i;
            dijkstra_initContact(&allContact[i], map, index[i], col, size);
            j++;
        }
    }
    index[0] = 0; while(map[index[0]] != 'S') index[0]++; id[index[0]] = 0; dijkstra_initContact(&allContact[0], map, index[0], col, size); /* id:0 スタートを設定 */
    index[1] = 0; while(map[index[1]] != 'G') index[1]++; id[index[1]] = 1; dijkstra_initContact(&allContact[1], map, index[1], col, size); /* id:1 ゴールを設定 */
    /* リスト初期化 */
    for(i=0;i<count;i++){
        uninvestigation[i] = i; /* 全てのノードは初めは未調査リストへ入れる。 */
        candidate[i] = -1;
        SPT[i]       = -1;
    }
    /* スタートをSPTに追加しこれをVとする。 */
    V = 0;
    dijkstra_add(allPath[V].route, index[V], count);
    dijkstra_remove(uninvestigation, V, count);
    dijkstra_add(SPT, V, count);
    while(!dijkstra_contains(SPT, 1, count))
    {   /* SPTにゴールが含まれるまで繰り返す。 */
        for(i=0;i<allContact[V].count;i++)
        {    /* Vの隣接ノードすべてに対する処理 */
            W = id[allContact[V].index[i]]; /* 隣接ノードをWとする */
            if(dijkstra_contains(SPT, W, count))
            {   /* WがSPTに含まれる場合は何もしない。 */
                continue;
            }
            if(dijkstra_contains(candidate, W, count))
            {   /* Wが候補リストに含まれる場合は、その距離が候補より小さければ置き換える。*/
                if(allPath[W].length < allPath[V].length + 1)
                {
                    /* 距離の置き換え */
                    allPath[W].length = allPath[V].length + 1;
                    /* ルートの置き換え */
                    memcpy(allPath[W].route, allPath[V].route, count * sizeof(int));
                    dijkstra_add(allPath[W].route, index[W], count);
                }
                continue;
            }
            /* WがSPTにも候補リストにも含まれない場合は候補リストに追加する。 */
            dijkstra_remove(uninvestigation, W, count);
            dijkstra_add(candidate, W, count);
            /* 距離の設定 */
            allPath[W].length = allPath[V].length + 1;
            /* ルートの設定 */
            memcpy(allPath[W].route, allPath[V].route, count * sizeof(int));
            dijkstra_add(allPath[W].route, index[W], count);
        }
        /* 最もコストの小さい候補をSPTに追加しこれをVとする。 */
        V = dijkstra_shortestPath(allPath, candidate, count);
        dijkstra_remove(candidate, V, count);
        dijkstra_add(SPT, V, count);
    }
    /* スタートからゴールまでのルートの座標と距離を設定する */
    V = allPath[1].length;
    memcpy(route, allPath[1].route, count * sizeof(int));
    /* 領域の解放 */
    for(i=0;i<count;i++)
    {
        free(allPath[i].route);
        free(allContact[i].index);
    }
    free(allPath);
    free(allContact);
    free(index);
    free(id);
    free(uninvestigation);
    free(candidate);
    free(SPT);
    return V;
}

  /* 初期化 */
char *init(int *col, int *row)
{
    *col = 26;
    *row = 13;
    return
    "**************************"
    "*S* *                    *"
    "* * *  *  *************  *"
    "* *   *    ************  *"
    "*    *                   *"
    "************** ***********"
    "*                        *"
    "** ***********************"
    "*      *              G  *"
    "*  *      *********** *  *"
    "*    *        ******* *  *"
    "*       *                *"
    "**************************";
}
 /* ルートを記したマップを表示 */
void routePrint(char *map, int *route, int col, int row, int size)
{
    int r, r2; /* ルート判定用 */
    int i, j;
    char c;
    /* ルートの最小値取得 */
    j = 0; r2 = -1; r = route[0];
    while(route[++j]!=-1) if(r>route[j] && route[j]>r2) r=route[j];
    /* 表示 */
    for(i=0;i<size;i++)
    {
        c = map[i];
        if(i%col == 0) putchar('\n'); /* 改行 */
        if(i == r)
        {    /* 経路上 */
            if(c == ' ') c = '$';
            /* 次のルート取得 */
            j = 0; r2 = r; r = size + 1;
            while(route[++j]!=-1) if(r>route[j] && route[j]>r2) r=route[j];
        }
        putchar(c);
    }
}

int main(void)
{
    int col, row, size; /* col:列数; row:行数; size:セル数; */
    char *map;      /* 迷路のマップ */
    int *route;     /* 最短ルート */
    int length;     /* 最短距離 */
    
    /* 初期化 */
    map = init(&col, &row);
    size = col * row;
    route = (int*)calloc(dijkstra_count(map, size), sizeof(int));
    
    /* 探索 */
    length = dijkstra(route, map, col, size);
    
    /* 表示 */
    routePrint(map, route, col, row, size);
    printf("\nShortest-Length:%d\n", length);
    
    /* 終了 */
    free(route);
    return 0;
}

**************************
*S* *$$$$                *
*$* *$ *$ *************  *
*$*$$$* $$$************  *
*$$$ *    $$$$$          *
**************$***********
* $$$$$$$$$$$$$          *
**$***********************
* $$$$$*$$$$$$$$$$$$$$G  *
*  *  $$$ *********** *  *
*    *        ******* *  *
*       *                *
**************************
Shortest-Length:60