////////////////////////////////////////////////////////////////////
//
// 実環境データに基づいた精密測位シミュレーション　
// 主要な処理
//
////////////////////////////////////////////////////////////////////

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <math.h>
#include <errno.h>
#include "timer.h"
//#include <windows.h>

using namespace std;

//ファイルオープン用のポインタの定義
FILE *ele,*azi,*average,*sat_pos,*dop,*pos,*st;
FILE *can,*satv,*dd_code,*cs,*change,*fix_info;
FILE *final,*mp_code,*mp_carrier,*temp,*correlator,*visible;

//外部変数（どの関数でも使用可能、取り扱いは注意）
//解析時間について
const int ITER	        =3600;//計算回数
const double STEP       = 1;//間隔（秒数)
double START_TIME;//計算開始GPS時刻
//解析グリッドについて
double Start_latitude;//解析領域の左下
double Start_longitude;//
double End_latitude;//右上
double End_longitude;//右上
double Height;//高度は全ての地点で一定
const int SATMAX             = 36;//GPSの最大総数

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

//各ＧＰＳ衛星の設定
const int GPS                = 32;//GPS衛星総数
const int NMAX               = 36;//衛星数の最大数

//使用可能衛星の決定に関する設定
double Mask_angle;//GPS衛星に対するマスク角
double RTK_MASK;//GPS衛星に対するマスク角(RTK)

//定数
const double CS       = 299792458.0;//光速
const double F1		  = 1575420000.0;
const double F2       = 1227600000.0;
const double MYU      = 3.986005e+14;//地心重力定数
const double OMEGAE   = 7.2921151467e-5;//地球回転角速度
const double PI       = 3.1415926535898;//円周率
const double F        = -4.442807633e-10;//相対論補正に使用（今回は使用しないか？）

//以下計算に使用する外部変数
int Enable[DIVMAX][DIVMAX],Enable_sat[DIVMAX][DIVMAX][SATMAX];//使用可能衛星
int Actual_satn;//実際のＧＰＳ，ＧＥＯ衛星の運用数を入れる
int Enable_sat_stock[DIVMAX][DIVMAX][SATMAX];//2000/11/04
//予備
int Enable2[DIVMAX][DIVMAX];
int Enable_sat2[DIVMAX][DIVMAX][SATMAX];//使用可能衛星
int Total_sat;//トータル衛星数
int Ref_sat;//最大仰角衛星のこと
int UNUSE_GPS[16];//アルマナック情報のない衛星をストックする
int Com_sat[SATMAX],Com_satn,RTK_satn;
int MSV[12],Pre_sv[4];
int CHANGE_FLAG,NO_SOLUTION_FLAG,Initialization_time;
int QZSS,UN_FIX,NO_RTK,CS_check[SATMAX],CS_check2[SATMAX],CS_FLAG;
int Integer_r1[SATMAX],Integer_m1[SATMAX],DD_integer1[SATMAX];
int Integer_r2[SATMAX],Integer_m2[SATMAX],DD_integer2[SATMAX];
int Integer_r5[SATMAX],Integer_m5[SATMAX],DD_integer5[SATMAX];
int MP_FLAG;
int Correct_wide_integer[SATMAX],Correct_uwide_integer[SATMAX];

//double Average_com,Average_rtk;
double Sat_xpos[SATMAX],Sat_ypos[SATMAX],Sat_zpos[SATMAX];//地球中心座標系
double Sat_xpos2[DIVMAX][DIVMAX][SATMAX],Sat_ypos2[DIVMAX][DIVMAX][SATMAX];
double Sat_zpos2[DIVMAX][DIVMAX][SATMAX];//地平直交座標系
double Sat_ele[DIVMAX][DIVMAX][SATMAX],Sat_azi[DIVMAX][DIVMAX][SATMAX];//衛星の仰角、方位角
double Lat_pos[DIVMAX],Lon_pos[DIVMAX],Hei_pos[DIVMAX][DIVMAX];//ユーザ位置の座標
double Sigma[DIVMAX][DIVMAX][SATMAX];//σ値
double HDOP[DIVMAX][DIVMAX],VDOP[DIVMAX][DIVMAX];//精度計算用にストック
double GDOP[DIVMAX][DIVMAX],PDOP[DIVMAX][DIVMAX],TDOP[DIVMAX][DIVMAX];
double GPSTIME;
double Ground_x[16],Ground_y[16],Ground_z[16];//地上局位置(地球中心)
double Ground[16][3];//地上局位置(緯度、経度、高度)
double POSx[DIVMAX][DIVMAX],POSy[DIVMAX][DIVMAX],POSz[DIVMAX][DIVMAX];//解析位置
double Cp1ref[SATMAX],Cp1mov[SATMAX],Cp2ref[SATMAX],Cp2mov[SATMAX],Cp5ref[SATMAX],Cp5mov[SATMAX];
double Pr1ref[SATMAX],Pr1mov[SATMAX],Pr2ref[SATMAX],Pr2mov[SATMAX],Pr5ref[SATMAX],Pr5mov[SATMAX];
double DD_carrier1[SATMAX],DD_code1[SATMAX],DD_carrier2[SATMAX],DD_code2[SATMAX],DD_carrier5[SATMAX],DD_code5[SATMAX];
double Dx_code,Dy_code,Dz_code,RHDOP,RVDOP;
double Cn[SATMAX],Cn2[SATMAX],RGDOP,Relative_amp;
double MP_rov[SATMAX],MP_rov2[SATMAX],MP_rov3[SATMAX];
double MPG_rov[SATMAX],MPG_ref[SATMAX];
double MP_rov5[SATMAX],MP_rov5_2[SATMAX],MP_rov5_3[SATMAX];
double MPG_ref5[SATMAX],MPG_rov5[SATMAX];
double OBST1_MASK,Ultra_wide_carrier[SATMAX];
double Ant_height_ref,Ant_height_rover;


void read_alma(double data[][13]);
void trans_user_point();
void calc_sat_pos(double data[][13]);
void trans_sat_pos();
void calc_angle();
void calc_dop(int []);
void make_carrier_l1();
void make_carrier_l2();
void make_carrier_l5();
void make_code();
void select_sat();
void ambiguity_l1();
void calc_pos1();
void calc_cn();
void carrier_smoothing();
void carrier_smoothing2();
void make_dd_code();
void ambiguity_wide();
void make_multipath();
void make_multipath_l5_obstacle();
void calc_pos0();
double gauss_gen(double,double,int *);
void correlator_simulation();
void ambiguity_ultra_wide();
double set_visible_sat(int []);
void ground_multipath_l1();
void ground_multipath_l5();
void obstacle_multipath_l1();
void obstacle_multipath_l5();

int main()
{
	int i,k,sv[32]={0};
	int com[13]={0},rtk[13]={0};
	double Average_com=0,Average_rtk=0,stock;
	double local_time=0;


	//計算に要する時間の開始時間
	//start();

	//sapporo  43.04 141.21
	//fukuoka  33.35 130.23
	//seoul    37.32 126.58
	//beijing  39.55 116.26
	//shanghai 31.06 121.22
	//sydney  -33.53 151.10
	//manila   14.36 121.10

	Start_latitude  = 35.666259669;//解析領域の左下
	Start_longitude = 139.79231630;//
	End_latitude    = 35.66;//右上
	End_longitude   = 139.79;//右上

//	Start_latitude  = 14.36;//解析領域の左下
//	Start_longitude = 121.10;//
//	End_latitude    = 14.35;//右上
//	End_longitude   = 121.11;//右上

//	Start_latitude  = 31.06;//解析領域の左下
//	Start_longitude = 121.22;//
//	End_latitude    = 32.0;//右上
//	End_longitude   = 122.2;//右上
	Height          = 60;//高度は全ての地点で一定
	Relative_amp    = 0.25;

	Initialization_time = 300;
	Mask_angle      = 15.0;
	RTK_MASK        = 15.0;
	OBST1_MASK      = 30.0;
	START_TIME      = 0;
	QZSS            = 0;
	UN_FIX          = 0;
	NO_RTK          = 0;
	CS_FLAG         = 1;//キャリアスムージングを行うかどうか？
	MP_FLAG         = 1;
	int wide        = 0;
	Ant_height_ref=40.0;
	Ant_height_rover=30.0;

	//DOPの計算で緯度、経度の位置用
	double alma[NMAX][13]={0};//alma[0][*]は使用しない
	//アルマナックのデータをストック

	//テスト用結果出力ファイルのオープン
	//どの関数に存在するかを示す
	ele        = fopen("result/ele.csv","w");	  //calc_angle
	azi        = fopen("result/azi.csv","w");	  //calc_angle
	mp_carrier = fopen("result/mp_carrier.csv","w");
	mp_code    = fopen("result/mp_code.csv","w");
	dop        = fopen("result/dop.csv","w");//calc_dop
	pos        = fopen("result/pos.csv","w");
	st         = fopen("result/st.csv","w");
	can        = fopen("result/candidate.csv","w");
	satv       = fopen("result/satv.csv","w");
	dd_code    = fopen("result/dd_code.csv","w");
	cs         = fopen("result/cs.csv","w");
	change     = fopen("result/change.csv","w");
	fix_info   = fopen("result/fix.csv","w");
	final      = fopen("result/final.csv","w");
	temp       = fopen("result/temp.csv","w");
	correlator = fopen("result/correlator.csv","w");
	visible    = fopen("data/sat_cn0520.txt","r");

	//0-604800
	GPSTIME = START_TIME;//解析時刻の最初のGPS時刻を設定
	
	cout.precision(15);//表示の桁精度を指定しているだけ

	//各種コリレータのシミュレーション
	//correlator_simulation();

	//アルマナックの読み込み
	read_alma(alma);
	//これ以降はalma[33][13]にアルマナック情報が入っている
			 
	//ユーザ位置を指定し、地球中心座標系に変換
	trans_user_point();

	//整数値バイアスの設定
	for(i=0;i<=GPS+QZSS;i++){
		Integer_r1[i] = i*5;
		Integer_m1[i] = i*2;
		Integer_r2[i] = i*14;
		Integer_m2[i] = i*22;
		Integer_r5[i] = i*6;
		Integer_m5[i] = i*9;
	}

	//ここまでが初期設定
	//ここから繰り返しの計算に入る
	for(i=1;i<=ITER;i++){	
		//GPSTIME=GPSTIME+STEP;//解析時間ステップの決定

//		if((int(GPSTIME)%600)==0)
//			printf("%f\n",GPSTIME);
		
		if(GPSTIME>=604800)
			GPSTIME=GPSTIME-604800;
		
		//衛星位置算出	
		calc_sat_pos(alma);
		
		//各衛星をユーザ位置を平面とする座標に変換
		trans_sat_pos();
		
		//各衛星の方位角、仰角(ユーザ位置からの)の計算
		calc_angle();
		
		//ファイル入力による使用衛星決定
		//if(GPSTIME-0.1-local_time>0)
		//	local_time = set_visible_sat(sv);//読み込んだデータのGPS時刻を返す
//		while(GPSTIME-0.05>local_time)
//			local_time = set_visible_sat(sv);
//		if(local_time-0.1-GPSTIME>0){
//			GPSTIME = GPSTIME+STEP;
//			continue;
//		}

		//DOPの計算(使用衛星の決定も含む)
		//ここで使用衛星を決定しているので、障害物を想定している場合は
		//その障害物に遮られている衛星はここで排除すること
		calc_dop(sv);

		//信号強度を衛星仰角より算出
		calc_cn();

		//基準局と移動局を決めて搬送波位相を作成（二重位相差まで）
		//共通衛星も抜き出している
		//widelane
		//基準局及び移動局のマルチパスについても考慮
		//L1,L2,L5
		make_carrier_l1();
		make_carrier_l2();
		make_carrier_l5();

		//障害物によるコードのマルチパス誤差を求める
		obstacle_multipath_l1();
		obstacle_multipath_l5();

		//地面反射によるコードのマルチパス誤差を求める
		ground_multipath_l1();//地面反射(L1)
		ground_multipath_l5();//地面反射(L5)

		//コード位相を生成（二重位相差まで）
		make_code();

		if(CS_FLAG == 1){
			//キャリアスムージング(Reference)
			carrier_smoothing();

			//キャリアスムージング(Rover)
			carrier_smoothing2();

			//再度コードの二重位相差を作成する必要がある
			make_dd_code();
		}

		if(Com_satn>=4){
			//主衛星と従衛星を決定する
			if((int(GPSTIME)%Initialization_time)==0){
				//移動局の最大仰角衛星を求めておく
				for(k=1;k<=GPS+QZSS;k++){
					if(Sat_ele[DIV1][DIV1][k]>=stock){
						stock = Sat_ele[DIV1][DIV1][k];
						Ref_sat = k;
						//ここで求めた基準衛星は以下の計算において
						//使用衛星の中に含まれない可能性がある。重大なバグ
					}
				}
				select_sat();
			}
			//select_sat();

			//単独測位
			calc_pos0();

			//コードによるDGPS測位(L1 or L5)
			calc_pos1();

			//L2,L5の線形結合によるambiguity決定について
			if(NO_SOLUTION_FLAG == 0){//NO_SOLUTION_FLAGはRTKに利用できる衛星が5個以上のとき0
				ambiguity_ultra_wide();
			}

			//ワイドレーンを利用する場合
/*			if(wide == 1){
				//ワイドレーンの二重位相差を利用
				if(NO_SOLUTION_FLAG == 0)
					ambiguity_wide();
				else
					wide=wide;
					//cout << GPSTIME << "　衛星が4個以下" << endl;
			}
			else{
				//二重位相差を利用して探索空間を求め、残差等を計算
				if(NO_SOLUTION_FLAG == 0)
					ambiguity_l1();
				else
					i=i;
					//cout << GPSTIME << "　衛星が4個以下" << endl;
			}*/
			
		}


//		if((i%100)==0 && wide==0)
//			cout << i << " " << UN_FIX << " " << NO_RTK << endl;
//		if((int(GPSTIME)%300)==0 && RTK_satn<=4){
//			fprintf(final,"%f,0,0,nosol\n",GPSTIME);
//			printf("%f,0,0,nosol\n",GPSTIME);
//		}

		//関数のチェック
//		int gggg[5];
//		double va;
//		gggg[0]=i;
//		va = gauss_gen(0,0.5,gggg);
//		fprintf(temp,"%f,%f\n",GPSTIME,va);

		GPSTIME=GPSTIME+STEP;//解析時間ステップの決定
//		fprintf(temp,"%f %d\n",GPSTIME-172800,Com_satn);

		//平均値算出
		Average_com = Average_com+Com_satn;
		Average_rtk = Average_rtk+RTK_satn;

		if(Com_satn<4)
			com[0]++;
		if(Com_satn==4)
			com[1]++;
		if(Com_satn==5)
			com[2]++;
		if(Com_satn==6)
			com[3]++;
		if(Com_satn==7)
			com[4]++;
		if(Com_satn==8)
			com[5]++;
		if(Com_satn==9)
			com[6]++;
		if(Com_satn==10)
			com[7]++;
		if(Com_satn==11)
			com[8]++;
		if(Com_satn==12)
			com[9]++;
		if(Com_satn==13)
			com[10]++;
		if(Com_satn==14)
			com[11]++;
		if(Com_satn==15)
			com[12]++;

		if(RTK_satn<4)
			rtk[0]++;
		if(RTK_satn==4)
			rtk[1]++;
		if(RTK_satn==5)
			rtk[2]++;
		if(RTK_satn==6)
			rtk[3]++;
		if(RTK_satn==7)
			rtk[4]++;
		if(RTK_satn==8)
			rtk[5]++;
		if(RTK_satn==9)
			rtk[6]++;
		if(RTK_satn==10)
			rtk[7]++;
		if(RTK_satn==11)
			rtk[8]++;
		if(RTK_satn==12)
			rtk[9]++;
		if(RTK_satn==13)
			rtk[10]++;
		if(RTK_satn==14)
			rtk[11]++;
		if(RTK_satn==15)
			rtk[12]++;
	}

	Average_com = Average_com/ITER;
	Average_rtk = Average_rtk/ITER;


	//最終結果の書き出し
	double percentage;
	percentage = float(ITER-UN_FIX-NO_RTK)/(float)ITER;
	fprintf(final,"%d,%d,%d\n",ITER,UN_FIX,NO_RTK);
	fprintf(final,"%f\n",percentage);
	fprintf(final,"%f,%f\n",Average_com,Average_rtk);
	for(i=0;i<=12;i++)
		fprintf(final,"%d,",com[i]);
	fprintf(final,"\n");
	for(i=0;i<=12;i++)
		fprintf(final,"%d,",rtk[i]);
	fprintf(final,"\n");

	fclose(ele);
	fclose(azi);
	fclose(dop);
	fclose(pos);
	fclose(st);
	fclose(can);
	fclose(satv);
	fclose(dd_code);
	fclose(cs);
	fclose(change);
	fclose(fix_info);
	fclose(final);
	fclose(mp_code);
	fclose(mp_carrier);
	fclose(temp);
	fclose(correlator);
	fclose(visible);

	cout << endl;
	cout << i << " " << UN_FIX << " " << NO_RTK << endl;
	cout << Average_com << " " << Average_rtk << endl;
	for(i=0;i<=12;i++)
		cout << com[i] << " ";
	cout << endl;
	for(i=0;i<=12;i++)
		cout << rtk[i] << " ";
	cout << endl;

	cout << "**********  計算終了  **********" << endl;

	//かかった時間の書き出し
	//end();

	return 0;
}