箱入り娘パズルを解く

## 箱入り娘パズルをプログラムで解く
### 概略
先日、とある科学博物館に立ち寄って「箱入り娘」というパズルに出会いました。結論から言うと、閉館までに解くことはできず、消化不良な感じで終わりました。なんかプログラムで解けそうな気がしたのでやってみたら解けました。

### 問題設定
15パズルを変形したような問題設定です。
以下の図ように4x5のマスの中に図のような様々な形の長方形が押し込まれています。この各長方形を人に見立てたパズルです。中央下の白い場所を玄関として、全ての人が外に出ることなく、２マスだけある隙間を譲り合いながら中央上にいる娘を外に出す、というパズルです。
![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3746743/c9d9fe0e-f9b6-d90f-9578-9289ed7fd798.png)
これちゃんと解ける問題なんだろうな？と疑いたくなる設定でした。

### 時間のない人へ
一番下までスクロールしたら、娘が出ていくところまでの動画が見られますのでスクロールして下さい。

### 戦略
あんまり深く考えたくないなー。
考えるのが面倒なので、全探索かなー。
というわけで、以下の戦略で行きます。

(1) <b><font color="Blue">一手で行ける状態</font></b>を全部書き出す。
  　<b>その中に娘が玄関に到達したものがあったら終わり。</b>なかったら(2)へ。
   
(2) <b><font color="Blue">「一手で行ける状態」</font><font color="DarkGreen">から一手で行ける状態</font></b>を全部書き出す。
　　でも<b><font color="Blue">元の状態</font></b>に戻っちゃう手は除く。
  　<b>その中に娘が玄関に到達したものがあったら終わり。</b>なかったら(3)へ。
   
(3) <b><font color="DarkGreen">「</font><font color="Blue">「一手で行ける状態」</font><font color="DarkGreen">から一手で行ける状態」</font><font color="Red">から一手で行ける状態</font></b>を全部書き出す。
　　でも<b><font color="Blue">元の状態</font></b>や<b><font color="DarkGreen">「一手で行ける状態」</font></b>に戻っちゃう手は除く。
  　<b>その中に娘が玄関に到達したものがあったら終わり。</b>なかったら(4)へ。
   
(4) <b><font color="Red">「</font><font color="DarkGreen">「</font></font><font color="Blue">「一手で行ける状態」</font><font color="DarkGreen">から一手で行ける状態」</font><font color="Red">から一手で行ける状態」</font><font color="DarkGoldenRod ">から一手で行ける状態</font></b>を全部書き出す。
　　でも<b><font color="Blue">元の状態</font></b>や<b><font color="DarkGreen">「一手で行ける状態」</font></b>や<b><font color="Red">「「一手で行ける状態」から一手で行ける状態」</font></b>に戻っちゃう手は除く。
  　<b>その中に娘が玄関に到達したものがあったら終わり。</b>なかったら(5)へ。
   
(5) <b>以下略！</b>

### 同じ状態をどうやって比較しようかな。
「元の状態に戻っちゃう手は除く」を実現するためには、今の状態とこれまでの状態を簡単に比較する方法を考えなきゃいけない。
色々考えるのが面倒なので、2桁の16進数で、各マスを表現することにします。

・10の位に長方形の横の長さを書く
・1の位に長方形の縦の長さを書く
・左上のマスは0x80をorしといて、分かりやすくしとく。

このルールに従うと初期状態は以下のように書けます。
```
92 a2 22 92 
12 22 22 12 
92 a1 21 92 
12 91 91 12 
91 00 00 91 
```
これを一列に並べて、こんなデータ列作って、このデータ列が一致したら同じものとみなす感じでいけそうな気がした。
```
92 a2 22 92 12 22 22 12 92 a1 21 92 12 91 91 12 91 00 00 91 
```
もっと良い方法もありそうだけど時間かけて考え出した良い方法とこの方法の速度差って僅かな気がするしいいや。

### 状態遷移を出力するプログラム
謎の老人から「時間かかりそうな処理はC++で書きなされ。」と言われたので、探索部分はC++で書いてみました。
以下全ソース。時間のない人は何も考えずコピペしてコンパイルしちゃって下さい。
``` c
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <vector>

enum {
	ID_FATHER = 0,
	ID_DAUGHTER,
	ID_MOTHER,
	ID_GROUND_FATIER,
	ID_GROUND_MOTHER,
	ID_BANTO,
	ID_SERVITOR,
	ID_SISTER,
	ID_DRIVEL1,
	ID_DRIVEL2,
	
	NUM_OF_PERSONS,
	
	MOVE_UP = 0,
	MOVE_LEFT,
	MOVE_DOWN,
	MOVE_RIGHT,
	
	NUM_OF_DIRECTIONS,
	
	ROOM_WIDTH = 4,
	ROOM_HEIGHT = 5,
};

struct POS {
	int x;
	int y;
};

struct PERSON {
	POS pos;
	int w;
	int h;
};


struct MOVE_INFO {
	int prev_info; // 1個前の状態の番号
	int step; // 初期状態から何手でいけるか
	int id;
	int dir;
};


struct STATE_INFO {
	PERSON persons[NUM_OF_PERSONS];  // 人の位置
	unsigned char phash[ROOM_WIDTH * ROOM_HEIGHT];      // 人の位置関係を数値化したもの
	MOVE_INFO minfo; // 何をどの方向に移動するか
};

static void init_state(STATE_INFO &data)
{
	const STATE_INFO sinfo = {
		{
			{{0, 0,}, 1, 2,}, // 父親
			{{1, 0,}, 2, 2,}, // 娘
			{{3, 0,}, 1, 2,}, // 母親
			{{0, 2,}, 1, 2,}, // 爺や
			{{3, 2,}, 1, 2,}, // 婆や
			{{1, 2,}, 2, 1,}, // 番頭
			{{1, 3,}, 1, 1,}, // 従僕
			{{2, 3,}, 1, 1,}, // 姉や
			{{0, 4,}, 1, 1,}, // 召使い1
			{{3, 4,}, 1, 1,}, // 召使い2
		},
		{0, 0, 0, 0,
		 0, 0, 0, 0,
		 0, 0, 0, 0,
		 0, 0, 0, 0,
		 0, 0, 0, 0,},
		{-1, 0, 0, 0,}, // 移動IDと移動方向
	};
	memcpy(&data, &sinfo, sizeof(STATE_INFO));
}

static bool empty_check(const unsigned char *phash, int x, int y)
{
	if (x < 0 || x >= ROOM_WIDTH || y < 0 || y >= ROOM_HEIGHT) return false;
	return (phash[x + y * ROOM_WIDTH] == 0);
}

static bool movability(const STATE_INFO &data, int id, int move)
{
	const PERSON &p = data.persons[id];
	const POS &pos = p.pos;
	bool ret = true;
	switch (move) {
	case MOVE_UP:
		for (int i = 0; i < p.w; i++) {
			ret &= empty_check(data.phash, pos.x + i, pos.y - 1);
		}
		break;
	case MOVE_LEFT:
		for (int i = 0; i < p.h; i++) {
			ret &= empty_check(data.phash, pos.x - 1, pos.y + i);
		}
		break;
	case MOVE_DOWN:
		for (int i = 0; i < p.w; i++) {
			ret &= empty_check(data.phash, pos.x + i, pos.y + p.h);
		}
		break;
	case MOVE_RIGHT:
		for (int i = 0; i < p.h; i++) {
			ret &= empty_check(data.phash, pos.x + p.w, pos.y + i);
		}
		break;
	default:
		return false;
	}
	return ret;
}

static void move_person(PERSON *next_p, const PERSON *p, int id, int move)
{
	memcpy(next_p, p, sizeof(PERSON) * NUM_OF_PERSONS);
	
	PERSON &np = next_p[id];
	switch (move) {
	case MOVE_UP:
		np.pos.y--;
		break;
	case MOVE_LEFT:
		np.pos.x--;
		break;
	case MOVE_DOWN:
		np.pos.y++;
		break;
	case MOVE_RIGHT:
		np.pos.x++;
		break;
	default:
		break;
	}
}

static void print_all_process(const std::vector<STATE_INFO> &slist, const STATE_INFO &data)
{
	std::vector<STATE_INFO> answer;
	const STATE_INFO *p = &data;
	while (1) {
		answer.push_back(*p);
		const MOVE_INFO &minfo = p->minfo;
		if (minfo.prev_info == -1) break;
		p = &slist[minfo.prev_info];
	}
	printf("------------------------------\n");
	FILE *fp = fopen("output.txt", "wb");
	if (fp == NULL) {
		printf("Can't open output.txt\n");
		return;
	}
	for (int i = (int)answer.size() - 1; i >= 0 ; i--) {
		p = &answer[i];
		const MOVE_INFO &minfo = p->minfo;
		if (minfo.prev_info != -1) {
			const POS &pos = p->persons[minfo.id].pos;
			int prevX = pos.x, prevY = pos.y;
			switch (minfo.dir) {
			case MOVE_UP:
				printf(" ↑ ");
				prevY++;
				break;
			case MOVE_LEFT:
				printf(" ← ");
				prevX++;
				break;
			case MOVE_DOWN:
				printf(" ↓ ");
				prevY--;
				break;
			case MOVE_RIGHT:
				printf(" → ");
				prevX--;
				break;
			default:
				break;
			}
			printf("%d  (%d,%d) -> (%d,%d)\n", minfo.prev_info, prevX, prevY, pos.x, pos.y);
		}
		for (int i = 0; i < ROOM_WIDTH * ROOM_HEIGHT; i++) {
			printf("%02x ", p->phash[i]);
			if ((i + 1) % ROOM_WIDTH == 0) printf("\n");
			fprintf(fp, "%02x ", p->phash[i]);
		}
		printf("\n");
		fprintf(fp, "\n");
	}
	fclose(fp);
	printf("------------------------------ (%d)\n", (int)answer.size());
}


static bool check_daughter(std::vector<STATE_INFO> &slist, int data_id)
{
	const STATE_INFO &data = slist[data_id];
	const POS &p = data.persons[ID_DAUGHTER].pos;
	if (p.x == 1 && p.y == 3) { // ゴール位置に娘が来てたら終了
		print_all_process(slist, data);
		return true;
	}
	return false;
}

static int check_same_state_hash(std::vector<STATE_INFO> &slist, STATE_INFO &next_data)
{
	for (int i = 0; i < (int)slist.size(); i++) {
		// 一か所でも一致してたら終わり
		if (memcmp(slist[i].phash, next_data.phash, sizeof(next_data.phash)) == 0) return i;
	}
	return -1;
}


static void set_person_hash(STATE_INFO &data)
{
	memset(data.phash, 0, sizeof(data.phash));
	for (int id = 0; id < NUM_OF_PERSONS; id++) {
		const PERSON &p = data.persons[id];
		const POS &pos = p.pos;
		unsigned char sflag = 0x80;
		for (int j = 0; j < p.h; j++) {
			for (int i = 0; i < p.w; i++) {
				data.phash[(pos.x + i) + (pos.y + j) * ROOM_WIDTH] = sflag | (p.w * 0x10 + p.h);
				sflag = 0;
			}
		}
	}
}

// slist:これまでの状態の蓄積
// data_id:一手前のid
// step:一手前が初期状態から何手目か
static void find_next_step(std::vector<STATE_INFO> &slist, int data_id, int step)
{
	STATE_INFO data;
	memcpy(&data, &slist[data_id], sizeof(STATE_INFO));
	
	// 最短の前の状態を探して、未チェックのリストを作る
	for (int id = 0; id < NUM_OF_PERSONS; id++) {
		// 上下左右を1つづつ見ていく
		for (int dir = 0; dir < NUM_OF_DIRECTIONS; dir++) {
			if (movability(data, id, dir)) { // まずはその方向に行けるか確認する
				// 行ける場合
				STATE_INFO next_data;
				// 移動した後の各人の位置を調べる
				move_person(next_data.persons, data.persons, id, dir);
				set_person_hash(next_data);
				// 移動後の各人の位置が移動前の各人の位置と一致してたら調べない。(別人でも形が一致してたら同一状態とみなす)
				int ret = check_same_state_hash(slist, next_data);
				if (ret == -1) { // 過去にないパターンの場合
					MOVE_INFO &minfo = next_data.minfo;
					minfo.prev_info = data_id;
					minfo.step = step + 1;
					minfo.id = id;
					minfo.dir = dir;
					slist.push_back(next_data);
				}
			}
		}
	}
}

int main(void)
{
	STATE_INFO data;
	init_state(data);
	set_person_hash(data);
	
	std::vector<STATE_INFO> slist;
	slist.push_back(data); // 追加する
	int start = 0;
	int end = 1;
	int step = 0;
	while (1) {
		for (int i = start; i < end; i++) {
			// 娘が出口にいたらそこまでの手を表示して終了する
			if (check_daughter(slist, i)) return 0;
			find_next_step(slist, i, step);
		}
		start = end;
		end = (int)slist.size();
		step++;
		if (start == end) break;
	}
	
	return 0;
	
}

```

### 実行方法
雑にコンパイルしても実行できます。
```bash
g++ -Wall -Wextra -O4 hakoiri.cpp
./a.out
```

### プログラムの説明
| 関数名 | 説明 |
| ---- | ---- |
| init_state() | 人の位置の初期状態を返す。 |
| empty_check() | 指定した位置が誰もいない場所かどうかを返す。 |
| movability() | 指定した人が指定した方向に動けるか返す。 |
| move_person() | 指定した人の今の座標から指定した方向に動かしてその座標を返す。 |
| print_all_process() | ゴールまでの手順を画面に表示して、ファイルには最初から最後までの状態を保存する。<br>最初に、ゴールから一個前の状態、一個前の状態から二個前の状態、と逆にたどって答えのリストanswerを作って後ろから順に表示してく感じ。 |
| check_daughter() | 娘が玄関にいたらtrue、いなかったらfalseを返す。|
| check_same_state_hash() | slistの中に与えられた状態と一致する状態があったらその状態のidを返す。<br>無かったら-1を返す。 |
| set_person_hash() | 「同じ状態をどうやって比較しようかな。」の項目で説明したルールで比較用データを作る関数。 |
| find_next_step() | 与えられた状態で部屋の中の人を一人ずつ見ていって、上下左右のどれかに動かせるやつがあったら動かしてみる。<br>動かした後の状態を見て、slistの過去の状態全部と比較して、一致するのがなかったら新状態発見ということで、slistに追加する。<br>ついでに自分が初手から何手目なのかも記録しとく。 |
| main() | dataに最初の状態を保存して、slistの1個目のデータとする。<br>娘が出口にいる状態になるまでslistにあるデータでまた見てないやつをfind_next_step()で次の一手を全部書き出す。 |

| 構造体名 | 説明 |
| ---- | ---- |
| POS | 人の左上の座標格納用 |
| PERSON | 人の左上の座標と幅・高さ格納用 |
| MOVE_INFO | 移動情報。前の状態が何か、初期状態から何手で来れるか、だれが動くか、どの方向に動くか。 |
| STATE_INFO | 状態の格納用。PERSON、MOVE_INFOと状態を表すデータ列。 |


### 実行結果
冒頭の抜粋。わけわからん。
```
92 a2 22 92 12 22 22 12 92 a1 21 92 12 91 91 12 91 00 00 91 
92 a2 22 92 12 22 22 12 92 a1 21 92 12 91 91 12 00 91 00 91 
92 a2 22 92 12 22 22 12 00 a1 21 92 92 91 91 12 12 91 00 91 
92 a2 22 92 12 22 22 12 a1 21 00 92 92 91 91 12 12 91 00 91 
92 a2 22 92 12 22 22 12 a1 21 91 92 92 91 00 12 12 91 00 91 
92 a2 22 92 12 22 22 12 a1 21 91 92 92 91 00 12 12 91 91 00 
92 a2 22 92 12 22 22 12 a1 21 91 00 92 91 00 92 12 91 91 12 
92 a2 22 92 12 22 22 12 a1 21 00 91 92 91 00 92 12 91 91 12 
92 a2 22 92 12 22 22 12 00 a1 21 91 92 91 00 92 12 91 91 12 
....
```

### 状態に結果を動画にするプログラム
謎の通行人から、「図や動画にしたい場合はPythonで書いた方がいいよ！」と言われたので、Pythonで動画化してみます。
以下ソース。
```Python
import cv2
import numpy as np

IMG_WIDTH = 640
IMG_HEIGHT = 480

ROOM_WIDTH = 4
ROOM_HEIGHT = 5

BLOCK_SIZE = 90

X_LEFT_UP = (IMG_WIDTH - BLOCK_SIZE * 4) // 2
Y_LEFT_UP = (IMG_HEIGHT - BLOCK_SIZE * 5) // 2

COLOR_INFO = [
    (128, 255, 255),
    (128, 255, 128),
    (255, 128, 128),
    (128, 128, 255),
]


def draw_rectangle(img, xpos, ypos, w, h, color):
    cv2.rectangle(
        img,
        (xpos, ypos),
        (xpos + w * BLOCK_SIZE, ypos + h * BLOCK_SIZE),
        color,
        thickness=-1,
    )
    cv2.rectangle(
        img,
        (xpos, ypos),
        (xpos + w * BLOCK_SIZE, ypos + h * BLOCK_SIZE),
        (0, 0, 0),
    )


def make_png_files(videoWriter, i, xlist):
    if len(xlist) != ROOM_WIDTH * ROOM_HEIGHT:
        return
    img = np.full((IMG_HEIGHT, IMG_WIDTH, 3), 128, dtype=np.uint8)
    cv2.rectangle(
        img,
        (X_LEFT_UP - 8, Y_LEFT_UP - 8),
        (X_LEFT_UP + ROOM_WIDTH * BLOCK_SIZE + 8, Y_LEFT_UP + ROOM_HEIGHT * BLOCK_SIZE + 8),
        (0, 0, 0),
        thickness=10,
    )
    cv2.rectangle(
        img,
        (1 * BLOCK_SIZE + X_LEFT_UP, ROOM_HEIGHT * BLOCK_SIZE + Y_LEFT_UP),
        (1 * BLOCK_SIZE + X_LEFT_UP + 2 * BLOCK_SIZE, 480),
        (255, 255, 255),
        thickness=-1,
    )
    x = 0
    y = 0
    for val16 in xlist:
        val = int(val16, 16)
        xpos = int(x * BLOCK_SIZE + X_LEFT_UP)
        ypos = int(y * BLOCK_SIZE + Y_LEFT_UP)
        if (val & 0x80) == 0x80:
            data = val & 0x7F
            w = (data & 0xF0) // 0x10
            h = data & 0x0F
            draw_rectangle(img, xpos, ypos, w, h, COLOR_INFO[(w - 1) * 2 + (h - 1)])
        x += 1
        if x == ROOM_WIDTH:
            x = 0
            y += 1

    # 動画で保存
    videoWriter.write(img)

    # 画像で一手ずつ保存したい場合はこちらのコメントを外す
    #cv2.imwrite(f"out_sample{i}.png", img)


def main():
    f = open("output.txt", "r")
    data = f.read().split(" \n")
    f.close()

    videoWriter = cv2.VideoWriter(
        "output.mp4", cv2.VideoWriter_fourcc(*"mp4v"), 1, (IMG_WIDTH, IMG_HEIGHT)
    )
    for i, x in enumerate(data):
        make_png_files(videoWriter, i, x.split(" "))

    videoWriter.release()


if __name__ == "__main__":
    main()

```
### プログラムの説明
| 関数名 | 説明 |
| ---- | ---- |
| draw_rectangle() | 指定された座標に指定された色で長方形を描く。 |
| make_png_files() | 画像ファイルと動画ファイルも作る。<br>状態を表す16進数のデータ列を解読してどんな四角形を描くかを決める。<br>  |
| main() | C++で書いたコードを実行して作られるoutput.txtを読み込んで、動画を作る。 |

### 結果
こんな感じの出力となりました。いい感じ。
![output.gif](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3746743/2a5627bc-68ee-8605-b782-4d9abd07d1d4.gif)

### その他
謎の老人A「時間かかりそうな処理はC++で書きなされ。」
謎の老人B「C++が吐くバイナリはオーバーヘッドがあるからCで書きなされ。」
謎の老人C「コンパイラの最適化はたかが知れてるからアセンブラで書きなされ。」