#include <stdio.h>
#include <time.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <pbm.h>
#include <pgm.h>
#include <ppm.h>
#include <assert.h>
#include <cmacs.h>


/*
   best fit plane solution

      t1 = Sxx*Syz;  t2 = t1*Sxz;   t3 = Sxz*Syy;   t4 = t3*Syz;
      t5 = Syz*Syz;  t6 = t5*Sxy;   t7 = Sxz*Sxz;   t8 = Sxy*t7;
      t14 = Syy*Syy; t17 = Sxx*Syy; t19 = Sxz*t5;   t20 = t1*Sxy;
      t21 = Sxy*Sxy; t22 = Sxz*t21; t23 = Sxx*Szz;  t26 = Szz*Syz*Sxy;
      t35 = Szz*Szz;
      troot = Cubic((-t2+t4-t6+t8),
		t7*Sxz-Sxz*t14+t3*Szz+t17*Sxz+t19+t20
			-2.0*t22-t23*Sxz-2.0*t26+Syz*Syy*Sxy,
		t6+t21*Sxy+Szz*Syy*Sxy+t23*Sxy-t35*Sxy
			-2.0*t8+Sxz*Syz*Szz+t2-t17*Sxy-2.0*t4,
		t22+t26-t20-t19,roots);

	yoz = troot;
	xoz = troot*(Syy+Syz-troot*Syz-Szz)/(-Sxy+troot*Sxz);
*/


int Cubic(double a0, double a1, double a2, double a3, double *roots)
{
    double Q, R, RQ, QQQ, RR, theta, MR;  int i;

    if (a0 != 0) /* a real cubic */
      { 
	  if (a0 != 1) { a1 /= a0;  a2 /= a0;  a3 /= a0; };
	  Q = (a1*a1-3*a2)/9;
	  R = (2*a1*a1*a1-9*a1*a2+27*a3)/54;
	  if ((QQQ=Q*Q*Q) >= (RR=R*R))  /* 3 real roots */
	    {
		RQ = SQRT(Q);  theta = ACOS(R/(RQ*Q));
		roots[0] = -2*RQ*COS(theta/3)-a1/3;
		roots[1] = -2*RQ*COS((theta+TWOPI)/3)-a1/3;
		roots[2] = -2*RQ*COS((theta+2*TWOPI)/3)-a1/3;
		return 3;
	    }
	  else /*  only one real root */
	    {
		RQ = SQRT(RR-QQQ);  MR = R<0?-R:R;
		RQ = POW(RQ+MR,1.0/3.0);
		RQ = RQ + Q/RQ;
		if (R>0) RQ = -RQ;
		roots[0] = RQ - a1/3;
		return 1;
	    };
      }
    else /* a quadratic */
      {
	  if ((RQ = a2*a2-4*a1*a3)<0) return 0;  /* no real roots */
	  RQ = SQRT(RQ);  if (a2<0) RQ = -RQ;
	  RQ = (a2+RQ)/2;
	  i = 0;
	  if (a1 != 0) roots[i++] = RQ/a1;
	  if (RQ != 0) roots[i++] = a3/RQ;
	  return i;
      };
}

/* compute eigenvectors */

#define ROTATE(a,i,j,k,l) g=a[i][j];h=a[k][l];a[i][j]=g-s*(h+g*tau);\
	a[k][l]=h+s*(g-h*tau);

void jacobi(a,n,d,v,nrot)
double a[4][4],d[4],v[4][4];
int n,*nrot;
{
	int j,iq,ip,i;
	double tresh,theta,tau,t,sm,s,h,g,c,b[n+1],z[n+1];

	for (ip=1;ip<=n;ip++) {
		for (iq=1;iq<=n;iq++) v[ip][iq]=0.0;
		v[ip][ip]=1.0;
	}
	for (ip=1;ip<=n;ip++) {
		b[ip]=d[ip]=a[ip][ip];
		z[ip]=0.0;
	}
	*nrot=0;
	for (i=1;i<=50;i++) {
		sm=0.0;
		for (ip=1;ip<=n-1;ip++) {
			for (iq=ip+1;iq<=n;iq++)
				sm += fabs(a[ip][iq]);
		}
		if (sm == 0.0) {
			return;
		}
		if (i < 4)
			tresh=0.2*sm/(n*n);
		else
			tresh=0.0;
		for (ip=1;ip<=n-1;ip++) {
			for (iq=ip+1;iq<=n;iq++) {
				g=100.0*fabs(a[ip][iq]);
				if (i > 4 && (double)(fabs(d[ip])+g) == (double)fabs(d[ip])
					&& (double)(fabs(d[iq])+g) == (double)fabs(d[iq]))
					a[ip][iq]=0.0;
				else if (fabs(a[ip][iq]) > tresh) {
					h=d[iq]-d[ip];
					if ((double)(fabs(h)+g) == (double)fabs(h))
						t=(a[ip][iq])/h;
					else {
						theta=0.5*h/(a[ip][iq]);
						t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
						if (theta < 0.0) t = -t;
					}
					c=1.0/sqrt(1+t*t);
					s=t*c;
					tau=s/(1.0+c);
					h=t*a[ip][iq];
					z[ip] -= h;
					z[iq] += h;
					d[ip] -= h;
					d[iq] += h;
					a[ip][iq]=0.0;
					for (j=1;j<=ip-1;j++) {
						ROTATE(a,j,ip,j,iq)
					}
					for (j=ip+1;j<=iq-1;j++) {
						ROTATE(a,ip,j,j,iq)
					}
					for (j=iq+1;j<=n;j++) {
						ROTATE(a,ip,j,iq,j)
					}
					for (j=1;j<=n;j++) {
						ROTATE(v,j,ip,j,iq)
					}
					++(*nrot);
				}
			}
		}
		for (ip=1;ip<=n;ip++) {
			b[ip] += z[ip];
			d[ip]=b[ip];
			z[ip]=0.0;
		}
	}
	printf("Too many iterations in routine JACOBI");
}

#undef ROTATE

double gasdev() /* gaussian random numbers, mean = 0, sd = 1 */
{
	static int iset=0;
	static double gset;
	double fac,r,v1,v2;
	double ran1();

	if  (iset == 0) {
		do {
			v1=2.0*RAND-1.0;
			v2=2.0*RAND-1.0;
			r=v1*v1+v2*v2;
		} while (r >= 1.0 || r == 0.0);
		fac=SQRT(-2.0*LOG(r)/r);
		gset=v1*fac;
		iset=1;
		return v2*fac;
	} else {
		iset=0;
		return gset;
	}
}


void testgasdev()  /* print check of gaussian curve */
{
    int cnt[30], k, d, i;

    for (i=0; i<30; i++) cnt[i]=0;
    for (i = 0; i<1000000; i++) 
      { k = gasdev()*2 + 15;  if (k>=0 && k<30) cnt[k]++; }

    for (d = 1000000; d>0; d /= 10)
      {
          for (i=0; i<30; i++) printf(" %c",(k=cnt[i]/d)?(k%10)+'0':' ');
	  printf("\n");
      };
}

#define npoints 10000
void main ()
{
    int i, j, nrot;
    double xc, yc, zc, xd, yd, zd, rd, xv, yv, zv;
    double x[npoints], y[npoints], z[npoints], tx, ty, tz;
    double Sxx, Sxy, Sxz, Syy, Syz, Szz;

    double a[4][4], d[4], v[4][4];

    xc = 2;  yc = 5;  zc = 9;
    xd = 1;  yd = 1;  zd = 1;
    xv = 2;  yv = 3;  zv = 4;

    rd = SQRT(xd*xd+yd*yd+zd*zd);
    xd /= rd;  yd /= rd;  zd /= rd;

    printf("Original center [%g %g %g]  direction [%g %g %g] variance [%g %g %g]\n",
	   xc, yc, zc, xd, yd, zd, xv, yv, zv);

    for (i=0; i<npoints; i++)
      {
	  tx = xv*gasdev();  ty = yv*gasdev();  tz = zv*gasdev();

	  x[i] = tx;  y[i] = ty;  z[i] = tz;

      };

    Sxx = Sxy = Sxz = Syy = Syz = Szz = 0;
    for (i=0; i<npoints; i++)
      {
	  x[i] = tx;  y[i] = ty;  z[i] = tz;
	  Sxx += tx*tx;  Sxy += tx*ty;  Sxz += tx*tz;
	  Syy += ty*ty;  Syz += ty*tz;  Szz += tz*tz;
      };

    a[1][1] = Sxx/npoints;   a[1][2] = a[2][1] = Sxy/npoints;
    a[1][3] = a[3][1] = Sxz/npoints;   a[2][2] = Syy/npoints;
    a[2][3] = a[3][2] = Syz/npoints;   a[3][3] = Szz/npoints;

    jacobi(a,3,d,v,&nrot);

    printf("%d iterations\n",nrot);
    printf("\n Eigenvalues %g %g %g\n",d[1],d[2],d[3]);
    for (i=1; i<=3; i++)
      {
	  printf("Eigenvector %g %g %g\n",v[i][1],v[i][2],v[i][3]);
      };
    printf("\n");

};