////////////////////////////////////////////////////////////////	
//	DOPの計算(可視衛星で行う:最低仰角以上)
//	使用可能衛星の決定(ユーザからの最低仰角を満足するか) 
//
//	呼び込み関数
//	minver()  
//
////////////////////////////////////////////////////////////////

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

using namespace std;

extern FILE *dop;

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

const int GPS                = 32;//GPS衛星総数
const int SATMAX             = 36;//GPSの最大総数
//const double Mask_angle      = 20.0;//GPS衛星に対するマスク角
const double PI       = 3.1415926535898;//円周率

extern int Enable[DIVMAX][DIVMAX],Enable_sat[DIVMAX][DIVMAX][SATMAX];//使用可能衛星
extern int Enable2[DIVMAX][DIVMAX];
extern int UNUSE_GPS[16];//アルマナック情報のない衛星をストックする
extern int QZSS,Ref_sat;
extern double Sat_ele[DIVMAX][DIVMAX][SATMAX],Sat_azi[DIVMAX][DIVMAX][SATMAX];//衛星の仰角、方位角
extern double HDOP[DIVMAX][DIVMAX],VDOP[DIVMAX][DIVMAX];//精度計算用にストック
extern double GDOP[DIVMAX][DIVMAX],PDOP[DIVMAX][DIVMAX],TDOP[DIVMAX][DIVMAX];
extern double GPSTIME,Mask_angle;
extern double OBST1_MASK;

int minver(double [],double);

void calc_dop(int sv[32])
{
	int i,j,k,l,m;
	int sat;
	double ele[DIVMAX][DIVMAX],azi[DIVMAX][DIVMAX];
	double g[64][4]={0},gt[4][64]={0},a[16]={0};
	double eps;
	double gdop[DIVMAX][DIVMAX],pdop[DIVMAX][DIVMAX],tdop[DIVMAX][DIVMAX],
		hdop[DIVMAX][DIVMAX],vdop[DIVMAX][DIVMAX];
	
		
	cout.precision(5);
		
	for(i=0;i<=DIV1;i++){
		for(j=0;j<=DIV1;j++){
		
			sat  = 0;//GPS
			//使用衛星数の初期化は必要
			Enable[i][j]=0;

			for(k=1;k<=GPS+QZSS;k++){
				if(k==UNUSE_GPS[0] || k==UNUSE_GPS[1] || k==UNUSE_GPS[2] || 
					k==UNUSE_GPS[3] || k==UNUSE_GPS[4])
					continue;

				//観測行列Gの計算
				//使用衛星の決定
				//各衛星は最低仰角以上であること
/*				if(Sat_ele[i][j][k]>=Mask_angle){
					//ラジアンへの変換					
					ele[i][j]=Sat_ele[i][j][k]*PI/180.0;
					azi[i][j]=Sat_azi[i][j][k]*PI/180.0;

					g[sat][0]=-cos(ele[i][j])*sin(azi[i][j]);
					g[sat][1]=-cos(ele[i][j])*cos(azi[i][j]);
					g[sat][2]=-sin(ele[i][j]);
					g[sat][3]=1.0;
					
					//使用衛星をストック
					//if(k==3 || k==15 || k==17 || k==31){//使用衛星を限定するときに有効にする
						Enable_sat[i][j][sat]=k;
						sat++;
					//}
				}
*/

				//ファイル読み込みによる使用衛星を決定する
/*				int ok=0;
				if(i==DIV1 && j==DIV1){
					for(l=1;l<=sv[0];l++){
						if(sv[l]==k)
							ok=1;
						else
							;
					}
					if(ok==1)
						ok=0;
					else
						Sat_ele[DIV1][DIV1][k]=0;
				}
*/

				//ここで障害物による衛星可視条件を入れる 2003/8/19
				if(Sat_ele[i][j][k]>=Mask_angle){

					//下の条件を満たす衛星は障害物に遮られている
					if(Sat_ele[i][j][k]<=OBST1_MASK && Sat_azi[i][j][k]>=225 && Sat_azi[i][j][k]<=315){
						i=i;
					}
					else if(Sat_ele[i][j][k]<=OBST1_MASK && Sat_azi[i][j][k]>=130 && Sat_azi[i][j][k]<=160){
						i=i;
					}
					else if(Sat_ele[i][j][k]<=OBST1_MASK && Sat_azi[i][j][k]>=20 && Sat_azi[i][j][k]<=50){
						i=i;
					}
					else{
						//ラジアンへの変換					
						ele[i][j]=Sat_ele[i][j][k]*PI/180.0;
						azi[i][j]=Sat_azi[i][j][k]*PI/180.0;

						g[sat][0]=-cos(ele[i][j])*sin(azi[i][j]);
						g[sat][1]=-cos(ele[i][j])*cos(azi[i][j]);
						g[sat][2]=-sin(ele[i][j]);
						g[sat][3]=1.0;
						
						//使用衛星をストック
						//if(k==3 || k==15 || k==17 || k==31){//使用衛星を限定するときに有効にする
						Enable_sat[i][j][sat]=k;
						sat++;
						//}
					}
				}

				Enable[i][j]=sat;//使用衛星総数をストック
				Enable2[i][j]=sat;//予備		
			}//for
			
			//観測行列Gの転置の計算
			for(l=0;l<=sat-1;l++){
				gt[0][l]=g[l][0];
				gt[1][l]=g[l][1];
				gt[2][l]=g[l][2];
				gt[3][l]=g[l][3];
			}
			//Gの転置とGを掛ける(4×4行列を配列で表す)
			for(l=0;l<=sat-1;l++){
				a[0]  += gt[0][l]*g[l][0];
				a[1]  += gt[0][l]*g[l][1];
				a[2]  += gt[0][l]*g[l][2];
				a[3]  += gt[0][l];
				a[4]  += gt[1][l]*g[l][0];
				a[5]  += gt[1][l]*g[l][1];
				a[6]  += gt[1][l]*g[l][2];
				a[7]  += gt[1][l];					
				a[8]  += gt[2][l]*g[l][0];
				a[9]  += gt[2][l]*g[l][1];
				a[10] += gt[2][l]*g[l][2];
				a[11] += gt[2][l];
				a[12] += g[l][0];
				a[13] += g[l][1];
				a[14] += g[l][2];
				a[15] += 1.0;
			}
			double xx[] = {a[0], a[1], a[2], a[3],
				       a[4], a[5], a[6], a[7],
				       a[8], a[9], a[10],a[11],
				       a[12],a[13],a[14],a[15]};
			eps = 1.0e-25;

			//逆行列を算出する
			minver(xx,eps);
				
			gdop[i][j]=sqrt(xx[0]+xx[5]+xx[10]+xx[15]);
			pdop[i][j]=sqrt(xx[0]+xx[5]+xx[10]);
			tdop[i][j]=sqrt(xx[15]);
			hdop[i][j]=sqrt(xx[0]+xx[5]);
			vdop[i][j]=sqrt(xx[10]);
			
			HDOP[i][j]=hdop[i][j];
			VDOP[i][j]=vdop[i][j];		
			
			for(m=0;m<=15;m++){
				a[m]=0.0;	
				xx[m]=0.0;
			}
			for(m=0;m<=DIVMAX-1;m++){
				g[m][0]=0;
				g[m][1]=0;
				g[m][2]=0;
				g[m][3]=0;
				gt[0][m]=0;
				gt[1][m]=0;
				gt[2][m]=0;
				gt[3][m]=0;
			}
			while(sat <= SATMAX-1){//使用可能衛星が35個無い場合はクリアする
				Enable_sat[i][j][sat]=0;
				sat++;
			}
		}//for j
	}//for i

	//DOP値と使用衛星数の書き出し
	if((int(GPSTIME)%10)==0){
		fprintf(dop,"%f,%d,%f,%f,%f,%f,%f",GPSTIME,Enable[0][0],gdop[0][0],pdop[0][0],tdop[0][0],hdop[0][0],vdop[0][0]);
		for(i=1;i<=Enable[0][0];i++)
			fprintf(dop,",%d",Enable_sat[0][0][i-1]);
		fprintf(dop,"\n");
	}

	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;
			//ここで求めた基準衛星は以下の計算において
			//使用衛星の中に含まれない可能性がある。重大なバグ
		}
	}
}