coding Shift_JIS;

import GUI;
import Graphics;
import Graphics2D;
import Math;

// GUI用の変数
int WIDTH = 600; // ウィンドウの幅
int HEIGHT = 600; // ウィンドウの高さ
int window; // ウィンドウのID
int displayLabel; // 表示ラベルのID

// グラフィックス用の変数
int PIXEL_PER_LENGTH = 100; // 長さとピクセルの変換係数
int graphics; // グラフィックスのID
int renderer; // レンダラーのID

// モデルの性質を表すパラメータ変数
float gravityAccel = 9.8; // 重力加速度
float deltaT = 0.00001; // 時間刻み
float length1 = 1.4; // 振り子1の長さ
float length2 = 1.2; // 振り子2の長さ
float mass1 = 1.2; // 振り子1の重さ
float mass2 = 0.1; // 振り子2の重さ

// モデルの状態を表す自由度変数
float theta1 = 3.1; // 振り子1の角度
float theta2 = -2.0; // 振り子2の角度
float theta1Dot = 0.0; // 振り子1の角速度
float theta2Dot = 0.0; // 振り子2の角速度

// メインループ制御用の変数
bool loopState = true; // メインループを脱出させるための変数
int loopWait = 24; // メインループ1周ごとの待機時間（ミリ秒）


// プログラムはここから開始する
void main( string args[] ){

	init(); // 初期化を行う

	// メインループ
	while( loopState ){
		paint(); // 描画
		update(); // 更新
		sleep( loopWait ); // 待機
	}

	exit(); // メインループを抜けたらプログラムを終了

}

// 起動後の初期化処理を行う関数
void init(){
	// グラフィックスデータを生成
	graphics = newGraphics();
	// 上で生成したgraphicsに描き込む2DCGレンダラーを生成
	renderer = newGraphics2DRenderer( WIDTH, HEIGHT, graphics );

	// ウィンドウを生成
	window = newWindow( 0, 0, WIDTH, HEIGHT+20, "Duplex Pendulum" );
	// graphicsの内容を表示するグラフィックスラベルを生成
	displayLabel = newImageLabel( 0, 0, WIDTH, HEIGHT, graphics );
	// ラベルをウィンドウに配置
	mountComponent( displayLabel, window );
}

// 毎周期の画面描画を行う関数
void paint(){
	// 背景を白で塗りつぶし
	setDrawColor( renderer, 255, 255, 255, 255 );
	drawRectangle( renderer, 0, 0, WIDTH, HEIGHT, true );

	// 振り子のボールの座標を求める
	int centerX = WIDTH / 2;
	int centerY = HEIGHT / 2;
	int point1X = PIXEL_PER_LENGTH * length1 * sin( theta1 ) + centerX;
	int point1Y = PIXEL_PER_LENGTH * length1 * cos( theta1 ) + centerY;
	int point2X = PIXEL_PER_LENGTH * length2 * sin( theta2 ) + point1X;
	int point2Y = PIXEL_PER_LENGTH * length2 * cos( theta2 ) + point1Y;

	// 黒色で、振り子のバーを描画
	setDrawColor( renderer, 0, 0, 0, 255 );
	drawLine( renderer, centerX, centerY, point1X, point1Y );
	drawLine( renderer, point1X, point1Y, point2X, point2Y );

	// それぞれの色で、振り子のボールを描画
	int r = 10;
	setDrawColor( renderer, 0, 255, 0, 255 );
	drawPoint( renderer, centerX, centerY, r, true );

	setDrawColor( renderer, 0, 0, 255, 255 );
	drawPoint( renderer, point1X, point1Y, r, true );

	setDrawColor( renderer, 255, 0, 0, 255 );
	drawPoint( renderer, point2X, point2Y, r, true );

	// GUIコンポーネントの再描画
	paintComponent( displayLabel );
	paintComponent( window );
}

// 毎周期の更新処理を行う関数
void update(){
	float theta1DotDot, theta2DotDot;
	float a1, a2, b, d1, d2;

	// パラメータA1,A2の計算
	a1 = (mass1 + mass2) * length1 * length1;
	a2 = mass2 * length2 * length2;

	// アニメーションの1フレームあたりの処理
	for( int skip=0; skip<5000; skip++ ){
		// パラメータB,D1,D2の計算
		b = mass2 * length1 * length2 * cos( theta1 - theta2 );
		d1 = -mass2 * length1 * length2 * theta2Dot * theta2Dot * sin( theta1 - theta2 ) - ( mass1 + mass2 ) * gravityAccel * length1 * sin( theta1 );
		d2 = mass2 * length1 * length2 * theta1Dot * theta1Dot * sin( theta1 - theta2 ) - mass2 * gravityAccel * length2 * sin( theta2 );

		// 振り子をDTの時間だけ運動させる
		theta1DotDot = ( a2*d1 - b*d2 ) / ( a1*a2 - b*b );
		theta2DotDot = ( a1*d2 - b*d1 ) / ( a1*a2 - b*b );
		theta1Dot += theta1DotDot * deltaT;
		theta2Dot += theta2DotDot * deltaT;
		theta1 += theta1Dot * deltaT;
		theta2 += theta2Dot * deltaT;
	}
}

// イベントハンドラ、ウィンドウが閉じられた際にコールされる
void onWindowClose( int id ){
	loopState = false; // メインループを脱出、プログラム終了
}