/** MAIN *******************************************************************/
/*                                                                         */
/*  SOURCE1.CPP                                                            */
/*                                                                         */
/***************************************************************************/

/** INCLUDES ***************************************************************/
//- C/C++ -------------------------------------------------------------------
#include "stdlib.h"
#include "stdio.h"
#include "vector"
#include "direct.h"
//- TECGRAF -----------------------------------------------------------------
#include "im.h"
#include "im_image.h"
//- PROJECT -----------------------------------------------------------------
#include "img_striper.h"
#include "algebra.h"
#include "correpondence.h"
#include "camera_position.h"
#include "stereo.h"
#include "structured_color.h"
//- EXTRA -------------------------------------------------------------------
#include "string_usefull.h"

#define img_E_42 "Photo Data\\cano_amassado_vista_2\\4_2\\" 
#define img_D_42 "Photo Data\\cano_amassado_vista_2\\4_2\\" 


imImage * processPictures( int base,  char **nomes ) ;

char *nomes_esq[] = {   "E_V_P (1).JPG",
                        "E_V_P (2).JPG",
                        "E_V_N (1).JPG",
                        "E_V_N (2).JPG",
                        "E_V_a_Discrete (1).BMP",
                        "E_V_b_Valid (1).BMP",
                        "E_V_c_T_Candidate (1).BMP",
                        "E_V_a_Discrete (2).BMP",
                        "E_V_b_Valid (2).BMP",
                        "E_V_c_T_Candidate (2).BMP",
                        "E_V_Transitions.BMP"
                     } ;

char *nomes_dir[] = {   "D_V_P (1).JPG",
                        "D_V_P (2).JPG",
                        "D_V_N (1).JPG",
                        "D_V_N (2).JPG",
                        "D_V_a_Discrete (1).BMP",
                        "D_V_b_Valid (1).BMP",
                        "D_V_c_T_Candidate (1).BMP",
                        "D_V_a_Discrete (2).BMP",
                        "D_V_b_Valid (2).BMP",
                        "D_V_c_T_Candidate (2).BMP",
                        "D_V_Transitions.BMP"
                     } ;


#define VP_1      0
#define VP_2      1
#define VN_1      2
#define VN_2      3
#define V_DISC_1  4
#define V_VALD_1  5
#define V_CAND_1  6
#define V_DISC_2  7
#define V_VALD_2  8
#define V_CAND_2  9
#define V_TRANST 10



/* Lines and Points used in Process */


static float cam_esquerda[7][4] = 
{
   {  1213 ,  443 ,  1054 , 1560 } ,
   {  1276 ,  402 ,  2938 ,  650 } ,
   {  1814 ,  509 ,  1761 ,  889 } ,
   {  1808 ,  945 ,  2876 , 1103 } ,
   {  2403 ,  601 ,  2343 , 1000 } ,
   {  1102 , 1614 ,  2760 , 1834 } ,
   {  2914 , 1152 ,  2815 , 1787 } 
};

static float cam_direita[7][4] = 
{
   {   631 ,  502 ,   648 , 1356 },
   {   664 ,  462 ,  1784 ,  488 },
   {   996 ,  494 ,   999 ,  795 },
   {  1031 ,  837 ,  1785 ,  887 },
   {  1389 ,  504 ,  1391 ,  837 },
   {   682 , 1395 ,  1768 , 1525 },
   {  1816 ,  915 ,  1812 , 1485 }
};



int main ( int argc, char ** argv )
{
   if( argc < 4 )
   {
      printf( "\n\t3D_Scanner [base] [dir_fotos] [arq_pontos]\n\n" );
      return 0 ;
   }

   int  error ;
   int  base = atoi( argv[1] ) ;

   float T_Esq[3], R_Esq[9], T_Dir[3], R_Dir[9], MV_Esq[16], MV_Dir[16] ;
   float pos_E[3], pos_D[3], f_E, f_D ;

   printf( "Camera Direita\n") ;
   f_D = 7509.3333333333f ;
   imImage *img_dir = imFileImageLoad ( "Photo Data\\D_Calib.jpg", 0, &error ) ;
   cameraPosition( img_dir, cam_direita, pos_D, &f_D, R_Dir, T_Dir, MV_Dir ) ;
   imFileImageSave( "Photo Data\\D_Calib_pnts.jpg", "JPEG", img_dir ) ;
   imImageDestroy ( img_dir ) ;

   printf( "Completo\n\nCamera Esquerda\n") ;
   f_E = 8448.000f ;
   imImage *img_esq = imFileImageLoad ( "Photo Data\\E_Calib.jpg", 0, &error ) ;
   cameraPosition( img_esq,cam_esquerda, pos_E, &f_E, R_Esq, T_Esq, MV_Esq ) ;
   imFileImageSave( "Photo Data\\E_Calib_pnts.jpg", "JPEG", img_esq ) ;
   imImageDestroy ( img_esq ) ;
   printf( "Completo\n\n") ;


   _chdir( argv[2] ) ;

   printf( "Transicoes Direita..." ) ;
   img_dir = processPictures( base, nomes_dir ) ;
   printf( "...completo\nTransicoes Esquerda...");
   img_esq = processPictures( base, nomes_esq ) ;
   printf( "...completo\n\n" );

   // O  ' +2 ' é pelas bordas, transicao q vai do nada pra 1a faixa e da ultima faixa pro nada
   std::vector<int> * table_esq = new std::vector<int>[stC_MAX_STRIPES[base]+2] ;
   std::vector<int> * table_dir = new std::vector<int>[stC_MAX_STRIPES[base]+2] ;

   printf( "Tabela Esquerda\n") ;
   generateTransTable( table_esq, img_esq, stC_MAX_STRIPES[base]+2 ) ;
   printf( "Completo\n\nTabela Direita\n") ;
   generateTransTable( table_dir, img_dir, stC_MAX_STRIPES[base]+2 ) ;
   printf( "Completo\n\n") ;

   int w_d = (unsigned int) img_dir->width  ;
   int h_d = (unsigned int) img_dir->height ;
   int w_e = (unsigned int) img_esq->width  ;
   int h_e = (unsigned int) img_esq->height ;

   Vector pos_esq = algVector( pos_E[0], pos_E[1], pos_E[2], 1 ) ;
   Vector pos_dir = algVector( pos_D[0], pos_D[1], pos_D[2], 1 ) ;

   Matrix R_esq_epipolar = matriz_r ( R_Esq ) ;
   Matrix R_dir_epipolar = matriz_r ( R_Dir ) ;
   Matrix K_esq_epipolar = matriz_k ( pos_E, (float)w_e, (float)h_e, f_E ) ;
   Matrix K_dir_epipolar = matriz_k ( pos_D, (float)w_d, (float)h_d, f_D ) ;


   printf( "Reconstruindo...") ;

   Matrix fund = stereoFundamentalMatrix( K_esq_epipolar, R_esq_epipolar, pos_esq,  K_dir_epipolar, R_dir_epipolar, pos_dir) ;

   std::vector<int> *corresp = new std::vector<int>[stC_MAX_STRIPES[base]+2] ;

   getCorrespondent (  stC_MAX_STRIPES[base]+2, table_esq, table_dir, fund, corresp ) ;


   Vector ponto ;
   FILE *file  = fopen( argv[3], "wt" ) ;
   int p ;
   int erro = 1 ;
   int t ;
   int s ;

   for( t=0 ; t< (stC_MAX_STRIPES[base]+2) ; t++ )
   {
      s = (int)corresp[t].size() ;

      for( p=0 ; p<s ; p+=4 )
      {
         double xa = (double) corresp[t][p] ;
         double ya = (double) corresp[t][p+1] ;
         double xb = (double) corresp[t][p+2] ;
         double yb = (double) corresp[t][p+3] ;
         ponto =  stereoReconstruct( xa, ya, f_E, R_esq_epipolar, pos_esq, (double)(w_e/2), (double)(h_e/2),
                                     xb, yb, f_D, R_dir_epipolar, pos_dir, (double)(w_d/2), (double)(h_d/2),
                                    &erro );
         if( erro == 1 )
         {
            fprintf( file, "%.3f %.3f %.3f\n", ponto.x, ponto.y, ponto.z ) ;
         }
         else
         {
          //  printf( "ERRO %d %d == %d %d\n", corresp[t][p],corresp[t][p+1], corresp[t][p+2], corresp[t][p+3] ) ;
         }
      }
   }
   fclose(file);


   printf( "...completo\n\n\n\n" ) ;



   delete[] corresp ;
   delete[] table_esq ;
   delete[] table_dir ;
}



imImage *processPictures( int base,  char ** nomes )
{
   int error ;

   imImage *img_positive      ;
   imImage *img_negative      ;
   imImage *img_discrete      ;
   imImage *img_valid         ;
   imImage *img_tran_cand_1   ;
   imImage *img_tran_cand_2   ;
   imImage *img_transition_V  ;

   unsigned int width  = 0 ;
   unsigned int height = 0 ;


// FIRST PAIR OF PICTURES ------------------------------------------

   img_positive = imFileImageLoadBitmap ( nomes[ VP_1 ], 0, &error ) ;
   img_negative = imFileImageLoadBitmap ( nomes[ VN_1 ], 0, &error ) ;

   width  = (unsigned int)( ( img_positive->width  == img_negative->width  ) ? img_positive->width  : 0 ) ;
   height = (unsigned int)( ( img_positive->height == img_negative->height ) ? img_positive->height : 0 ) ;

   if ( !(width*height) )
   {
      imImageDestroy ( img_positive ) ;
      imImageDestroy ( img_negative ) ;
      return 0 ;
   }
   img_discrete      = imImageCreate ( width, height, IM_RGB, IM_BYTE ) ;
   img_valid         = imImageCreate ( width, height, IM_RGB, IM_BYTE ) ;
   img_tran_cand_1   = imImageCreate ( width, height, IM_RGB, IM_BYTE ) ;
   img_tran_cand_2   = imImageCreate ( width, height, IM_RGB, IM_BYTE ) ;
   img_transition_V  = imImageCreate ( width, height, IM_RGB, IM_BYTE ) ;


   ImgS_Discretize ( img_positive, img_negative, img_discrete ) ;
   ImgS_ValidateV  ( base, img_discrete, img_valid, 5 ) ;
   ImgS_TransCandidateV ( base, img_valid, img_tran_cand_1, 7 ) ;

   imFileImageSave ( nomes[ V_DISC_1 ] , "BMP", img_discrete ) ;
   imFileImageSave ( nomes[ V_VALD_1 ] , "BMP", img_valid ) ;
   imFileImageSave ( nomes[ V_CAND_1 ] , "BMP", img_tran_cand_1 ) ;

   imImageDestroy ( img_positive ) ;
   imImageDestroy ( img_negative ) ;


// SECOND PAIR OF PICTURES ------------------------------------------
   img_positive = imFileImageLoadBitmap ( nomes[ VP_2 ], 0, &error ) ;
   img_negative = imFileImageLoadBitmap ( nomes[ VN_2 ], 0, &error ) ;

   width  = (unsigned int)( ( img_positive->width  == img_negative->width  ) ? img_positive->width  : 0 ) ;
   height = (unsigned int)( ( img_positive->height == img_negative->height ) ? img_positive->height : 0 ) ;

   if ( !(width*height) )
   {
      imImageDestroy ( img_positive ) ;
      imImageDestroy ( img_negative ) ;
      imImageDestroy ( img_discrete ) ;
      imImageDestroy ( img_valid    ) ;
      imImageDestroy ( img_tran_cand_1  ) ;
      imImageDestroy ( img_tran_cand_2  ) ;
      imImageDestroy ( img_transition_V ) ;
      return 0 ;
   }

   ImgS_Discretize ( img_positive, img_negative, img_discrete ) ;
   ImgS_ValidateV  ( base, img_discrete, img_valid, 5 ) ;
   ImgS_TransCandidateV ( base, img_valid, img_tran_cand_2, 7 ) ;

   imFileImageSave ( nomes[ V_DISC_2 ] , "BMP", img_discrete ) ;
   imFileImageSave ( nomes[ V_VALD_2 ] , "BMP", img_valid ) ;
   imFileImageSave ( nomes[ V_CAND_2 ] , "BMP", img_tran_cand_2 ) ;

   imImageDestroy ( img_positive ) ;
   imImageDestroy ( img_negative ) ;
   imImageDestroy ( img_discrete ) ;
   imImageDestroy ( img_valid    ) ;

// TRANSITIONS ------------------------------------------

   ImgS_TransitionsV( base, img_tran_cand_1, img_tran_cand_2, img_transition_V, 11, 11 ) ;
   imFileImageSave (  nomes[ V_TRANST ], "BMP", img_transition_V ) ;

   imImageDestroy ( img_tran_cand_1  ) ;
   imImageDestroy ( img_tran_cand_2  ) ;

   return img_transition_V ;
}
