#include "algebra.h"

void matriz_r ( float *r9, Matriz r16 )
{
   r16.m.m
   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 ;
}

void matriz_k ( float *eye, float w, float h, float f, Matriz k16 )
{
   k16[0] =  -f ;  k16[4] =  0 ;  k16[8]  = w/2 ;  k16[12] = 0.00f ;
   k16[1] =   0 ;  k16[5] = -f ;  k16[9]  = h/2 ;  k16[13] = 0.00f ;
   k16[2] =   0 ;  k16[6] =  0 ;  k16[10] = 1   ;  k16[14] = 0.00f ;
   k16[3] =  0.00f ;  k16[7] =  0.00f ;  k16[11] =  0.00f ;  k16[15] = 1.00f ;
}


void main()
{

}


void getCorrespondent ( int n_trans, std::vector<float> *left_trans, std::vector<float> *right_trans, float * fundamental )
{
   int t, p, s ;

   for( t=0 ; t<n_trans ; t++ )
   {
      s = 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] ;

         getCorrespondent ( x, y, t, fundamental, right_trans[t] ) ;
      }
   }
}


int getCorrespondent ( int x_left, int y_left, int transition, float * Fundamental, std::vector<float> * right_trans , float *x_r, float*y_r)
{
   void stereoEpipolarLineEquation(double x, double y, Matrix F, double* a, double *b, double *c) 
   int t, p, s ;

   int min_d , int p_min = -1 ;

      s = right_trans[t].size() ;

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

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

         if( dist <= min || p=0 )
         {
            min_d = dist ;
            min_p = p ;
         }
      }
   }
   if( p_min != -1 )
   {
   *x_r = right_trans[p_min] ;
   *y_r = right_trans[p_min+1] ;
   }
  return p_min ;
}

