/**
 *	trajectory_with_booster_2stage.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.73)          /* 推進剤質量比,既定値           */
#define PL (1000)           /* Payload[kg],規定値            */
#define ReqV (6200)         /* 所要速度[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 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 gamdeg,gam;          /*経路角(γ)[deg],[rad]*/
	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 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 trst;                /*ブースター推力[N]*/
	double at,vel;              /*ブースト加速度[N],速度[m/s]*/
	double axt,ayt;             /*ブースト加速度[N]の(x,y)成分*/
	double temp;                /*一時変数*/

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

	revolution = CCW;

	xisp1 = ISP;
	xisp2 = ISP;

	xmf1 = XMF;
	xmf2 = XMF;

	wpay = PL;

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

	amax1 = MaxA;
	amax2 = MaxA;

	gamdeg = GAMMA;
	gam = gamdeg / RAD;

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

	trst2 = amax2 * (wpay + ws2);
	tb2 = xisp2 * wp2 / trst2;

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

	trst1 = amax1 * (wtot2 + ws1);
	tb1 = xisp1 * wp1 / trst1;

	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 < 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(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;
}