#include "Pv.h"
#include"time.h"
#include"matrix.h"

#ifndef _NO_NAMESPACE
using namespace std;
using namespace math;
#define STD std
#else
#define STD
#endif

#ifndef _NO_TEMPLATE
typedef matrix<double> Matrix;
#else
typedef matrix Matrix;
#endif

#ifndef _NO_EXCEPTION
#  define TRYBEGIN()	try {
#  define CATCHERROR()	} catch (const STD::exception& e) { \
						cerr << "Error: " << e.what() << endl; }
#else
#  define TRYBEGIN()
#  define CATCHERROR()
#endif

bool PVOne(GMOREC gmoRecord,/*觀測(cè)值信息記錄*/
		   GMO gmo,/*觀測(cè)值文件*/
		   vector<GMNREC> navRecord,/*導(dǎo)航電文文件*/
		   PPVONERESULT pvresult/*輸出結(jié)果*/)
{
	//	CRDCARTESIAN crdSite;//測(cè)站坐標(biāo)
		CRDCARTESIAN crdSat;//衛(wèi)星坐標(biāo)

	//double Xsat,Ysat,Zsat;//衛(wèi)星坐標(biāo)
//	double Xsite,Ysite,Zsite;//測(cè)站坐標(biāo)
		SatVel satv;//衛(wèi)星的速度
	//double Xsatv,Ysatv,Zsatv;//衛(wèi)星速度
	double Xsitev=0,Ysitev=0,Zsitev=0;//測(cè)站速度
	double Xsitev2=0,Ysitev2=0,Zsitev2=0;//測(cè)站速度

	double dx,dy,dz;

	PPONERESULT m_presult;
//	PVONERESULT m_vresult;

	double Pdot=0;
	double D=0;
	double R=0;
    
	GMNREC m_nearRec;

	int len=0;

	if(PPOne(gmoRecord,gmo,navRecord,&m_presult))
		len=m_presult.sat_num;
    else
			return false;

	Matrix B(len,4),W(len,1),Result(4,1);

	COMMONTIME   tj;
    tj.year=m_presult.epoch.year;
	tj.month=m_presult.epoch.month;
	tj.day=m_presult.epoch.day;
	tj.hour=m_presult.epoch.hour;
	tj.minute=m_presult.epoch.minute;
    tj.second=m_presult.epoch.second;

	pvresult->epoch.year=m_presult.epoch.year;
	pvresult->epoch.month=m_presult.epoch.month;
	pvresult->epoch.day=m_presult.epoch.day;
	pvresult->epoch.hour=m_presult.epoch.hour;
	pvresult->epoch.minute=m_presult.epoch.minute;
	pvresult->epoch.second=m_presult.epoch.second;
	///////////////////////////////////////////////////
    
	int m_value=0;//用到的觀測(cè)文件的哪種偽距C1,P1,P2
	int m_p1=0;
	int m_p2=0;
    
	//確定使用偽距所在的位置
	int tf=0;
	for(int type=0;type<gmo.hdr.MeasureTypeNum;type++)
	{
		if(gmo.hdr.ObsType[type].substr(0,2).compare(0,2,"D1")==0)
		{
			m_value=type;
			tf=1;
			break;
		}

		 
	}

	if(tf==0)
		return false;

	//確定能使用的衛(wèi)星數(shù)目
    int bz=0,vp=0,vh=0,t=0;
	int sat_value[MAXNUM]={0};
	int sat_pos[MAXNUM]={0};
	int PN;
    
	int pos,PRN;//有效衛(wèi)星在觀測(cè)星歷中的位置

	for(int j=0;j<gmoRecord.satsum;j++)
	{
		PN=atoi(gmoRecord.PRN_list[j].substr(1,2).c_str());
		for(int k=0;k<navRecord.size();k++)
		{
			if(PN==navRecord[k].PRN&&gmoRecord.obsValue[j][m_value]!=0)
			{
				sat_value[vp]=PN;
				vp++;
				sat_pos[vh]=j;
				vh++;
				bz=1;
				break;
			}

		}
		if(bz==0)
			t++;
		else
			bz=0;
	}

	int sat_valuesum=gmoRecord.satsum-t;
	int flag=0;
	/////////////////////////////////////////////////////
	if(sat_valuesum>=4)
	{
	 do{
		 flag++;
		 Xsitev=Xsitev2;
		 Ysitev=Ysitev2;
		 Zsitev=Zsitev2;

	//	len=sat_valuesum;

		 for(int i=0;i<len;i++)
		 {
			 PRN=sat_value[i];
			 GetOrbNClk(navRecord,PRN,&tj, &crdSat);//計(jì)算衛(wèi)星的位置
			 GetSatVelocity(navRecord,PRN,&tj,&satv);//計(jì)算衛(wèi)星的速度

			 dx=crdSat.x-m_presult.crd.x;
			 dy=crdSat.y-m_presult.crd.y;
			 dz=crdSat.z-m_presult.crd.z;

			 R=sqrt(dx*dx+dy*dy+dz*dz);
			 B(i,0)=-dx/R;
			 B(i,1)=-dy/R;
			 B(i,2)=-dz/R;
			 B(i,3)=1;

             pos=sat_pos[i];  
			  //得到最靠近所給時(shí)刻的歷元單位
             m_nearRec=GetBestGMNREC(navRecord,PRN,&tj);//用于得到a1
             
			 D=(dx*satv.xv+dy*satv.yv+dz*satv.zv)/R;
             
			 Pdot=gmoRecord.obsValue[pos][m_value]*0.1903;
             
			 cout<<"第幾號(hào)衛(wèi)星："<<pos<<"  ";
			 cout<<"D1:"<<gmoRecord.obsValue[pos][m_value]<<"  ";
             cout<<"衛(wèi)星數(shù)：" <<sat_valuesum<<endl;

			 W(i,0)=Pdot-D+c*m_nearRec.a1;

            cout<<"W(i,0):"<<W(i,0)<<"   "<<"Pdot:"<<Pdot<<"   "<<"D:"<<D<<endl;
			 cout<<"flag:"<<flag<<endl;
		 }

		 Result=!(~B*B)*~B*W;

         Xsitev=Result(0,0);
		 Ysitev=Result(1,0);
		 Zsitev=Result(2,0);

		
       if(flag>10)
		   break;

	}while(fabs(Zsitev-Zsitev2)>0.1);
		 
		 

		///////////////////////////////
		
	pvresult->m_sitev.xv=Xsitev;
	pvresult->m_sitev.yv=Ysitev;
	pvresult->m_sitev.zv=Zsitev;

     return true;
	}
  else
	  return false;
	

}