/*
 *  find_axis.c
 *  copyright 2003 Scott Hotton
 *
 *  This program finds the minimal variation center for a finite set of points.
 *  It does this by implementing gradient descent via Euler's method on a 
 *  gradient flow.  The gradient is formed from the "sum of squares" of the 
 *  divergence angles made by the points.
 */
#include<math.h>
#include<stdio.h>

main()
{
 int j, J=0, count=0;
 double u[501], v[501], w[501]; /* assumes a maximum of 500 data points */
 double phi0, psi0, x[501], y[501], dist_sq;
 double theta[501];
 double Dphi_theta[501], Dpsi_theta[501], Dx_theta[501], Dy_theta[501];      
 double delta[500];
 double Dphi_delta[501], Dpsi_delta[501], Dx_delta[500], Dy_delta[500];  
 double Dphi_x[501], Dpsi_x[501], Dpsi_y[501];
 double sum_delta; double sum_Dx_delta, sum_delta_Dx_delta;
 double sum_Dy_delta, sum_delta_Dy_delta;
 double sum_Dphi_delta, sum_delta_Dphi_delta;
 double sum_Dpsi_delta, sum_delta_Dpsi_delta;
 double Dx_S, Dy_S, Dphi_S, Dpsi_S, epsilon=0.01, tolerance=0.000000000000001;

 for( j=1 ; j<=500 ; ++j){             /* reads in coordinates of each point */
    if( scanf("%lf",u+j)==EOF ) break; /* ignores everything after 500 points*/
    if( scanf("%lf",v+j)==EOF ){       /* checks there is a v-coordinate for */
      fprintf(stderr,"last line incomplete\n");  /* each u-coordinate */
      exit(1);                         
    }
    if( scanf("%lf",w+j)==EOF ){       /* checks there is a w-coordinate for */
      fprintf(stderr,"last line incomplete\n");  /* for each u-coordinate   */
      exit(1);
    }
    J=j;
 }

 phi0=M_PI/2.0;
 psi0=M_PI/2.0;  /* Starting estimates for axis is w-axis */
 x[0]=0.0;
 y[0]=0.0;

 printf("Starting estimate for center: (%lf, %f, %lf, %lf)\n",phi0*180.0/M_PI,psi0*180.0/M_PI,x[0],y[0]);

 do{

   for( j=1 ; j<=J ; j++){
      x[j]=-u[j]*sin(phi0)+cos(phi0)*(v[j]*cos(psi0)+w[j]*sin(psi0));
      y[j]=-v[j]*sin(psi0)+w[j]*cos(psi0);
      theta[j]=atan2(y[j]-y[0],x[j]-x[0]);
      dist_sq=(x[j]-x[0])*(x[j]-x[0])+(y[j]-y[0])*(y[j]-y[0]);
      Dphi_x[j]=-u[j]*cos(phi0)-sin(phi0)*(v[j]*cos(psi0)+w[j]*sin(psi0));
      Dpsi_x[j]=cos(phi0)*(-v[j]*sin(psi0)+w[j]*cos(psi0));
      Dpsi_y[j]=-v[j]*cos(psi0)-w[j]*sin(psi0);
      Dphi_theta[j]=(y[0]-y[j])*Dphi_x[j]/dist_sq;
      Dpsi_theta[j]=((x[j]-x[0])*Dpsi_y[j]-(y[j]-y[0])*Dpsi_x[j])/dist_sq;
      Dx_theta[j]=(y[j]-y[0])/dist_sq;
      Dy_theta[j]=(x[0]-x[j])/dist_sq;
   }
   sum_delta=0.0;
   sum_Dx_delta=sum_Dy_delta=sum_delta_Dx_delta=sum_delta_Dy_delta=0.0;
   sum_Dphi_delta=sum_Dpsi_delta=sum_delta_Dphi_delta=sum_delta_Dpsi_delta=0.0;
   for( j=1 ; j<J ; j++){
      delta[j]=theta[j+1]-theta[j];
 
       /* put delta[j] into the interval [-pi,pi] */

      while( delta[j]<-M_PI ) delta[j]+=2*M_PI;
      while( M_PI<delta[j]  ) delta[j]-=2*M_PI;
    
      sum_delta += delta[j];

      Dphi_delta[j] = Dphi_theta[j+1]-Dphi_theta[j];
      sum_Dphi_delta+=Dphi_delta[j];
      sum_delta_Dphi_delta+=delta[j]*Dphi_delta[j];

      Dpsi_delta[j] = Dpsi_theta[j+1]-Dpsi_theta[j];
      sum_Dpsi_delta+=Dpsi_delta[j];
      sum_delta_Dpsi_delta+=delta[j]*Dpsi_delta[j];

      Dx_delta[j] = Dx_theta[j+1]-Dx_theta[j];  /* intermediate terms in the */
      sum_Dx_delta += Dx_delta[j];              /* computation of the grad */
      sum_delta_Dx_delta += delta[j]*Dx_delta[j];

      Dy_delta[j]=Dy_theta[j+1]-Dy_theta[j];
      sum_Dy_delta+=Dy_delta[j];
      sum_delta_Dy_delta+=delta[j]*Dy_delta[j];
   }

   Dphi_S = (sum_delta_Dphi_delta) - (sum_delta)*(sum_Dphi_delta)/(J-1);
   Dpsi_S = (sum_delta_Dpsi_delta) - (sum_delta)*(sum_Dpsi_delta)/(J-1);
   Dx_S = (sum_delta_Dx_delta) - (sum_delta)*(sum_Dx_delta)/(J-1);
   Dy_S = (sum_delta_Dy_delta) - (sum_delta)*(sum_Dy_delta)/(J-1);
   phi0 -= epsilon*Dphi_S;
   psi0 -= epsilon*Dpsi_S;
   x[0] -= epsilon*Dx_S;
   y[0] -= epsilon*Dy_S;  /* update estimate for the center */

   ++count;                            /* output estimate for center every */
   if( count%1000==0 ){             /* million iterations */
     printf("Estimate for center: (%lf, %lf, %lf, %lf)",phi0*180.0/M_PI,psi0*180.0/M_PI,x[0],y[0]);
     printf(" after %d iterations\n",count);
   }

   if( 0.05>phi0 || phi0>M_PI-0.05 ){ /* If we're beyond these bounds then */
     fprintf(stderr,"After %d iterations phi0=%lf\n",count, phi0*180.0/M_PI);
     exit(1);                         /* the initial estimate can't be right */
   }
   if( 0.05>psi0 || psi0>M_PI-0.05 ){
     fprintf(stderr,"After %d iterations psi0=%lf\n",count, psi0*180.0/M_PI);
     exit(1);
   }

  /* run this do-loop while the change to the estimate is large enough */

 }while( fabs(epsilon*Dphi_S)>tolerance || fabs(epsilon*Dpsi_S)>tolerance || 
         fabs(epsilon*Dx_S)>tolerance || fabs(epsilon*Dy_S)>tolerance );

 printf("Final estimate for center: (%lf, %lf, %lf, %lf)\n",phi0*180.0/M_PI,psi0*180.0/M_PI,x[0],y[0]);
}