// gps.cpp: implementation of the gps class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "maintest.h"
#include "gps.h"
#include <math.h>

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

gps::gps()
{
	l=0.0;
    b=0.0;
	h=0.0;
	x=0.0;
	y=0.0;
	z=0.0;
}

gps::~gps()
{
}

void gps::setgps(double a,double o,double e)
{  
	l=a;
	b=o;
	h=e;
}

gps operator-(gps &obj,gps &stage)
{   gps delta ;
	delta.x=obj.x-stage.x;
	delta.y=obj.y-stage.y;
	delta.z=obj.z-stage.z;
	return delta;
}

void gps::convert()       //轉換成直角坐標
{   
    const double e=1/298.257223563;    //地球橢圓偏心率
    const double a=6378137.0;            //地球橢圓長半徑
	const double pi=3.1415927;
	l*=pi/180;
	b*=pi/180;
	double N=a/sqrt(1-pow(e*sin(b),2));       //N為卵酉面曲率半徑
	x=(N+h)*cos(b)*cos(l);  
	y=(N+h)*cos(b)*sin(l);
	z=(N*(1-pow(e,2))+h)*sin(b);
}

gps transform(gps &stage,gps &delta)  // 轉換到法線直角坐標系中
{
	gps aim;
	const double pi=3.1415927;
	stage.l*=pi/180;
	stage.b*=pi/180;
	aim.x=-sin(stage.b)*cos(stage.l)*delta.x-sin(stage.l)*sin(stage.b)*delta.y+cos(stage.b)*delta.z;
	aim.y=-sin(stage.l)*delta.x+cos(stage.l)*delta.y ;
	aim.z=cos(stage.b)*cos(stage.l)*delta.x+cos(stage.b)*sin(stage.l)*delta.y+sin(stage.b)*delta.z;
    return aim;
}
  
void gps::compute(gps &goal,int output[])    //求仰角,方位角，結果是度數
{
	double elevation,azimuth;  
	const double pi=3.1415926;
	elevation=atan(goal.z/sqrt(pow(goal.x,2)+pow(goal.y,2)))*180/pi;   //仰角
	//x軸順時針定方位角
	azimuth=atan(goal.y/goal.x)*180/pi;
	if(goal.x<0 && goal.y<0)
		azimuth=azimuth+180;
	if(goal.x<0 && goal.y>0)
		azimuth=azimuth+180;
	if(goal.x>0 && goal.y<0)
		azimuth=azimuth+360;
	output[0]=(int)(azimuth/0.08);        //方位角 轉換成數字格式
    output[1]=(int)(elevation/0.08); 	   // 仰角
}