/**
 *   terminal_phase_simulation.cpp
 *  ターミナル・フェーズにおける弾道ミサイルの飛翔経路をシミュレーションする
 *
 **/

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

#define G (9.799)
#define RAD (57.2958)

#define ALT (50000.0)      /* ターゲット再突入高度 [m],規定値   */
#define VEL (3000.0)       /* ターゲット再突入速度 [m/s],規定値 */
#define GAMMA (45.0)       /* ターゲット再突入角度 [deg],規定値 */
#define BETA (2000.0)      /* ターゲット弾道係数 [kg/m^2]       */
#define DATA_OUT (0.9999)  /* データ出力間隔 [s],規定値　　　   */

int main()
{
	int step;              /* 計算ステップ */

	double xt,yt;          /* ターゲット再突入位置(x,y) [m] */
	double vt;             /* ターゲット再突入速度 [m/s] */
	double gamtdeg,gamma;  /* ターゲット再突入角度 [deg],[rad] */
	double beta;           /* ターゲット弾道係数 [Kg/m^2] */
	double vtx,vty;        /* ターゲット再突入速度(x,y)成分 [m/s] */
	double t;              /* ターゲット飛翔時間 [s] */
	double h;              /* 積分刻み幅 [s] */
	double dout;           /* データ出力間隔 [s] */
	double xtold,ytold;    /* １計算ステップ前の(xt,yt)の値 [m] */
	double vtxold,vtyold;  /* １計算ステップ前の(vtx,vty)の値 [m/s]*/
	double atg,atx,aty;    /* ターゲットに加わる加速度,(x,y)成分 [m/s^2] */
	double xtk,ytk;        /* (x,y)	[km] 表現値 [Km] */
	double rho;            /* 大気密度ρ [Kg/m^3] */
	double q;              /* ターゲット単位面積当たりの動圧 [N]] */
	double gamt;           /* ターゲット経路角 [rad] */

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

    xt = 0.0;
	yt = ALT;
	ytk= yt / 1000.0;

	vt = VEL;

	gamtdeg = GAMMA;
	gamma = gamtdeg / RAD;

	beta = BETA;

	vtx = vt * cos(gamma);
	vty =-vt * sin(gamma);

 	t = 0.0;
	h = 0.01;
	dout = 0.0;

	while( yt >= 0.0 )
	{
		dout = dout + h;

		xtold = xt;
		ytold = yt;

		vtxold = vtx;
		vtyold = vty;

 		step = 1;

		do
		{
			ytk = yt/ 1000.0;

			if(yt <= 90000){
				rho = 1.225 * exp(-0.1051 * ytk) / G;
			}
			else{
				rho = 2.020 * exp(-0.1558 * ytk) / G;
			}

			vt = sqrt(vtx*vtx + vty*vty);

			q =0.5 * rho * vt*vt;
			gamt = atan2( -vty, vtx );

			atx = -G * q * cos(gamt) / beta;
			aty = -G + G * q * sin(gamt) / beta;

			switch( step )
			{
				case 1:

					step = 2;

					xt = xt + h * vtx;
					yt = yt + h * vty;

					vtx = vtx + h * atx;
					vty = vty + h * aty;

 					t = t + h;

					break;

				case 2:

					step = 1;

					xt = 0.5 * (xtold + xt + h * vtx);
					yt = 0.5 * (ytold + yt + h * vty);

					vtx = 0.5 * (vtxold + vtx + h * atx);
					vty = 0.5 * (vtyold + vty + h * aty);

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

						atg = sqrt(atx*atx + aty*aty) / G;

						xtk = xt / 1000.0;
						ytk = yt / 1000.0;

						vt = sqrt(vtx*vtx + vty*vty);

						atg = sqrt(atx*atx + aty*aty) / G;

						printf("%10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,atg,vt);
						fprintf(fptr,"%10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,atg,vt);
 					}
					break;

					default:;

			} /* switch-case */

		}while(step >= 2); /* do-while */

	} /* whike */

 	atg = sqrt(atx*atx + aty*aty) / G;

	xtk = xt / 1000.0;
	ytk = yt / 1000.0;

	vt = sqrt(vtx*vtx + vty*vty);

	atg = sqrt(atx*atx + aty*aty) / G;

	printf("%10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,atg,vt);

	fprintf(fptr,"%10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,atg,vt);
	fclose (fptr);

	return 0;
}