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

#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_l1()
{
	int i,j,k=0;
	int sat,prn;
	int arr[1];
	double wn,cn;
	double pll_sigma[SATMAX];
	double mp1[SATMAX],mp2[SATMAX],mp[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 mp_wall[SATMAX]={0},mpf2_wall[SATMAX]={0},mpf5_wall[SATMAX]={0};
	double mp_wall2[SATMAX]={0},mp_wall3[SATMAX]={0};
	double dis=15.0;
	double dis2=45,dis3=45;
	double defuse = 1.0;//壁による反射係数を少し減少させる
	double defuse1 = 5.0;//アンテナによる減衰効果値

	static int kaisu=0;

	static int incr_carrier1[36]={1,1,-10,10,-20,20,-30,30,-40,40,-50,50,-60,60,-70,70,-80,80,-90,90,-100,100,-110,110,-120,120,-130,130,-140,140,-150,150,-160,160,-170,170};
	static int incr_carrier2[36]={1,10,-100,100,-200,200,-300,300,-400,400,-500,500,-600,600,-700,700,-800,800,-900,900,-1000,1000,-1100,1100,-1200,1200,-1300,1300,-1400,1400,-1500,1500,-1600,1600,-1700,1700};

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

	kaisu++;

	//まず基準局と移動局における共通衛星を抜き出す
	for(i=0;i<Enable[0][0];i++){
		for(j=0;j<Enable[DIV1][DIV1];j++){
			if(Enable_sat[0][0][i] == Enable_sat[DIV1][DIV1][j])
				Com_sat[k++] = Enable_sat[0][0][i];
		}
	}
	Com_satn = k;

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

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

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

		//粗面の鏡面反射係数
		double g,s;
		s=0.02;//sは第1フレネルゾーン内の局所的な粗面の高さの標準偏差
		g=4*PI*s/(CS/F1)*sin(PI*Sat_ele[0][0][sat]/180);
		//最終的な逓減係数
		g=exp(-0.5*g*g);

		//multipath effects on L1

		//基準局は下面からのマルチパスなので、全ての可視衛星について計算		
		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;

		//アンテナの下面から入射されるので、アンテナによる減衰効果があり
		//60-90 30dB以上減衰 0.0316
		//30 20dB減衰 0.1
		//0 13dB減衰 0.224

		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.03;
		if(Sat_ele[0][0][sat]>=30 && Sat_ele[0][0][sat]<=60)
			keisu=-0.07/30*Sat_ele[0][0][sat]+0.17;
		if(Sat_ele[0][0][sat]>=0 && Sat_ele[0][0][sat]<=30)
			keisu=-0.124/30*Sat_ele[0][0][sat]+0.224;

		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;
//		delay[sat] = delay[sat]+0.001;

		amp[sat] = ampl;
		delay[sat] = dl;
		mp1[sat] = amp[sat]*sin(delay[sat]*2.0*PI*F1/CS);
		mp2[sat] = 1.0+amp[sat]*cos(delay[sat]*2.0*PI*F1/CS);
		mp[sat] = (CS/F1/2.0/PI)*atan2(mp1[sat],mp2[sat]);
		if(MP_FLAG==0)
			mp[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]);
		Cp1ref[sat] = true_range_ref[sat]+Integer_r1[sat]*CS/F1+wn+mp[sat];
	}



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

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

		arr[0] = incr_carrier2[sat];
		cn = pow(10.0,Cn[sat]/10.0);
		pll_sigma[sat] = CS/F1/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 L1
		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);

		//アンテナの下面から入射されるので、アンテナによる減衰効果があり
		//60-90 30dB以上減衰 0.0316
		//30 20dB減衰 0.1
		//0 13dB減衰 0.224

		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.03;
		if(Sat_ele[0][0][sat]>=30 && Sat_ele[0][0][sat]<=60)
			keisu=-0.07/30*Sat_ele[0][0][sat]+0.17;
		if(Sat_ele[0][0][sat]>=0 && Sat_ele[0][0][sat]<=30)
			keisu=-0.124/30*Sat_ele[0][0][sat]+0.224;

		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[DIV1][DIV1][sat]+0.254;
//		delay_rov[sat] = delay_rov[sat]+0.001;

		amp[sat] = 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[sat] = (CS/F1/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;
			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(Sat_azi[DIV1][DIV1][sat]>=200 && Sat_azi[DIV1][DIV1][sat]<=230 && Sat_ele[DIV1][DIV1][sat]<=OBST1_MASK){
			dl = sqrt(dis2*2*dis2*2-(dis2*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180))*(dis2*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_wall2[sat] = (CS/F1/2.0/PI)*atan2(mp1[sat],mp2[sat]);
//			mp_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(dis3*2*dis3*2-(dis3*2*sin(PI*fabs(90-Sat_azi[DIV1][DIV1][sat])/180))*(dis3*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_wall3[sat] = (CS/F1/2.0/PI)*atan2(mp1[sat],mp2[sat]);
//			mp_wall[sat]=0;
		}


		if(MP_FLAG==0){
			mp[sat]=0;
			mp_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]);
		Cp1mov[sat] = true_range_rov[sat]+Integer_m1[sat]*CS/F1+wn+mp[sat]
			+mp_wall[sat]+mp_wall2[sat]+mp_wall3[sat];
	
		mp_file[sat] = wn+mp[sat]+mp_wall[sat];
	}


	//二重位相差の計算
	for(i=0;i<Com_satn;i++){
		prn = Com_sat[i];
		if(Com_sat[i] != Ref_sat){
			DD_carrier1[prn] = (Cp1mov[prn] - Cp1ref[prn])
				-(Cp1mov[Ref_sat] - Cp1ref[Ref_sat]);
		}
	}
	//整数値バイアスの二重差を計算
	for(i=0;i<Com_satn;i++){
		prn = Com_sat[i];
		if(Com_sat[i] != Ref_sat){
			DD_integer1[prn] = (Integer_m1[prn] - Integer_r1[prn])
				-(Integer_m1[Ref_sat] - Integer_r1[Ref_sat]);
		}
	}

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