#include<iostream.h>
#include<stdio.h>
#include<math.h>
#include"Matrix.h"
//#include"Matrix.cpp"

#define f 153.24
#define m 50000
#define N 4
#define PI 3.141593

void input(float*,float*,float*,float*,float*);
void output(float*,float*,float*,float*,float*,float*);

int main()
{
    float x[N],y[N],X[N],Y[N],Z[N];
    float Xs=0.0,Ys=0.0,Zs=0.0,phi=0.0,omega=0.0,kappa=0.0;
    int i;
    input(x,y,X,Y,Z);
    for(i=0;i<N;i++)
    {
        Xs+=X[i];
        Ys+=Y[i];
    }
    Xs/=N;
    Ys/=N;
    Zs=f*m;
    //迭代過程
    float XIANCHA;
    XIANCHA=0.1/60/180*PI;
    i=-1;
    do
    {
         //構(gòu)造R矩陣 
         float a1,a2,a3,b1,b2,b3,c1,c2,c3;
         a1=cos(phi)*cos(kappa)-sin(phi)*sin(omega)*sin(kappa);
         a2=-cos(phi)*sin(kappa)-sin(phi)*sin(omega)*cos(kappa);
         a3=-sin(phi)*cos(omega);
         b1=cos(omega)*sin(kappa);
         b2=cos(omega)*cos(kappa);
         b3=-sin(omega);
         c1=sin(phi)*cos(kappa)+cos(phi)*sin(omega)*sin(kappa);
         c2=-sin(phi)*sin(kappa)+cos(phi)*sin(omega)*cos(kappa);
         c3=cos(phi)*cos(omega);
         float data_R[3][3]={a1,a2,a3,
                             b1,b2,b3,
                             c1,c2,c3};
         Matrix R(3,3,data_R);
         //
         i++;
         float data_S[3][1]={X[i]-Xs,Y[i]-Ys,Z[i]-Zs};
         Matrix S(3,1,data_S);
         Matrix _S(3,1);
         _S=R.Invers()*S;
         float _x,_y;
         _x=-f*(_S.M[1][1]/_S.M[3][1]);
         _y=-f*(_S.M[2][1]/_S.M[3][1]);
         //求誤差方程的系數(shù)矩陣和常數(shù)項矩陣 
         float a[2][6];
         a[1][1]=(a1*f+a3*x)/_S.M[3][1];
         a[1][2]=(b1*f+b3*x)/_S.M[3][1];
         a[1][3]=(c1*f+c3*x)/_S.M[3][1];
         a[2][1]=(a2*f+a3*y)/_S.M[3][1];
         a[2][2]=(b2*f+b3*y)/_S.M[3][1];
         a[2][3]=(c2*f+c3*y)/_S.M[3][1];
         a[1][4]=y*sin(omega)-(x/f(x*cos(kappa)-y*sin(kappa))+f*cos(kappa))*cos(omega);
         a[1][5]=-f*sin(kappa)-x/f(x*sin(kappa)+y*cos(kappa));
         a[1][6]=y;
         a[2][4]=-x*sin(omega)-(y/f(x*cos(kappa)-y*sin(kappa))-f*sin(kappa))*cos(omega);
         a[2][5]=-f*cos(kappa)-y/f(x*sin(kappa)+y*cos(kappa));
         a[2][6]=-x;
         Matrix B(2,6,a);
         float data_L[2][1]={x-_x,y-_y};
         Matrix L(2,1,data_L);
         //利用法方程求各外方位元素的改正數(shù) 
         Matrix X(6,1);
         X=(B.Trans()*B).Invers()*B.Trans()*L;
         Xs+=X.M[1][1];
         Ys+=X.M[2][1];
         Zs+=X.M[3][1];
         phi+=X.M[4][1];
         omega+=X.M[5][1];
         kappa+=X.M[6][1];
    }while((X.M[4][1]>XIANCHA||X.M[5][1]>XIANCHA||X.M[6][1]>XIANCHA)&&i<N);
    //output
    output(Xs,Ys,Zs,phi,omega,kappa);
    cout<<"各外方位元素的近似值：\n"; 
    cout<<"Xs:\t"<<Xs<<endl;
    cout<<"Ys:\t"<<Ys<<endl;
    cout<<"Zs:\t"<<Zs<<endl;
    cout<<"phi:\t"<<phi<<endl;
    cout<<"omega:\t"<<omega<<endl;
    cout<<"kappa:\t"<<kappa<<endl;
    return 0;
}

void input(float* x,float* y,float* X,float* Y,float* Z)
{
     FILE * fp;
     if((fp=fopen("input.txt","r"))==NULL)
     {
         printf("不能打開輸入文件!");
         exit(0);
     }
     for(int i=0;i<N;i++)
       fscanf(fp,"%f,%f,%f,%f,%f",x++,y++,X++,Y++,Z++);
     fclose(fp); 
}

void output(float Xs,float Ys,float Zs,float phi,float omega,float kappa)
{
     FILE * fp;
     if((fp=fopen("output.txt","w"))==NULL)
     {
         printf("不能打開輸出文件!");
         exit(0);
     }
     fprintf(fp,"各外方位元素的近似值：\n");
     fprintf(fp,"Xs:\t%f\nYs:\t%f\nZs:\t%f\nphi:\t%f\nomega:\t%f\nkappa:\t%f\n",Xs,Ys,Zs,phi,omega,kappa);
     fclose(fp);
}