/* Image Processing and Recognition Module for the AR application. */
/* It should not contain interface code.                           */

#ifdef WIN32            
#include <windows.h>    /* necessary because of the Microsoft OpenGL headers dependency */
#endif
#include <GL/gl.h>
#include <GL/glu.h>

#include "jv.h"
#include "math.h"
#include "model.h"
#include "funcoes_reta.h"
#include "tsai.h"
#include "jipe.h"
#include "funcoes_desenho.h"
#include "stdio.h"


/* Temporary buffers  */
static unsigned char *tmpBufferA = NULL ;
static unsigned char *tmpBufferB = NULL ;
static unsigned char *tmpBufferC = NULL ;

/* Model */
Model  model ;

/* Lines and Points used in Process */
#define MAX_DETECTABLE_LINES 256

static jvLine linesInLeftImage[ MAX_DETECTABLE_LINES ] ;
static jvLine linesInLeftImage_interpreted[ MAX_MODEL_LINES ] ;
static jvLine linesInLeftImage_expanded   [ MAX_MODEL_LINES ] ;
static jvLine linesInLeftImage_corrected  [ MAX_MODEL_LINES ] ;
static jvLine linesInLeftImage_ideais     [ MAX_MODEL_LINES ] ;

static jvLine linesInRightImage[ MAX_DETECTABLE_LINES ] ;

static nLinesInLeftImage  = 0 ;
static nLinesInRightImage = 0 ;

static jvPoint2D pnts_model[MAXPTS] ;
static jvPoint2D pnts_image[MAXPTS] ;
static jvPoint2D pnts_corrected[MAXPTS] ;
static jvPoint2D pnts_built_mdl[MAXPTS] ;
static jvPoint2D pnts_built_img[MAXPTS] ;


/* Flags */
#define FLAG_NADA 0 
#define FLAG_DETECT 1 
#define FLAG_INTERPRET 2 
#define FLAG_CALC_HOMOG 3 
#define FLAG_DRAW_3D 4  
#define FLAG_INV_GRAY 5
#define FLAG_LOG 6
#define FLAG_IDEAL_INTERPRET 7
#define FLAG_IDEAL_3D 8

int tem_interpretacao = 0 ;
int FLAG_SUP = 0 ;
int FLAG_INF = 0 ;

char *TEXT[] = { "FI: -----" , 
                 "FII: Edge Detection",
                 "FIII: Interpretation",
                 "FIV: Homog & Tsai Points",
                 "FV: 3D model",
                 "FA: Inv of Gray Img",
                 "FB: Log of Gaussian",
                 "FB: Ideal Interpretation",
                 "FB: 3D Model with GLU"
};



/* for Homog and tsai */
int w_window ;
int h_window ;
float transf_matrix[9] ;
static int n_pairs ;
static int n_pnts_correct ;
float tsai_projection[16] ;
float tsai_modelview[16] ;
float cam_x, cam_y, cam_z ;


/* for interpretation */
int avalia_linhas( float* prev_diff, jvLine *interpretation ) ;
int avalia_interpretacao( void *userdata, int nlines_img, jvLine* lines_img, int nlines_mdl, jvLine* lines_mdl, unsigned char* current_interpretation ) ;



/*---------------------------------------------------------------------------*/
// userOpen 
/*---------------------------------------------------------------------------*/
void userOpen(int width, int height, char *model_file_name)
{
   if (tmpBufferA==NULL) tmpBufferA=(unsigned char *) malloc(width*height);
   if (tmpBufferB==NULL) tmpBufferB=(unsigned char *) malloc(width*height);

   w_window = width ;
   h_window = height ;



   jvInit(width,height,10,10,model_file_name ) ;
   glEnable(GL_POINT_SMOOTH);
	glEnable(GL_COLOR_MATERIAL);
	glDisable(GL_CULL_FACE);
	glEnable(GL_DEPTH_TEST);
   model = create_model( model_file_name ) ;
   create_img_ideal_interpretation( model, linesInLeftImage_ideais, (float)w_window, (float)h_window ) ;
}

/*---------------------------------------------------------------------------*/
// userClose 
/*---------------------------------------------------------------------------*/
void userClose( )
{
   if (tmpBufferA) free(tmpBufferA);
   if (tmpBufferB) free(tmpBufferB);
   jvClose( );
}

/*---------------------------------------------------------------------------*/
// userProcessLeft 
/*---------------------------------------------------------------------------*/
void userProcessLeft(int width, int height, unsigned char* inFrame, int* flags, float* values, unsigned char* outFrame, char *label )
{
   int k,x,y ;
   unsigned char *outBuffer ;

   if( flags[0] )
      FLAG_SUP = FLAG_NADA ;
   else
   if ( flags[1] )
      FLAG_SUP = FLAG_DETECT ;
   else
   if ( flags[2] )
      FLAG_SUP = FLAG_INTERPRET ;
   else
   if ( flags[3] )
      FLAG_SUP = FLAG_CALC_HOMOG ;
   else
   if ( flags[4] )
      FLAG_SUP = FLAG_DRAW_3D ;

   if( flags[5] )
      FLAG_INF = FLAG_INV_GRAY ;
   else if( flags[6] )
      FLAG_INF = FLAG_LOG ;



   inverseOfGrayImage( height, width, inFrame, tmpBufferA )  ;
   outBuffer = tmpBufferA ;

   if( FLAG_SUP == FLAG_NADA )
   {    
      tem_interpretacao = 0 ;
   }

   if( FLAG_SUP > FLAG_NADA )
   {    
      jvLoGFilter( tmpBufferA, (int)(1+49*values[0]), tmpBufferB ) ;
   }

   if( FLAG_SUP == FLAG_DETECT || !tem_interpretacao )
   {
      nLinesInLeftImage = jvEdgeDetect( tmpBufferB, 1+49*values[1], linesInLeftImage, 1 ) ;
      tem_interpretacao = 0 ;
   }

   if( FLAG_SUP >= FLAG_INTERPRET )
   {  
      int l, nvalid=0 ;
      if( !tem_interpretacao )
      { 
         float DIFF = 3.402823466e+38F ;
         jvInterpret( &DIFF, nLinesInLeftImage, linesInLeftImage, 1, 1, avalia_interpretacao ) ;
         n_pairs = generatePairs( linesInLeftImage_interpreted, model, pnts_image, pnts_model ) ;
         printf( "\nINTERPRETACAO\t" ) ;
         tem_interpretacao = 1 ;
      }
      else
      {	
         jvAdjust( tmpBufferB, model->n_lines, linesInLeftImage_interpreted, 4 ) ;
         printf( "ajustando\t") ;
      }

      for( l=0 ; l<model->n_lines ; l++)
      {
         if( linesInLeftImage_interpreted[l].valid )
            nvalid++ ;
      }
      n_pairs = generatePairs( linesInLeftImage_interpreted, model, pnts_image, pnts_model ) ;
      if( n_pairs < 6 )
      {
         tem_interpretacao = 0 ;
         printf( "fudeu pq: pares = %d", n_pairs);
      }

      printf( "linhas: %d\tpares de pontos: %d\n", nvalid, n_pairs ) ;
      if( flags[6])
         FLAG_INF = FLAG_IDEAL_INTERPRET ;
   }

   if( FLAG_SUP >= FLAG_CALC_HOMOG && tem_interpretacao )
   { 
      float T[16], R[16], F ;
      float mdl_xs[MAXPTS],  mdl_ys[MAXPTS],  img_xs[MAXPTS],  img_ys[MAXPTS] ;

      jvFindTransf( transf_matrix, n_pairs, pnts_model, pnts_image ) ;

      correctLines( transf_matrix, model, linesInLeftImage_corrected ) ;
      n_pnts_correct = correctPoints( transf_matrix, model, pnts_corrected ) ;

      jvBuildNewData( model->min_x, model->min_y, model->max_x, model->max_y, transf_matrix, 7, 9, pnts_built_mdl, pnts_built_img ) ;
      convertPoints( 63, pnts_built_mdl, pnts_built_img, mdl_xs, mdl_ys, img_xs, img_ys, (float)w_window, (float)h_window ) ;

      tsaiCalibration2D( 63,  mdl_xs,  mdl_ys,  img_xs,  img_ys,  &F,  T,  R ) ;
      tsaiCameraPosition( &cam_x,  &cam_y, &cam_z, T, R ) ;

      {
         float far_p = (float)(2*sqrt( (cam_x*cam_x)+(cam_y*cam_y)+(cam_z*cam_z) )) ;
         tsaiOGLProjectionMatrix( F,  100.0f,  far_p,  w_window,  h_window,  tsai_projection ) ;
         tsaiOGLModelViewMatrix( T,  R,  tsai_modelview) ;
      }
      if( flags[6])
         FLAG_INF = FLAG_LOG ;
   }


  if( flags[4] && flags[6])
         FLAG_INF = FLAG_IDEAL_3D ;
   
// DEFININDO AS IMAGENS DE SAIDA
// imagem cinza invertida é o padrao
   outBuffer = tmpBufferA ;

   if( flags[5] )
   {
      outBuffer = tmpBufferA ; 
   k=0;
   for(y=0;y<height;y++)
      for(x=0;x<width;x++)
      {
         outFrame[k]=outFrame[k+1]=outFrame[k+2]=outBuffer[y*width+x];
         k+=3;
      }

   }
   if( flags[6] && (!flags[0]) ) 
   {   // L of G
      outBuffer = tmpBufferB ;
   k=0;
   for(y=0;y<height;y++)
      for(x=0;x<width;x++)
      {
         outFrame[k]=outFrame[k+1]=outFrame[k+2]= outBuffer[y*width+x];// 16 * (unsigned char)(sqrt( outBuffer[y*width+x]) ) ;
         k+=3;
      }

   }
      sprintf( label, "%s , %s   - Values:  %4.2f   %4.2f", TEXT[ FLAG_SUP], TEXT[FLAG_INF], 1+49*values[0], 1+49*values[1]  ) ;
 
}

/*---------------------------------------------------------------------------*/
// userProcessRight 
/*---------------------------------------------------------------------------*/
void userProcessRight(int width, int height, unsigned char* inFrame, int* flags, float* values, unsigned char* outFrame)
{
   int x,y,k; 
   unsigned char *outBuffer=tmpBufferA;
   k=0;
   nLinesInRightImage=0;
   for(y=0;y<height;y++)
      for(x=0;x<width;x++)
      {
         tmpBufferA[y*width+x]= 255-((unsigned char) (0.2126f*inFrame[k]+0.7152f*inFrame[k+1]+0.0722f*inFrame[k+2]));
         k+=3;
      }
      if (flags[0]) {
         jvLoGFilter(tmpBufferA,230,tmpBufferB);
         nLinesInRightImage=jvEdgeDetect(tmpBufferB,20,linesInRightImage, 1);
         outBuffer=tmpBufferB;
      }
      k=0;
      for(y=0;y<height;y++)
         for(x=0;x<width;x++) {
            outFrame[k]=outFrame[k+1]=outFrame[k+2]=outBuffer[y*width+x];
            k+=3;
         }
}

/*---------------------------------------------------------------------------*/
// userDrawLeft
/*---------------------------------------------------------------------------*/
void userDrawUpperLeft(int width, int height)
{
   if( FLAG_SUP == FLAG_DETECT )
   {
       desenha_linhas_detectadas( linesInLeftImage, nLinesInLeftImage ); 
   }
   else
   if( FLAG_SUP == FLAG_INTERPRET && tem_interpretacao )
   {
      desenha_linhas_interpretadas( linesInLeftImage_interpreted, model->n_lines ); 
   }
   else
   if( FLAG_SUP == FLAG_CALC_HOMOG && tem_interpretacao )
   {
      desenha_pontos( pnts_image, n_pairs, pnts_corrected, n_pnts_correct, pnts_built_img, 63 ) ;
   }
   else
   if( FLAG_SUP == FLAG_DRAW_3D && tem_interpretacao )
   {      float cor[3] ={ .7f, .0f, 0 } ;
      float pos[3] ;
      pos[0] = cam_x ;
      pos[1] = cam_y ;
      pos[2] = cam_z ;



      desenha_linhas_interpretadas( linesInLeftImage_interpreted, model->n_lines ); 
    //  desenha_pontos( pnts_image, n_pairs, pnts_corrected, n_pnts_correct, pnts_built_img, 63 ) ;

      glEnable   ( GL_DEPTH_TEST ) ;
      glDepthFunc( GL_LEQUAL ) ;
      glClear    ( GL_DEPTH_BUFFER_BIT ) ;
	   glDisable  ( GL_CULL_FACE ) ;
	   glEnable   ( GL_LIGHTING ) ;
	   glEnable   ( GL_LIGHT0   ) ;
      glLightfv  ( GL_LIGHT0, GL_COLOR, cor ) ;
      glEnable   ( GL_COLOR_MATERIAL ) ;

      glMatrixMode( GL_PROJECTION ) ;
      glPushMatrix( ) ;
      glLoadMatrixf( tsai_projection ) ;

      glMatrixMode( GL_MODELVIEW ) ;
      glPushMatrix( ) ;
      glLoadMatrixf( tsai_modelview ) ;

      glShadeModel( GL_SMOOTH ) ; 

      glTranslatef( 136.15f,66,0 ) ;
      desenha_jipe() ;

      glShadeModel( GL_FLAT ); 
      glMatrixMode( GL_MODELVIEW ) ;
      glPopMatrix( ) ;
      glMatrixMode( GL_PROJECTION ) ;
      glPopMatrix( ) ;

  	   glDisable  ( GL_COLOR_MATERIAL ) ;
  	   glDisable  ( GL_LIGHTING ) ;
   }
}
/*---------------------------------------------------------------------------*/
void userDrawLowerLeft(int width, int height)
{
   if( FLAG_INF == FLAG_IDEAL_INTERPRET )
   {
      glClear( GL_COLOR_BUFFER_BIT ) ;
      desenha_linhas_interpretadas( linesInLeftImage_ideais, model->n_lines ); 
   }
   if( FLAG_INF == FLAG_IDEAL_3D )
   {
      float pos[3] ;
      float cor[3] ={ .7f, .0f, 0 } ;
      pos[2] = 250 ;

      if( cam_x > 0 )
         pos[0] = 300 ;
      else if( cam_x < 0 )
         pos[0] = -300;
      else
         pos[0] = 0 ;

      if( cam_y > 0 )
         pos[1] = 300 ;
      else if( cam_y < 0 )
         pos[1] = -300;
      else
         pos[1] = 0 ;


      glEnable   ( GL_DEPTH_TEST ) ;
      glDepthFunc( GL_LEQUAL ) ;
      glClear    ( GL_DEPTH_BUFFER_BIT ) ;
      glClear    ( GL_COLOR_BUFFER_BIT ) ;
      glDisable  ( GL_CULL_FACE ) ;
      glEnable   ( GL_LIGHTING ) ;
      glEnable   ( GL_LIGHT0   ) ;
      glLightfv  ( GL_LIGHT0, GL_COLOR, cor ) ;
      glEnable   ( GL_COLOR_MATERIAL ) ;

      glMatrixMode( GL_PROJECTION ) ;
      glPushMatrix( ) ;
      glLoadIdentity();
      gluPerspective( 40, ((float)w_window)/((float)h_window), 10, 2000 ) ;

      glMatrixMode( GL_MODELVIEW ) ;
      glPushMatrix( ) ;
      glLoadIdentity();
      gluLookAt( pos[0], pos[1], pos[2], 0,0,0, 0,0,1 ) ;

      glShadeModel( GL_SMOOTH ) ; 
      
      glTranslatef( -84.49f,-0.14f,0 ) ;
      desenha_jipe() ;

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

      glDisable  ( GL_COLOR_MATERIAL ) ;
      glDisable  ( GL_LIGHTING ) ;
   }

}
/*---------------------------------------------------------------------------*/
// userDrawRight
/*---------------------------------------------------------------------------*/
void userDrawRight(int width, int height)
{
   int n;
   glColor3f(1.0,0.0,0.0);
   glBegin(GL_LINES);
   for(n=0;n<nLinesInRightImage;n++){
      glVertex2f(linesInRightImage[n].v1x,linesInRightImage[n].v1y);
      glVertex2f(linesInRightImage[n].v2x,linesInRightImage[n].v2y);
   }
   glEnd();
   glFlush();
}






int avalia_interpretacao(
                         void *userdata,
                         int nlines_img,
                         jvLine* lines_img,
                         int nlines_mdl,
                         jvLine* lines_mdl,
                         unsigned char* current_interpretation )
{

   jvLine interpretation[ MAX_MODEL_LINES ] ;
   int l, i ;
   int n_lin = 0 ;

   // Faz uma copia ordenada de lines_in_left_image
   for( i=0 ; i<nlines_mdl; i++ )
   {
      interpretation[i].valid = 0 ;
   }
   for( i=0 ; i<nlines_img; i++ )
   {
      l = current_interpretation[i] ;
      if( l!= 255 )
      {
         n_lin++ ;
         interpretation[l].v1x = lines_img[i].v1x ;
         interpretation[l].v1y = lines_img[i].v1y ;
         interpretation[l].v2x = lines_img[i].v2x ;
         interpretation[l].v2y = lines_img[i].v2y ;
         interpretation[l].valid = 1 ;
      }
   }
   if( n_lin < 5 )
      return 0 ;

   return avalia_linhas( (float*)userdata, interpretation ) ;
}

int avalia_linhas( float* prev_diff, jvLine *interpretation )
{
   jvLine mdl_proj[ MAX_MODEL_LINES ] ;
   float t_matrix[9] ;
   jvPoint2D p_mdl_proj[100];
   float sqr_diff = 0 ;
   int np ;
   int i ;
   jvPoint2D p_inter[100], p_mdl[100] ;

// CALCULA HOMOGRAFIA
   {   
      np = generatePairs ( interpretation, model, p_inter, p_mdl ) ;
      if( np < 8 )
         return 0 ;

      jvFindTransf( t_matrix, np, p_mdl, p_inter ) ;
      correctLines( t_matrix, model, mdl_proj  ) ;
      correctPoints2( t_matrix, np, p_mdl, p_mdl_proj  ) ;
   }
// Calcula a Homografia e erro minimo quadratico
   {
      for( i=0 ; i<np ; i++ )
      {
         float dx = p_inter[i].x - p_mdl_proj[i].x ;
         float dy = p_inter[i].y - p_mdl_proj[i].y ;
         sqr_diff += dx*dx + dy*dy ;
      }
      sqr_diff /= (float)(np) ;
   }
// ACHA DIAGONAIS, PONTOS E DIFF
   {
      jvLine  dig_1, dig_2,dig_3,dig_4,dig_5,dig_6,dig_7,dig_8;
      float X, Y, XA, YA, XB, YB, XC, YC, XD, YD ;

      intersecao( mdl_proj[0], mdl_proj[5], &X, &Y ) ;
      dig_1.v1x = X ;  dig_1.v1y = Y ;
      dig_3.v1x = X ;  dig_3.v1y = Y ;

      intersecao( mdl_proj[0], mdl_proj[1], &X, &Y ) ;
      dig_2.v1x = X ;  dig_2.v1y = Y ;
      dig_4.v1x = X ;  dig_4.v1y = Y ;

      intersecao( mdl_proj[2], mdl_proj[5], &X, &Y ) ;
      dig_5.v1x = X ;  dig_5.v1y = Y ;
      dig_7.v1x = X ;  dig_7.v1y = Y ;

      intersecao( mdl_proj[2], mdl_proj[1], &X, &Y ) ;
      dig_6.v1x = X ;  dig_6.v1y = Y ;
      dig_6.v1x = X ;  dig_6.v1y = Y ;

      intersecao( mdl_proj[4], mdl_proj[5], &X, &Y ) ;
      dig_4.v2x = X ;  dig_4.v2y = Y ;
      dig_8.v2x = X ;  dig_8.v2y = Y ;

      intersecao( mdl_proj[4], mdl_proj[1], &X, &Y ) ;
      dig_3.v2x = X ;  dig_3.v2y = Y ;
      dig_7.v2x = X ;  dig_7.v2y = Y ;

      intersecao( mdl_proj[6], mdl_proj[5], &X, &Y ) ;
      dig_2.v2x = X ;  dig_2.v2y = Y ;
      dig_6.v2x = X ;  dig_6.v2y = Y ;

      intersecao( mdl_proj[6], mdl_proj[1], &X, &Y ) ;
      dig_1.v2x = X ;  dig_1.v2y = Y ;
      dig_5.v2x = X ;  dig_5.v2y = Y ;


      intersecao( dig_1, dig_2, &XA, &YA ) ;
      intersecao( dig_3, dig_4, &XB, &YB ) ;
      intersecao( dig_5, dig_6, &XC, &YC ) ;
      intersecao( dig_7, dig_8, &XD, &YD ) ;

      {
         float dx, dy ;

         dx = XA - XD ;
         dy = YA - YD ;
         sqr_diff += (float)sqrt(dx*dx + dy*dy) ;

         sqr_diff += distancia_ponto( model->lines[2], XB, YB) ;
         sqr_diff += distancia_ponto( model->lines[4], XC, YC) ;
      }
   }
   printf( "%f\t",  sqr_diff);

// se é menor q a interpretacao anterior aceita
   if( sqr_diff < (*prev_diff) )
   {

      for( i=0 ; i<model->n_lines; i++ )
      {
         linesInLeftImage_interpreted[i].v1x = interpretation[i].v1x ;
         linesInLeftImage_interpreted[i].v1y = interpretation[i].v1y ;
         linesInLeftImage_interpreted[i].v2x = interpretation[i].v2x ;
         linesInLeftImage_interpreted[i].v2y = interpretation[i].v2y ;
         linesInLeftImage_interpreted[i].valid = interpretation[i].valid ;
      }
      *prev_diff  = sqr_diff ;
   }

  return 0 ;
}