////////////////////////////////////////////////////////////////
//	人工的に搬送波位相を生成
//	基準局と移動局の共通衛星抜き出し
////////////////////////////////////////////////////////////////

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

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

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

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

extern FILE *mp_carrier;
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 Integer_r2[SATMAX],Integer_m2[SATMAX],DD_integer2[SATMAX];
extern int Integer_r5[SATMAX],Integer_m5[SATMAX],DD_integer5[SATMAX];
extern int QZSS,MP_FLAG,Correct_wide_integer[SATMAX],Correct_uwide_integer[SATMAX];

extern double Cp1ref[SATMAX],Cp1mov[SATMAX],Cp2ref[SATMAX],Cp2mov[SATMAX],Cp5ref[SATMAX],Cp5mov[SATMAX];
extern double DD_carrier1[SATMAX],DD_carrier2[SATMAX],DD_carrier5[SATMAX];
extern double DD_code1[SATMAX],DD_code2[SATMAX],DD_code5[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],Cn2[SATMAX],GPSTIME,Relative_amp;
extern double Sat_ele[DIVMAX][DIVMAX][SATMAX],Sat_azi[DIVMAX][DIVMAX][SATMAX];//衛星の仰角、方位角
extern double OBST1_MASK,Ultra_wide_carrier[SATMAX];
extern double Ant_height_ref,Ant_height_rover;

double gauss_gen(double,double,int *);
double random(int *);

void make_carrier_l5()
{
	int i,k=0;
	int sat,prn;
	int arr[1];
	double wn,cn;
	double pll_sigma[SATMAX];
	double mp1[SATMAX],mp2[SATMAX],mpf5[SATMAX];
	double wide_carrier[SATMAX];
	double mp_file[SATMAX]={0},true_range_ref[SATMAX],true_range_rov[SATMAX];

	//multipath parameters
//	double delay[SATMAX]      = {0.0};
//	static double delay_rov[SATMAX]  = {0.02};
	static double delay2[SATMAX]     = {0.00};
	static double delay2_rov[SATMAX] = {0.02};
	double amp[SATMAX]               = {0.0};
	double amph,ampl,ampv,dl;
	double permt,permt_wall;
	double repermt=7;
	double cond=0.04;
	double mpf5_wall[SATMAX]={0},mpf5_wall2[SATMAX]={0},mpf5_wall3[SATMAX]={0};
	double dis=15.0;
	double dis2=45,dis3=45;
	double defuse = 1.00;//壁による反射係数を少し減少させる
	double defuse1 = 5.0;//アンテナによる減衰効果値

	static int kaisu5=2000;

	static int incr_carrier5[36]={34,-23,12,-45,56,-67,23,-54,83,-15,49,-42,58,-59,32,-19,43,-45,10,-98,103,-984,234,-765,298,-613,579,-982,346,-129,873,-459,285,-578,235,-653};
	static int incr_carrier6[36]={1030,-940,860,710,-660,-580,430,-350,280,-110,1450,-980,820,-760,630,-570,-410,330,-280,130,1450,-960,810,-700,660,-530,480,-320,-240,180,224,577,375,719,573,381};

	permt = repermt-60.0*CS/F5*cond;
	permt_wall = 3-60.0*CS/F5*0.00002;

	kaisu5++;

////////////////////////   L5   ////////////////////////
	//基準局の搬送波位相の生成(L5)
	for(i=0;i<Enable[0][0];i++){
		sat = Enable_sat[0][0][i];

		//PLLの1σ値を利用
		incr_carrier5[sat]++;
		if(incr_carrier5[sat] == 0)
			incr_carrier5[sat]++;

		arr[0] = incr_carrier5[sat];
		cn = pow(10.0,Cn2[sat]/10.0);
		pll_sigma[sat] = CS/F5/2.0/PI*sqrt(10.0*(1.0+1.0/2.0/0.02/cn)/cn);
		wn = gauss_gen(0,pll_sigma[sat],arr);

		//multipath effects on L2

		dl = 2.0*Ant_height_ref*cos(PI*(90.0-Sat_ele[0][0][sat])/180);		

		amph = cos(PI*Sat_ele[0][0][sat]/180) - sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180));
		amph = amph/(cos(PI*Sat_ele[0][0][sat]/180) + sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180)));
		amph = fabs(amph);

		ampv = permt*cos(PI*Sat_ele[0][0][sat]/180) - sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180));
		ampv = ampv/(permt*cos(PI*Sat_ele[0][0][sat]/180) + sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180)));
		ampv = fabs(ampv);

		ampl = (amph+ampv)/2.0;

		//アンテナの下面から入射されるので、アンテナによる減衰効果があり(L2)
		//60-90 25dB以上減衰 0.056
		//30 17dB減衰 0.141
		//0 12dB減衰 0.251
		double keisu;
//		dec = 0.23*Sat_ele[0][0][sat]*2;//GPS600
//		keisu = 1/(pow(defuse1,dec/20));
		if(Sat_ele[0][0][sat]>=60)
			keisu=0.056;
		if(Sat_ele[0][0][sat]>=30 && Sat_ele[0][0][sat]<=60)
			keisu=-0.085/30*Sat_ele[0][0][sat]+0.226;
		if(Sat_ele[0][0][sat]>=0 && Sat_ele[0][0][sat]<=30)
			keisu=-0.11/30*Sat_ele[0][0][sat]+0.251;

		ampl = ampl*keisu*5;

//		amp[sat] = Relative_amp - Relative_amp*Sat_ele[0][0][sat]/90.0;
//		amp[sat] = -0.24/85.0*Sat_ele[0][0][sat]+0.254;
//		delay2[sat] = delay2[sat]+0.001;

		amp[sat] = ampl;
		delay2[sat] = dl;
		mp1[sat] = amp[sat]*sin(delay2[sat]*2.0*PI*F5/CS);
		mp2[sat] = 1.0+amp[sat]*cos(delay2[sat]*2.0*PI*F5/CS);
		mpf5[sat] = (CS/F5/2.0/PI)*atan2(mp1[sat],mp2[sat]);
		if(MP_FLAG==0)
			mpf5[sat]=0;
		
		true_range_ref[sat] = (Sat_xpos[sat]-POSx[0][0])*(Sat_xpos[sat]-POSx[0][0])
			+ (Sat_ypos[sat]-POSy[0][0])*(Sat_ypos[sat]-POSy[0][0])
			+ (Sat_zpos[sat]-POSz[0][0])*(Sat_zpos[sat]-POSz[0][0]);
		true_range_ref[sat] = sqrt(true_range_ref[sat]);

		Cp5ref[sat] = true_range_ref[sat]+Integer_r5[sat]*CS/F5+wn+mpf5[sat];
	}
	//移動局の搬送波位相の生成(L5)
	for(i=0;i<Enable[DIV1][DIV1];i++){
		sat = Enable_sat[DIV1][DIV1][i];

		//PLLの1σ値を利用
		incr_carrier6[sat]++;
		if(incr_carrier6[sat] == 0)
			incr_carrier6[sat]++;

		arr[0] = incr_carrier6[sat];
		cn = pow(10.0,Cn2[sat]/10.0);
		pll_sigma[sat] = CS/F5/2.0/PI*sqrt(10.0*(1.0+1.0/2.0/0.02/cn)/cn);
		wn = gauss_gen(0,pll_sigma[sat],arr);
		kaisu5++;

		//multipath effects on L5
		dl = 2.0*Ant_height_rover*cos(PI*(90.0-Sat_ele[0][0][sat])/180);		

		amph = cos(PI*Sat_ele[0][0][sat]/180) - sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180));
		amph = amph/(cos(PI*Sat_ele[0][0][sat]/180) + sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180)));
		amph = fabs(amph);

		ampv = permt*cos(PI*Sat_ele[0][0][sat]/180) - sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180));
		ampv = ampv/(permt*cos(PI*Sat_ele[0][0][sat]/180) + sqrt(permt-sin(PI*Sat_ele[0][0][sat]/180)*sin(PI*Sat_ele[0][0][sat]/180)));
		ampv = fabs(ampv);

		ampl = (amph+ampv)/2.0;

		//アンテナの下面から入射されるので、アンテナによる減衰効果があり(L2)
		//60-90 25dB以上減衰 0.056
		//30 17dB減衰 0.141
		//0 12dB減衰 0.251
		double keisu;
//		dec = 0.23*Sat_ele[0][0][sat]*2;//GPS600
//		keisu = 1/(pow(defuse1,dec/20));
		if(Sat_ele[0][0][sat]>=60)
			keisu=0.056;
		if(Sat_ele[0][0][sat]>=30 && Sat_ele[0][0][sat]<=60)
			keisu=-0.085/30*Sat_ele[0][0][sat]+0.226;
		if(Sat_ele[0][0][sat]>=0 && Sat_ele[0][0][sat]<=30)
			keisu=-0.11/30*Sat_ele[0][0][sat]+0.251;

		ampl = ampl*keisu;

//		amp[sat] = Relative_amp - Relative_amp*Sat_ele[0][0][sat]/90.0;
//		amp[sat] = -0.24/85.0*Sat_ele[0][0][sat]+0.254;
//		delay2_rov[sat] = delay2_rov[sat]+0.001;

		amp[sat] = ampl;
		delay2_rov[sat] = dl;
		mp1[sat] = amp[sat]*sin(delay2_rov[sat]*2.0*PI*F2/CS);
		mp2[sat] = 1.0+amp[sat]*cos(delay2_rov[sat]*2.0*PI*F2/CS);
		mpf5[sat] = (CS/F5/2.0/PI)*atan2(mp1[sat],mp2[sat]);

		//壁に反射するマルチパスについて
		if(Sat_azi[DIV1][DIV1][sat]>=45 && Sat_azi[DIV1][DIV1][sat]<=135 && Sat_ele[DIV1][DIV1][sat]<=OBST1_MASK){			
			dl = sqrt(dis*2*dis*2-(dis*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180))*(dis*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180)));
			dl = sqrt(dl*dl+dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180)*dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180));

			amph = cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			amph = amph/(cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			amph = fabs(amph);

			ampv = permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			ampv = ampv/(permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			ampv = fabs(ampv);

			ampl = (amph+ampv)/2.0;

			amp[sat] = defuse*ampl;
			delay2_rov[sat]=dl;
			mp1[sat] = amp[sat]*sin(delay2_rov[sat]*2.0*PI*F5/CS);
			mp2[sat] = 1.0+amp[sat]*cos(delay2_rov[sat]*2.0*PI*F5/CS);
			mpf5_wall[sat] = (CS/F5/2.0/PI)*atan2(mp1[sat],mp2[sat]);
//			mpf5_wall[sat]=0;
		}

		if(Sat_azi[DIV1][DIV1][sat]>=200 && Sat_azi[DIV1][DIV1][sat]<=230 && Sat_ele[DIV1][DIV1][sat]<=OBST1_MASK){			
			dl = sqrt(dis*2*dis*2-(dis*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180))*(dis*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180)));
			dl = sqrt(dl*dl+dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180)*dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180));

			amph = cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			amph = amph/(cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			amph = fabs(amph);

			ampv = permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			ampv = ampv/(permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			ampv = fabs(ampv);

			ampl = (amph+ampv)/2.0;

			amp[sat] = defuse*ampl;
			delay2_rov[sat]=dl;
			mp1[sat] = amp[sat]*sin(delay2_rov[sat]*2.0*PI*F5/CS);
			mp2[sat] = 1.0+amp[sat]*cos(delay2_rov[sat]*2.0*PI*F5/CS);
			mpf5_wall2[sat] = (CS/F5/2.0/PI)*atan2(mp1[sat],mp2[sat]);
//			mpf5_wall[sat]=0;
		}

		if(Sat_azi[DIV1][DIV1][sat]>=310 && Sat_azi[DIV1][DIV1][sat]<=340 && Sat_ele[DIV1][DIV1][sat]<=OBST1_MASK){			
			dl = sqrt(dis*2*dis*2-(dis*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180))*(dis*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180)));
			dl = sqrt(dl*dl+dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180)*dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180));

			amph = cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			amph = amph/(cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			amph = fabs(amph);

			ampv = permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			ampv = ampv/(permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			ampv = fabs(ampv);

			ampl = (amph+ampv)/2.0;

			amp[sat] = defuse*ampl;
			delay2_rov[sat]=dl;
			mp1[sat] = amp[sat]*sin(delay2_rov[sat]*2.0*PI*F5/CS);
			mp2[sat] = 1.0+amp[sat]*cos(delay2_rov[sat]*2.0*PI*F5/CS);
			mpf5_wall3[sat] = (CS/F5/2.0/PI)*atan2(mp1[sat],mp2[sat]);
//			mpf5_wall[sat]=0;
		}


/*		else if(Sat_azi[DIV1][DIV1][sat]>=225 && Sat_azi[DIV1][DIV1][sat]<=315 && Sat_ele[DIV1][DIV1][sat]<=OBST1_MASK){
			dl = sqrt(dis*2*dis*2-dis*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180));
			dl = sqrt(dl*dl+dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180)*dl*tan(PI*Sat_ele[DIV1][DIV1][sat]/180));
		
			amph = cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			amph = amph/(cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			amph = fabs(amph);

			ampv = permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) - sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180));
			ampv = ampv/(permt*cos(PI*Sat_ele[DIV1][DIV1][sat]/180) + sqrt(permt_wall-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			ampv = fabs(ampv);

			ampl = (amph+ampv)/2.0;

			amp[sat] = defuse*ampl;
			delay_rov[sat]=dl;
			mp1[sat] = amp[sat]*sin(delay_rov[sat]*2.0*PI*F1/CS);
			mp2[sat] = 1.0+amp[sat]*cos(delay_rov[sat]*2.0*PI*F1/CS);
			mp_wall[sat] = (CS/F1/2.0/PI)*atan2(mp1[sat],mp2[sat]);
//			mp_wall[sat]=0;
		}
		*/

		if(MP_FLAG==0){
			mpf5[sat]=0;
			mpf5_wall[sat]=0;
		}

		true_range_rov[sat] = (Sat_xpos[sat]-POSx[DIV1][DIV1])*(Sat_xpos[sat]-POSx[DIV1][DIV1])
			+ (Sat_ypos[sat]-POSy[DIV1][DIV1])*(Sat_ypos[sat]-POSy[DIV1][DIV1])
			+ (Sat_zpos[sat]-POSz[DIV1][DIV1])*(Sat_zpos[sat]-POSz[DIV1][DIV1]);
		true_range_rov[sat] = sqrt(true_range_rov[sat]);

		Cp5mov[sat] = true_range_rov[sat]+Integer_m5[sat]*CS/F5+wn+mpf5[sat]
			+mpf5_wall[sat]+mpf5_wall2[sat]+mpf5_wall3[sat];
		
		mp_file[sat] = wn+mpf5[sat]+mpf5_wall[sat];
	}

	//二重位相差の計算
	for(i=0;i<Com_satn;i++){
		prn = Com_sat[i];
		if(Com_sat[i] != Ref_sat){
			DD_carrier5[prn] = (Cp5mov[prn] - Cp5ref[prn])
				-(Cp5mov[Ref_sat] - Cp5ref[Ref_sat]);
		}
	}
	//整数値バイアスの二重差を計算
	for(i=0;i<Com_satn;i++){
		prn = Com_sat[i];
		if(Com_sat[i] != Ref_sat){
			DD_integer5[prn] = (Integer_m5[prn] - Integer_r5[prn])
				-(Integer_m5[Ref_sat] - Integer_r5[Ref_sat]);
		}
	}

	//wide,ultra_wideの二重位相差
	//二重位相差の計算
	for(i=0;i<Com_satn;i++){
		prn = Com_sat[i];
		if(Com_sat[i] != Ref_sat){
			wide_carrier[prn] = DD_carrier1[prn]*F1/CS - DD_carrier2[prn]*F2/CS-DD_code1[prn]*(F1-F2)/CS;
			Ultra_wide_carrier[prn] = DD_carrier2[prn]*F2/CS - DD_carrier5[prn]*F5/CS-DD_code2[prn]*(F2-F5)/CS;
		}
	}
	//正しい整数値バイアスの計算
	for(i=0;i<Com_satn;i++){
		prn = Com_sat[i];
		if(Com_sat[i] != Ref_sat){
			//正しい整数値バイアスは？
			Correct_wide_integer[prn] = DD_integer1[prn]-DD_integer2[prn];
			Correct_uwide_integer[prn] = DD_integer2[prn]-DD_integer5[prn];
		}
	}




//	if((int(GPSTIME)%10)==0){
//		fprintf(mp_carrier,"%f",GPSTIME);
//		for(i=1;i<=31;i++)
//			fprintf(mp_carrier,",%f",wide_carrier[i]);
//		fprintf(mp_carrier,"\n");
//	}


}