#include "vector"
#include "algebra.h"
#include "math.h"
#include "im.h"
#include "im_image.h"
#include "stereo.h"
#include "stdio.h"

Matrix matriz_r ( float *r9 )
{
   Matrix r16 ;

   r16.m[0] =  r9[0] ;  r16.m[4] =  r9[1] ;  r16.m[8]  = -r9[2] ;  r16.m[12] = 0.00f ;
   r16.m[1] =  r9[3] ;  r16.m[5] =  r9[4] ;  r16.m[9]  = -r9[5] ;  r16.m[13] = 0.00f ;
   r16.m[2] = -r9[6] ;  r16.m[6] = -r9[7] ;  r16.m[10] =  r9[8] ;  r16.m[14] = 0.00f ;
   r16.m[3] =  0.00f ;  r16.m[7] =  0.00f ;  r16.m[11] =  0.00f ;  r16.m[15] = 1.00f ;

   return r16 ;   
}

Matrix matriz_k ( float *eye, float w, float h, float f )
{
   Matrix k16 ;

   k16.m[0] = -f ;  k16.m[4] =  0 ;  k16.m[8]  = w/2  ;  k16.m[12] = 0;
   k16.m[1] =  0 ;  k16.m[5] = -f ;  k16.m[9]  = h/2 ;  k16.m[13] = 0 ;
   k16.m[2] =  0 ;  k16.m[6] =  0 ;  k16.m[10] =  1  ;  k16.m[14] = 1 ;
   k16.m[3] =  0 ;  k16.m[7] =  0 ;  k16.m[11] =  1  ;  k16.m[15] = 0 ;

   return k16 ;
}


void generateTransTable( std::vector<int> * table, imImage *img_trans, int n_trans )
{
// Inicializing variables
   unsigned int width  = (unsigned int)img_trans->width  ;
   unsigned int height = (unsigned int)img_trans->height ;

   unsigned int dim = width*height ;

   unsigned char *dat = (unsigned char *)(img_trans->data[0]) ;

// if images differ in at least one dimensions, no process is done
   if ( !dim )
      return ;

// processing...
   unsigned int w, h ;

   for ( h=0 ; h<height ; h++ )
   {
      for ( w=0 ; w<width ; w++ )
      {
         int t = (int)dat[ 2*dim + h*width + w ] ;

         if( t > 0 && t < n_trans )
         {
            table[t].push_back( w ) ;
            table[t].push_back( h ) ;
         }
      }
   }
}

int getCorrespondentxy ( int x_left, int y_left, int transition, Matrix Fundamental, std::vector<int> * right_trans , int *x_r, int*y_r, double *dist);

void getCorrespondent ( int n_trans, std::vector<int> *left_trans, std::vector<int> *right_trans, Matrix fundamental, std::vector<int> *correspondence )
{

   int t, p, s ;

   for( t=0 ; t<n_trans ; t++ )
   {
      s = (int)left_trans[t].size() ;

      for( p=0 ; p+1 < s ; p+=2 )
      {
         int x = left_trans[t][p] ;
         int y = left_trans[t][p+1] ;
         int x_r=0, y_r=0;

         double dist = 1000 ;

         getCorrespondentxy ( x, y, t, fundamental, right_trans, &x_r, &y_r, &dist ) ;

         if( dist <= 100 )
         {
            correspondence[t].push_back( x ) ;
            correspondence[t].push_back( y ) ;
            correspondence[t].push_back( x_r ) ;
            correspondence[t].push_back( y_r ) ;
         }
         else
         {
         }
      }
   //   printf( "%4d completa\n", t) ;
   }
}


int getCorrespondentxy ( int x_left, int y_left, int transition, Matrix Fundamental, std::vector<int> * right_trans , int *x_r, int*y_r, double *dist )
{
   double a, b, c ;
   int p, s ;

   stereoEpipolarLineEquation( x_left, y_left, Fundamental, &a, &b, &c)  ;

   double min_d =  0 ;
   int    min_p = -1 ;

   s = (int)right_trans[transition].size() ;

   for( p=0 ; p+1 < s ; p+=2 )
   {
      double x = (double)right_trans[transition][p] ;
      double y = (double)right_trans[transition][p+1] ;

      double dist = fabs( a*x + b*y + c ) ;

      if( dist <= min_d || p==0 )
      {
         min_d = dist ;
         min_p = p ;
      }
   }

   if( min_p != -1 )
   {
      *x_r = right_trans[transition][min_p] ;
      *y_r = right_trans[transition][min_p+1] ;
      *dist = min_d ;
   }
   return min_p ;
}

