/**
 *		engagement_simulation.cpp
 *
 *  ターミナル・フェーズにおける弾道ミサイル迎撃をシミュレートする
 *
 **/

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

#define G (9.799)                 /* 重力の加速度 [m/s^2] */
#define RAD (57.2958)             /* radian to degree 変換係数 */

#define AUG (0)                   /* augmented prop. navi. 不採用:規定値 */
#define DATA_OUT (0.1999)         /* データ出力間隔[s] 1秒:規定値*/
#define ReEntryAlt (60000.0)      /* 再突入高度[m]:規定値 */
#define ReEntryVel (3000.0)       /* 再突入速度[m/s]:規定値 */
#define ReEntryAng (45.0)         /* 再突入角度[deg]:規定値 */
#define BETA (2000)               /* 再突入体の弾道係数[Kg/m^2]:規定値 */
#define BETA_EST (2000)           /* 再突入体の弾道係数の推定値:規定値 */
#define XM_InitPosition (54550.0) /* 迎撃ミサイル初期位置(x-座標)[m]:規定値 */
#define InterceptAlt (15000)      /* 迎撃所望高度[m]:規定値 */
#define GLIMIT (11)               /* 迎撃ミサイル横加速度限界[G]:規定値 */

/* 所望迎撃点算定関数 */
void intercept_point(double ytdes,double xt,double yt,double vtx,
	double vty,double betest,double *xtf,double *ytf,double *tfdes);

int main()
{
	int step;                   /*計算のステップ*/
	int augment;                /*augmented prop. navi.を使うか否か*/
	double navconst;            /*有効比例航法定数*/
	double xt,yt;               /*target の (x,y) 位置 [m]*/
	double xm,ym;               /*迎撃ミサイルの(x,y)位置[m]*/
	double vt,vtx,vty;          /*target の速度,(x,y)成分[m/s]*/
	double ytdes;               /*所望(desired)のtarget迎撃高度[m]*/
	double gamt,gamtdeg;        /*target再突入角(γ)[rad],[deg]*/
	double beta,betest;         /*targetの弾道係数(β),同推定値[Kg/m^2]*/
	double xnclimg,xnclim;      /*迎撃ミサイル横加速度コマンド限界[G],[N]*/
	double xtf,ytf,tfdes;       /*target 迎撃未来位置(x,y)[m],経過秒時[s]*/
	double rtmf,xtmf,ytmf;      /*迎撃位置：target-迎撃ミサイル間の距離[m],時間[s]*/
	double gamm,gammdeg;        /*迎撃ミサイルと迎撃位置を結ぶ直線の垂直面角[rad],[deg]*/
	double vm,vmx,vmy;          /*迎撃ミサイルの速度,(x,y)成分[m/s]*/
	double rtm,rtmx,rtmy;       /*targetと迎撃ミサイルの距離,(x,y)成分[m]*/
	double vc,vtmx,vtmy;        /*target-迎撃ミサイルの相対接近速度,(x,y)成分[m/s]*/
	double t;                   /*経過秒時[s]*/
	double h;                   /*積分時間刻み幅[s]*/
	double dout;                /*データ出力間隔[s]*/
	double xncg,xnc;            /*迎撃ミサイル横加速度コマンド[G],[N]*/
	double xtold,ytold;         /*１計算ステップ前の(xt,yt)の値[m]*/
	double vtxold,vtyold;       /*１計算ステップ前の(vtx,vty)の値[m/s]*/
	double xmold,ymold;         /*１計算ステップ前の(xm,ym)の値[m]*/
	double vmxold,vmyold;       /*１計算ステップ前の(vmx,vmy)の値[m/s]*/
	double atg,atx,aty;         /*target の加速度 [G],(x,y)成分[N]*/
	double amx,amy;             /*迎撃ミサイルの横加速度(x,y)成分[N]*/
	double xtk,ytk;             /*(xt,yt)の[Km]表示*/
	double xmk,ymk;             /*(xm,ym)の[Km]表示 */
	double atplos,atplosg;      /*target：加速度のLOS直交成分[N],[G]*/
	double rho;                 /*大気密度(ρ) [Kg/m~3]*/
	double q;                   /*大気による動圧 [N/m^2]*/
	double xlam,xlamdot;        /*LOSの水平面に対する角(λ)[rad],同角速度[rad/s]*/

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

	augment = AUG;
	navconst = 3.0;

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

	xm = XM_InitPosition;
	ym = 0.0;

	vt = ReEntryVel;

	ytdes = InterceptAlt;

	gamtdeg = ReEntryAng;
	gamt = gamtdeg / RAD;

	beta = BETA;
	betest = BETA_EST;

	xnclimg = GLIMIT;
	xnclim = xnclimg * G;

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

 /* 関数呼び出し */

	intercept_point(ytdes,xt,yt,vtx,vty,betest,&xtf,&ytf,&tfdes);

	xtmf = xtf - xm;
	ytmf = ytf - ym;

	gammdeg = RAD * atan2(ytmf, xtmf);
	gamm = gammdeg / RAD;

	rtmf =sqrt(xtmf*xtmf + ytmf*ytmf);
	vm = rtmf / tfdes;
	vmx = vm * cos(gamm);
	vmy = vm * sin(gamm);

	rtmx = xt - xm;
	rtmy = yt - ym;
	rtm = sqrt(rtmx*rtmx + rtmy*rtmy);

	vtmx = vtx - vmx;
	vtmy = vty - vmy;
	vc = -(rtmx*vtmx + rtmy*vtmy) / rtm;

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

	xnc = 0.0;

	while( vc >= 0)
	{
 		if(rtm < 300){
			h = 0.0002;
		}
		else{
			h = 0.01;
		}

		dout = dout + h;

		xtold = xt;
		ytold = yt;

		vtxold = vtx;
		vtyold = vty;

		xmold = xm;
		ymold = ym;

		vmxold = vmx;
		vmyold = vmy;

 		step = 1;

		do
		{
			ytk = yt / 1000.0;

 			if( yt < 10000.0){
				rho = 1.225 * exp(-0.1051 * ytk) / G;
			}
			else{
				rho = 2.2020 * 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 * q * sin(gamt) / beta - G;

			rtmx = xt - xm;
			rtmy = yt - ym;
			rtm = sqrt(rtmx*rtmx + rtmy*rtmy);

			vtmx = vtx - vmx;
			vtmy = vty - vmy;
			vc = -(rtmx*vtmx + rtmy*vtmy) / rtm;

			xlam = atan2(rtmy, rtmx);
			xlamdot = (rtmx*vtmy - rtmy*vtmx) /(rtm*rtm);
			atplos = -atx * sin(xlam) + aty * cos(xlam);

			if(augment==1){     /* augmented proportional navigation */
				xnc = navconst * vc * xlamdot + 0.5 * navconst * atplos;
			}
			else{
				xnc = navconst * vc * xlamdot;
			}

			if(xnc > xnclim){
				xnc = xnclim;
			}

			if(xnc < -xnclim){
				xnc = -xnclim;
			}

			amx = -xnc * sin(xlam);
			amy =  xnc * cos(xlam);
 
			switch(step)
			{
				case 1:
					step = 2;

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

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

					xm = xm + h * vmx;
					ym = ym + h * vmy;

					vmx = vmx + h * amx;
					vmy = vmy + h * amy;

 					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);

					xm = 0.5 * (xmold + xm + h * vmx);
					ym = 0.5 * (ymold + ym + h * vmy);

					vmx = 0.5 * (vmxold + vmx + h * amx);
					vmy = 0.5 *(vmyold + vmy + h * amy);

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

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

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

						xmk = xm / 1000.0;
						ymk = ym / 1000.0;

						xncg = xnc / G;
						atplosg = atplos / G;

						vm = sqrt(vmx*vmx + vmy*vmy);

						printf("%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,xmk,ymk,vm,atg,xncg,atplosg);

						fprintf(fptr,"%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,xmk,ymk,vm,atg,xncg,atplosg);
 					}
				    break;

				default:;

			} /* switch-case */

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

	} /* while */

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

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

	xmk = xm / 1000.0;
	ymk = ym / 1000.0;

	xncg = xnc / G;
	atplosg = atplos / G;

	vm = sqrt(vmx*vmx + vmy*vmy);

	printf("%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,xmk,ymk,vm,atg,xncg,atplosg);

	fprintf(fptr,"%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,xtk,ytk,xmk,ymk,vm,atg,xncg,atplosg);
 	
	printf("%10.4f\n",rtm);
	fclose (fptr);

	return 0;
}

void intercept_point(double ytdes,double xt,double yt,double vtx,
	double vty,double betest,double *xtf,double *ytf,double *tfdes)
{
	int step;               /*計算のステップ*/
	double t;               /*経過秒時[s]*/
	double h;               /* 積分時間刻み幅[s]*/
	double xtold,ytold;     /*１計算ステップ前の(xt,yt)の値[m]*/
	double vtxold,vtyold;   /*１計算ステップ前の(vtx,vty)の値[m/s]*/
	double atx,aty;         /*target の加速度 (x,y)成分[N]*/
	double rho;             /*大気密度(ρ)[Kg/m^3]*/
	double vt;              /*target速度[m/s]*/
	double q;               /*大気による動圧 [N/m^2]*/
	double gamt;            /*target再突入角(γ)[rad]*/
	double beta;            /*target弾道係数[Kg/m^2]*/

	t = 0.0;
	h = 0.01;
	beta = betest;

	while( yt >= ytdes )
	{
		xtold = xt;
		ytold = yt;

		vtxold = vtx;
		vtyold = vty;
 		step = 1;

		do
		{
			if( yt < 10000.0){
				rho = 1.225 * exp(-0.1051 * yt/1000.0) / G;
			}
			else{
				rho = 2.2020 * exp(-0.1558 * yt/1000.0) / 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 * q * sin(gamt) / beta - G;
 
			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);

					break;

				default :;

			} /* switch-case */

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

	} /* while */

 	*xtf = xt;
	*ytf = yt;

	*tfdes = t;
}