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

#include <GL/glut.h>
/*
#include "im.h"
#include "tsai.h"

#define SQR(x) ((x)*(x))

#define IMAGE_W 320
#define IMAGE_H 240

#define NEARPLANE  50
#define FARPLANE  200

#ifndef _PI_
#define _PI_ 3.1415926535897932384626433832795
#endif

static GLubyte image[3*IMAGE_W*IMAGE_H];
static int wtext, htext;
static int np;
static double *xw, *yw, *xf, *yf;
static double mtsaiopengl_modelview[16];
static double mtsaiopengl_projection[16];

static double f, T[3], r[9];
static double Tb[3], rb[9];

#define NPTSX 8
#define NPTSY 8
static double xwn[NPTSX*NPTSY], ywn[NPTSX*NPTSY], xfn[NPTSX*NPTSY], yfn[NPTSX*NPTSY];

void load_image(char *arq, GLubyte *image)
{
 int i;
 GLubyte red[IMAGE_W*IMAGE_H];
 GLubyte green[IMAGE_W*IMAGE_H];
 GLubyte blue[IMAGE_W*IMAGE_H];
 imLoadRGB(arq, red, green, blue);
 for (i=0; i<IMAGE_W*IMAGE_H; i++) {
  image[3*i  ] = red[i];
  image[3*i+1] = green[i];
  image[3*i+2] = blue[i];
 }
 glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, IMAGE_W, IMAGE_H, GL_RGB, GL_UNSIGNED_BYTE, image);
}

void load_data(char *arq,
               int *np, double **xw, double **yw, double **xf, double **yf)
{
 int i;
 int npaux;
 double *xwaux, *ywaux, *xfaux, *yfaux;
 FILE *fp = fopen (arq, "r");
 fscanf(fp, "%d", &npaux);
 xwaux = (double *)malloc(npaux*sizeof(double));
 ywaux = (double *)malloc(npaux*sizeof(double));
 xfaux = (double *)malloc(npaux*sizeof(double));
 yfaux = (double *)malloc(npaux*sizeof(double));
 for (i=0; i<npaux; i++) {
  fscanf (fp, "%lf %lf %lf %lf", &(xwaux[i]), &(ywaux[i]), &(xfaux[i]), &(yfaux[i]));
  xfaux[i] -= IMAGE_W/2.0;
  yfaux[i] -= IMAGE_H/2.0;
 }
 fclose(fp);
 *np = npaux;
 *xw = xwaux;
 *yw = ywaux;
 *xf = xfaux;
 *yf = yfaux;
}

void save_result(char *arq, 
                 int np, double *xw, double *yw, double *xf, double *yf,
                 double f, double T[3], double r[9])
{
 int i;
 double u, v;
 double aux = 0;
 FILE *fp = fopen (arq, "w");
 for (i=0; i<np; i++) {
  tsaiProject(xw[i], yw[i], 0.0, &u, &v, f, T, r);
  aux += sqrt(SQR(u-xf[i]) + SQR(v-yf[i]));
  u += IMAGE_W/2.0;
  v += IMAGE_H/2.0;
  fprintf(fp, "%.16g %.16g - %.16g %.16g - %.16g %.16g\n", xw[i], yw[i], xf[i]+IMAGE_W/2.0, yf[i]+IMAGE_H/2.0, u, v);
 }
 fprintf(fp, "\n\n\n");
 fprintf(fp, "erro medio = %f\n", aux/np);
 fclose(fp);
}

static void init(void)
{ 
 GLubyte *Image;
 wtext = pow(2, ceil((log(IMAGE_W)/log(2))));
 htext = pow(2, ceil((log(IMAGE_H)/log(2))));
 Image = (GLubyte *)malloc(wtext*htext*3);
 glDisable(GL_DEPTH_TEST);
 glDisable(GL_BLEND);
 glShadeModel(GL_FLAT);
 glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
 glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
 glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, wtext, htext, 0, GL_RGB, GL_UNSIGNED_BYTE, Image);
 glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);
 glEnable(GL_TEXTURE_2D);
 free(Image);
}

static void display(void)
{
 int i;
 int viewport[4];
 float sx = ((float)IMAGE_W)/((float)wtext);
 float sy = ((float)IMAGE_H)/((float)htext);
 glGetIntegerv(GL_VIEWPORT, viewport);
 glEnable(GL_TEXTURE_2D);
 glDrawBuffer(GL_BACK);
 glBegin(GL_QUADS);
  glTexCoord2f(0.0, 0.0); glVertex3i(          0,           0, 0);
  glTexCoord2f( sx, 0.0); glVertex3i(viewport[2],           0, 0);
  glTexCoord2f( sx,  sy); glVertex3i(viewport[2], viewport[3], 0);
  glTexCoord2f(0.0,  sy); glVertex3i(          0, viewport[3], 0);
 glEnd();

 glDisable(GL_TEXTURE_2D);
 glMatrixMode (GL_PROJECTION);
 glPushMatrix();
 glLoadIdentity ();
 glLoadMatrixd (mtsaiopengl_projection);
 glMatrixMode (GL_MODELVIEW);
 glPushMatrix();
 glLoadIdentity ();
 glLoadMatrixd (mtsaiopengl_modelview);

 glColor3d(1.0, 0.0, 0.0);
 glPointSize(5.0);
 glBegin(GL_POINTS);
#if 0
  for (i=0; i<np; i++)
   glVertex3d(xw[i], yw[i], 0.0);
#else
  for (i=0; i<NPTSX*NPTSY; i++)
   glVertex3d(xwn[i], ywn[i], 0.0);
#endif
  glVertex3d(105, 30.34, 2.44);
  glVertex3d(105, 37.66, 2.44);
 glEnd();

 glutSwapBuffers();

 glMatrixMode (GL_PROJECTION);
 glPopMatrix();
 glMatrixMode (GL_MODELVIEW);
 glPopMatrix();
}

static void reshape(int w, int h)
{
 glViewport(0, 0, (GLsizei)w, (GLsizei)h);
 glMatrixMode(GL_PROJECTION);
 glLoadIdentity();
 gluOrtho2D (0.0, w, 0.0, h);
 glMatrixMode(GL_MODELVIEW);
 glLoadIdentity();
}

int main(int argc, char** argv)
{
 load_data("anim_a0001.ref", &np, &xw, &yw, &xf, &yf);
#if 0
 tsaiCalibration2D(np, xw, yw, xf, yf, &f, T, r);
 tsaiCalibration2D_Tr(np, xw, yw, xf, yf, f, Tb, rb);
#else
 tsaiBuildNewData(np, xw, yw, xf, yf, NPTSX, NPTSY, xwn, ywn, xfn, yfn);
 tsaiCalibration2D(NPTSX*NPTSY, xwn, ywn, xfn, yfn, &f, T, r);
 tsaiCalibration2D_Tr(NPTSX*NPTSY, xwn, ywn, xfn, yfn, f, T, r);
#endif
 save_result("tsai.saida", NPTSX*NPTSY, xwn, ywn, xfn, yfn, f, T, r);
 tsaiOGLModelViewMatrix(T, r, mtsaiopengl_modelview);
 tsaiOGLProjectionMatrix(f, NEARPLANE, FARPLANE, IMAGE_W, IMAGE_H, mtsaiopengl_projection);

 glutInit(&argc, argv);
 glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
 glutInitWindowSize(IMAGE_W, IMAGE_H);
 glutCreateWindow("TSAI");
 init();
 load_image("anim_a0001.jpg", image);
 glutDisplayFunc(display);
 glutReshapeFunc(reshape);
 glutIdleFunc(display);
 glutMainLoop();
 return 0;
}*/