////////////////////////////////////////////////////////////////////
//
//　単独測位用ルーチン
//
///////////////////////////////////////////////////////////////////

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

const double PI       = 3.1415926535898;//円周率

#define DIV1     1//何個(縦横)にグリッドを区切るか
#define DIVMAX  4//グリッドを格納する配列の最大値を設定
const int SATMAX             = 36;//GPSの最大総数
extern FILE *temp;
extern int Com_sat[SATMAX],Com_satn;
extern int Enable[DIVMAX][DIVMAX],Enable_sat[DIVMAX][DIVMAX][SATMAX];//使用可能衛星
extern double Pr1ref[SATMAX],Pr1mov[SATMAX],Pr2ref[SATMAX],Pr2mov[SATMAX];
extern double Sat_xpos[SATMAX],Sat_ypos[SATMAX],Sat_zpos[SATMAX];//地球中心座標系
extern double GPSTIME;
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 End_latitude;//右上
extern double End_longitude;//右上

void sokui(int,double [],int,int,int []);
int minver(double [],double);
void trans_xyz_llh(double,double,double,double[]);

void calc_pos0()
{
	int i;
	int iter=1;
	int rcvn=1;
	int sat,satn=0,sat_n[24];
	double init[4];

	//最小二乗法で測位を行う場合の初期値
	init[0] = -3940000.0;
	init[1] = 3370000.0;
	init[2] = 3690000.0;
	init[3] = 0.0;

//	satn = Com_satn;

	for(i=0;i<Enable[DIV1][DIV1];i++){
		sat = Enable_sat[DIV1][DIV1][i];
		sat_n[satn]=sat;
		satn++;
//		sat_n[i] = sat;
//		if(Sat_azi[DIV1][DIV1][sat]<=315 && Sat_azi[DIV1][DIV1][sat]>=225){
//			i=i;
//		}
//		else{
//			sat_n[satn]=sat;
//			satn++;
//		}
	}

	if(satn>=4){
		//本来は残差が閾値以下になるまでまわす。
		//通常は2，3回で収束している
		while(iter <= 5)
		{
			//最小二乗法による測位計算
			sokui(rcvn,init,iter,satn,sat_n);
			iter = iter+1;
		}

		double a1,a2,a3,llh2[3];//地球中心のx,y,z
		a1=init[0];
		a2=init[1];
		a3=init[2];
		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;

//		fprintf(temp,"%f,%f,%f,%f,%d\n",GPSTIME,lat_diff,
//			lon_diff,llh2[2],satn);
	}
	else{
//		fprintf(temp,"%f,0,0,0,%d\n",GPSTIME,satn);
	}
}


//測位計算用の反復//
//--- iteration ---//
void sokui(int rcvn, double init[4], int iter, int satn, int sat_n[24])
{
	double r2[32],r3[32],a[32],b[32],c[32],a2[16],a3[32],a4[32],
		aa[32],bb[32],cc[32],dd[32],g[4];
	double SV_omomi[36];
	int i,prn;
	static int ii=0;
	ii++;

	for(i=0;i<satn;i++){
		prn = sat_n[i];

		r2[i] = sqrt((Sat_xpos[prn]-init[0])*(Sat_xpos[prn]-init[0])
			+(Sat_ypos[prn]-init[1])*(Sat_ypos[prn]-init[1])
			+(Sat_zpos[prn]-init[2])*(Sat_zpos[prn]-init[2]));

		//シミュレーションなので、まだ電離層や対流圏補正はない
		r3[i] = Pr1mov[prn] - r2[i];
		
		a[i] = -(Sat_xpos[prn]-init[0])/r2[i];
		b[i] = -(Sat_ypos[prn]-init[1])/r2[i];
		c[i] = -(Sat_zpos[prn]-init[2])/r2[i];

		SV_omomi[prn] = 1.0;

		aa[i] = a[i]/SV_omomi[prn];
		bb[i] = b[i]/SV_omomi[prn];
		cc[i] = c[i]/SV_omomi[prn];
		dd[i] = 1/SV_omomi[prn];
	}

	for(i=0; i<16; i++){
		a2[i] = 0.0;
		a4[i] = 0.0;
	}

	for(i=0;i<satn;i++){
		a2[0] += aa[i]*a[i];
		a2[1] += aa[i]*b[i];
		a2[2] += aa[i]*c[i];
		a2[3] += aa[i];
		a2[4] += bb[i]*a[i];
		a2[5] += bb[i]*b[i];
		a2[6] += bb[i]*c[i];
		a2[7] += bb[i];
		a2[8] += cc[i]*a[i];
		a2[9] += cc[i]*b[i];
		a2[10] += cc[i]*c[i];
		a2[11] += cc[i];
		a2[12] += dd[i]*a[i];
		a2[13] += dd[i]*b[i];
		a2[14] += dd[i]*c[i];
		a2[15] += dd[i];

		a4[0] += a[i]*a[i];
		a4[1] += a[i]*b[i];
		a4[2] += a[i]*c[i];
		a4[3] += a[i];
		a4[4] += b[i]*a[i];
		a4[5] += b[i]*b[i];
		a4[6] += b[i]*c[i];
		a4[7] += b[i];
		a4[8] += c[i]*a[i];
		a4[9] += c[i]*b[i];
		a4[10] += c[i]*c[i];
		a4[11] += c[i];
		a4[12] += a[i];
		a4[13] += b[i];
		a4[14] += c[i];
		a4[15] += 1;
	}

	//DOP計算用
	double pdop,gdop;
	double xx_dop[16] = {a4[0],a4[1],a4[2],a4[3],a4[4],a4[5],a4[6],a4[7],
		a4[8],a4[9],a4[10],a4[11],a4[12],a4[13],a4[14],a4[15]};
	double eps = 1.0e-20;
	//逆行列計算//
	minver(xx_dop,eps);
	pdop = sqrt(xx_dop[0]+xx_dop[5]+xx_dop[10]);
	gdop = sqrt(xx_dop[0]+xx_dop[5]+xx_dop[10]);

	double xx[16] = {a2[0],a2[1],a2[2],a2[3],a2[4],a2[5],a2[6],a2[7],
		a2[8],a2[9],a2[10],a2[11],a2[12],a2[13],a2[14],a2[15]};
	for(i=0;i<=15;i++)
		xx[i]=a2[i];
	//double eps = 1.0e-20;
	//逆行列計算//
	minver(xx,eps);

	g[0] = 0.0;
	g[1] = 0.0;
	g[2] = 0.0;
	g[3] = 0.0;

	for(i=0;i<satn;i++){
		a3[i]    = xx[0]*aa[i]+xx[1]*bb[i]+xx[2]*cc[i]+xx[3]*dd[i];
		a3[i+satn]  = xx[4]*aa[i]+xx[5]*bb[i]+xx[6]*cc[i]+xx[7]*dd[i];
		a3[i+satn*2] = xx[8]*aa[i]+xx[9]*bb[i]+xx[10]*cc[i]+xx[11]*dd[i];
		a3[i+satn*3] = xx[12]*aa[i]+xx[13]*bb[i]+xx[14]*cc[i]+xx[15]*dd[i];
		g[0] += a3[i]*r3[i];
		g[1] += a3[i+satn]*r3[i];
		g[2] += a3[i+satn*2]*r3[i];
		g[3] += a3[i+satn*3]*r3[i];
	}
	for (i=0; i<3; i++){
		init[i] += g[i];
	}
	init[3] = g[3];
}