/*
遺伝的アルゴリズムを用いた例：倒立振子を制御するアルゴリズムを獲得する
Written by M.Nagaku 1999
*/

#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <mem.h>
#include <math.h>

// POPULATION_COUNT：集団の大きさ、個体数。
#define POPULATION_COUNT 500

// MUTATION_ODDS：突然変異を起こす確率。
#define MUTATION_ODDS 0.1

// DT：Δt
#define DT 0.02

// TIME：目標時間(秒)
#define TIME 10

// STOP_COST：このコスト以上の解を得た時点で終了する。
#define STOP_COST TIME/DT

// F：加える力の大きさ(単位：N)
#define F 1.

// M：棒の質量(単位：kg)
#define M 0.1

// L：棒の長さ(単位：m)
#define L 1.0

// G：重力加速度(単位：m/s^2)
#define G 9.8

// PI：π
#define PI 3.1415

/*
individual：個体データ。個体は制御アルゴリズム(control)で表現され、
            そのアルゴリズムで制御した場合、
            どれだけ倒れないでいられるか(cost)評価される。
アルゴリズム表現
Fの大きさは固定された値とし、Fを加える方向(+or-)を制御するものとする。
x',θ',θの値を見る事によって制御を行う。
x',θ',θからなる状態空間を分割し、各領域毎に制御方法を定めるものとする。
区間の分割は次の通り
x'：〜 -1.5 〜 -0.5 〜 0 〜 0.5 〜 1.5 〜 (6区間)
θ'：〜 -π/4 〜 -π/5 〜 0 〜 π/5 〜 π/4 〜 (6区間)
θ：〜 -π/180 〜 -π/30 〜 0 〜 π/30 〜 π/180 〜 (6区間)
但し、θについては、負の値の場合は、正の場合の逆の状態と考えられるので、
正の状態の制御方向を取り出してそれを反転して用いるものとする。
したがって、3区間分しか用意しない。
*/
typedef struct {
	int control[6][6][3];
	int cost;
} individual;

// population：対象となる集団の実体。
individual **population;


/*---------------------------------------------------------------------
x方向の加速度、x''を求める
*/
double dx2(double f, double theta, double dtheta, double dtheta2)
{
	return(f / M + dtheta * dtheta * sin(theta) - dtheta2 * cos(theta));
}
/*---------------------------------------------------------------------
軸回りの角加速度、θ''を求める
*/
double dtheta2(double f, double theta, double dtheta)
{
	return((G * sin(theta) - cos(theta) * (f / M + L * dtheta * sin(theta)))
		/ (L * (4. / 3. - cos(theta) * cos(theta))));
}
/*---------------------------------------------------------------------
個体のcostの計算を行う。
実際に倒立振子のシミュレーションを行い、何秒間耐えられるかを評価値とする。
*/
int calculate_cost(individual *target)
{
	int i, j, k, l, m, n, o;
	double x, dx, theta, dtheta;

// 異なる初期状態でシミュレーションを5回行い、耐えた秒数の平均値を評価値とする。
	for(i = 0, j = 0; i < 5; i++) {
		x = 0.;
		dx = 0.;
		theta = (i - 2.) * PI / 45.;
		dtheta = 0.;

// 手の位置が初期位置より±3m以上動いた場合、傾きが±π/6を超えた場合、
// 目標値に達した場合は、シミュレーションを停止する。
		for(o=0; fabs(x)<3. && fabs(theta)<PI/6. && o<STOP_COST; j++, o++) {

// 速度を区間分割に当てはめる。
			if(dx < -1.5)
				k = 0;
			else if(dx < -0.5)
				k = 1;
			else if(dx < 0.)
				k = 2;
			else if(dx < 0.5)
				k = 3;
			else if(dx < 1.5)
				k = 4;
			else
				k = 5;

// 角速度を区間分割に当てはめる。
			if(dtheta < -(PI / 4.))
				l = 0;
			else if(dtheta < -(PI / 5.))
				l = 1;
			else if(dtheta < 0.)
				l = 2;
			else if(dtheta < PI / 5.)
				l = 3;
			else if(dtheta < PI / 4.)
				l = 4;
			else
				l = 5;

// 角度を区間分割に当てはめる。
			if(fabs(theta) < PI / 180.)
				m = 0;
			else if(fabs(theta) < PI / 30.)
				m = 1;
			else
				m = 2;

// 角度が負の場合は、Fの符号を反転して使用する。
			n = 1;
			if(theta < 0.)
				n *= -1;

// 加速度から速度を求め、速度から位置を求める。
			dx += dx2(target->control[k][l][m] * n * F, theta, dtheta,
				dtheta2(target->control[k][l][m] * n * F, theta, dtheta)) * DT;
			dtheta += dtheta2(target->control[k][l][m] * n * F, theta, dtheta)
				* DT;
			x += dx * DT;
			theta += dtheta * DT;
		}
	}

	target->cost = j / 5;
	return j / 5;
}
/*---------------------------------------------------------------------
個体の突然変異体を作る。
*/
void mutation(individual *target)
{
// 染色体の1ヶ所をランダムに選んで符号を反転。
	target->control[rand() % 6][rand() % 6][rand() % 3] *= -1;
}
/*---------------------------------------------------------------------
2つ個体から1点交差によって新しい個体を得る。
*/
void crossover(individual *target, individual *source1, individual *source2)
{
	int point;
	int i;

// 交差位置を乱数で決定。
	point = rand() % 107 + 1;

// 染色体の前半をsource1からコピー。
	for(i = 0; i < point; i++)
		target->control[i % 6][(i / 6) % 6][i / 36]
			= source1->control[i % 6][(i / 6) % 6][i / 36];

// 染色体の後半をsource2からコピー。
	for(; i < 108; i++)
		target->control[i % 6][(i / 6) % 6][i / 36]
			= source2->control[i % 6][(i / 6) % 6][i / 36];
}
/*---------------------------------------------------------------------
GAに関する全ての操作の前に初期化を行う。
*/
void ga_init()
{
	int i, j;

// 集団のメモリ領域を確保
	population = malloc(sizeof(individual *) * POPULATION_COUNT);
	for(i = 0; i < POPULATION_COUNT; i++)
		population[i] = malloc(sizeof(individual));

// 集団の初期状態を生成する。
// 1,-1の何れかの値で初期化する。
	for(i = 0; i < POPULATION_COUNT; i++)
		for(j = 0; j < 108; j++)
			population[i]->control[j%6][(j/6)%6][j/36] = (rand() % 2) * 2 - 1;
}
/*---------------------------------------------------------------------
各個体のcostを計算し順列に並べ替える。
*/
void ga_ranking()
{
	int i, j;
	individual *tmp;

// 各個体のcostを計算。
	for(i = 0; i < POPULATION_COUNT; i++)
		calculate_cost(population[i]);

// costに基づき並べ替える。
	for(i = 0; i < POPULATION_COUNT - 1; i++)
		for(j = i + 1; j < POPULATION_COUNT; j++)
			if(population[i]->cost < population[j]->cost) {
				tmp = population[i];
				population[i] = population[j];
				population[j] = tmp;
			}
}
/*---------------------------------------------------------------------
次世代の集団を生成する。
*/
void ga_next_genelation()
{
	static int count;
	int i, j, cut_line;

// 評価値の平均値を求め、平均値以下の個体を淘汰する。
	for(i = 0, cut_line = 0; i < POPULATION_COUNT; i++)
		cut_line += population[i]->cost;
	cut_line /= POPULATION_COUNT;
	for(i = 0; i < POPULATION_COUNT; i++)
		if(cut_line > population[i]->cost)
			break;
	cut_line = i;

// 平均値以上の個体が10%未満の場合、10%の生き残りを保証する。
	if(cut_line < POPULATION_COUNT / 10) {
		cut_line = POPULATION_COUNT / 10;
		count = 0;
	}

// 平均値以上の個体が60%以上の場合、上位60%のみ生き残らせる。
	else if(cut_line > POPULATION_COUNT * 6 / 10) {
		cut_line = POPULATION_COUNT * 6 / 10;
		count++;

// 平均値以上の個体が60%以上の状態が続いた場合、
// 集団内の染色体が平均化されてきたと考え
// 上位25%のみを残して、25%の個体をランダム生成し、
// 残り50%をそれらの交叉で生成する。
		if(count > 4) {
			for(i = POPULATION_COUNT / 4; i < POPULATION_COUNT / 2; i++)
				for(j = 0; j < 108; j++)
					population[i]->control[j%6][(j/6)%6][j/36]
						= (rand() % 2) * 2 - 1;
			cut_line = POPULATION_COUNT / 2;
			count = 0;
		}
	}
	else
		count = 0;

// 集団は順位付けられているものとし、CUT_LINE以下の個体を
// CUT_LINE以上の個体から1点交差によって生成される個体に置き換える。
	for(i = cut_line; i < POPULATION_COUNT; i++)
		crossover(population[i], population[rand() % cut_line],
			population[rand() % cut_line]);

// MUTATION_ODDSの確率で突然変異をおこす。
// 但し、最優秀個体は保護される。
	for(i = 1; i < POPULATION_COUNT; i++)
		if((rand() % (int)(1.0 / MUTATION_ODDS)) == 0)
			mutation(population[i]);
}
/*---------------------------------------------------------------------
*/
void main()
{
	int i, j;
	time_t now_t, old_t;

	srand(0);

	ga_init();

	time(&old_t);

	for(i = 0; 1; ga_next_genelation(), i++) {
		ga_ranking();
		printf("%3d: cost %4d (%dsec)\n",
			i, population[0]->cost, (int)(population[0]->cost * DT));
		if(population[0]->cost >= STOP_COST)
			break;
	}
	printf("\n");

	time(&now_t);
	printf("%d sec\n", now_t - old_t);

	printf("control\n");
	for(j = 0; j < 108; j++)
		printf("%d, ", population[0]->control[j%6][(j/6)%6][j/36]);
	printf("\n");

	for(i = 0; i < POPULATION_COUNT; i++)
		free(population[i]);
	free(population);
}