とりあえずメインプログラムざっくり読みました。
既に見つけてたら悪いんですけど、いくつかバグあったので報告します。
76行目 UMAPに1を入れてるやつが要らないです。あとUMAP[X,Y]が1の時だけGFILLを実行するようにして下さい。
また、一行下のIF文はループで確認するより、既に分かっている座標から処理を行った方が良さそうです。
108行目 UMAP,0をUMAP,1に変えて下さい。
349行目 MIN(D_RIGHT2,…をMIN(D_RIGHT,…に変えて下さい。
追記:259~262行目のD_HPは全てD_PHPに変えて下さい。