/* Last edited on 2002-12-11 17:40:44 by ra007998 */

#include <epipolar.h>


int leparametros(char *arqpar, double *Nfx, double *Ncx, double *dx, double *dy, double *dpx, double *dpy, double *Cx, double *Cy, double *sx, double *f, double *kappa1, double *Tx, double *Ty, double *Tz, double *Rx, double *Ry, double *Rz, double *p1, double *p2, double *sa, double *ca, double *sb, double *cb, double *sg, double *cg)
{
  FILE *fp;

  if ( !(fp = fopen(arqpar, "r")) )
    {printf("Nao abriu %s.\n", arqpar); return -1;}
  
  fscanf (fp, "%lf", Ncx);
  fscanf (fp, "%lf", Nfx);
  fscanf (fp, "%lf", dx);
  fscanf (fp, "%lf", dy);
  fscanf (fp, "%lf", dpx);
  fscanf (fp, "%lf", dpy);
  fscanf (fp, "%lf", Cx);
  fscanf (fp, "%lf", Cy);
  fscanf (fp, "%lf", sx);
  fscanf (fp, "%lf", f);
  fscanf (fp, "%lf", kappa1);
  fscanf (fp, "%lf", Tx);
  fscanf (fp, "%lf", Ty);
  fscanf (fp, "%lf", Tz);
  fscanf (fp, "%lf", Rx);
  fscanf (fp, "%lf", Ry);
  fscanf (fp, "%lf", Rz);
  fscanf (fp, "%lf", p1);
  fscanf (fp, "%lf", p2);

  fclose(fp);
  
  SINCOS (*Rx, *sa, *ca);
  SINCOS (*Ry, *sb, *cb);
  SINCOS (*Rz, *sg, *cg);
  
  return 0;
}

int matriz_real_para_imagem(char *arqpar, r4x4_t *M)
{
  int i, j;
  double Nfx, Ncx, dx, dy, dpx, dpy, Cx, Cy, sx, f, kappa1;
  double Tx, Ty, Tz, Rx, Ry, Rz, p1, p2, sa, ca, sb, cb, sg, cg;
  r4x4_t R, T, P, C, M1, M2;
  /* R: matriz de rotação
     T: translação
     P: projeção */
  

  leparametros(arqpar, &Nfx, &Ncx, &dx, &dy, &dpx, &dpy, &Cx, &Cy, &sx, &f, &kappa1, &Tx, &Ty, &Tz, &Rx, &Ry, &Rz, &p1, &p2, &sa, &ca, &sb, &cb, &sg, &cg);


  /* T, R, C inicializadas com a identidade*/
  for (i=0; i<4; i++)
    for (j=0; j<4; j++)
      { T.c[i][j] = C.c[i][j] = R.c[i][j] = ((i==j) ? 1 : 0); }

  R.c[1][1] = cb * cg;
  R.c[2][1] = cg * sa * sb - ca * sg;
  R.c[3][1] = sa * sg + ca * cg * sb;
  R.c[1][2] = cb * sg;
  R.c[2][2] = sa * sb * sg + ca * cg;
  R.c[3][2] = ca * sb * sg - cg * sa;
  R.c[1][3] = -sb;
  R.c[2][3] = cb * sa;
  R.c[3][3] = ca * cb;

  T.c[0][1] = Tx;
  T.c[0][2] = Ty;
  T.c[0][3] = Tz;

  C.c[0][1] = Cx;
  C.c[0][2] = Cy;
  C.c[1][1] = sx/dpx;
  C.c[2][2] = 1.0/dpy;
  C.c[3][3] = 1.0/sqrt(dpx*dpy);

  /* Matriz P */
  for (i=0; i<4; i++)
    for (j=0; j<4; j++)
      { P.c[i][j] = ((i==j) ? 1.0 : 0); }

  P.c[0][0] = 0;
  P.c[3][0] = 1.0/f;
  P.c[0][3] = -f;

  r4x4_mul(&R, &T, &M1);
  r4x4_mul(&M1, &P, &M2); M1 = M2;
  r4x4_mul(&M1, &C, &M2); M1 = M2;
  /* M1 = RTPC */
  *M = M1;
  
  /* Debug: */
/*   fprintf(stderr, "matriz World->Image\n"); */
/*   r4x4_gen_print(stderr, M, "%8.4f", "[ ", "[", ",", "]", "\n  ", "]"); */
/*   fprintf(stderr, "\n"); */
  
/*   fprintf(stderr, "Pontos da grade:\n"); */
  { int x, y, z;
    for (z = 0; z <= 50; z += 50)
      for (x = 0; x <= 250; x += 50)
        for (y = 0; y <= 170; y += 170)
          {
	    r4_t p, q;
            p = (r4_t){{1.0, (double)x, (double)y, (double)z}};
/*             fprintf(stderr, "(%3d, %3d, %3d)", x, y, z); */
            r4x4_map_row(&p, M, &q);
/*             fprintf(stderr, " -- > (%8.3f, %8.3f, %8.3f)\n",  */
/*                     q.c[1]/q.c[0], q.c[2]/q.c[0], q.c[3]/q.c[0]); */
          }
  }
  return 0;
}


/* arqpar1 e 2 são os nomes dos arquivos de parametros e de pontos*/
/* gerados pelo programa de calibração */
/* provavelmente .cal */
int encontra_epipolares(char *arqpar1, char *arqpar2)
{
  r4x4_t M, M1, M1i, M2;
  r4_t Pi1, Pi2, Pi2n;
  int alfa;
  
  matriz_real_para_imagem(arqpar1, &M1);
  matriz_real_para_imagem(arqpar2, &M2);

  /*duas matrizes: uma para cada foto*/

  /*inverte uma matriz e multiplica essa inversa pela outra*/
  r4x4_inv (&M1, &M1i);  

  /*m=m1i*m2*/
  r4x4_mul (&M1i, &M2, &M);

  /*  M(4x4) é a matriz que leva de uma imagem para a outra */

/*   fprintf(stderr, "matriz Image1->Image2\n"); */
/*   r4x4_gen_print(stderr, &M, "%8.4f", "[ ", "[", ",", "]", "\n  ", "]"); */
/*   fprintf(stderr, "\n"); */
  
  /* [ponto 1 real(mm)] x [matriz 1] = [ponto 1 na imagem 1(pixels)]*/
  /* [ponto 1 real(mm)] x [matriz 2] = [ponto 1 na imagem 2(pixels)]*/
  /* [ponto 1 real] = [ponto 1 na imagem 1] x [matriz 1]^ -1*/
  /* [ponto 1 na imagem 1]x[matriz 1]^-1 x[matriz 2] = [ponto 1 na imagem 2]*/

  for (alfa = 4000; alfa <= 8000; alfa += 100)
    {
      Pi1.c[0] = 1;
      Pi1.c[1] = 1000;
      Pi1.c[2] = 600;
      Pi1.c[3] = (double)alfa;

      r4x4_map_row (&Pi1, &M, &Pi2);

      r4_scale (1/Pi2.c[0], &Pi2, &Pi2n);
/*       r4_gen_print(stderr, &Pi2n, "%8.3f", "[", ", ", "]"); */
/*       fprintf(stderr, "\n"); */
    }

  return 0;
}