/**
 *      boost_phase_engagement_simulation.cpp
 *
 *    弾道ミサイルに対するブーストフェーズ交戦シミュレーション
 *
 **/


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

#define R (6367448)         /* 地球の平均半径 [m] */
#define GM (3.986004e14)    /* 地心引力定数 [m^3/s^2] */
#define G (9.80665)         /* 地球表面における重力の加速度 [m/s~2] */
#define PI (3.14159265)     /* 円周率 */
#define RAD (57.29578)      /* [rad]→[deg]変換係数 */

#define GAMMA (61)          /* 軌道投入角度 {deg] */
#define ISP (350)           /* 比推力 [s] */
#define XMF (0.73)          /* 推進剤質量比 */
#define PL (1000)           /* Payload [kg] */
#define ReqV (6200)         /* 所要速度 [m/s] */
#define MaxA (20)           /* 最大軸加速度 [G] */

#define PNR (3)             /* 有効比例定数 */
#define THETA_M (86)        /* ミサイル発射位置(偏角) [deg] */
#define TOF (50)            /* Time of Flight [s] */
#define PredErr (0)         /* 迎撃点予測誤差 */
#define DATA_OUT (0.99999)  /* データ出力時間間隔 [s] 規定値 1 [s] */
#define AUG (0)             /* augmented prop. navi. 不採用:規定値(0) */


/* 迎撃ミサイル所要速度計算関数 */
void lambert(double xm,double ym,double tf,double xtf,double ytf,
	double *vrxm,double *vrym,double xlongm,double xlongt);
/* Downrange 計算関数 */
void distance(double xt,double yt,double xfirstt,double yfirstt,double *distkt);
/* 予想迎撃点計算関数 */
void predict (double tf,double xt,double yt,double vxt,double vyt,
		double *xtf,double *ytf,double wp1,double wtot,double tb1,
		double trst1,double tb2,double wp2,double wtot2,
		double trst2,double wpay);

int main()
{
	int step;                   /* 計算ステップ */
	int augment;                /*augmented prop. navi.を使うか否か*/
	
	double revolution;          /* 偏角(θ)の回転の向き*/
	double xnclim;              /* ミサイルコママンド加速度制限[m/s^2] */ 
	double h;                   /* 積分時間刻み幅[s] */
	double xisp1,xisp2;         /* 比推力(1段,2段) */
	double xmf1,xmf2;           /* 燃料質量比(1段,2段) */
	double wpay;                /* Payload[Kg]*/
	double delv,delv1,delv2;    /* 速度増分[m/s](1段,2段) */
	double amax1,amax2;         /* 軸加速度[G](1段,2段) */
	double xkickdeg;            /* 軌道投入角度[deg] */
	double numer2,denom2,wp2;   /* 第２段燃料質量Wp[Kg]公式:分子,分母 */
	double ws2,wtot2;           /* 第２段構造質量,全質量[Kg] */
	double trst2,tb2;           /* 第２段推力[N],燃焼時間[s] */
	double numer1,denom1,wp1;   /* 第１段燃料質量Wp[Kg]公式:分子,分母 */
	double ws1,wtot;            /* 第１段構造質量,全質量[Kg] */
	double trst1,tb1;           /* 第１段推力[N],燃焼時間[s] */
	double xlongmdeg,xlongtdeg; /* ミサイル,target位置偏角[deg] */
	double altktic,altkmic;     /* target,ミサイル初期高度[m] */
	double tf;                  /* 誘導飛翔時間[s] */
	double altt,altm;           /* target,ミサイル高度[m] */
	double s;                   /* 時間カウンター */
	double dout;                /* データ出力時間間隔[s] */
	double xlongm,xlongt;       /* xlongmdeg,xlongtdeg[deg]→[rad] */
	double xm,ym;               /* ミサイル位置(x,y)[m] */
	double xt,yt;               /* target位置(x,y)[m] */
	double xfirstt,yfirstt;     /* target初期位置[m] */
	double vxt,vyt;             /* target速度(x,y)成分[m/s] */
	double t;                   /* 交戦時間[s] */
	double xtf,ytf;             /* target未来位置(x,y)[m] */
	double vxm,vym;             /* ミサイル速度(x,y)[m/s] */
	double xtm,ytm,rtm;         /* target位置-ミサイル位置[m] */
	double vxtm,vytm;           /* target速度-ミサイル速度[m/s] */
	double vc;                  /* 相対接近速度[m/s] */
	double tgo;                 /* Time-to-Go[s] */
	double xtold,ytold;         /* (xt,yt)の旧値[m] */
	double vxtold,vytold;       /* (vxt,vyt)の旧値[m/s] */
	double xmold,ymold;         /* (vxt,vyt)の旧値[m/s] */
	double vxmold,vymold;       /* (vxm,vym)の旧値[v] */
	double delvold;             /* delvの旧値[m/s] */
	double xtdot,ytdot;         /* (xt,yt)の時間微分値[m/s] */
	double vxtdot,vytdot;       /* (vxt,Vyt)の時間微分値[m/s^2] */
	double xmdot,ymdot;         /* (xm,ym)の時間微分値[m/s] */
	double vxmdot,vymdot;       /* ミサイル誘導コマンドの絶対値[m/s^2] */
	double delvd;               /* ミサイル誘導コマンドの絶対値[m/s^2] */
	double xkt,ykt;             /* (xt,yt)の[km]表示[km] */
	double xkm,ykm;             /* (xm,ym)の[km]表示[km] */
	double altkt,altkm;         /* altt,altm の[km]表示[km] */
	double distkt;              /* targetのdownrange[km] */
	double wgt;                 /* ミサイル重量[Kg]*/
	double trst;                /* ブースター推力[N]*/
	double at,vel;              /* ブースト加速度[m/s^2],速度[m/s]*/
	double axt,ayt;             /* ブースト加速度[m/s^2],(x,y)成分*/
	double tembott;             /* ミサイル運動方程式の分母*/
	double atplos;              /* ブースト加速度[m/s^2]の(x,y)成分*/
	double xlam,xlamd;          /* LOS角度(λ)[rad],dλ/dt[rad/s] */
	double xnc,axm,aym;         /* ミサイル誘導コマンド[m/s^2] */
	double tembotm;             /* ミサイル運動方程式の分母 */
	//double tgolam;              /* Time-to-Go(lambert()に渡す変数)[s] */
	double vrxm,vrym;           /* ミサイル所要速度(x,y)成分[m/s] */
	double distkm;              /* distm の[km]表示[km] */
	double xncg;                /* ミサイル誘導コマンド [G] */
	double atplosg;             /* ブースト加速度[G]の(x,y)成分*/

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

	revolution = 0;

	xnclim = 20 * G;
	
	augment = AUG;

 	h = 0.01;

	xisp1 = ISP;
	xisp2 = ISP;

	xmf1 = XMF;
	xmf2 = XMF;

	wpay = PL;

	delv = ReqV;
	delv1 = delv / 3;
	delv2 = delv * 2 / 3;

	amax1 = MaxA;
	amax2 = MaxA;

	xkickdeg = GAMMA;

	numer2 = wpay * (exp(delv2 / (xisp2 * G)) - 1.0);
	denom2 = 1 / xmf2 - ((1.0 - xmf2) / xmf2) * exp(delv2 / (xisp2 * G));
	wp2 = numer2 / denom2;

	ws2 = wp2 * (1 - xmf2) / xmf2;
	wtot2 = wp2 + ws2 + wpay;
	trst2 = amax2 * (wpay + ws2);
	tb2 = xisp2 * wp2 / trst2;

	numer1 = wtot2*(exp(delv1/(xisp1*G)) - 1.0);
	denom1 = 1 / xmf1 - ((1.0 - xmf1) / xmf1) * exp(delv1 / (xisp1 * G));
	wp1 = numer1 / denom1;

	ws1 = wp1*(1 - xmf1) / xmf1;
	wtot = wp1 + ws1 + wtot2;
	trst1 = amax1 * (wtot2 + ws1);
	tb1 = xisp1 * wp1 / trst1;

	xlongmdeg = THETA_M ;
	xlongtdeg = 90;

	altktic = 0;
	altkmic = 0;

	tf = TOF;

	altt = altktic * 1000;
	altm = altkmic * 1000;

	s = 0.0;
	dout = 0.0;

	xlongm = xlongmdeg / RAD;
	xlongt = xlongtdeg / RAD;

	xm = (R + altm) * cos(xlongm);
	ym = (R + altm) * sin(xlongm);

	xt = (R + altt) * cos(xlongt);
	yt = (R + altt) * sin(xlongt);

	xfirstt = xt;
	yfirstt = yt;

	if (revolution == 1){                           /* in case of CCW */
	  vxt = cos(PI/2.0 - xkickdeg / RAD + xlongt);
	  vyt = sin(PI/2.0 - xkickdeg  /RAD + xlongt);
	}
	else{                                           /* in case of CW */
	  vxt = cos(-PI /2.0 + xkickdeg / RAD + xlongt);
	  vyt = sin(-PI /2.0 + xkickdeg / RAD + xlongt);
	}

	xfirstt = xt;
	yfirstt = yt;

	t=0.0;

/* 予想迎撃点計算関数 呼び出し*/
	predict (tf,xt,yt,vxt,vyt,&xtf,&ytf,wp1,wtot,tb1,
		trst1,tb2,wp2,wtot2,trst2,wpay);

	ytf = ytf + PredErr;

/* 迎撃ミサイル所要速度計算関数 呼び出し*/
	lambert(xm,ym,tf,xtf,ytf,&vrxm,&vrym,xlongm,xlongt);

	vxm = vrxm;
	vym = vrym;

	xtm = xt - xm;
	ytm = yt - ym;
	rtm = sqrt(xtm*xtm + ytm*ytm);

	vxtm = vxt - vxm;
	vytm = vyt - vym;
	vc = -(xtm*vxtm + ytm*vytm) / rtm;

	delv = 0.0;

	while(vc >= 0)
	{
	 	tgo = rtm / vc;

		if(tgo > 0.1)
			h = 0.01;
		else
			h = 0.0001;

		xtold = xt;
		ytold = yt;

		vxtold = vxt;
		vytold = vyt;

		xmold = xm;
		ymold = ym;

		vxmold = vxm;
		vymold = vym;

		delvold = delv;

		step = 1;

		do
		{
		 	if(t < tb1){
				wgt = -wp1 * t / tb1 + wtot;
				trst = trst1;
			}
			else if(t < (tb1 + tb2)){
				wgt = -wp2 * t / tb2 + wtot2 + wp2 * tb1 / tb2;
				trst = trst2;
			}
			else{
				wgt = wpay;
				trst = 0.0;
			}

			at = G *trst / wgt;
			vel = sqrt(vxt*vxt + vyt*vyt);

			axt = at * vxt / vel;
			ayt = at * vyt / vel;

			tembott = pow((xt*xt + yt*yt),1.5);
			vxtdot = -GM * xt / tembott + axt;
			vytdot = -GM * yt / tembott + ayt;

			xtdot = vxt;
			ytdot = vyt;

			xtm = xt - xm;
			ytm = yt - ym;
			rtm = sqrt(xtm*xtm + ytm*ytm);

			vxtm = vxt - vxm;
			vytm = vyt - vym;
			vc = -(xtm*vxtm + ytm*vytm) / rtm;

			tgo = rtm / vc;

			xlam = atan2(ytm,xtm);
			xlamd = (xtm*vytm - ytm*vxtm) / (rtm*rtm);

			atplos = vytdot * cos(xlam) - vxtdot * sin(xlam);

			if( t > 25.0)
			
				if(augment==1){     /* augmented proportional navigation */
					xnc = PNR * vc * xlamd + 0.5 * PNR * atplos;
				}
				else{
					xnc = PNR * vc *xlamd;
				}
		
			else
				xnc = 0.0;

			if(xnc > xnclim)
				xnc = xnclim;

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

			delvd = fabs(xnc);

			axm = -xnc * sin(xlam);
			aym =  xnc * cos(xlam);

			tembotm = pow((xm*xm + ym*ym),1.5);
			vxmdot = -GM * xm / tembotm + axm;
			vymdot = -GM * ym / tembotm + aym;

			xmdot = vxm;
			ymdot = vym;

			switch(step)
			{
				case 1:
					step = 2;

					xt = xt + h * xtdot;
					yt = yt + h * ytdot;

					vxt = vxt + h * vxtdot;
					vyt = vyt + h * vytdot;

					xm = xm + h * xmdot;
					ym = ym + h * ymdot;

					vxm = vxm + h *vxmdot;
					vym = vym + h * vymdot;

					delv = delv + h * delvd;

					t = t + h;

					break;

				case 2:
					step = 1;

					xt = (xtold + xt) / 2 + 0.5 * h * xtdot;
					yt = (ytold + yt) / 2 + 0.5 * h * ytdot;

					vxt = (vxtold + vxt) / 2 + 0.5 * h * vxtdot;
					vyt = (vytold + vyt) / 2 + 0.5 * h * vytdot;

					xm = (xmold + xm) / 2 + 0.5 * h * xmdot;
					ym = (ymold + ym) / 2 + 0.5 * h * ymdot;

					vxm = (vxmold + vxm) / 2 + 0.5 * h * vxmdot;
					vym = (vymold + vym) / 2 + 0.5 * h * vymdot;

					delv = (delvold + delv) / 2 + 0.5 * h * delvd;

					altt = sqrt(xt*xt + yt*yt) - R;
					altm = sqrt(xm*xm + ym*ym) - R;

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

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

						xkt = xt / 1000;
						ykt = yt / 1000;

						xkm = xm / 1000;
						ykm = ym / 1000;

						altkt = altt / 1000;

						/* Downrange 計算関数呼び出し */
						distance(xt,yt,xfirstt,yfirstt,&distkt);

						altkm = altm / 1000;

						/* Downrange 計算関数呼び出し */
						distance(xm,ym,xfirstt,yfirstt,&distkm);

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

						printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distkt
								,altkt,distkm,altkm,xncg,delv,atplosg,vxm,vym);

						fprintf(fptr,"%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distkt
								,altkt,distkm,altkm,xncg,delv,atplosg,vxm,vym);
				 	}
					break;

				default:;
			} /* switch-case */
		}while(step >= 2); /* do-while */
	} /* while */

 	xkt = xt / 1000;
	ykt = yt / 1000;

	xkm = xm / 1000;
	ykm = ym / 1000;

	altkt=altt/1000;

    /* Downrange 計算関数呼び出し */
	distance(xt,yt,xfirstt,yfirstt,&distkt);

	altkm = altm / 1000;

    /* Downrange 計算関数呼び出し */
	distance(xm,ym,xfirstt,yfirstt,&distkm);

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

	printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distkt
			,altkt,distkm,altkm,xncg,delv,atplosg,vxm,vym);

	fprintf(fptr,"%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distkt
			,altkt,distkm,altkm,xncg,delv,atplosg,vxm,vym);

	printf("%10.3f %10.3f %10.3f\n",t,rtm,delv);
 	
	fclose (fptr);

	return 0;
}

/* Downrange 計算関数 */
void distance(double xt,double yt,double xfirstt,double yfirstt,double *distkt)
{
	double r;      /* target動径 [m] */
	double cbeta;  /* cos(β) */
	double beta;   /* β */

	r = sqrt(xt*xt + yt*yt);

	cbeta = (xt * xfirstt + yt * yfirstt) / (r * R);

	if(cbeta < 1.0){
		beta = atan2(sqrt(1.0 - cbeta*cbeta), cbeta);

		*distkt = R * beta / 1000;
	}
	else
		*distkt = (xfirstt - xt) / 1000;
}

/* 予想迎撃点計算関数 */
void predict (double tf,double xt,double yt,double vxt,double vyt,
		double *xtf,double *ytf,double wp1,double wtot,double tb1,
		double trst1,double tb2,double wp2,double wtot2,
		double trst2,double wpay)
{
	int step;            /* β */
	double h;            /* 積分刻み幅 [s] */
	double t;            /* 所望迎撃時間 [s] */
	double x,y;          /* target位置 [m] */
	double vx,vy;        /* target速度 [m/s] */
	double xold,yold;    /* (x,y)の旧値 [m] */
	double vxold,vyold;  /* (vx,vy)の旧値 [m/s] */
	double xdot,ydot;    /* (x,y)の時間微分値 [m/s] */
	double vxdot,vydot;  /* (x,y)の時間微分値 [m/s^2] */
	double wgt;          /* ミサイル重量 [kg] */
	double trst;         /* ブースター推力 [N]  */
	double at,vel;       /* ブースツ加速度 [m/s^2],速度 [m/s] */
	double axt,ayt;      /* ブースト加速度の(x,y)成分 [m/s^2] */
	double tembott;      /* ミサイルの運動方程式の分母 */

	h = 0.01;
	t = 0.0;

	x = xt;
	y = yt;

	vx = vxt;
	vy = vyt;

	while(t <= (tf - 0.00001))
	{
		xold = x;
		yold = y;

		vxold = vx;
		vyold = vy;

		step = 1;

		do
		{
			if(t<tb1){
				wgt = -wp1 * t / tb1 + wtot;
				trst = trst1;
			}
			else if(t < (tb1 + tb2)){
				wgt = -wp2 * t / tb2 + wtot2 + wp2 * tb1 / tb2;
				trst = trst2;
			}
			else{
				wgt = wpay;
				trst = 0.0;
			}

			at = G * trst / wgt;
			vel = sqrt(vx*vx + vy*vy);

			axt = at * vx / vel;
			ayt = at * vy / vel;

			tembott = pow((x*x + y*y),1.5);
			vxdot = -GM * x / tembott + axt;
			vydot = -GM * y / tembott + ayt;

			xdot = vx;
			ydot = vy;

			switch(step)
			{
				case 1:
					step = 2;

					x = x + h * xdot;
					y = y + h * ydot;

					vx = vx + h * vxdot;
					vy = vy + h * vydot;

					t = t + h;

					break;

				case 2:
					step = 1;

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

					vx = (vxold + vx) / 2 + 0.5 * h * vxdot;
					vy = (vyold + vy) / 2 + 0.5 * h * vydot;

					break;

				default:;

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

 	*xtf = x;
	*ytf = y;
}

/* 迎撃ミサイル所要速度計算関数 */
void lambert(double xm,double ym,double tfdes,double xt,double yt,
		double*vrx,double*vry,double xlongm,double xlongt)
{
	int icount;               /* カウンター */
	double rm;                /* ミサイル動径 */
	double rt;                /* target動径 */
	double cphi,phi,sphi;     /* cos(Φ),Φ,sin(Φ) */
	double r0;                /* ミサイル動径初期値 */
	double gmin,gmax,gam;     /* 経路角:γmin,γmax,γ */
	double numer,temp,denom;  /* 速度公式:分子,一時変数,分母 */
	double v;                 /* ミサイル速度 */
	double xlam;              /* λ(旋回遠心力：地心引力) */
	double top1,bot1p,bot1;   /* 所望到達時間公式(1/3) */
	double top2,bot2;         /* 所望到達時間公式(2/3) */
	double top3,bot3;         /* 所望到達時間公式(3/3) */
	double tf;                /* Time of Fight [s] */
	double xnext;             /* 次の探索値 */
	double told;              /* tf の旧値 */
	double gamold;            /* gam の旧値 */

	rm = sqrt(xm*xm + ym*ym);
	rt = sqrt(xt*xt + yt*yt);

	cphi =(xm*xt + ym*yt) / (rm * rt);
	phi = atan2(sqrt(1.0 - cphi*cphi), cphi);
	sphi = sin(phi);

	r0 = rm;

	gmin = atan2((sphi - sqrt(2.0 * r0 * (1.0 - cphi) / rt)), (1.0 - cphi));
	gmax = atan2((sphi + sqrt(2.0 * r0 * (1.0 - cphi) / rt)), (1.0 - cphi));
	gam = (gmin + gmax) / 2.0;

	for (icount = 1; icount <= 100; icount++){

		numer = GM * (1.0 -cos(phi));
		temp = r0 * cos(gam) / rt - cos(phi + gam);
		denom = r0 * cos(gam) * temp;
		v = sqrt(numer / denom);

		if(xlongt > xlongm){
	  		*vrx = v * cos(PI/2.0 - gam + xlongm);
	  		*vry = v * sin(PI/2.0 - gam + xlongm);
	  	}

		if(xlongt <= xlongm){
	  		*vrx = v * cos(-PI/2.0 + gam + xlongm);
	  		*vry = v * sin(-PI/2.0 + gam + xlongm);
	  	}

		xlam = r0 * v * v / GM;

		top1 = tan(gam) * (1.0 - cos(phi)) + (1.0 - xlam) * sin(phi);
		bot1p = (1.0 - cos(phi)) / (xlam * cos(gam) * cos(gam));
		bot1 = (2.0 - xlam) * (bot1p + cos(gam+phi) / cos(gam));

		top2 = 2.0 * cos(gam);
		bot2 = xlam * pow((2.0 / xlam - 1.0), 1.5);

		top3 = sqrt(2.0 / xlam - 1.0);
		bot3 = cos(gam) / tan(phi / 2.0) - sin(gam);

		temp = (top2 / bot2) * atan2(top3, bot3);

		tf = r0 * (top1 / bot1 + temp) / (v * cos(gam));

		if((fabs(tfdes - tf) <= 0.00000001 * tfdes))
			break;

		if(tf > tfdes)
			gmax =gam;
		else
			gmin = gam;
		
		if(icount == 1)
			xnext = (gmax + gmin) / 2.0;
		else{
			xnext = gam + (gam - gamold) * (tfdes - tf) / (tf - told);

			if(xnext > gmax || xnext < gmin)
				xnext = (gmax + gmin) / 2.0;
		}

		gamold = gam;
		told = tf;
		gam = xnext;
	}
}