////////////////////////////////////////////////////////////////
//
//	人工的にコードマルチパス誤差を生成
//　（地面以外の障害物によるマルチパスについて）
//
////////////////////////////////////////////////////////////////

#include <stdio.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 *dd_code,*temp;

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 MP_rov5[SATMAX],MP_rov5_2[SATMAX],MP_rov5_3[SATMAX];
extern double OBST1_MASK;

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

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

	double dl,dm[SATMAX];
	double amp,amph,ampv,permt;
	double repermt=3;
	double cond=0.00002;
	double lm=CS/F5;
	double ph;
	double dis = 15.0;//アンテナと壁との距離
	double dis2=45.0,dis3=45.0;
	double corr=1.0;
	double chip=CS/10230000;
	double td=corr*chip/2.0;
	double defuse = 2.0;//壁による反射係数を少し減少させる

	double a[SATMAX]={0},d[SATMAX]={0};
	double mp_rov2[SATMAX]={0};
	static int kaisu=0;

	//反射係数の計算
	permt = repermt-60.0*CS/F5*cond;

	kaisu++;

	//移動局のコード位相の生成
	for(i=0;i<Enable[DIV1][DIV1];i++){
		sat = Enable_sat[DIV1][DIV1][i];
		MP_rov5[sat]=0;
		MP_rov5_2[sat]=0;
		MP_rov5_3[sat]=0;

		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-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-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-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-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			ampv = fabs(ampv);

			amp = defuse*(amph+ampv)/2.0;
			a[sat]=amp;
			d[sat]=dl;

//////////////////////////////

			//遅延距離による場合分けが必要
			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_rov5[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_rov5[sat] = dm[sat];
				}
				else{//逆相側の場合
					ph = 2*PI*ph;
					dm[sat] = amp*(chip+td-dl)*cos(ph);
					dm[sat] = dm[sat]/(2-amp*cos(ph));
					MP_rov5[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));
				MP_rov5[sat] = dm[sat];
			}
			else{
				dm[sat]=0;
				MP_rov5[sat] = dm[sat];
			}
////////////////////////////////////////////////////////////
		}//if

		//2個目
		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-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-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-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-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			ampv = fabs(ampv);

			amp = defuse*(amph+ampv)/2.0;
			a[sat]=amp;
			d[sat]=dl;

//////////////////////////////

			//遅延距離による場合分けが必要
			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_rov5_2[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_rov5_2[sat] = dm[sat];
				}
				else{//逆相側の場合
					ph = 2*PI*ph;
					dm[sat] = amp*(chip+td-dl)*cos(ph);
					dm[sat] = dm[sat]/(2-amp*cos(ph));
					MP_rov5_2[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));
				MP_rov5_2[sat] = dm[sat];
			}
			else{
				dm[sat]=0;
				MP_rov5_2[sat] = dm[sat];
			}
////////////////////////////////////////////////////////////
		}//if

		//3個目
		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-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-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-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-sin(PI*Sat_ele[DIV1][DIV1][sat]/180)*sin(PI*Sat_ele[DIV1][DIV1][sat]/180)));
			ampv = fabs(ampv);

			amp = defuse*(amph+ampv)/2.0;
			a[sat]=amp;
			d[sat]=dl;

//////////////////////////////

			//遅延距離による場合分けが必要
			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_rov5_3[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_rov5_3[sat] = dm[sat];
				}
				else{//逆相側の場合
					ph = 2*PI*ph;
					dm[sat] = amp*(chip+td-dl)*cos(ph);
					dm[sat] = dm[sat]/(2-amp*cos(ph));
					MP_rov5_3[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));
				MP_rov5_3[sat] = dm[sat];
			}
			else{
				dm[sat]=0;
				MP_rov5_3[sat] = dm[sat];
			}
////////////////////////////////////////////////////////////
		}//if


	}//for
//	fprintf(temp,"%f,%f,%f,%f,%f,%f,%f\n",GPSTIME,dm[3],a[3],d[3],dm[31],a[31],d[31]);
}