#ifdef _VS_WALK
#include "wkClassDefine.h"
#endif

//因为GPS获取的高程值精度很低，所以求解地心坐标时，不使用点的高程值。
#define USE_H  0

string proj4_xian80;//="+proj=tmerc +lon_0=120 +x_0=500000 +ellps=IAU76";
string proj4_xian80_ellps;//="+ellps=IAU76";
string proj4_wgs84;//="+proj=latlong +ellps=WGS84";

//由数组构成towgs84串
string towgs84_byR(array &R)
{
	string towgs84;
	if (R.getSize()==7)
		towgs84.format(" +towgs84=%.8lf,%.8lf,%.8lf,%.10e,%.10e,%.10e,%.10e",
				R[0],R[1],R[2],R[3],R[4],R[5],R[6]);
	else if (R.getSize()==3)
		towgs84.format(" +towgs84=%.8lf,%.8lf,%.8lf", R[0],R[1],R[2]);
	else
		message("7参数错误");
	return towgs84;
}

//求西安80高斯坐标转西安80的地心坐标
//	【in】pt中x=E，y=N；以米为单位
void xian80_to_centro(wkPoint &pt)
{
	wkTrans xian80, centro80;
	xian80.projectCreate(proj4_xian80);
	centro80.projectCreate("+proj=geocent "+proj4_xian80_ellps);
	if (!USE_H) pt.z=0;
	xian80.project(pt, centro80);
	xian80.projectFree();
	centro80.projectFree();
}

//使用七参数求西安80高斯坐标转WGS84地心坐标
//	【in】pt中x=E，y=N；以米为单位
void xian80_to7_centro(wkPoint &pt, array &R)
{
	wkTrans xian80, centro80;
	xian80.projectCreate(proj4_xian80+towgs84_byR(R));
	centro80.projectCreate("+proj=geocent "+proj4_xian80_ellps);
	if (!USE_H) pt.z=0;
	xian80.project(pt, centro80);
	xian80.projectFree();
	centro80.projectFree();
}

//求WGS84经纬度坐标转WGS84的地心坐标
//	pt中x=经度，y=纬度。以度为单位
void wgs84_to_centro(wkPoint &pt)
{
	wkTrans wgs84, centro84;
	wgs84.projectCreate(proj4_wgs84);
	centro84.projectCreate("+proj=geocent +ellps=WGS84");
	if (!USE_H) pt.z=0;
	wgs84.project(pt, centro84);
	wgs84.projectFree();
	centro84.projectFree();
}

//使用七参数将WGS84坐标转换为西安80坐标
void wgs84_to7_xian80(wkPoint &pt, array &R)
{
	wkTrans xian80;
	xian80.projectCreate(proj4_xian80+towgs84_byR(R));
	wkTrans wgs84;
	wgs84.projectCreate("+proj=latlong +ellps=WGS84");
	if (!USE_H) pt.z=0;
	wgs84.project(pt, xian80);
	wgs84.projectFree();
	xian80.projectFree();
}

//将度分秒转换为度
double dms_deg(double d, double m, double s)
{
	return d+m/60.+s/3600.;
}


//+++++++++++++++++++++++++++++++
array xian80;
array wgs84;
array xian80C;
array wgs84C;
array X;
int pt_count=0;
//+++++++++++++++++++++++++++++++

void init_geocentric()
{
	int i;
	//求xian80的地心坐标：
	for (i=0; i<pt_count; i++)
	{
		wkPoint pt;
		pt.y=xian80[i][0];
		pt.x=xian80[i][1];
		pt.z=xian80[i][2];
		xian80_to_centro(pt);
		xian80C.add(pt);
	}
	//求wgs84的地心坐标：
	for (i=0; i<pt_count; i++)
	{
		wkPoint pt;
		pt.y=wgs84[i][0];
		pt.x=wgs84[i][1];
		pt.z=wgs84[i][2];
		wgs84_to_centro(pt);
		wgs84C.add(pt);
	}
	//输出两套地心坐标
	trace("\n地心坐标：\n");
	for (i=0; i<pt_count; i++)
	{
		trace("%d 80= %.3lf, %.3lf, %.3lf\n  84= %.3lf, %.3lf, %.3lf\n",
			i+1, xian80C[i].x, xian80C[i].y, xian80C[i].z,
			wgs84C[i].x, wgs84C[i].y, wgs84C[i].z);
	}
}

bool solve7X()
{
	//组成系数方程矩阵 A*X=L:
	/*
	r1 +  0 +  0 +     0    + r5*z_in - r6*y_in + r7*x_in = x_out
	 0 + r2 +  0 -  r4*z_in +    0    + r6*x_in + r7*y_in = y_out
	 0 +  0 + r3 +  r4*y_in - r5*x_in +    0    + r7*z_in = z_out
	*/
	array A, L;
	array line, l0; line.setSize(7); l0.setSize(1);
	for (int i=0; i<pt_count; i++)
	{
		//【1】	r1 +  0 +  0 +     0     + r5*z_in - r6*y_in + r7*x_in  - x_out = 0
		line[0]=1.; line[1]=0.; line[2]=0.;
		line[3]=0.; line[4]=xian80C[i].z; line[5]=-xian80C[i].y; line[6]=xian80C[i].x;
		A.add(line);
		l0[0]=wgs84C[i].x;
		L.add(l0);
		//【2】	0 + r2 +  0 -   r4*z_in +    0     + r6*x_in + r7*y_in - y_out = 0
		line[0]=0.; line[1]=1.; line[2]=0.;
		line[3]=-xian80C[i].z; line[4]=0.; line[5]=xian80C[i].x; line[6]=xian80C[i].y;
		A.add(line);
		l0[0]=wgs84C[i].y;
		L.add(l0);
		//【3】	 0 +  0 + r3 +  r4*y_in - r5*x_in +    0     + r7*z_in  - z_out = 0
		line[0]=0.; line[1]=0.; line[2]=1.;
		line[3]=xian80C[i].z; line[4]=-xian80C[i].x; line[5]=0.; line[6]=xian80C[i].z;
		A.add(line);
		l0[0]=wgs84C[i].z;
		L.add(l0);
	}

	//按最小二乘法组成法方程： (A'*A)*X = (A'*L) -- A'为A的转置矩阵
	// 设： N=(A'*A), W=(A'*L) 法方程为 N*X=W
	array At=A.mtTranspose();
	array N=At.mtMultiply(A);
	array W0=At.mtMultiply(L);
	array W; for (i=0; i<7; i++) W.add(W0[i][0]);
	//高斯主元素消元法求X
	int o1 = X.mtGauss(N,W);
	if (o1 == 1)
	{
		trace("\n唯一解   X = " + toString(X)+"\n");
		//	Dx = r1,Dy = r2,Dz = r3,
		//	Rx = r4/r7,	Ry = r5/r7,	Rz = r6/r7,	M = r7
		X[3]/=X[6];
		X[4]/=X[6];
		X[5]/=X[6];
		trace("伪七参数 = " + toString(X)+"\n");

		//转换为towgs84的真七参数——towgs84={r1,r2,r3,r4,r5,r6,r7}
		/*
			f = pi/180/3600 //SEC_TO_RAD
			r4 /= f;
			r5 /= f;
			r6 /= f;
			r7 = (r7-1.)*1000000.0;
		*/
		double f=atan(1)*4./180./3600.;
		X[3]/=f;
		X[4]/=f;
		X[5]/=f;
		X[6]=(X[6]-1.)*1000000.;
		trace("真七参数 = " + toString(X)+"\n");
	}
	else if (o1 == -1)
	{
		trace("无解\n"); return false;
	}else if (o1 == 0)
	{
		trace("无数组解\n"); return false;
	}
	return true;
}

bool solve3X()
{
	//组成系数方程矩阵 A*X=L:
	/*
	r1 +  0 +  0 = x_out - x_in
	 0 + r2 +  0 = y_out - y_in
	 0 +  0 + r3 = z_out - z_in
	*/
	array A, L;
	array line, l0; line.setSize(3); l0.setSize(1);
	for (int i=0; i<pt_count; i++)
	{
		//【1】	r1 +  0 +  0 = x_out - x_in
		line[0]=1.; line[1]=0.; line[2]=0.;
		A.add(line);
		l0[0]=wgs84C[i].x-xian80C[i].x;
		L.add(l0);
		//【2】	0 + r2 +  0 = y_out - y_in
		line[0]=0.; line[1]=1.; line[2]=0.;
		A.add(line);
		l0[0]=wgs84C[i].y-xian80C[i].y;
		L.add(l0);
		//【3】	 0 +  0 + r3 = z_out - z_in
		line[0]=0.; line[1]=0.; line[2]=1.;
		A.add(line);
		l0[0]=wgs84C[i].z-xian80C[i].z;
		L.add(l0);
	}

	//按最小二乘法组成法方程： (A'*A)*X = (A'*L) -- A'为A的转置矩阵
	// 设： N=(A'*A), W=(A'*L) 法方程为 N*X=W
	array At=A.mtTranspose();
	array N=At.mtMultiply(A);
	array W0=At.mtMultiply(L);
	array W; for (i=0; i<3; i++) W.add(W0[i][0]);
	//高斯主元素消元法求X
	int o1 = X.mtGauss(N,W);
	if (o1 == 1)
	{
		//	Dx = r1,	Dy = r2,	Dz = r3
		trace("\n三参数 = " + toString(X)+"\n");
	}
	else if (o1 == -1)
	{
		trace("无解\n"); return false;
	}else if (o1 == 0)
	{
		trace("无数组解\n"); return false;
	}
	return true;
}

void check_by_geocentric()
{
	trace("\n\n========== check_by_geocentric ===========\n");
	//验证：
	double sxy=0, sxyz=0;
	//求xian80的地心坐标：
	for (int i=0; i<pt_count; i++)
	{
		wkPoint pt;
		pt.y=xian80[i][0];
		pt.x=xian80[i][1];
		pt.z=xian80[i][2];
		xian80_to7_centro(pt, X);
		trace("\n %16.3lf, %16.3lf, %16.3lf == %d", pt.x, pt.y, pt.z, i+1);
		wkPoint p2=wgs84C[i];
		trace("\n %16.3lf, %16.3lf, %16.3lf", p2.x, p2.y, p2.z);
		double dx=pt.x-p2.x;
		double dy=pt.y-p2.y;
		double dz=pt.z-p2.z;
		double dxy=dx*dx+dy*dy;
		double dxyz=dxy+dz*dz;
		trace("\n dx=%13.3lf,  dy=%13.3lf,  dz=%13.3lf;    dxy=%.3lf;  dxyz=%.3lf\n",
			dx, dy, dz, sqrt(dxy), sqrt(dxyz));
		sxy+=dxy;
		sxyz+=dxyz;
	}
	sxy=sqrt(sxy/(pt_count*3-X.getSize()));
	sxyz=sqrt(sxyz/(pt_count*3-X.getSize()));
	trace("\n\n按地心坐标评定参数求解中误差： 【sxy】=%.4lf;  sxyz=%.4lf", sxy, sxyz);
}

void check_by_xy()
{
	trace("\n\n========== check_by_xy ===========\n");
	//验证：
	double sxy=0, sxyz=0;
	//求xian80的地心坐标：
	for (int i=0; i<pt_count; i++)
	{
		wkPoint pt;
		pt.y=wgs84[i][0];
		pt.x=wgs84[i][1];
		pt.z=wgs84[i][2];
		wgs84_to7_xian80(pt, X);
		//高程仍取xian80的z值
		pt.z=xian80[i][2];
		trace("\n %16.3lf, %16.3lf, %16.3lf == %d", pt.x, pt.y, pt.z, i+1);
		wkPoint p2;
		p2.y=xian80[i][0];
		p2.x=xian80[i][1];
		p2.z=xian80[i][2];
		trace("\n %16.3lf, %16.3lf, %16.3lf", p2.x, p2.y, p2.z);
		double dx=pt.x-p2.x;
		double dy=pt.y-p2.y;
		double dz=pt.z-p2.z;
		double dxy=dx*dx+dy*dy;
		double dxyz=dxy+dz*dz;
		trace("\n dx=%13.3lf,  dy=%13.3lf,  dz=%13.3lf;    dxy=%.3lf;  dxyz=%.3lf\n",
			dx, dy, dz, sqrt(dxy), sqrt(dxyz));
		sxy+=dxy;
		sxyz+=dxyz;
	}
	sxy=sqrt(sxy/(pt_count*3-X.getSize()));
	sxyz=sqrt(sxyz/(pt_count*3-X.getSize()));
	trace("\n\n按投影坐标评定参数求解中误差：【sxy】=%.4lf;  sxyz=%.4lf", sxy, sxyz);
}

void main()
{
	//原始数据 =============== 同名点（按点序分别输入xian80与wgs84坐标）
	//西安80坐标
	xian80={// N, E, H
{3380944.668,593496.294,2.710},
{3378038.655,586164.246,3.220},
{3387628.221,598072.556,6.06},
{3382491.170,587427.068,2.783},
{3384460.139,583869.364,3.708},
{3377901.152,581137.738,3.001}
};
	//WGS84坐标
	wgs84={// B, L, H
{dms_deg(30,32,42.08768),dms_deg(120,58,32.11529),13.2835},
{dms_deg(30,31,9.71186),dms_deg(120,53,56.20056),13.5321},
{dms_deg(30,36,17.76898),dms_deg(121,01,26.05918),16.7657},
{dms_deg(30,33,33.95061),dms_deg(120,54,44.91182),13.1270},
{dms_deg(30,34,38.79744),dms_deg(120,52,32.00539),13.9162},
{dms_deg(30,31,6.51000),dms_deg(120,50,47.65012),13.1723}
};
	pt_count=xian80.getSize();

	/* ------ proj4 投影定义说明： ------
	北京54：椭球为 +ellps=krass
			投影为 +proj=tmerc +lon_0=120 +x_0=500000 +ellps=krass
	西安80：椭球为 +ellps=IAU76
			投影为 +proj=tmerc +lon_0=120 +x_0=500000 +ellps=IAU76
	其中 +lon_0=120 为高斯投影中央子午线
		 +x_0=500000 为高斯投影的E含500公里常数，若含带号，如40带，则 +x0=40500000
	*/
	//西安80的椭球
	proj4_xian80_ellps="+ellps=IAU76";
	//西安80的投影——proj4
	proj4_xian80="+proj=tmerc +lon_0=120 +x_0=500000 "+proj4_xian80_ellps;

	//WGS84的投影——proj4
	proj4_wgs84="+proj=latlong +ellps=WGS84";

	//=============================================

	//分别求控制点的西安80和WGS84地心坐标
	init_geocentric();
	wkView vw;
	bool bCal7=vw.dlgQuestion("计算【七参数】?\n 否则计算三参数");
	if (bCal7)
	{	//求解七参数：组成系数数组和常量数组，组成法方程，高斯消元
		if (!solve7X())
			return;
	}else
	{	//求三参数
		if (!solve3X())
			return;
	}
	//按地心坐标评定参数求解中误差
	check_by_geocentric();
	//按投影坐标评定参数求解中误差
	check_by_xy();
}

