////////////////////////////////////////////////////////////////
//
//	各種コリレータのシミュレーション
//
////////////////////////////////////////////////////////////////

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

const double PI       = 3.1415926535898;//円周率
const double CS       = 299792458.0;//光速
const double F1		  = 1575420000.0;
const double F2       = 1227600000.0;
const double F5       = 1176450000.0;

const int SATMAX             = 36;//GPSの最大総数
const int GPS                = 32;//GPSの最大総数

#define DIV1     1//何個(縦横)にグリッドを区切るか
#define DIVMAX  4//グリッドを格納する配列の最大値を設定

extern FILE *correlator;

extern int Enable[DIVMAX][DIVMAX],Enable_sat[DIVMAX][DIVMAX][SATMAX];//使用可能衛星
extern int Actual_satn;//実際のＧＰＳ，ＧＥＯ衛星の運用数を入れる
extern int Enable_sat_stock[DIVMAX][DIVMAX][SATMAX];//2000/11/04
//予備
extern int Enable2[DIVMAX][DIVMAX];
extern int Enable_sat2[DIVMAX][DIVMAX][SATMAX];//使用可能衛星
extern int Total_sat;//トータル衛星数
extern int Ref_sat;//最大仰角衛星のこと
extern int UNUSE_GPS[16];//アルマナック情報のない衛星をストックする
extern int Com_sat[SATMAX],Com_satn;
extern int Integer_r1[SATMAX],Integer_m1[SATMAX],DD_integer1[SATMAX];
extern int QZSS;

extern double Pr1ref[SATMAX],Pr1mov[SATMAX],Pr2ref[SATMAX],Pr2mov[SATMAX];
extern double DD_code1[SATMAX],DD_code2[SATMAX];
extern double Sat_xpos[SATMAX],Sat_ypos[SATMAX],Sat_zpos[SATMAX];//地球中心座標系
extern double POSx[DIVMAX][DIVMAX],POSy[DIVMAX][DIVMAX],POSz[DIVMAX][DIVMAX];//解析位置
extern double Cn[SATMAX],GPSTIME;
extern double Sat_ele[DIVMAX][DIVMAX][SATMAX],Sat_azi[DIVMAX][DIVMAX][SATMAX];//衛星の仰角、方位角
extern double MP_rov[SATMAX];
extern double OBST1_MASK;

void correlator_simulation()
{
	int i,k=0;
	int sat;

	double dl=0,dm[SATMAX];
	double amp=0.1;
	double repermt=3;
	double cond=0.00002;
	double lm=CS/F1;
	double corr,td,ph;
	double chip=CS/1023000;
	double a[SATMAX]={0},d[SATMAX]={0};
	double mp_rov2[SATMAX]={0},narrow[SATMAX]={0},strobe[SATMAX]={0},L5[SATMAX]={0};

	for(i=1;i<=14000;i++)
	{
		sat=1;
		dl=dl+0.025;
/////////////　narrow correlatorの実装(0.1)　/////////////////
		lm=CS/F1;
		corr=0.1;
		chip=CS/1023000;
		td=corr*chip/2.0;

		//遅延距離による場合分けが必要
		if(dl>=0 && dl<(1.0-amp)*td){//1
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = dl*amp*cos(ph)/(1+amp*cos(ph));
			MP_rov[sat] = dm[sat];
		}
		else if(dl>=(1.0-amp)*td && dl<(1.0+amp)*td){//2
			ph = dl/lm-int(dl/lm);
			//同相側の場合
			if(ph<=0.25 || ph>=0.75){
				ph = 2*PI*ph;
				dm[sat] = dl*amp*cos(ph)/(1+amp*cos(ph));
				MP_rov[sat] = dm[sat];
			}
			else{//逆相側の場合
				ph = 2*PI*ph;
				dm[sat] = amp*cos(ph)*td;
				MP_rov[sat] = dm[sat];
			}

		}
		else if(dl>=(1.0+amp)*td && dl<chip-(1.0+amp)*td){//3
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = amp*cos(ph)*td;
			MP_rov[sat] = dm[sat];
		}
		else if(dl>=chip-(1.0+amp)*td && dl<chip-(1.0-amp)*td){//4
			ph = dl/lm-int(dl/lm);
			//同相側の場合
			if(ph<=0.25 || ph>=0.75){
				ph = 2*PI*ph;
				dm[sat] = amp*cos(ph)*td;
				MP_rov[sat] = dm[sat];
			}
			else{//逆相側の場合
				ph = 2*PI*ph;
				dm[sat] = amp*(chip+td-dl)*cos(ph);
				dm[sat] = dm[sat]/(2-1.0*amp*cos(ph));
				MP_rov[sat] = dm[sat];
			}
		}
		else if(dl>=chip-(1.0-amp)*td && dl<chip+td){//5
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = amp*(chip+td-dl)*cos(ph);
			dm[sat] = dm[sat]/(2-1.0*amp*cos(ph));
			MP_rov[sat] = dm[sat];
		}
		else{
			dm[sat]=0;
			MP_rov[sat] = dm[sat];
		}
		narrow[sat]=MP_rov[sat];
////////////////////////////////////////////////////////////

/////////////　strobe correlatorの実装(0.2)　//////////////
		lm=CS/F1;
		corr=0.2;
		chip=CS/1023000;
		td=corr*chip/2.0;

		//遅延距離による場合分けが必要
		if(dl>=0 && dl<(1.0-amp)*td){
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = dl*amp*cos(ph)/(1+amp*cos(ph));
			mp_rov2[sat] = dm[sat];
		}
		else if(dl>=(1.0-amp)*td && dl<(1.0+amp)*td){
			ph = dl/lm-int(dl/lm);
			//同相側の場合
			if(ph<=0.25 || ph>=0.75){
				ph = 2*PI*ph;
				dm[sat] = dl*amp*cos(ph)/(1+amp*cos(ph));
				mp_rov2[sat] = dm[sat];
			}
			else{//逆相側の場合
				ph = 2*PI*ph;
				dm[sat] = amp*cos(ph)*td;
				mp_rov2[sat] = dm[sat];
			}

		}
		else if(dl>=(1.0+amp)*td && dl<chip-(1.0+amp)*td){
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = amp*cos(ph)*td;
			mp_rov2[sat] = dm[sat];
		}
		else if(dl>=chip-(1.0+amp)*td && dl<chip-(1.0-amp)*td){
			ph = dl/lm-int(dl/lm);
			//同相側の場合
			if(ph<=0.25 || ph>=0.75){
				ph = 2*PI*ph;
				dm[sat] = amp*cos(ph)*td;
				mp_rov2[sat] = dm[sat];
			}
			else{//逆相側の場合
				ph = 2*PI*ph;
				dm[sat] = amp*(chip+td-dl)*cos(ph);
				dm[sat] = dm[sat]/(2-1.0*amp*cos(ph));
				mp_rov2[sat] = dm[sat];
			}
		}
		else if(dl>=chip-(1.0-amp)*td && dl<chip+td){
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = amp*(chip+td-dl)*cos(ph);
			dm[sat] = dm[sat]/(2-1.0*amp*cos(ph));
			mp_rov2[sat] = dm[sat];
		}
		else{
			dm[sat]=0;
			mp_rov2[sat] = dm[sat];
		}
		strobe[sat]=2*MP_rov[sat]-mp_rov2[sat];


///////////////////////// L5の場合のシミュレーション ////////////////////
		lm=CS/F5;
		corr=1.0;
		chip=CS/10230000;
		td=corr*chip/2.0;

		//遅延距離による場合分けが必要
		if(dl>=0 && dl<(1.0-amp)*td){
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = dl*amp*cos(ph)/(1+amp*cos(ph));
			L5[sat] = dm[sat];
		}
		else if(dl>=(1.0-amp)*td && dl<(1.0+amp)*td){
			ph = dl/lm-int(dl/lm);
			//同相側の場合
			if(ph<=0.25 || ph>=0.75){
				ph = 2*PI*ph;
				dm[sat] = dl*amp*cos(ph)/(1+amp*cos(ph));
				L5[sat] = dm[sat];
			}
			else{//逆相側の場合
				ph = 2*PI*ph;
				dm[sat] = amp*(chip+td-dl)*cos(ph);
				dm[sat] = dm[sat]/(2-amp*cos(ph));
				L5[sat] = dm[sat];
			}

		}
		else if(dl>=(1.0+amp)*td && dl<chip+td){
			ph = dl/lm-int(dl/lm);
			ph = 2*PI*ph;
			dm[sat] = amp*(chip+td-dl)*cos(ph);
			dm[sat] = dm[sat]/(2-amp*cos(ph));
			L5[sat] = dm[sat];
		}
		else{
			dm[sat]=0;
			L5[sat] = dm[sat];
		}
//			MPG_rov[sat] = 0;
		L5[sat] = L5[sat];




		
		fprintf(correlator,"%f,%f,%f,%f\n",dl,narrow[sat],strobe[sat],L5[sat]);


////////////////////////////////////////////////////////////
	}//繰り返しの終わり
	exit(1);
}