#include <windows.h>
#include <math.h>
#include <stdio.h>
#include "CoorTrans.h"
////////////////////////////////////////////
// 公用函數
////////////////////////////////////////////
//角度轉換為弧度
double CoorTrans_Dms2Rad(double Dms)
{
	double Degree, Miniute;
	double Second;
	int Sign;
	double Rad;
	if(Dms >= 0)
	Sign = 1;
	else
	Sign = -1;
	Dms = fabs(Dms);
	Degree = floor(Dms);
	Miniute = floor(fmod(Dms * 100.0, 100.0));
	Second = fmod(Dms * 10000.0, 100.0);
	Rad = Sign * (Degree + Miniute / 60.0 + Second / 3600.0) * PI / 180.0;
	return Rad;
}
//弧度轉換為角度
double CoorTrans_Rad2Dms(double Rad)
{
	double Degree, Miniute;
	double Second;
	int Sign;
	double Dms;
	if(Rad >= 0)
	Sign = 1;
	else
	Sign = -1;
	Rad = fabs(Rad * 180.0 / PI);
	Degree = floor(Rad);
	Miniute = floor(fmod(Rad * 60.0, 60.0));
	Second = fmod(Rad * 3600.0, 60.0);
	Dms = Sign * (Degree + Miniute / 100.0 + Second / 10000.0);
	return Dms;
}


///////////////////////////////////////////////////
// 坐標系參數初始化
///////////////////////////////////////////////////
void CoorTrans_Init(CoorTrans_Gaosi_Parameter p,unsigned int type)
{
	double f;
	if (type==CoorTrans_Type_IUGG1975)
	{	//IUGG1975坐標系參數初始化
		p.a = 6378140;
		f = 298.257;
		p.e2 = 1 - ((f - 1) / f) * ((f - 1) / f);
		//p.e12 = (f / (f - 1)) * (f / (f - 1)) - 1;
		p.A1 = 111133.0047;
		p.A2 = -16038.5282;
		p.A3 = 16.8326;
		p.A4 = -0.0220;
	}
	else if(type==CoorTrans_Type_WGS84)
	{
		p.a = 6378137;
		f = 298.257;
		p.e2 = 1 - ((f - 1) / f) * ((f - 1) / f);
		p.A1=0;
		p.A2=0;
		p.A3=0;
		p.A4=0;
	}
	else//Krasovsky坐標系參數初始化
	{
		p.a = 6378245;
		f = 298.3;
		p.e2 = 1 - ((f - 1) / f) * ((f - 1) / f);
		//p.e12 = (f / (f - 1)) * (f / (f - 1)) - 1;
		p.A1 = 111134.8611;
		p.A2 = -16036.4803;
		p.A3 = 16.8281;
		p.A4 = -0.0220;
	}
}

///////////////////////////////////////////////////
// 
///////////////////////////////////////////////////
bool CoorTrans_BL2xy(double L0,double L,double B,double x,double y,int type)
{
	double X, N, t, t2, m, m2, ng2;
	double sinB, cosB;
	CoorTrans_Gaosi_Parameter Para;
	CoorTrans_Init(para,type);
	L0 = Dms2Rad(L0);
	B = Dms2Rad(B);
	L = Dms2Rad(L);
	X = Para.A1 * B * 180.0 / PI + Para.A2 * sin(2 * B) + Para.A3 * sin(4 * B) + Para.A4 * sin(6 * B);
	sinB = sin(B);
	cosB = cos(B);
	t = tan(B);
	t2 = t * t;
	N = Para.a / sqrt(1 - Para.e2 * sinB * sinB);
	m = cosB * (L - L0);
	m2 = m * m;
	ng2 = cosB * cosB * Para.e2 / (1 - Para.e2);
	x = X + N * t * ((0.5 + ((5 - t2 + 9 * ng2 + 4 * ng2 * ng2) / 24.0 + (61 - 
		58 * t2 + t2 * t2) * m2 / 720.0) * m2) * m2);
	y = N * m * ( 1 + m2 * ( (1 - t2 + ng2) / 6.0 + m2 * ( 5 - 18 * t2 + t2 * t
		2 + 14 * ng2 - 58 * ng2 * t2 ) / 120.0));
	y += 500000;
	return true;
}
bool CoorTrans_xy2BL(double L0,double x,double y,double L,double B,int type)
{
	double sinB, cosB, t, t2, N ,ng2, V, yN;
	double preB0, B0;
	double eta;
	CoorTrans_Gaosi_Parameter Para;
	CoorTrans_Init(para,type);
	L0 = Dms2Rad(L0);
	B = Dms2Rad(B);
	L = Dms2Rad(L);
	y -= 500000;
	B0 = x / A1;
	do
	{
	preB0 = B0;
	B0 = B0 * PI / 180.0;
	B0 = (x - (Para.A2 * sin(2 * B0) + Para.A3 * sin(4 * B0) + Para.A4 * sin(6 * B0))) / Para.A1;
	eta = fabs(B0 - preB0);
	}while(eta > 0.000000001);
	B0 = B0 * PI / 180.0;
	B = Rad2Dms(B0);
	sinB = sin(B0);
	cosB = cos(B0);
	t = tan(B0);
	t2 = t * t;
	N = Para.a / sqrt(1 - Para.e2 * sinB * sinB);
	ng2 = cosB * cosB * Para.e2 / (1 - Para.e2);
	V = sqrt(1 + ng2);
	yN = y / N;
	B = B0 - (yN * yN - (5 + 3 * t2 + ng2 - 9 * ng2 * t2) * yN * yN * yN * yN /
		12.0 + (61 + 90 * t2 + 45 * t2 * t2) * yN * yN * yN * yN * yN * yN / 360.0)
		* V * V * t / 2;
	L = L0 + (yN - (1 + 2 * t2 + ng2) * yN * yN * yN / 6.0 + (5 + 28 * t2 + 24 
		* t2 * t2 + 6 * ng2 + 8 * ng2 * t2) * yN * yN * yN * yN * yN / 120.0) / cosB
		;
	return true;
}