/////////////////////////////////////////////////////////////////////
//
//	ユーザの天頂方向にZ軸を持ち東の方向にX軸を持つ地平直交座標系
//	の場合の各衛星の位置を算出(仰角などを算出するのに不可欠)
//
/////////////////////////////////////////////////////////////////////

#include <math.h>

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

extern int UNUSE_GPS[16];//アルマナック情報のない衛星をストックする
extern int 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 Lat_pos[DIVMAX],Lon_pos[DIVMAX],Hei_pos[DIVMAX][DIVMAX];//ユーザ位置の座標
extern double Sat_xpos2[DIVMAX][DIVMAX][SATMAX],Sat_ypos2[DIVMAX][DIVMAX][SATMAX];
extern double Sat_zpos2[DIVMAX][DIVMAX][SATMAX];//地平直交座標系

void trans_sat_pos()
{
	int i,j,k;
	int l[DIVMAX][DIVMAX]={0};
	double xpos1[SATMAX],ypos1[SATMAX],zpos1[SATMAX];
	double xpos2[SATMAX],ypos2[SATMAX],zpos2[SATMAX];
	double lat,lon;
	double kk = 0.5;
	
	
	for(i=0;i<=DIV1;i++){
		for(j=0;j<=DIV1;j++){
			for(k=1;k<=GPS+QZSS;k++){//GPS衛星分回す

				if(k==UNUSE_GPS[0] || k==UNUSE_GPS[1] || k==UNUSE_GPS[2] || 
					k==UNUSE_GPS[3] || k==UNUSE_GPS[4] || k==UNUSE_GPS[5] || k==UNUSE_GPS[6])
					continue;

				lat=(90.0-Lat_pos[i])*2.0*PI/360.0;
				lon=Lon_pos[j]*2.0*PI/360.0;
				
				//Z軸回りに経度分回転
				xpos1[k]=cos(lon)*(Sat_xpos[k]-POSx[i][j])
					+sin(lon)*(Sat_ypos[k]-POSy[i][j]);
				ypos1[k]=-sin(lon)*(Sat_xpos[k]-POSx[i][j])
					+cos(lon)*(Sat_ypos[k]-POSy[i][j]);
				zpos1[k]=Sat_zpos[k]-POSz[i][j];
				
				//Y軸回りに(90-緯度)分回転
				xpos2[k]=cos(lat)*xpos1[k]-sin(lat)*zpos1[k];
				ypos2[k]=ypos1[k];
				zpos2[k]=sin(lat)*xpos1[k]+cos(lat)*zpos1[k];
				
				//Z軸回りに90度回転(?????)
				Sat_xpos2[i][j][k]=cos(kk*PI)*xpos2[k]+sin(kk*PI)*ypos2[k];
				Sat_ypos2[i][j][k]=-sin(kk*PI)*xpos2[k]+cos(kk*PI)*ypos2[k];
				Sat_zpos2[i][j][k]=zpos2[k];
				
				if(Sat_zpos2[i][j][k]>=0.0)
					l[i][j]++;
			}
		}
	}							
}