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

matrix  setb(matrix e,int n,double x0,double y0)
{   matrix b(n,6);
	int i=0;
   for(i=0;i<n;i++)
   {
	double dx=e.getele(i,0)-x0,dy=e.getele(i,1)-y0,w=e.getele(i,2)-e.getele(i,3);
	b.setele(i,0,1.0);
    b.setele(i,1,dx);
	b.setele(i,2,dy);
	b.setele(i,3,dx*dx);
	b.setele(i,4,dx*dy);
	b.setele(i,5,dy*dy);
  }
  return b;
}

matrix  setl(matrix e,int n)
{ matrix l(n,1);
  int i=0;
  for(i=0;i<n;i++)
  {
	double w=e.getele(i,2)-e.getele(i,3);
	cout<<"\n l["<<i<<"]="<<w;
	l.setele(i,0,w);
  }
  return l;
}

 matrix deal(matrix d,int n,matrix a, double x0,double y0)
 {   matrix zh(n,8);
	 for(int i=0;i<n;i++)
	 { double dx=d.getele(i,0)-x0,dy=d.getele(i,1)-y0,H=d.getele(i,2);
	   zh.setele(i,0,d.getele(i,0));
	   zh.setele(i,1,d.getele(i,1));
       zh.setele(i,2,d.getele(i,2));
	   double value=a.getele(0,0)+a.getele(1,0)*dx
	   +a.getele(2,0)*dy
	   +a.getele(3,0)*dx*dx
	   +a.getele(4,0)*dx*dy 
	   +a.getele(5,0)*dy*dy;
	   zh.setele(i,3,H-value);
	   double dis=(sqrt(dx*dx+dy*dy))/1000.0;
	   zh.setele(i,4,dis);
       zh.setele(i,7,sqrt(dis)*20/1000.0);
	 }
     for(i=0;i<n;i++)
	 {zh.setele(i,5,d.getele(i,3));
      zh.setele(i,6,zh.getele(i,3)-d.getele(i,3));
	 }
	 return zh;
 }

void main()
{    
   int n=2;
   char y='y';
   //double x0=4592000,y0=770000,dx=0,dy=0,w=0;
   double x0=0,y0=0,dx=0,dy=0,w=0,d0=0;
   /* Next input your matrix,
      for example:e=inputmatrix(m,n,"01.txt");
      the style of 01.txt is a sery number on a line between blank*/
   cout<<"\n★★★★★GPS大地高二次曲面擬合水準高程(六參數(shù))\n";
   cout<<"\n please input the number of points which are known:\n n=";cin>>n;
   matrix l(n,1),b(n,6),e(1),N(6),L(6,n),a(6,6),d(2,3),zh(10),v(n,1);

   e=inputmatrix(n,4);
   cout<<"\n 已知點信息如下(請核對)";
   e.output();

   for(int i=0;i<n;i++)//計算測區(qū)中央坐標
   {x0+=e.getele(i,0)/n;
    y0+=e.getele(i,1)/n;
   }
   cout<<"\n \n 測區(qū)中央坐標大約為：\n";
   cout<<"x0="<<x0<<"    y0="<<y0;

   b=setb(e,n,x0,y0);
   cout<<"\n \nmatrix l is(即各已知點處高程異常值): ";
   l=setl(e,n); //系數(shù)陣  
   L=b.T()*l;N=b.T()*b; a=~N*L;//求取六參數(shù)
   cout<<"\n擬合的參數(shù)如下\n";a.output();
   v=b*a-l;        //求得改正數(shù)向量
   cout<<"\n matrix v is: ";v.output();
   for(i=0;i<n;i++)
   d0+=v.getele(i,0)*v.getele(i,0);d0=sqrt(d0/(n-1));//計算高程擬合的內(nèi)符合精度
   cout<<"\n \nplease input the number of points are to deal:\n n=";
   cin>>n;

   d=inputmatrix(n,4,"dnh.txt");
   cout<<"\n following is the data to deal:(請核對) ";
   cout<<"\n      x            y        大地高    已知海拔高";
   d.output();

   zh=deal(d,n,a,x0,y0);//進行高程擬合
   cout<<"\n\n 擬合高程如下:     \n   x                y       大地高    擬合高程 到已知點距離  已知海拔高   擬合殘差  容許誤差（三等）：";zh.output();
   cout<<"\n   內(nèi)符合精度d0= "<<d0<<"mm"<<endl;

   for(i=0;i<n;i++)
	   d0+=zh.getele(i,6)*zh.getele(i,6);
       d0=sqrt(d0/(n-1));//計算擬合點的外符合精度

  
   cout<<"   外符合精度M= "<<d0<<"mm"<<endl;
   cout<<"\n\nDo you want to input the result to the file(y/n)"<<endl;
   cin>>y;
   if(y=='y') 
   {
	   cout<<"★Your result have been saved as a file in output.txt!\n";
       outputmatrix(zh);
   }
   else cout<<"\n   Thank you to use this process!\n ";
}