#include <stdio.h>
#include <math.h>
#include <r4x4.h>
/* Last edited on 2002-12-09 20:51:43 by ra007998 */

#define SINCOS(x,s,c)   s=sin(x);c=cos(x)

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 main(int argc, char **argv)
{
  char *arqpar, *arqpon;/*nomes dos arquivos de parametros e de pontos*/
  FILE *fp2;
  r4_t pr, pi;
  int i,j, n;
  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 */

  if (argc==3)
    {
      arqpar = argv[1];
      arqpon = argv[2];
    }
  else
    {
      printf("Sintaxe: real2image [arquivo gerado por nccal] [arquivo com os pontos a converter].\n");
      return 0;
    }
  
  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;

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

  P.c[0][0] = 0;
  P.c[3][0] = 1;

  r4x4_mul(&R, &T, &M1);
  r4x4_mul(&M1, &P, &M2); M1 = M2;
  r4x4_mul(&M1, &C, &M2); M1 = M2;
  /* M1 = RTPC */

  /*  printf("Quantos pontos? ");
  scanf("%i", &n);
  printf("\n");*/
  n=7;

  if ( !(fp2 = fopen(arqpon, "r")) ) printf("Nao abriu %s.\n", arqpon);
      
  for (i=0; i<n; i++)
    {
      pr.c[0] = 1;/*W*/
      //      printf("[Xreal] = ");
      fscanf(fp2, "%lf", &(pr.c[1]));
      //      printf("[Yreal] = ");
      fscanf(fp2, "%lf", &(pr.c[2]));
      //      printf("[Zreal] = ");
      fscanf(fp2, "%lf", &(pr.c[3]));
      
      r4x4_map_row (&pr, &M1, &pi);
      
      printf("[");
      for (j=1; j<4; j++) printf("%9.4lf ", pr.c[j]/pr.c[0]);
      printf("]");
      printf("--> ");
      printf("[");
      for (j=1; j<4; j++) printf("%9.4lf ", pi.c[j]/pi.c[0]);
      printf("]\n");
    }

  fclose(fp2);

  return 0;
}