////////////////////////////////////////////////////////
//  コードによる基線解析
////////////////////////////////////////////////////////

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

const double CS       = 299792458.0;//光速
const double F1		  = 1575420000.0;
const double F5       = 1176450000.0;//L5
const double PI       = 3.1415926535898;//円周率
const int SATMAX             = 36;//GPSの最大総数
const int GPS         = 32;//GPSの最大総数
#define DIV1     1//何個(縦横)にグリッドを区切るか
#define DIVMAX  4//グリッドを格納する配列の最大値を設定

extern FILE *pos;

extern int MSV[16];
extern int Ref_sat,RTK_satn,Com_satn,Com_sat[SATMAX];
extern int CHANGE_FLAG,QZSS;
extern double Sat_xpos[SATMAX],Sat_ypos[SATMAX],Sat_zpos[SATMAX];//地球中心座標系
extern double POSx[DIVMAX][DIVMAX],POSy[DIVMAX][DIVMAX],POSz[DIVMAX][DIVMAX];//解析位置
extern double Sat_ele[DIVMAX][DIVMAX][SATMAX],Sat_azi[DIVMAX][DIVMAX][SATMAX];//衛星の仰角、方位角
extern double DD_carrier1[SATMAX],DD_code1[SATMAX];
extern double DD_carrier5[SATMAX],DD_code5[SATMAX];
extern double GPSTIME;
extern double Dx_code,Dy_code,Dz_code;
extern double End_latitude;//右上
extern double End_longitude;//右上
extern double RHDOP;

int minver3(double [],double ,int);
void trans_xyz_llh(double,double,double,double[]);
void choose_primary(int,int []);

void calc_pos1()
{
	int i,j;
	int prn;
	int sv[16];
	int sat=3;
	int iter;
	int L1=1,L5=0;
	double F;
	double ele[16]={0};
	double g[128]={0};
	double gt[128]={0},sg[128]={0},g3[9]={0},g2[128]={0};
	double lrl[16],lrln[16];
	double lrkn[16][16],lrk[16][16];
	double dx=0,dy=0,dz=0;
	double distance,real_distance;
	double r0[16]={0};
	double c[144]={0},gtc[128]={0};
	double sig_carrier = 0.005;
	double ref_diff[16]={0},rov_diff[16]={0};

	//衛星の位置をずらすためのもの？
	static double time=0;
	time=time+0.004;

	if(L1==1)
		F=F1;
	else if(L5==1)
		F=F5;
	else
		F=F1;

//	double stock=0.1;
//	//基準衛星をここで求める。
//	for(k=1;k<=GPS+QZSS;k++){
//		if(Sat_ele[DIV1][DIV1][k]>=stock){
//			stock = Sat_ele[DIV1][DIV1][k];
//			Ref_sat = k;
//			//ここで求めた基準衛星は以下の計算において
//			//使用衛星の中に含まれない可能性がある。重大なバグ
//		}
//	}

	for(i=0;i<=15;i++){
		sv[i] = Com_sat[i];
		if(Com_sat[i] == Ref_sat){
			sv[i] = sv[0];
			sv[0] = Ref_sat;
		}
	}

	//衛星の位置をわざとずらす
//	for(i=1;i<=32;i++){
//		Sat_xpos[i] = Sat_xpos[i] + 5.0*sin(0.5+time+i*0.05);
//		Sat_ypos[i] = Sat_ypos[i] + 5.0*sin(1.0+time+i*0.05);
//		Sat_zpos[i] = Sat_zpos[i] + 5.0*sin(1.5+time+i*0.05);
//	}

	//衛星の方向の単位ベクトルを算出
	//基準局と基準衛星の単位ベクトル
	lrl[0] = Sat_xpos[Ref_sat]-POSx[0][0];
	lrl[1] = Sat_ypos[Ref_sat]-POSy[0][0];
	lrl[2] = Sat_zpos[Ref_sat]-POSz[0][0];
	lrln[0] = lrl[0]/sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);
	lrln[1] = lrl[1]/sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);
	lrln[2] = lrl[2]/sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);

	//基準局と選択衛星の単位ベクトル
	//衛星数分まわす（主衛星＋従衛星分）
	//最大仰角の衛星はすでに選択されている

	for(i=0;i<Com_satn-1;i++){
		//ここで0,1,2には主衛星が入らなければならないことに注意
		//従衛星の番号は3以降に順番に代入
		switch(i){
			case 0:prn=sv[1];break;
			case 1:prn=sv[2];break;
			case 2:prn=sv[3];break;
			case 3:prn=sv[4];break;
			case 4:prn=sv[5];break;
			case 5:prn=sv[6];break;
			case 6:prn=sv[7];break;
			case 7:prn=sv[8];break;
			case 8:prn=sv[9];break;
			case 9:prn=sv[10];break;
			case 10:prn=sv[11];break;
			case 11:prn=sv[12];break;
			case 12:prn=sv[13];break;
			case 13:prn=sv[14];break;
			case 14:prn=sv[15];break;
			default:i=i;break;
		}
		lrk[0][i] = Sat_xpos[prn]-POSx[0][0];
		lrk[1][i] = Sat_ypos[prn]-POSy[0][0];
		lrk[2][i] = Sat_zpos[prn]-POSz[0][0];
		lrkn[0][i] = lrk[0][i]/sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i]);
		lrkn[1][i] = lrk[1][i]/sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i]);
		lrkn[2][i] = lrk[2][i]/sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i]);
	}

	//基線ベクトルではなく移動局側の測位として解析した場合
	for(iter=1;iter<=4;iter++){
		for(i=0;i<=2;i++){
			for(j=0;j<Com_satn-1;j++){
				g[i+j*3]=-1.0*(lrkn[i][j]-lrln[i])*F/CS;
			}
		}
		//G行列を残す
		for(i=0;i<3*(Com_satn-1);i++)
			sg[i]=g[i];

		//Gの転置行列の指定
		for(i=0;i<(Com_satn-1);i++){
			gt[i]=g[i*3];
			gt[i+(Com_satn-1)]=g[i*3+1];
			gt[i+(Com_satn-1)*2]=g[i*3+2];
		}
		for(i=0;i<=8;i++)
			g3[i]=0;
		//上で求めた行列G(t)とGを掛ける
		for(i=0;i<Com_satn-1;i++){
			g3[0]=g3[0]+gt[i]*g[i*3];
			g3[1]=g3[1]+gt[i]*g[i*3+1];
			g3[2]=g3[2]+gt[i]*g[i*3+2];
			g3[3]=g3[3]+gt[i+Com_satn-1]*g[i*3];
			g3[4]=g3[4]+gt[i+Com_satn-1]*g[i*3+1];
			g3[5]=g3[5]+gt[i+Com_satn-1]*g[i*3+2];
			g3[6]=g3[6]+gt[i+(Com_satn-1)*2]*g[i*3];
			g3[7]=g3[7]+gt[i+(Com_satn-1)*2]*g[i*3+1];
			g3[8]=g3[8]+gt[i+(Com_satn-1)*2]*g[i*3+2];
		}
		double eps = 1.0e-25;
		sat=3;
		minver3(g3,eps,sat);

		//全ての衛星を利用して測位計算
		sat=Com_satn-1;

		for(i=0;i<sat*3;i++)
			g2[i]=0;

		//逆行列に行列G(t)を掛ける
		//ここで3衛星にするか全ての衛星を使用するか注意
		for(i=0;i<sat;i++){
			for(j=0;j<3;j++){
				g2[i]      =g2[i]+g3[j]*gt[i+sat*j];
				g2[sat+i]  =g2[sat+i]+g3[j+3]*gt[i+sat*j];
				g2[sat*2+i]=g2[sat*2+i]+g3[j+6]*gt[i+sat*j];
			}
		}
		//左辺に移行した基準局のほうの距離分を計算しておく
		if(iter==1){
			for(i=0;i<Com_satn-1;i++){
				ref_diff[i] = sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i])
					- sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);
				ref_diff[i] = ref_diff[i]*F/CS;
			}
		}
		for(i=0;i<Com_satn-1;i++){
			rov_diff[i] = sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i])
				- sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);
			rov_diff[i] = rov_diff[i]*F/CS;
		}

		if(L1==1){//L1-DGPS
			for(i=0;i<sat;i++){
				prn = sv[i+1];
				dx = dx+g2[i]*(DD_code1[sv[i+1]]*F/CS+ref_diff[i]-rov_diff[i]);
				dy = dy+g2[i+sat]*(DD_code1[sv[i+1]]*F/CS+ref_diff[i]-rov_diff[i]);
				dz = dz+g2[i+sat*2]*(DD_code1[sv[i+1]]*F/CS+ref_diff[i]-rov_diff[i]);
			}
		}
		if(L5==1){//L5-DGPS
			for(i=0;i<sat;i++){
				prn = sv[i+1];
				dx = dx+g2[i]*(DD_code5[sv[i+1]]*F/CS+ref_diff[i]-rov_diff[i]);
				dy = dy+g2[i+sat]*(DD_code5[sv[i+1]]*F/CS+ref_diff[i]-rov_diff[i]);
				dz = dz+g2[i+sat*2]*(DD_code5[sv[i+1]]*F/CS+ref_diff[i]-rov_diff[i]);
			}
		}

		distance = sqrt(dx*dx+dy*dy+dz*dz);

		//実際の基線長を計算
		real_distance = sqrt((POSx[0][0]-POSx[1][1])*(POSx[0][0]-POSx[1][1])+
			(POSy[0][0]-POSy[1][1])*(POSy[0][0]-POSy[1][1])+
			(POSz[0][0]-POSz[1][1])*(POSz[0][0]-POSz[1][1]));

		//衛星の方向の単位ベクトルを算出
		//基準局と基準衛星の単位ベクトル
		lrl[0] = Sat_xpos[Ref_sat]-(POSx[0][0]+dx);
		lrl[1] = Sat_ypos[Ref_sat]-(POSy[0][0]+dy);
		lrl[2] = Sat_zpos[Ref_sat]-(POSz[0][0]+dz);
		lrln[0] = lrl[0]/sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);
		lrln[1] = lrl[1]/sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);
		lrln[2] = lrl[2]/sqrt(lrl[0]*lrl[0]+lrl[1]*lrl[1]+lrl[2]*lrl[2]);

		//基準局と選択衛星の単位ベクトル
		//衛星数分まわす（主衛星＋従衛星分）
		//最大仰角の衛星はすでに選択されている

		for(i=0;i<Com_satn-1;i++){
			//ここで0,1,2には主衛星が入らなければならないことに注意
			//従衛星の番号は3以降に順番に代入
			switch(i){
				case 0:prn=sv[1];break;
				case 1:prn=sv[2];break;
				case 2:prn=sv[3];break;
				case 3:prn=sv[4];break;
				case 4:prn=sv[5];break;
				case 5:prn=sv[6];break;
				case 6:prn=sv[7];break;
				case 7:prn=sv[8];break;
				case 8:prn=sv[9];break;
				case 9:prn=sv[10];break;
				case 10:prn=sv[11];break;
				case 11:prn=sv[12];break;
				case 12:prn=sv[13];break;
				case 13:prn=sv[14];break;
				case 14:prn=sv[15];break;
				default:i=i;break;
			}
			lrk[0][i] = Sat_xpos[prn]-(POSx[0][0]+dx);
			lrk[1][i] = Sat_ypos[prn]-(POSy[0][0]+dy);
			lrk[2][i] = Sat_zpos[prn]-(POSz[0][0]+dz);
			lrkn[0][i] = lrk[0][i]/sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i]);
			lrkn[1][i] = lrk[1][i]/sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i]);
			lrkn[2][i] = lrk[2][i]/sqrt(lrk[0][i]*lrk[0][i]+lrk[1][i]*lrk[1][i]+lrk[2][i]*lrk[2][i]);
		}

	}//iterの終わり

	//外部変数に格納
	Dx_code = dx;
	Dy_code = dy;
	Dz_code = dz;

	//正確な基線ベクトル
	double codx,cody,codz;
	codx=POSx[1][1]-POSx[0][0];
	cody=POSy[1][1]-POSy[0][0];
	codz=POSz[1][1]-POSz[0][0];

	double a1,a2,a3,llh2[3];//地球中心のx,y,z
	a1=dx+POSx[0][0];
	a2=dy+POSy[0][0];
	a3=dz+POSz[0][0];
	trans_xyz_llh(a1,a2,a3, llh2);

	double lat_diff,lon_diff;
	lat_diff = (End_latitude-llh2[0])*111317.1;
	lon_diff = (End_longitude-llh2[1])*cos(llh2[0]*PI/180.0)*111317.1;

	//RHDOPが必要
	choose_primary(2,sv);

	//測位結果の書き出し
	if((int(GPSTIME)%3)==0 && Com_satn>=4)
		fprintf(pos,"%f,%f,%f,%f,%f,%f,%d,%d\n",GPSTIME,lat_diff,lon_diff,
		llh2[2],sqrt(lat_diff*lat_diff+lon_diff*lon_diff),RHDOP,Com_satn,RTK_satn);
}