////////////////////////////////////////////////////////////
//	各衛星位置の計算
//
//	呼び込み関数 
//	gps_newton()
//
////////////////////////////////////////////////////////////

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

const double MYU      = 3.986005e+14;//地心重力定数
const double OMEGAE   = 7.2921151467e-5;//地球回転角速度
const double PI       = 3.1415926535898;//円周率

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

extern FILE *sat_pos;

extern int UNUSE_GPS[16];//アルマナック情報のない衛星をストックする
extern int QZSS;
extern double GPSTIME;
extern double Sat_xpos[SATMAX],Sat_ypos[SATMAX],Sat_zpos[SATMAX];//地球中心座標系


double gps_newton(double,double,int);

void calc_sat_pos(double alma[NMAX][13])	
{
	int i;
	double time_toc[NMAX];
	double a[NMAX],n[NMAX],mk[NMAX],e[NMAX],ek[NMAX],sinvk[NMAX],cosvk[NMAX];
	double vk[NMAX],dk[NMAX],rk[NMAX],ik[NMAX],omegak[NMAX];
	double xpos2[NMAX],ypos2[NMAX],xpos[NMAX],ypos[NMAX],zpos[NMAX];

	for(i=1;i<=GPS+QZSS;i++){//31衛星分回す

		//アルマナック情報のない衛星を前もって排除
		if(i==UNUSE_GPS[0] || i==UNUSE_GPS[1] || i==UNUSE_GPS[2] || 
			i==UNUSE_GPS[3] || i==UNUSE_GPS[4] || i==UNUSE_GPS[5] || i==UNUSE_GPS[6])
			continue;
			
//位置を計算するGPS時刻と放送歴基準時刻の差より(time_toc)を決定
		time_toc[i] = GPSTIME-alma[i][3];
		
//主要処理部(衛星位置計算)
//ケプラーの６要素(衛星はある楕円軌道上を楕円運動しているものとする)
// alma[i][9] mean anomaly
// alma[i][2] eccentricity
// alma[i][6] semi major axis
// alma[i][7] right ascention of ascending node
// alma[i][4] orbital inclination
// alma[i][8] argument of perigee
//
// alma[i][5] rate of right ascension
/////////////////////////////////////////////////////////////////////

		a[i]     = alma[i][6]*alma[i][6];//衛星の軌道長半径(長い)
		n[i]     = sqrt(MYU/(a[i]*a[i]*a[i]));//平均運動
		mk[i]    = alma[i][9]+n[i]*time_toc[i];//平均近点角
		e[i]     = alma[i][2];//離心率

		ek[i]    = gps_newton(mk[i],e[i],i);//離心近点離角(ニュートン法)

		sinvk[i] = sqrt(1.0-e[i]*e[i])*sin(ek[i])/(1.0-e[i]*cos(ek[i]));
		cosvk[i] = (cos(ek[i])-e[i])/(1.0-e[i]*cos(ek[i]));		
		if(cosvk[i] == 0 || e[i]*cos(vk[i]) == -1.0){
			if(i==32)
				i=i;
			else{
				printf("### calc_sat_pos error ###\n");
				exit(1);
			}
		}		
		vk[i]    = atan2(sinvk[i],cosvk[i]);//真近点離角算出
		
		if(ek[i]>=0)
		    ek[i]= acos((e[i]+cos(vk[i]))/(1.0+e[i]*cos(vk[i])));
		else
		    ek[i]= -acos((e[i]+cos(vk[i]))/(1.0+e[i]*cos(vk[i])));
	       			
		dk[i]    = vk[i] + alma[i][8];
		rk[i]    = a[i]*(1.0-e[i]*cos(ek[i]));
		ik[i]    = alma[i][4];
		omegak[i]= alma[i][7] + (alma[i][5] - OMEGAE)*time_toc[i]
				-OMEGAE*alma[i][3];
		xpos2[i] = rk[i]*cos(dk[i]);
		ypos2[i] = rk[i]*sin(dk[i]);
		
		xpos[i]  = xpos2[i]*cos(omegak[i]) - ypos2[i]*cos(ik[i])*sin(omegak[i]);
		ypos[i]  = xpos2[i]*sin(omegak[i]) + ypos2[i]*cos(ik[i])*cos(omegak[i]);
		zpos[i]  = ypos2[i]*sin(ik[i]);
		
		//外部変数に収納
		Sat_xpos[i] = xpos[i];
		Sat_ypos[i] = ypos[i];
		Sat_zpos[i] = zpos[i];
	}//for
	
	//衛星位置の書き出し（衛星番号は指定）
	//fprintf(sat_pos,"%f,%f,%f,%f\n",GPSTIME,
	//	xpos[1],ypos[1],zpos[1]);


////////////////////////////////////////////////////////////////////
	//QZSSと地球表面との交点の緯度、経度を計算
	// l,m,nは方向ベクトル
	// x-x(k)l = y-y(k)m = z-z(k)n 空間の直線方程式

//	fprintf(sat_pos,"%f,",GPSTIME);

	int k;
	double x[3],y[3],z[3],xk[3],yk[3],zk[3],rr[3],r;
	double l,m,nn,aa,b,c,ans;
	r = 6378137;
	for(i=33;i<=33;i++){
		k=i-33;
		xk[k] = Sat_xpos[i];
		yk[k] = Sat_ypos[i];
		zk[k] = Sat_zpos[i];
		rr[k] = sqrt(xk[k]*xk[k]+yk[k]*yk[k]+zk[k]*zk[k]);
		l = xk[k]/rr[k];
		m = yk[k]/rr[k];
		nn = zk[k]/rr[k];
		aa = l*l+m*m+nn*nn;
		b = 2*xk[k]*l+2*yk[k]*m+2*zk[k]*nn;
		c = xk[k]*xk[k]+yk[k]*yk[k]+zk[k]*zk[k]-r*r;
		ans = -1*b+sqrt(b*b-4*aa*c);
		ans = ans/2/aa;
		x[k] = xk[k]+ans*l;
		y[k] = yk[k]+ans*m;
		z[k] = zk[k]+ans*nn;

// 地球中心のx,y,zを緯度、経度、高度に変換
		double p[3],f,b;
		double b1;
		double stock_p = 1000000000;
		double a1,a2,a3,xx[3][3];

		a1=x[k];
		a2=y[k];
		a3=z[k];

		f = 1/298.257223563;
		//a=6377397.155;
		//f=1/299.152813;
		b = r*(1-f);

		p[0] = atan2(a2,a1);
		b1 = sqrt(a1*a1+a2*a2);
		p[1] = atan2(a3,b1);
		p[1] = atan2(a3+r*f*(2-f)*sin(p[1])/
					sqrt(1-f*(2-f)*sin(p[1])*sin(p[1])),b1);

		while(fabs(stock_p-p[1]) >= 0.000000001){
			stock_p = p[1];
			p[1] = atan2(a3+r*f*(2-f)*sin(stock_p)/
				sqrt(1-f*(2-f)*sin(stock_p)*sin(stock_p)),b1);
				//cout << stock_p << " " << p[1] << endl;
		}
		xx[2][k] = b1/cos(p[1])-r/sqrt(1-f*(2-f)*sin(p[1])*sin(p[1]));
		xx[1][k] = p[0]*180/PI;
		xx[0][k] = p[1]*180/PI;
	}

/////////////////////////////////////////////////////////////////////////
}