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

/* Model */
Model  model ;

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

static jvLine lines[ MAX_DETECTABLE_LINES ] ;
static jvLine lines_interpreted[ MAX_MODEL_LINES ] ;
static jvLine lines_expanded   [ MAX_MODEL_LINES ] ;
static jvLine lines_corrected  [ MAX_MODEL_LINES ] ;

static nLines  = 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] ;

/* for Homog and tsai */
static float transf_matrix[9] ;
static int   n_pairs ;
static int   n_pnts_correct ;



void cameraPosition( imImage *image, float lines_img[7][4], float *pos, float *focus, float *R, float*T, float*MV) 
{
   unsigned char *data = (unsigned char *)image->data[0] ;
   int width  = image->width ;
   int height = image->height;


   unsigned char *invgray = new unsigned char[width*height] ;
   unsigned char *log     = new unsigned char[width*height] ;

   jvInit(width,height,16,16 , "modelo.mdl" ) ;
   model = create_model( "modelo.mdl" ) ;

   int i ;
   for( i=0; i<7 ; i++ )
   {
      lines_interpreted[i].v1x = lines_img[i][0] ;
      lines_interpreted[i].v1y = lines_img[i][1] ;
      lines_interpreted[i].v2x = lines_img[i][2] ;
      lines_interpreted[i].v2y = lines_img[i][3] ;
      lines_interpreted[i].valid = true ;
   }

   int p =0;
   n_pairs = generatePairs( lines_interpreted, model, pnts_image, pnts_model ) ;
   
   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, lines_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)width, (float)height ) ;

   tsaiCalibration2D_Tr( 63,  mdl_xs,  mdl_ys,  img_xs,  img_ys, *focus,  T,  R ) ;
// tsaiCalibration2D   ( 63,  mdl_xs,  mdl_ys,  img_xs,  img_ys,  focus,  T,  R ) ;

   tsaiCameraPosition( &(pos[0]),  &(pos[1]), &(pos[2]), T, R ) ;

   tsaiOGLModelViewMatrix( T, R, MV ) ;

   jvClose() ;

   p=0;
   int  j ;
   while (p<63)
   {
      for( i = (int)(pnts_built_img[p].x)-5 ; i<= (int)(pnts_built_img[p].x)+5 ; i++ )
      {
         for( j = (int)(pnts_built_img[p].y)-5 ; j<= (int)(pnts_built_img[p].y)+5 ; j++ )
         {
            if( j>6 && j< height-6 && i>6 && i < width-6 )
            {
               data[                      j*width + i ] = 255 ;
               data[   (height*width)  +  j*width + i ] = 0 ;
               data[ 2*(height*width)  +  j*width + i ] = 0 ;
            }
         }
      }
      p++;

   }

}