/**
 *	trajectory_with_booster_1stage.cpp
 *	Boost-Phase（１段ロケット）を有する弾道ミサイルの軌道を計算する
 *  出力：経過時間、ミサイルの位置(x,y)、Downrange,	高度、速度
 *
 **/

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

#define R (6367448)         /* 地球の平均半径[m]             */
#define GM (3.986004e14)    /* 地心重力定数[m^3/s^2]         */
#define G (9.799)           /* 地表面での重力の加速度[m/s^2] */
//#define M_PI_2 (1.5707963)  /* π/2                          */
#define RAD (57.2958)       /* radian to degree変換係数      */
#define CCW (1)             /* Counter-Clockwise             */

#define GAMMA (75.0)        /* 経路角(γ)[deg],規定値 		 */
#define DATA_OUT (4.99999)  /* データ出力間隔[s],規定値 5[s] */
#define ISP (350)           /* 比推力[s],規定値              */
#define XMF (0.67)          /* 推進剤質量比,既定値           */
#define PL (1000)           /* Payload[kg],規定値            */
#define ReqV (3400)         /* 所要速度[m/s],規定値          */
#define MaxA (20)           /* 最大軸加速度[G],規定値        */


/* Downrange を求める関数 */
void distance(double x,double y,double x0,double y0,double *distkm);

int main()
{
	int step;                   /*計算のステップ*/
	int revolution;             /*偏角(θ)の回転の向き*/
	double xisp;                /*比推力*/
	double xmf;                 /*燃料質量比*/
	double wpay;                /*Payload[Kg]*/
	double delv;                /*速度増分[m/s]*/
	double amax;                /*軸加速度[G]*/
	double gamdeg,gam;          /*経路角(γ)[deg],[rad]*/
	double numer,denom,wp;      /*燃料質量Wp[Kg]公式:分子,分母*/
	double ws,wtot;             /*構造質量,全質量[Kg]*/
	double trst,tb;             /*推力[N],燃焼時間[s]*/
	double delvk;               /*速度増分[Km/s]*/
	double h;                   /*積分時間刻み幅[s]*/
	double t;                   /*ミサイル飛翔時間[s]*/
	double dout;                /*データ出力間隔[s]*/
	double alt,altkm;           /*高度[m],[Km]*/
	double thetadeg,theta;      /*偏角(θ)[deg],[rad]*/
	double vx,vy;               /*ミサイル速度[m/s]：x,y成分*/
	double x,y;                 /*ミサイル(x,y)[m]*/
	double x0,y0;               /*ミサイル初期位置(x,y)[m]*/
	double vx0,vy0;             /*ミサイル初期速度(x,y)[m/s]*/
	double xold,yold;           /*１計算ステップ前の(x,y)の値*/
	double vxold,vyold;         /*１計算ステップ前の(vx,vy)の値*/
	double xdot,ydot;           /*(x,y)の微分値*/
	double x2dot,y2dot;         /*(x,y)の２次微分値*/
	double distkm;              /*Downrange[Km]*/
	double xkm,ykm;             /*(x,y)[m]から[Km]への変換値*/
	double wgt;                 /*ミサイル重量[Kg]*/
	double at,vel;              /*ブースト加速度[N],速度[m/s]*/
	double axt,ayt;             /*ブースト加速度[N]の(x,y)成分*/
	double temp;                /*一時変数*/

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

	revolution = CCW;

	xisp = ISP;

	xmf = XMF;

	wpay = PL;

	delv = ReqV;

	amax = MaxA;

	gamdeg = GAMMA;
	gam = gamdeg / RAD;

	numer = wpay * (exp(delv / (xisp * G)) - 1.0);
	denom = 1.0 / xmf - ((1.0 - xmf) / xmf) * exp(delv / (xisp * G));
	wp = numer / denom;
	ws = wp * (1.0 - xmf) / xmf;
	wtot = wp + ws + wpay;

	trst = amax * (wpay + ws);
	tb = xisp * wp / trst;

	delvk = delv / 1000.0;

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

	altkm = 0.0;
	alt = altkm * 1000.0;

	thetadeg = 0.0;
	theta = thetadeg / RAD;

	if( revolution == 1 ){
		vx = cos(M_PI_2 - gam + theta);
		vy = sin(M_PI_2 - gam + theta);
	}
	else{
		vx = cos(-M_PI_2 + gam + theta);
	  	vy = sin(-M_PI_2 + gam + theta);
	}

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

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

	x0 = x;
	y0 = y;

	vx0 = vx;
	vy0 = vy;


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

		vxold = vx0;
		vyold = vy0;

		step=1;

		do
		{
			if(t < tb){
				wgt = -wp * t / tb + wtot;
			}
			else{
				wgt = wpay;
				trst = 0.0;
			}

			at = G * trst / wgt;
			vel = sqrt(vx0*vx0 + vy0*vy0);

			axt = at * vx0 / vel;
			ayt = at * vy0 / vel;

			temp = pow(x*x + y*y,1.5);
			x2dot = -GM * x / temp + axt;
			y2dot = -GM * y / temp + ayt;

			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;
 					dout = dout + h;

					if(dout > DATA_OUT)
					{
 						dout = 0.0;

 						distance(x,y,x0,y0,&distkm);
						altkm = (sqrt(x*x + y*y) -R) / 1000.0;
						xkm = x / 1000.0;
						ykm = y / 1000.0;

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

					default:;

			}/*switch-case*/

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

	}/*while*/
					

 	distance(x,y,x0,y0,&distkm);
	altkm = (sqrt(x*x + y*y) -R) / 1000.0;

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

	printf("%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distkm,altkm,xkm,ykm,vel);
	fprintf(fptr,"%10.3f %10.3f %10.3f %10.3f %10.3f %10.3f\n",t,distkm,altkm,xkm,ykm,vel);

	fclose (fptr);

	return 0;
}

void distance(double x,double y,double x0,double y0,double *distkm)
{
	double r;       /* 動径[m]  */
	double cbeta;	/* cos(β)  */
	double 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;
}