/**
 * 弾道ミサイル飛翔経路を次の条件下に求める。
 *  1  大気の影響を無視する。
 *  2  飛翔経路は楕円軌道（亜軌道）の一部とする。
 *  3  ブースト段階を無視する。
 *  4  弾道ミサイルの射程が決まると、対応する初期速度と経路角の組合わせが
 *     決まるので、最初に経路角を与えて計算を開始する。
 *  5  出力は、経過時間、地球重心を原点とする（x,y）座標値、飛翔距離、
 *　   高度とする。
 *  6  弾道方程式（微分方程式）の解法は、Ruge-Kutta 法（２次）による。
 **/

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

#define R  (6367448)        /* 地球の平均半径 [m]               */
#define GM (3.986004e14)    /* 地心引力定数                     */
#define M_PI_2 (1.5707963)  /* π/2                             */

#define GAMMA (45.0)        /* 経路角(γ)[deg],規定値           */
#define RANGE (1300.0)      /* 射程[km],規定値                  */
#define DATA_OUT (4.99999)  /* データ出力間隔[sec],規定値 5 秒  */

/* 弾道ミサイルの地表面上の飛翔距離（Downrange）を求める関数 */
void distance(double x,double y,double x0,double y0,double *distkm);

int main()
{
	int step;                   /*計算のステップ*/
	double h;                   /*積分時間刻み幅[sec]*/
	double gamdeg,gam;          /*経路角(γ)[dgree],[rad]*/
	double distkm;              /*Downrange[km]*/
	double thetadeg,theta;      /*極座標動径偏角(θ)[degree],[rad]*/
	double phi;                 /*射程角(φ)[rad]*/
	double altkm,alt;           /*飛翔高度[km],[m]*/
	double r;                   /*極座標動径長[m]*/
	double numer,denom,temp1;   /*速度計算式の分子、分母、一時変数*/
	double v,vx,vy;             /*ミサイル速度,x,y成分[m/s]*/
	double lam;                 /*λ:ミサイルの遠心力/地心引力定数*/
	double s;                   /*積分時間[sec]*/
	double dout;                /*データ出力間隔(10[sec])*/
	double x,y;                 /*ミサイル現在位置(x,y)座標[m]*/
	double x0, y0;              /*ミサイル初期位置(x,y)座標[m]*/
	double vx0,vy0;             /*ミサイル初期速度(x,y)座標[m/s]*/
	double t;                   /*ミサイル飛翔時間[sec]*/
	double xold,yold;           /*1計算ステップ前の(x,y)の値*/
	double vxold,vyold;         /*1計算ステップ前の(vx,vy)の値*/
	double xdot,ydot;           /*(x,y)の微分値(dx/dt,dy/dt)*/
	double x2dot, y2dot;        /*(x,y)の２次微分値*/
	double xkm, ykm;            /*(x,y)の[m]から[km]への変換値*/
	double temp2;               /*一時変数*/

	FILE *fptr;
	fptr = fopen("DATFIL.TXT","w");

	h = 0.01;

	gamdeg = GAMMA;
	gam = gamdeg / 57.296;

	distkm = RANGE;

	thetadeg = 0;
	theta = thetadeg / 57.296;

	phi = distkm * 1000.0 / R;

	altkm = 0.0;
	alt = altkm * 1000.0;

	r = R + alt;

	numer = GM * ( 1.0 - cos( phi ) );
	temp1 = r * cos( gam ) / R - cos( phi + gam );
	denom = r * cos( gam ) * temp1;
	v = sqrt( numer / denom );

	lam = r * v * v / GM;

	vx = v * cos( M_PI_2 - gam + theta );	/* M_PI_2 = π/2 */
	vy = v * sin( M_PI_2 - gam + theta );

	s = 0;
	dout = 0;

	x = ( R + alt ) * cos( theta );
	y = ( R + alt ) * sin( theta );

	x0 = x;
	y0 = y;

	vx0 = vx;
	vy0 = vy;

	t = 0.0;

	while(alt >=0.0 )
	{
		xold = x;
		yold = y;

		vxold = vx0;
		vyold = vy0;

		step = 1;

		do
		{
			temp2 = pow( x * x + y * y, 1.5 );
			x2dot = - GM * x / temp2;
			y2dot = - GM * y / temp2;

			xdot = vx0;
			ydot = vy0;

			switch(step)
			{
				case 1:
					step = 2;
					
					x = x + h * xdot;
					y = y + h * ydot;

					vx0 = vx0 + h * x2dot;
					vy0 = vy0 + h * y2dot;

					t = t + h;

					break;

				case 2:
					step = 1;

					x = ( xold + x ) / 2 + 0.5 * h * xdot;
					y = ( yold + y ) / 2 + 0.5 * h * ydot;

					vx0 = ( vxold + vx0 ) / 2 + 0.5 * h * x2dot;
					vy0 = ( vyold + vy0 ) / 2 + 0.5 * h * y2dot;

					alt = sqrt( x * x + y * y ) - R;

					s = s + h;
					dout = dout + h;

					if( dout >= DATA_OUT )
					{
 						dout = 0.0;

						xkm = x / 1000.0;
						ykm = y / 1000.0;

						altkm = alt / 1000;

						v = sqrt( lam * GM / ( R + alt ));

						distance(x, y, x0, y0, &distkm );

						printf("%8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n",t,xkm,ykm,distkm,altkm,v );
						fprintf(fptr,"%8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n",t,xkm,ykm,distkm, altkm,v );
					}
					break;

				default:;

			} /*switch*/

		}while(step >= 2);

	} /*while*/

 	xkm = x / 1000.0;
	ykm = y / 1000.0;

	altkm = alt / 1000.0;

	distance( x, y, x0, y0, &distkm );

	printf("%8.2f %8.2f %8.2f %8.2f %8.2f %8.2f\n", t, xkm, ykm, distkm, altkm,v );
 	fclose ( fptr );

	return 0;
}

/**
 * Downrange を求める関数
 **/

void distance(double x,double y,double x0,double y0,double *distkm )
{
	double r, cbeta, beta;

	r = sqrt( x * x + y * y );
	cbeta = ( x * x0 + y * y0 ) / ( r * R );

	if( cbeta <1.0 ){
		beta = atan2( sqrt( 1.0 - cbeta * cbeta ), cbeta );
		*distkm = R * beta / 1000.0;
	}
	else{
		*distkm = ( x0 - x ) / 1000.0;
	}
}