/* 左手法による探索をするマウス */

/*
#define MOUSESIM
enum Action { Step, TurnLeft, TurnRight, TurnBack, End };
*/
/*
int senseForward();
int senseLeft();
int senseRight();
void MousePuts(char *message);
*/

/* マウスの把握している自分の現在位置と方向 */
int curx, cury, curdir ;
/* マウスの記憶しているマップ */
int myMap[16][16];
// bit 0 : その区画の西に壁があると1
// bit 1 : その区画の南に壁があると1
// bit 2 : その区画に立ち寄ったことがあると1
// bit 3 : 経路探索で見つかった、たどるべき道を示すパン屑

int myMode ;

int rotx(int dx, int dy) {
  switch (curdir) {
    case 0: return dx;
    case 1: return -dy;
    case 2: return -dx;
    case 3: return dy;
  }
}

int roty(int dx, int dy) {
  switch (curdir) {
    case 0: return dy;
    case 1: return dx;
    case 2: return -dy;
    case 3: return -dx;
  }
}

void setWall(int dir) {
  int direction = (curdir+dir) % 4;
  switch (direction) {
  case 0: if (curx < 15) { myMap[curx+1][cury] |= 2; } break;
  case 1: if (cury < 15) { myMap[curx][cury+1] |= 1; } break;
  case 2: myMap[curx  ][cury] |= 2 ; break;
  case 3: myMap[curx  ][cury] |= 1 ; break;
  }
}

/* 等高線法による、未到達区画への経路の発見 */

int routeMap[16][16];

int findUnvisited() {
  int i,j;

  for (i=0;i<16;i++) {
    for (j=0;j<16;j++) {
      routeMap[i][j] = 255;
      myMap[i][j] &= 0xf7; // ~8 で狂うのはCINTのバグだろ。
    }
  }
  // こういう計算、memfill とか union map[256] でやった方がちょっと速い。

  // 1.現在位置からの距離を書き込んでいく。未到達区画に達したら完了。
  routeMap[curx][cury] = 0;

  int stepflag = 1; // スキャンして1区画も進展しなかったら行き詰まり。
  int fx,fy; // 発見した未到達区画
  int v,v1;
  v = 0;
  v1 = 1;
  while  (stepflag) {
    stepflag = 0;
    for (i=0;i<16;i++) {
      for (j=0;j<16;j++) {
	if (routeMap[i][j] == v) {
	  // N
	  if ((i<15) && (v<routeMap[i+1][j]) && ((myMap[i+1][j]&2)==0)) {
	    routeMap[i+1][j] = v1;
	    stepflag = 1;
	    if ((myMap[i+1][j]&4)==0) { fx = i+1; fy = j; goto FOUND; }
	  }
	  // E
	  if ((j<15) && (v<routeMap[i][j+1]) && ((myMap[i][j+1]&1)==0)) {
	    routeMap[i][j+1] = v1;
	    stepflag = 1;
	    if ((myMap[i][j+1]&4)==0) { fx = i; fy = j+1; goto FOUND; }
	  }
	  // W
	  if ((0<j) && (v<routeMap[i][j-1]) && ((myMap[i][j]&1)==0)) {
	    routeMap[i][j-1] = v1;
	    stepflag = 1;
	    if ((myMap[i][j-1]&4)==0) { fx = i; fy = j-1; goto FOUND; }
	  }
	  // S
	  if ((0<i) && (v<routeMap[i-1][j]) && ((myMap[i][j]&2)==0)) {
	    routeMap[i-1][j] = v1;
	    stepflag = 1;
	    if ((myMap[i-1][j]&4)==0) { fx = i-1; fy = j; goto FOUND; }
	  }
	} // routeMap[i][j]==v
      } // for j
    } // for i
    v = v1++;
  } // while stepflag

  // 発見できなかった。
  return 0;

 FOUND:
  // 2.発見した未到達区画から、距離が減るマスに戻っていくことで
  //   たどるべき経路を myMap | 8 にチェック。

#if 1
  printf("I found ONE at %d, %d.\n",fx,fy);
  MouseUserCmd(2);
#endif

  v = routeMap[fx][fy];
  do {
    myMap[fx][fy] |= 8;
    v--;
    if (( 0<fx) && (routeMap[fx-1][fy]==v) && ((myMap[fx  ][fy]&2)==0)) { fx--; continue; }
    if ((fx<15) && (routeMap[fx+1][fy]==v) && ((myMap[fx+1][fy]&2)==0)) { fx++; continue; }
    if (( 0<fy) && (routeMap[fx][fy-1]==v) && ((myMap[fx  ][fy]&1)==0)) { fy--; continue; }
    if ((fy<15) && (routeMap[fx][fy+1]==v) && ((myMap[fx][fy+1]&1)==0)) { fy++; continue; }
    puts("Emergency."); G__pause();
  } while ((fx != curx) || (fy != cury)) ;
  
  return 1;
}

#define VALID(x,y) ((0 <= x) && (x <= 15) && (0 <= y) && (y <= 15))

Action MousedoAction() {
  int x1, y1;

  if (myMode == 1) { goto ROUTED; }

 LEFTHAND:
  // 左手法探索モード

  // 壁をセンスしてマップデータに格納
  if (senseForward()) { setWall(0) ; }
  if (senseLeft()) { setWall(3) ; }
  if (senseRight()) { setWall(1) ; }

  // 左側の区画が未到達かつそっちに壁がないならそっちを向く。
  // 次のターンで(下の場合分けになり)自動的に前進になる。
  x1 = curx + rotx(0,-1) ;
  y1 = cury + roty(0,-1) ;
  if (VALID(x1,y1) && (myMap[x1][y1] & 4) == 0 && (senseLeft() == 0)) {
    curdir = (curdir+3)%4;
    return TurnLeft;
  }
  // 上がだめなとき、今度は前進できるなら前進する。
  x1 = curx + rotx(1,0) ;
  y1 = cury + roty(1,0) ;
  if (VALID(x1,y1) && (myMap[x1][y1] & 4) == 0 && (senseForward() == 0)) {
    // マップデータと自分の座標を更新
    myMap[x1][y1] |= 4;
    curx = x1; cury = y1;
    return Step;
  }
  // それもだめなとき、右に行けるかどうか試す。
  x1 = curx + rotx(0,1) ;
  y1 = cury + roty(0,1) ;
  if (VALID(x1,y1) && (myMap[x1][y1] & 4) == 0 && (senseRight() == 0)) {
    curdir = (curdir+1)%4;
    return TurnRight;
  }

  // 左右と前の区画は未到達でないか壁がある。
  // 現在位置から最も近い未到達区間と、
  // そこに辿りつくための経路を計算する。

  if (! findUnvisited()) {
    // 未到達区画はない。探索終了。
    MousePuts("I visited all cell.\n");
    return End;
  }

  myMode = 1;
  MousePuts("Entering ROUTED mode.\n");

 ROUTED:
  // 未到達区画に向かって進むモード。

  // 1. 足元のマーカーを消す。
  myMap[curx][cury] &= 0xf7;

  // 2. 周囲にマーカーを探してそっちに動く。

  // 左側
  x1 = curx + rotx(0,-1) ;
  y1 = cury + roty(0,-1) ;
  if (VALID(x1,y1) && (myMap[x1][y1] & 8) && (senseLeft() == 0)) {
    curdir = (curdir+3)%4;
    return TurnLeft;
  }
  // 前進
  x1 = curx + rotx(1,0) ;
  y1 = cury + roty(1,0) ;
  if (VALID(x1,y1) && (myMap[x1][y1] & 8) && (senseForward() == 0)) {
    curx = x1; cury = y1;
    myMap[x1][y1] |= 4;
    return Step;
  }
  // 右側
  x1 = curx + rotx(0,1) ;
  y1 = cury + roty(0,1) ;
  if (VALID(x1,y1) && (myMap[x1][y1] & 8) && (senseRight() == 0)) {
    curdir = (curdir+1)%4;
    return TurnRight;
  }
  // 最初の1回だけ、Uターン
  x1 = curx + rotx(-1,0) ;
  y1 = cury + roty(-1,0) ;
  if (VALID(x1,y1) && (myMap[x1][y1] & 8)) {
    curdir = (curdir+2)%4;
    return TurnBack;
  }

  // 3. マーカーが無ければ、そこがゴール。左手探索に戻る。
  myMode = 0;
  MousePuts("Return to LEFTHAND method.\n");
  goto LEFTHAND;

}

void MouseInit() {
  int i,j;
  myMap[0][0] = 7;
  for (j=1;j<16;j++) { myMap[0][j] = 2; }
  for (i=1;i<16;i++) {
    myMap[i][0] = 1;
    for (j=1; j<16; j++) {
      myMap[i][j] = 0;
    }
  }
  myMode = 0;
  MouseBring2Start();
}

void MouseBring2Start() {
  curx = 0; cury = 0 ; curdir = 0;
}

void MouseUserCmd(int cmd) {
  char msg[30];
  int i,j;

  switch (cmd) {
  case 1: // 状態を表示
    printf("x : %2d / y : %2d / d : %d\n",curx,cury,curdir);
    break;
  case 2: // 内部マップと経路探索マップをダンプ
    for (i=0;i<16;i++) {
      for (j=0;j<16;j++) {
	printf("%1X",myMap[j][i] & 0x0f);
      }
      printf("  %02d  ",i);
      for (j=0;j<16;j++) {
	printf("%02X ",routeMap[j][i]);
      }
      printf("\n"); // puts("") も putchar('\n') も動作が変...
    }
    break;
  case 3: // スタートからゴールまでの最短経路を計算する準備
    for (i=0;i<16;i++) {
      for (j=0;j<16;j++) {
	myMap[i][j] |= 4;
      }
    }
    myMap[7][7] &= 0xfb;
    myMap[7][8] &= 0xfb;
    myMap[8][7] &= 0xfb;
    myMap[8][8] &= 0xfb;
    break;
  } // switch cmd
} // MouseUserCmd