#include "stdio.h"

#include <math.h>



#define  PI  3.141592653

/*

该程序根据GPS.G1-X-00006.pdf文档，实现了WGS84和ECEF坐标的转换

*/
 
 

void LLAtoECEF(double latitude, double longitude,double height,double *X,double *Y,double *Z)
{

  double a = 6378137;

  double b = 6356752.314245;// b=(1-f)

  double E = (a*a - b*b)/(a*a);



  double COSLAT = cos(latitude*PI/180);

  double SINLAT = sin(latitude*PI/180);

  double COSLONG = cos(longitude*PI/180);

  double SINLONG = sin(longitude*PI/180);



  double N = a /(sqrt(1 - E*SINLAT*SINLAT));

  double NH = N + height;



  *X = NH * COSLAT * COSLONG;

  *Y = NH * COSLAT * SINLONG;

  *Z = (b*b*N/(a*a) + height) * SINLAT;

  
}

 
//经纬度函数传参
typedef struct BLH
{
	double B;//维度
	double L;//经度
	double H;//高
}BLH;
#define aaa  6378137.0   //长半轴
#define f  (1 / 298.257223563)    //扁率
#define e2  (f*(2-f))   //第一偏心率平方
 
//经纬度转换(弧度)
BLH XYZtoLB(double X, double Y, double Z)
{
	BLH res = { 0 };
	double B = 0.0, N = 0.0, H = 0.0, R0, R1, deltaH, deltaB;
	R0 = sqrt(pow(X, 2) + pow(Y, 2));
	R1 = sqrt(pow(X, 2) + pow(Y, 2) + pow(Z, 2));
	//经度直接求解
	res.L = atan2(Y, X)* 180.0 / PI;;
	//迭代求大地维度和大地高
	N = aaa;
	H = R1 - aaa;
	B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
	do
	{
		deltaH = N;//判断收敛所用
		deltaB = B;
		N = aaa / sqrt(1 - e2 * pow(sin(B), 2));
		H = R0 / cos(B) - N;
		B = atan2(Z * (N + H), R0 * (N * (1 - e2) + H));
	} while (fabs(deltaH - H) > 1.0e-3 && fabs(deltaB - B) > 1.0e-9);
	res.B = B* 180.0 / PI;;
	res.H = H;
	return res;
}

//typedef union {
   typedef struct{
      unsigned int  :9;
      unsigned int high12:17;
      unsigned int mid12:4;
      unsigned int low20:3;
   
   
   }int20_num_t;
  
//#define NUM_PLUS(x, 12) 


//}int32_num_t;

int main(int argc, char* argv[])

{

  int i = 0;
  int20_num_t low20bit;
  
  int m = 1;
  
  for(i = 0; i < 20; i++)
  {
     m *=2;
     printf("%d\n", m);
  }

  while(1){
      sleep(10);
     printf("\n------[%d][%d][%d][%d]\n", low20bit.low20++, low20bit.mid12++, low20bit.high12++,sizeof(int20_num_t));
  }


  double latitude = 22.21555549969;

  double longitude = 113.367229848;

  double h = 100.0;

  double XX = 0.0;

  double YY = 0.0;

  double ZZ = 0.0;


  printf("\n------xx = [%.16f], yy = [%.16f], zz =%.16f\n", latitude, longitude, h);
  LLAtoECEF(latitude, longitude, h, &XX, &YY, &ZZ);
      printf("\n------xx = [%f], yy = [%f], zz =%f\n", XX, YY, ZZ);
 
    BLH  tmp = XYZtoLB(XX, YY, ZZ);
     printf("\n------xx = [%.16lf], yy = [%.16lf], zz =%.16lf\n", tmp.B, tmp.L, tmp.H); 
return 0;

}