#include "string.h"
#include "iostream.h"
#include "stdio.h"
#include "matrix.h"
#include "io_matrix.h"
#include "math.h"
#include "angle.h"

double a=6378137.00;
double A=1/298.3;//A=1/298.257223563;
double ee=2*A-A*A;
double A0=1+0.75*ee+45*pow(ee,2)/64+175*pow(ee,3)/256
		+11025*pow(ee,4)/16384 + 43659*pow(ee,5)/65536 + 693693*pow(ee,6)/1048576;
double B0=A0-1,
       C=15*pow(ee,2)/32+175*pow(ee,3)/256+3675*pow(ee,4)/8192+14553*pow(ee,5)/32768+231231*pow(ee,6)/524288,
	   D=35*pow(ee,3)/96+735*pow(ee,4)/2048+14553*pow(ee,5)/40960+231231*pow(ee,6)/655360,
	   E=315*pow(ee,4)/1024+6237*pow(ee,5)/20480+99099*pow(ee,6)/327680,
	   F=693*pow(ee,5)/2560+11011*pow(ee,6)/40960,
	   G=1001*pow(ee,6)/4096;
//下式求取子午線弧長，式中第二項中均減一項就可變為求兩緯度(B到B1)之間（某經度）的子午線
//弧長如將B0*sin(B)*cos(B)變為B*（sin(B)*cos(B)-B*sin(B1)*cos(B1)），其他項類似
double s(double B)//B為緯度值，求取緯度B處子午線弧長
{  
	double S=0.00;
	S=a*(1-ee)*(A0*B-B0*sin(B)*cos(B)-C*pow(sin(B),3)*cos(B)
		-D*pow(sin(B),5)*cos(B)-E*pow(sin(B),7)*cos(B)
		-F*pow(sin(B),9)*cos(B)-G*pow(sin(B),11)*cos(B));
	return S;
}

double sp(double Bk)
{  
	double S=0.00;
	S=a*(1-ee)*((A0-B0*cos(2*Bk))-C*pow(sin(Bk),2)*(cos(2*Bk)+2*pow(cos(Bk),2))
			-D*pow(sin(Bk),4)*(cos(2*Bk)+4*pow(cos(Bk),2))
			-E*pow(sin(Bk),6)*(cos(2*Bk)+6*pow(cos(Bk),2))
			-F*pow(sin(Bk),8)*(cos(2*Bk)+8*pow(cos(Bk),2)));
	return S;
}

matrix invert(double ee,double a,matrix BL,double l0)//高斯投影正算
{
	int n=BL.getrows(),i=0;
	matrix xy(n,2);
    double S=0.00, x=0.00,y=0.00,N,W,t,ep=0.00,k,l;//ep為第二偏心率
    for(i=0;i<n;i++)
	{
		double B=dtor(BL.getele(i,0)),L=dtor(BL.getele(i,1));	
		S=s(B);
		W=sqrt(1-ee*sin(B)*sin(B));
		N=a/W;
		t=tan(B);
		ep=sqrt(ee/(1-ee));
		k=ep*cos(B);
		l=L-l0;
        x=S+N*sin(B)*cos(B)*pow(l,2)/2
			+N*sin(B)*pow(cos(B),3)*pow(l,4)*(5-pow(t,2)+9*pow(k,2)+4*pow(k,4))/24
			+N*sin(B)*pow(cos(B),5)*pow(l,6)*(61-58*pow(t,2)+pow(t,4))/24;
		y=N*cos(B)*l+N*pow(cos(B),3)*pow(l,3)*(1-pow(t,2)+pow(k,2))/6
          +N*pow(cos(B),5)*pow(l,5)*(5-18*pow(t,2)+14*pow(k,2)+pow(t,4)-58*pow(t,2)*pow(k,2))/120;
		xy.setele(i,0,x);
		xy.setele(i,1,y);		
	}
    return xy;
}

matrix revert(double ee,double a,matrix xy,double l0)//高斯投影反算
{
	int n=xy.getrows(),i=0;
	matrix BL(n,2);
	double B,L,l,d,ep,t,k,N,W;
	double Mf=0.00;
    for(i=0;i<n;i++)
	{
		double x=xy.getele(i,0),y=xy.getele(i,1);
		double Bk,Sk,Sp,Bf;
		ep=sqrt(ee/(1-ee));
		//Sp為Sk的導數值,Sk為緯度1、2子午線弧長 ,ep為第二偏心率
        Bk=x/(a*(1-ee)*A0);
	    Sk=s(Bk);
		Sp=sp(Bk);
		B=Bk+(x-Sk)/Sp;
		while(fabs(x-Sk)>0.00001)
		{
			Bk=B;
            Sp=sp(Bk);
	        Sk=s(Bk);
		    B=Bk+(x-Sk)/Sp;
		} 
		Bf=B; //cout<<"\nBf=  "<<Bf<<endl;
		if(fabs(Bf-PI/2)<1e-10)  cout<<"所獲得的緯度值接近90度，其求解可能異常!\n";
		t=tan(Bf);
		k=ep*cos(Bf);
		W=sqrt(1-ee*sin(Bf)*sin(Bf));
		N=a/W;
		Mf=a*(1-ee)/(pow(W,3));
		d=y/N;
		l=d*(1-(1+2*pow(t,2)+pow(k,2))*pow(d,2)/6
		+(5+28*pow(t,2)+24*pow(t,4)+6*pow(k,2)+8*pow(k,2)*pow(t,2))*pow(d,4)/120)/cos(Bf);
		B=Bf-t*y*d*(1-(5+3*pow(t,2)+pow(k,2)-9*pow(k,2)*pow(t,2))*pow(d,2)/12
		+(61+90*pow(t,2)+45*pow(t,4))*pow(d,4)/360)/(2*Mf);
		L=l+l0;
		B=rtod(B);L=rtod(L);
		BL.setele(i,0,B);
		BL.setele(i,1,L);
	}
    return BL;
}