#include <stdio.h>
#include <math.h>

#include "algebra.h"
#include "simplemath.h"

/*- Inclusao das bibliotecas IUP e CD: ------------------------------------*/
#include <windows.h>                /* inclui as definicoes do windows para o OpenGL */
#include <gl/gl.h>                  /* prototypes do OpenGL */
#include <gl/glu.h>                 /* prototypes do OpenGL */

#define MAX_POINTS 50000
double x[MAX_POINTS],y[MAX_POINTS],z[MAX_POINTS];         /* nuvem de pontos */
int    n_points=0;





struct _axis {
   double cp[3];
   double e1[3];
   double e2[3];
   double e3[3];
} axis={ {0.,0.,0.}, {1.,0.,0.}, {0.,1.,0.}, {0.,0.,1.}};

static void findAxis(double* d, int* i1, int* i2, int* i3)
{
    if ((d[0]>=d[1])&&(d[1]>=d[2])) { *i1=0; *i2=1; *i3=2; return; }
    if ((d[0]>=d[2])&&(d[2]>=d[1])) { *i1=0; *i2=2; *i3=1; return; }

    if ((d[1]>=d[0])&&(d[0]>=d[2])) { *i1=1; *i2=0; *i3=2; return; }
    if ((d[1]>=d[2])&&(d[2]>=d[0])) { *i1=1; *i2=2; *i3=0; return; }

    if ((d[2]>=d[1])&&(d[1]>=d[0])) { *i1=2; *i2=1; *i3=0; return; }
    if ((d[2]>=d[0])&&(d[0]>=d[1])) { *i1=2; *i2=0; *i3=1; return; }
}

/*
 * Acha o centroide dos pontos e os eixos principais dele.
 */
static void fitPoints( double* xc, double*yc, double* zc, double* radius )
{
  double xm=x[0],xM=x[0],ym=y[0],yM=y[0],zm=z[0],zM=z[0];
  double  M[3*3]={0.,0.,0., 0.,0.,0., 0.,0.,0.};
  double  U[3*3];
  double  D[3];
  double  V[3*3];

  double  tmp[3]={0.,0.,0.};
  int i1,i2,i3;
  double  a,t;
  int i;

  axis.cp[0]=axis.cp[1]=axis.cp[2]=0.;
  for (i=0;i<n_points;i++) {
     xm=(xm<x[i])?xm:x[i];
     ym=(ym<y[i])?ym:y[i];
     zm=(zm<z[i])?zm:z[i];

     xM=(xM>x[i])?xM:x[i];
     yM=(yM>y[i])?yM:y[i];
     zM=(zM>z[i])?zM:z[i];

     axis.cp[0]+=x[i];
     axis.cp[1]+=y[i];
     axis.cp[2]+=z[i];
  }

  axis.cp[0]/=n_points;
  axis.cp[1]/=n_points;
  axis.cp[2]/=n_points;

  *xc=(xm+xM)/2;
  *yc=(ym+yM)/2;
  *zc=(zm+zM)/2;
  *radius=sqrt((xM-*xc)*(xM-*xc)+(yM-*yc)*(yM-*yc)+(zM-*zc)*(zM-*zc));

  /* Calcula a matriz de covvariancia */
  for(i=0;i<3*3;i++) M[i]=0.;
  for (i=0;i<n_points;i++) {
     tmp[0] = x[i]-axis.cp[0];
     tmp[1] = y[i]-axis.cp[1];
     tmp[2] = z[i]-axis.cp[2];
     smAddMatVecTensor(M,tmp,3);
  }
  smScaMat(M,(1.0/n_points),3,3,M);
  smShowMat("M",M,3,3);

  /* Calcula a decomposicao singular da matriz M */
  smSVD(M,3,3,U,D,V,tmp);

  /* acha os eixos principais */
  findAxis(D,&i1,&i2,&i3);

  axis.e1[0]=V[0*3+i1]; axis.e1[1]=V[1*3+i1]; axis.e1[2]=V[2*3+i1];
  axis.e2[0]=V[0*3+i2]; axis.e2[1]=V[1*3+i2]; axis.e2[2]=V[2*3+i2];
  axis.e3[0]=V[0*3+i3]; axis.e3[1]=V[1*3+i3]; axis.e3[2]=V[2*3+i3];

  for (i=0;i<n_points;i++) {
     t=axis.e3[0]*(x[i]-axis.cp[0])+axis.e3[1]*(y[i]-axis.cp[1])+axis.e3[2]*(z[i]-axis.cp[2]);
     if (i==0) 
        a=t;
     else
        a=(a<t)?a:t;
  }

  /* imprime os resultados */
  printf("\n\nEixos principais:\n");
  printf("e1=(%f,%f,%f)\n", axis.e1[0],axis.e1[1],axis.e1[2]);
  printf("e2=(%f,%f,%f)\n", axis.e2[0],axis.e2[1],axis.e2[2]);
  printf("e3=(%f,%f,%f)\n", axis.e3[0],axis.e3[1],axis.e3[2]);

  smShowMat("[U]",U,3,3);
  smShowVec("[D]",D,3);
  smShowMat("[V]",V,3,3);

}


void scnLoad(char* filename, double* xc, double*yc, double* zc, double* radius)
{
  FILE* fp=fopen(filename,"rt");
  int i=0;
  if (fp==NULL) return;
  /* Le os pontos */
  while(fscanf(fp," %lf %lf %lf",&x[i],&y[i],&z[i])==3) 
     i++;
  fclose(fp);

  n_points=i;
  printf("%d pontos lidos\n",n_points);

  if (n_points>0) fitPoints( xc,yc,zc,radius );
}

void scnDraw( int flag_eixos, double xc, double yc, double zc, double radius ) 
{
   int i;
   glPointSize(1.f);
   glBegin(GL_POINTS);
   for (i=0;i<n_points;i++)
   {
      glColor3d(0.3,0.3,0.3);
      glVertex3d(x[i],y[i],z[i]);
   }
   glEnd();


      /* Desenha os eixos x,y,z dos objetos */
   if (flag_eixos) {
      glBegin(GL_LINES);
         glColor3d(1.,0.,0.);
         glVertex3d(axis.cp[0], axis.cp[1], axis.cp[2]);
         glVertex3d(axis.cp[0]+axis.e1[0]*radius, axis.cp[1]+axis.e1[1]*radius, axis.cp[2]+axis.e1[2]*radius);

         glColor3d(0.,1.,0.);
         glVertex3d(axis.cp[0], axis.cp[1], axis.cp[2]);
         glVertex3d(axis.cp[0]+axis.e2[0]*radius, axis.cp[1]+axis.e2[1]*radius, axis.cp[2]+axis.e2[2]*radius);

         glColor3d(0.,0.,1.);
         glVertex3d(axis.cp[0], axis.cp[1], axis.cp[2]);
         glVertex3d(axis.cp[0]+axis.e3[0]*radius, axis.cp[1]+axis.e3[1]*radius, axis.cp[2]+axis.e3[2]*radius);
      glEnd();
   }

}
