/*
 群れをシミュレートする
 */

#include  <math.h>
#include  <stdlib.h>
#include  <stdio.h>
#include  <time.h>
#include  <eggx.h>
#include  <string.h>

#define WW 1000 // 画面の幅
#define HH 660 // 画面の高さ
#define TH 40  // 壁の位置（跳ね返る場所）
#define RR 9   // 円の直径
#define SS 8.0   // 標準的スピード
#define DD 30.0  // 群れの半径

#define MM 1200 // 座標点の上限数
double x[MM], y[MM];    // 座標点を保存するための配列
double xd[MM], yd[MM];  // 速度を保存するための配列
double xk[MM], yk[MM];  // 次回の速度を保存するための配列
int    c[MM];           // 色
int mm;                // 座標点が幾つあったかを数えておく
int interval;          // ミリ秒単位の待ち時間

// 繰り返し数を削減するための「存在位置記録枠」仕掛け
// （群れの半径の前後左右範囲しか注目しないように）
#define DW 30 // SS に等しい
#define DH 30
typedef struct cell {
	int idx; // インデックス番号 x[idx],y[idx] などで使う
	int flg; // そこに存在している場合は 1, その周辺の場合は 2
	struct cell *next;
} CELL;
CELL cellpool[MM*9];
CELL *cellmat[WW/DW+3][HH/DH+3];  // 添え字は座標位置から +1 して使う
// *cellmat[x[i]/DW+1][y[i]/DH+1] などとすれば良い
int cellm; // cellpool から幾つまで使ったか


int win;

// 0 〜 ratio までの乱数を作る
double rnd(double ratio)
{
	return ( rand()%10000 ) / 10000.0 * ratio;
}


// 現在位置に描く
int drawit(int i) 
{
	double ratio; // シッポの長さは頭の何倍か
	
	newhsvcolor(win, 0, 0, 200);
	ratio=(RR*3.0)/hypot(xk[i],yk[i]);
	line(win,x[i]-RR/2.0,            y[i]-RR/2.0,            PENUP);
	line(win,x[i]-RR/2.0-xk[i]*ratio,y[i]-RR/2.0-yk[i]*ratio,PENDOWN);
	
	newhsvcolor(win, c[i], 255, 255);
	fillarc(win,x[i]-RR/2.0,y[i]-RR/2.0,RR,RR,0.0,360.0,1) ;
	
	newhsvcolor(win, 0, 0, 128);
	circle(win,x[i]-RR/2.0,y[i]-RR/2.0,RR,RR);
	
	return 0;
}


// 次の移動位置を計算する
int moveit(int i, int w, int h) 
{
	int j, num1, num2;
	double x1, y1, x2, y2, x3, y3, dist, limit, kick, ratio;
	CELL *c;
	
	// 次回速度を前回速度を元にはじめる
	xk[i] = xd[i];
	yk[i] = yd[i];
	
	// 群れ全体の平均的移動ベクトルに沿う移動ベクトルを算出
	// ＆群れ全体の平均的な位置（中心位置）に向かう移動ベクトルを算出
	limit=DD; // チェックするのは群れ全体つまり DD 
	num1=0; x1=0.0; y1=0.0; x2=0.0; y2=0.0;
	num2=0; x3=0.0; y3=0.0;
	
	// チェック対象を同居＆周辺住人に絞る
	c=cellmat[w][h];
	while( c != NULL ) {
		j = c->idx; // チェック対象
		c=c->next;
		if(j==i) continue; // 自分自身とは相互作用無し！    
		dist=hypot(x[i]-x[j], y[i]-y[j]);
		if(dist<limit) { // 閾値以下の距離にいる（群れの対象である）
			x1+=xd[j]; // 移動ベクトルを積算
			y1+=yd[j];
			x2+=x[j]; // 位置を積算
			y2+=y[j];
			num1++;
		}
		if(dist<(limit*0.6)) { // 閾値(群れより小さめ)以下の距離にいる（近すぎる）
			kick=1.0-(dist/(limit*0.6)); // どれだけ反発(kickback)するか
			x3+=(x[i]-x[j])*kick; // 移動ベクトルを積算
			y3+=(y[i]-y[j])*kick;
			num2++;
		}
	}
	
	if(num1!=0) {
		x1 = x1 / num1;    // 平均移動ベクトルを得る（ただし誤差つき）
		y1 = y1 / num1;
		x1 = xd[i] + ( x1 - xd[i] ); // 現在のベクトルを補正する成分を得る
		y1 = yd[i] + ( y1 - yd[i] );
		ratio=0.6+rnd(0.6);    // 追従率（ブレつき）
		if( hypot(x1, y1) < (SS/4.0) ) { // ある程度速度を保つように
			x1 *= (SS / hypot(x1, y1)) * ratio; // 但し ratio で影響度を減らす;
			y1 *= (SS / hypot(x1, y1)) * ratio; 
		}
		xk[i]=xd[i]*0.7+x1*0.3; // 元の速度にどの程度影響を与えるかを
		yk[i]=yd[i]*0.7+y1*0.3; // 適当に決める
		
		x2 = ( x2 / num1 ) - x[i];  // 平均位置を得て、そこへ向かうベクトルを得る
		y2 = ( y2 / num1 ) - y[i];
		if( SS < hypot(x2, y2) ) { // 距離がある場合は長さを SS に切る 
			x2 *= (SS / hypot(x2, y2));
			y2 *= (SS / hypot(x2, y2));
		}
		ratio=0.01+rnd(0.05);    // 追従率（ブレつき）
		xk[i] += x2 * ratio; // 影響度を ratio で下げる
		yk[i] += y2 * ratio;
	}
	
	if(num2!=0) {
		ratio=0.6+rnd(1.4);    // 反発率
		xk[i] += x3 * ratio / num2; // 影響度を ratio で下げる
		yk[i] += y3 * ratio / num2;
	}
	
	// 全く関係なくブレを入れる
	xk[i] += SS * (-0.1 + rnd(0.2));
	yk[i] += SS * (-0.1 + rnd(0.2));
	
	
	return 0;
}

// 存在位置枠をクリア
int cell_init()
{ 
	int w, h;
	
	for(w=0; w < WW/DW+3; w++) {
		for(h=0; h < HH/DH+3; h++) {
			cellmat[w][h]=NULL;
		}
	}
	cellm=0;
	
	return 0;
}

// 存在位置枠に自分の位置を追加（CELL のリストをつける）
int cell_append(int w, int h, int i, int flg)
{
	cellpool[cellm].idx=i;
	cellpool[cellm].flg=flg;
	cellpool[cellm].next=cellmat[w][h];
	cellmat[w][h] = &cellpool[cellm];
	
	cellm++;
	
	return 0;
}

// 存在位置枠に自分の位置を記録
int cell_record(double x, double y, int i)
{
	int w, h;
	
	w=x/DW+1;
	h=y/DH+1;
	
	// 当人の居場所
	cell_append(w, h, i, 1); 
	// その向こう三軒両隣
	cell_append(w-1, h-1, i, 2); 
	cell_append(w  , h-1, i, 2); 
	cell_append(w+1, h-1, i, 2); 
	cell_append(w-1, h  , i, 2); 
	cell_append(w+1, h  , i, 2); 
	cell_append(w-1, h+1, i, 2); 
	cell_append(w  , h+1, i, 2); 
	cell_append(w+1, h+1, i, 2); 
	
	return 0;
}

// --------------------

int main(int argc, char *argv[])
{
	int i, w, h;   // 一時的に使う変数（ループカウンタ）
	int l=0; // layer 番号
	
	if(argc != 3) {
		fprintf(stderr, "usage: %s num wait\n",argv[0]);
		return 1;
	}
	mm=atoi(argv[1]);
	if((mm<1)||(mm>MM)) {
		fprintf(stderr, "usage: %s num wait\n",argv[0]);
		fprintf(stderr, "     : given num (%s) is not from 1 to %d\n",
				argv[1], MM);
		return 1;
	}
	interval=atoi(argv[2]);
	if((interval<0)||(interval>1000)) {
		fprintf(stderr, "usage: %s num msec\n",argv[0]);
		fprintf(stderr, "     : given msec (%s) is not from 0 to 1000\n",
				argv[2]);
		return 1;
	}
	
	// 描画準備
	win=gopen(WW,HH);
	winname(win, "Raynolds");
	layer(win, l, (l+1)%2); l=(l+1)%2;
    
	srand(time(NULL));
	// 配列にいったん格納する（ m が座標点の個数）
	for(i=0;i<mm;i++) {
		double r;
		x[i]=rand()%(WW/3)+WW/3;
		y[i]=rand()%(HH/3)+HH/3;
		r=((rand()%100)/100.0)*(3.14159*2);
		xd[i]=SS*cos(r);
		yd[i]=SS*sin(r);
		c[i]=(double)(i) / (double)(mm) * 360.0;
	}
	
	while(1) {
		// 現在の位置で存在位置枠をチェック
		cell_init();
		for(i=0; i<mm; i++) {
			// printf("%f %f, %d\n", x[i], y[i], i);
			cell_record(x[i], y[i], i);
		}
		// 速度（＝移動距離）計算をする
		CELL *c;
		for(w=1; w < WW/DW+2; w++) {
			for(h=1; h < HH/DH+2; h++) {
				c = cellmat[w][h];
				while(c!=NULL) {
					if( c->flg == 1 ) {  // 住人が居たら移動処理
						moveit( c->idx , w, h );
					}
					c = c->next;
				}
			}
		}
		
		// 配列に納めたぶんだけ移動し、描く
		gclr(win);
		newhsvcolor(win, 0, 0, 128);
		drawrect(win, TH, TH, WW-2*TH, HH-2*TH);
		for(i=0; i<mm; i++) {
			x[i]+=xk[i]; y[i]+=yk[i]; // 移動
			drawit(i); // 描画
			xd[i]=xk[i]; yd[i]=yk[i]; // 移動距離を保存
			if(x[i]<TH)      xd[i]=fabs(xd[i]);
			if(x[i]>(WW-TH)) xd[i]=fabs(xd[i]) * -1.0;
			if(y[i]<TH)      yd[i]=fabs(yd[i]);
			if(y[i]>(HH-TH)) yd[i]=fabs(yd[i]) * -1.0;
		}
		layer(win, l, (l+1)%2); l=(l+1)%2;
		if(interval>0) msleep(interval);
	}
	
	// never stop
	
}