/**
 *     miss_due_to_step_target_displacement.cpp
 *
 *     ステップターゲット変位に対する５次の伝達関数を持つホーミングループのミスデスタンス
 **/


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

#define PNR (3) /* 有効航法定数 */

int main()
{
	int step;

	double tau;      /* System Time Constant [s] */
	double tf;       /* time of Flight [s] */
	double vc;       /* 相対接近速度 [m/s^2]*/
	double ytic;     /* 目標初期変位 [m] */
	double t;        /* 時間 [s] */
	double s;        /* データ出力間隔 [s] */
	double tp;       /* 時間カウンタ [s] */
	double x2;       /* impulse 入力 */
	double x3;       /* step 入力 */
	double x4;       /* ｼｰｶ部積分出力 */
	double x5;       /* ノイズフィルタ積分出力 */
	double x6,x7,x8; /* Flight Control部積分出力 */
	double h;        /* 積分時間刻み幅 [s] */
	double x2old,x3old,x4old,x5old,x6old,x7old,x8old;     /* 旧値 */
	double x2d,x3d,x4d,x5d,x6d,x7d,x8d;                   /* 微分値 */
	double xmyt;     /* ミスデスタンス [m] */
	double y1;       /* LOS角速度 [rad/s] */
	double tgo;      /* Time to Go [s] */

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

	tau = 1.0;

	tf = 10.0;

	vc = 5000;

	ytic = 1.0;

	t = 0.0;
	s = 0.0;

	tp = t + 0.00001;

	x2 = 0;
	x3 = 1;

	x4 = 0;
	x5 = 0;

	x6 = 0;
	x7 = 0;
	x8 = 0;

	h = 0.01;

	while(tp <= (tf - 0.00001))
	{ 
		s = s + h;

		x2old = x2;
		x3old = x3;
		x4old = x4;
		x5old = x5;
		x6old = x6;
		x7old = x7;
		x8old = x8;

		step = 1;

		do
		{
			x2d = x3;

			y1 = 5 * (5 * x5/tau + x4) / tau;

			tgo = tp + 0.00001;

			x3d = y1 / (vc * tgo);
			x4d = -y1;
			x5d = -5 * x5/tau + 5 * x6 * PNR * vc / tau;
			x6d = -5 * x6/tau + 5 * x7/tau;
			x7d = -5 * x7/tau + 5 * x8/tau;
			x8d = -5 * x8/tau - x2;
		

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

					x2 = x2 + h * x2d;
					x3 = x3 + h * x3d;
					x4 = x4 + h * x4d;
					x5 = x5 + h * x5d;
					x6 = x6 + h * x6d;
					x7 = x7 + h * x7d;
					x8 = x8 + h * x8d;

					tp = tp + h;

					break;

				case 2:
					step = 1:

					x2 = (x2old + x2) / 2 + 0.5 * h * x2d;
					x3 = (x3old + x3) / 2 + 0.5 * h * x3d;
					x4 = (x4old + x4) / 2 + 0.5 * h * x4d;
					x5 = (x5old + x5) / 2 + 0.5 * h * x5d;
					x6 = (x6old + x6) / 2 + 0.5 * h * x6d;
					x7 = (x7old + x7) / 2 + 0.5 * h * x7d;
					x8 = (x8old + x8) / 2 + 0.5 * h * x8d;
					
					if( s >= 0.09999)
					{
						s = 0.0;
						xmyt = ytic * x3;

						printf("%10.3f %10.3f \n",tp,xmyt);
						fprintf(fptr,"%10.3f %10.3f \n",tp,xmyt);
					}

					break;

				default:;

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

	fclose (fptr);
	return 0;
}