/**
 *     mid_course_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 PNR (3)             /* 有効比例定数 */
#define PredErr (0)         /* 目標位置予測誤差 [m] */
#define THETA_M (84)        /* ミサイル発射位置(偏角) [deg] */
#define TOF (120)           /* Time of Flight [s] */
#define DATA_OUT (0.99999)  /* データ出力時間間隔 [s] 規定値 1 [s] */

/* 迎撃ミサイル所要速度計算関数 */
void lambert(double xm,double ym,double tgolam,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);

int main()
{
	int step;                   /* 計算ステップ */
	double xlongmdeg,xlongtdeg; /* ミサイル,target位置偏角[deg] */
	double altktic,altkmic;     /* target,ミサイル初期高度[m] */
	double tf;                  /* 飛翔時間[s] */
	double gamdegt,gamt;        /* target経路角[deg],[rad] */
	double h;                   /* 積分時間刻み幅[s] */
	double distkt;              /* targetのdownrange[km] */
	double phit;                /* target射程角[rad] */
	double altt,altm;           /* target,ミサイル高度[] */
	double r0t;                 /* target動径[m] */
	double numer,temp1,denom;   /* 弾道ミサイル速度公式分子分母 */
	double vt;                  /* 弾道ミサイル速度[m/s] */
	double xlongm,xlongt;       /* xlongmdeg,xlongtdeg[deg]→[rad] */
	double vxt,vyt;             /* target速度(x,y)成分[m/s] */
	double xtf,ytf;             /* target未来位置(x,y)[m] */
	double dout;                /* データ出力時間間隔[s] */
	double xm,ym;               /* ミサイル位置(x,y)[m] */
	double xt,yt;               /* target位置(x,y)[m] */
	double xfirstt,yfirstt;     /* target初期位置[m] */
	double t;                   /* 交戦時間[s] */
	double vxm,vym;             /* ミサイル速度(x,y)[m/s] */
	double xtm,ytm,rtm;         /* target位置-ミサイル位置[m] */
	double vxtm,vytm;           /* target速度-ミサイル速度[m/s] */
	double vc;                  /* 相対接近速度[m/s] */
	double delv;                /* ΔV[m/s] */
	double tgo;                 /* Time-to-Go[s] */
	double xtold,ytold;         /* (xt,yt)の旧値[m] */
	double vxtold,vytold;       /* (vxt,vyt)の旧値[m/s] */
	double xmold,ymold;         /* (xm,ym)の旧値[m] */
	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;       /* (vxm,vym)の時間微分値[m/^2]  */
	double delvd;               /* ミサイル誘導コマンドの絶対値[N] */
	double xkt,ykt;             /* (xt,yt)の[km]表示[km] */
	double xkm,ykm;             /* (xm,ym)の[km]表示[km] */
	double altkt,altkm;         /* altt,altm の[km]表示[km] */
	double temp2;               /* target運動方程式の分母 */
	double xlam,xlamd;          /* LOS角度(λ)[rad],dλ/dt[rad/s] */
	double xnc;                 /* ミサイル誘導コマンド[m/s^2] */
	double axm,aym;             /* ミサイル誘導コマンド(x,y)成分[m/s^2] */
	double temp3;               /* ミサイル運動方程式の分母 */
	double tgolam;              /* Time-to-Go(lambert()に渡す変数)[s] */
	double vrxm,vrym;           /* ミサイル所要速度(x,y)成分[m/s] */
	double distkm;              /* distm の[km]表示[km] */

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

	xlongmdeg = THETA_M;
	xlongtdeg = 90.0;

	altktic = 0.0;
	altkmic = 0.0;

	tf = TOF;

	gamdegt = 35.0;

 	h = 0.01;

	gamt = gamdegt / RAD;
	distkt = 1300;
	phit = distkt * 1000 / R;

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

	r0t = R + altt;

	numer = GM * (1.0 - cos(phit));
	temp1 = r0t * cos(gamt) / R - cos(phit + gamt);
	denom = r0t * cos(gamt) * temp1;
	vt = sqrt( numer / denom );

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

	if (xlongm > xlongt){
	  vxt = vt * cos( PI/2.0 - gamt + xlongt);
	  vyt = vt * sin( PI/2.0 - gamt + xlongt);
	}
	else{
	  vxt = vt * cos(-PI/2.0 + gamt + xlongt);
	  vyt = vt * sin(-PI/2.0 + gamt + xlongt);
	}

	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;

	t = 0.0;

/* 予想迎撃点計算関数呼び出し */
	predict (tf,xt,yt,vxt,vyt,&xtf,&ytf);

	ytf = ytf + PredErr;
	tgolam = tf;

/* 迎撃ミサイル所要速度計算関数呼び出し */
	lambert(xm,ym,tgolam,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
		{
			temp2 = pow((xt*xt + yt*yt),1.5);
			vxtdot = -GM * xt / temp2;
			vytdot = -GM * yt / temp2;

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

			xnc = PNR * vc * xlamd;
			delvd = fabs(xnc);

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

			temp3 = pow((xm*xm + ym*ym),1.5);
			vxmdot = -GM * xm / temp3 + axm;
			vymdot = -GM * ym / temp3 + 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;

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

						printf("%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,distkt
			,altkt,distkm,altkm,xnc,delv);

						fprintf(fptr,"%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,distkt
			,altkt,distkm,altkm,xnc,delv);
					}
					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);

	printf("%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,distkt
			,altkt,distkm,altkm,xnc,delv);

	fprintf(fptr,"%10.4f %10.4f %10.4f %10.4f %10.4f %10.4f %10.4f\n",t,distkt
			,altkt,distkm,altkm,xnc,delv);
	printf("%10.4f %10.4f %10.4f \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)
{
	int step;             /* 計算ステップ */
	double h;             /* 積分刻み幅 */
	double t;             /* 所望迎撃時間 */
	double x,y;           /* target位置 */
	double vx,vy;         /* target速度 */
	double xold,yold;     /* (x,y)の旧値 */
	double vxold,vyold;   /* (vx,vy)の旧値 */
	double xdot,ydot;     /* (x,y)の時間微分値 */
	double vxdot,vydot;   /* (vx,vy)の時間微分値 */
	double temp;          /* 一時変数 */

	h = 0.01;

	t = 0.0;

	x = xt;
	y = yt;

	vx = vxt;
	vy = vyt;

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

		vxold = vx;
		vyold = vy;

		step = 1;

		do
		{
			temp = pow((x*x + y*y), 1.5);

			vxdot = -GM * x / temp;
			vydot = -GM * y / temp;

			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;                /* 所望到達時間 */
	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;
	}
}