//---------------------------------------------------------------------------

#include <vcl.h>
#pragma hdrstop
#include <iostream>
#include <stdio.h>
#include <string.h>
#include <windows.h>
#include "StereoG.h"
#include "funkce.c"
#include <jpeg.hpp>
#include <conio.h>
#include "sift.h"
#include "sift.cpp"
#include "stdafx.cpp"
#include "stdafx.h"
#include "imgfeatures.cpp"
#include "imgfeatures.h"
#include "utils.h"
#include "utils.cpp"
#include "kdtree.cpp"
#include "kdtree.h"






//---------------------------------------------------------------------------
#pragma package(smart_init)
#pragma link "PERFGRAP"
#pragma resource "*.dfm"
using namespace std;
TForm1 *Form1;
//---------------------------------------------------------------------------
__fastcall TForm1::TForm1(TComponent* Owner)
        : TForm(Owner)
{
}
//---------------------------------------------------------------------------
int Pan;
CvPoint l_sp;
CvPoint r_sp;
int point_count = 50;
int l1; int l2; int r1; int r2;
int i=1;
int j;
int fm_pocet=1;
int xl=0;
int xr=0;
int yl=0;
int yr=0; float alpha = 0.5;
CvMatrix3 FMAT ;
CvMat*l_s = cvCreateMat(1,point_count,CV_32FC2);
CvMat*r_s = cvCreateMat(1,point_count,CV_32FC2);
CvMat*status = cvCreateMat(1,point_count,CV_8UC1);
CvMat*fundamental_matrix = cvCreateMat(3,3,CV_32FC1);

IplImage* img1=cvLoadImage("2L.jpg"); //vytvoreni promenne typu IplImage a prirazeni konkretni
IplImage* img2=cvLoadImage("2R.jpg"); //cesty k souboru
IplImage* imgc1=cvLoadImage("2L.jpg"); //vytvoreni promenne typu IplImage a prirazeni konkretni
IplImage* imgc2=cvLoadImage("2R.jpg");

IplImage* stereo=cvCreateImage(cvSize(img1->width, img1->height),8,3);

IplImage* gimg1 = cvCreateImage(cvSize(img1->width, img1->height), IPL_DEPTH_8U, 1);
IplImage* gimg2 = cvCreateImage(cvSize(img1->width, img1->height), IPL_DEPTH_8U, 1);
IplImage* disp = cvCreateImage(cvSize(img1->width, img1->height), IPL_DEPTH_8U, 1);


 //---------------------------------------
void on_mouseL(int event, int xl, int yl, int flags, void* param)
{
if( event == CV_EVENT_MOUSEMOVE )     //Zobrazovani aktualnich souradnic v realnem case
{
Form1->Panel1->Caption= "Souřadnice X: " + IntToStr(xl) + ", Y: " + IntToStr(yl) ;
}
 if( event == CV_EVENT_LBUTTONDOWN )
    {      //po kliknuti nacteni souradnic bodu a pridani do vstupni matice souradnic pro vypocet Fund. matice

 l_s->data.fl[i*2]=xl;
 l_s->data.fl[i*2+1]=yl;
l_sp.x=xl;    //nacteni souradnic do promenne typu CvPoint, potrebne pro zobrazeni kruznice
l_sp.y=yl;

cvCircle( imgc1, l_sp, 5, CV_RGB(0,255,0), 2, 3, 0);  //vytvoreni kruznice kolem bodu
 cvShowImage("obrazek1",imgc1);   //obnoveni obrazku
  }
}
void on_mouseR(int event, int xr, int yr, int flags, void* param)
{ if( event == CV_EVENT_MOUSEMOVE )   //Zobrazovani aktualnich souradnic v realnem case
{Form1->Panel2->Caption= "Souřadnice X: " + IntToStr(xr) + ", Y: " + IntToStr(yr) ;}

 if( event == CV_EVENT_LBUTTONDOWN )
    { //po kliknuti nacteni souradnic bodu a pridani do vstupni matice souradnic pro vypocet Fund. matice

 r_s->data.fl[i*2]=xr;
 r_s->data.fl[i*2+1]=yr;
//r_sp.x = xr;        //nacteni souradnic do promenne typu CvPoint
 r_sp.x = r_s->data.fl[i*2];
r_sp.y = yr;


cvCircle( imgc2, r_sp, 5, CV_RGB(0,255,0), 2, 3,0);
 cvShowImage("obrazek2",imgc2);  //obnoveni obrazku
  }
}



//---------------------------------------------------------------------------

  //---------------------------------------------------------
 void __fastcall TForm1::FormActivate(TObject *Sender)
{  Form1->Width=246;
    //vytvoreni a ukazani obrazku
cvNamedWindow("obrazek1", CV_WINDOW_AUTOSIZE);     //CV_WINDOW_AUTOSIZE
cvMoveWindow("obrazek1",20,100);
cvShowImage("obrazek1",img1);

cvNamedWindow("obrazek2", CV_WINDOW_AUTOSIZE);  //CV_WINDOW_AUTOSIZE
cvMoveWindow("obrazek2",640,100);
cvShowImage("obrazek2",img2);

  //inicializace funkce, spoustejici se pri zmene stavu mysi
cvSetMouseCallback( "obrazek1" , on_mouseL);
cvSetMouseCallback( "obrazek2" , on_mouseR);
     
 Form1->AlphaValue->Caption= "Alpha: " + FloatToStr(alpha);
}


//---------------------------------------------------------------------------

void __fastcall TForm1::LoadLeftBtnClick(TObject *Sender)
{
                    //nacitani obrazku pomoci OpenPictureDialogu


 if (OpenPictureDialog1->Execute())
{cvReleaseImage(&img1);
cvDestroyWindow("obrazek1");
IplImage* img1=cvLoadImage("OpenPictureDialog1.FileName");
 cvNamedWindow("obrazek1", 0);
 cvShowImage("obrazek1",img1); }



}
//---------------------------------------------------------------------------

void __fastcall TForm1::LoadRightBtnClick(TObject *Sender)
{ cvDestroyWindow("obrazek2");
 if (OpenPictureDialog2->Execute())
{img2=cvLoadImage("(OpenPictureDialog2->FileName)");
 cvNamedWindow("obrazek2", 0);
 cvShowImage("obrazek2",img2); }

}

//---------------------------------------------------------------------------

void __fastcall TForm1::NextCoupleBtnClick(TObject *Sender)
{            //ukaze korespondujici body, ze kterych se bude pocitat fundamentalni matice

PointList1->Items->Add(IntToStr(i)+". X: " + IntToStr(l_sp.x) + ", Y: " +(IntToStr(l_sp.y)));
PointList2->Items->Add(IntToStr(i)+". X: " + IntToStr(r_sp.x) + ", Y: " +(IntToStr(r_sp.y)));
i++;
}

//---------------------------------------------------------------------------

void __fastcall TForm1::ExitBtnClick(TObject *Sender)
{
   //zavreni programu
//Sleep(2000);
cvDestroyAllWindows();
cvReleaseImage(&img1);
cvReleaseImage(&img2);
cvReleaseImage(&imgc1);
cvReleaseImage(&imgc2);
cvReleaseImage(&stereo);
cvReleaseImage(&gimg1);
cvReleaseImage(&gimg2);
cvReleaseImage(&disp);


exit(0);

}
//---------------------------------------------------------------------------


void __fastcall TForm1::GenerateClick(TObject *Sender)
{


//volba metody vypoctu fundamentalni matice
if (MethodBox->ItemIndex==0){
        fm_pocet=cvFindFundamentalMat( l_s,r_s,fundamental_matrix,
                                     CV_FM_8POINT);}  ;
if (MethodBox->ItemIndex==1){
        fm_pocet=cvFindFundamentalMat( l_s,r_s,fundamental_matrix,
                                     CV_FM_RANSAC,1.0,0.99,status ); }    ;
if (MethodBox->ItemIndex==2){
        fm_pocet=cvFindFundamentalMat( l_s,r_s,fundamental_matrix,
                                     CV_FM_LMEDS,1.0,0.99,status ); }  ;


int v;
FundMatLabel->Visible=true;
fund_mat->Visible=true;  //ukaze tabulku pro fundamentalni matici
if (fm_pocet==0){        //v pripade, ze nemohla byt vytvorena fundamentalni matice, ukaze do tabulky chybove hlaseni
                Form1->fund_mat->Cells[0][1]="NELZE";
                Form1->fund_mat->Cells[1][1]="VYTVOŘIT";
                Form1->fund_mat->Cells[2][1]="FUNDAMENTÁLNÍ";
                Form1->fund_mat->Cells[1][2]="MATICI!!!";}
else {     //vypis obsahu fundamentalni matice(neslo udelat "for cyklem"-chyba, proto primo
fund_mat->Visible=true;
v=cvmGet(fundamental_matrix, 0, 0);
Form1->fund_mat->Cells[0][0]=IntToStr(v);
v=cvmGet(fundamental_matrix, 0, 1);
Form1->fund_mat->Cells[0][1]=IntToStr(v);
v=cvmGet(fundamental_matrix, 0, 2);
Form1->fund_mat->Cells[0][2]=IntToStr(v);
v=cvmGet(fundamental_matrix, 1, 0);
Form1->fund_mat->Cells[1][0]=IntToStr(v);
v=cvmGet(fundamental_matrix, 1, 1);
Form1->fund_mat->Cells[1][1]=IntToStr(v);
v=cvmGet(fundamental_matrix, 1, 2);
Form1->fund_mat->Cells[1][2]=IntToStr(v);
v=cvmGet(fundamental_matrix, 2, 0);
Form1->fund_mat->Cells[2][0]=IntToStr(v);
v=cvmGet(fundamental_matrix, 2, 1);
Form1->fund_mat->Cells[2][1]=IntToStr(v);
v=cvmGet(fundamental_matrix, 2, 2);
Form1->fund_mat->Cells[2][2]=IntToStr(v);
CvMat*correspondent_linesL = cvCreateMat(3,point_count,CV_32F);
CvMat*correspondent_linesR = cvCreateMat(3,point_count,CV_32F);
 ///vypocet korespondujicich epipolarnich primek
cvComputeCorrespondEpilines(r_s, 2, fundamental_matrix, correspondent_linesR);
cvComputeCorrespondEpilines(l_s, 1, fundamental_matrix, correspondent_linesL);


  /*
ImgSize.width=img1->width*3;
ImgSize.height=img1->height*3;
                                   */



for (int t=0;t<3;t++)//nutná konverze fundamentální matice kvůli následovnému
                     //použití ve funkci cvMakeScanlines, která potřebuje
                     //vstupní matici typu CvMatrix3
	{
		FMAT.m[t+1][1]=fundamental_matrix->data.fl[t*3];
		FMAT.m[t+1][2]=fundamental_matrix->data.fl[t*3+1];
                FMAT.m[t+1][3]=fundamental_matrix->data.fl[t*3+2];

	}
               

  }

 }



//---------------------------------------------------------------------------



 void ReWarp(CvMatrix3 FMAT)
 {
int line_count=2400;
int* scanlines1; int* scanlines2; int* scanlines_p;
int* delkyL; int* delkyR; int* delky;
int* runs1; int* runs2;
int* first_cor; int* second_cor;


uchar*  buffer_1; uchar*  buffer_2; uchar* buffer_c;
CvSize ImgSize;
scanlines1 = (int*)(calloc( line_count* 2, sizeof(int) * 4));
scanlines2 = (int*)(calloc( line_count * 2, sizeof(int) * 4));
scanlines_p = (int*)(calloc( line_count * 2, sizeof(int) * 4));
runs1=(int*)(calloc(img1->width * img1->height *3* 2, 2 * sizeof (int)));  //ImgSize.width * ImgSize.height
runs2=(int*)(calloc(img1->width * img1->height *3* 2, 2 * sizeof(int)));
int* num_runs1;
int* num_runs2;
num_runs1=(int*)(calloc(img1->width * img1->height  *3 *2, 2 *sizeof(int)));
num_runs2=(int*)(calloc(img1->width * img1->height *3* 2, 2 *sizeof(int)));
first_cor=(int*)(calloc(img1->width * img1->height *3* 2, 2 * sizeof(int)));
second_cor=(int*)(calloc(img1->width * img1->height *3* 2, 2 *sizeof(int)));


delkyL = (int*)(calloc( line_count* 2, sizeof(int) * 4));
delkyR  = (int*)(calloc( line_count* 2, sizeof(int) * 4));
delky  = (int*)(calloc( line_count* 2, sizeof(int) * 4));
cvMakeScanlines(&FMAT, cvSize(img1->width, img1->height),0,0,0,0,&line_count);
cvMakeScanlines( &FMAT, cvSize(img1->width, img1->height), scanlines1, scanlines2,
                delkyL, delkyR, &line_count);

buffer_1 = (uchar*)(malloc(1200*line_count*3));
 buffer_2 = (uchar*)(malloc(1200*line_count*3));
 buffer_c=(uchar*)(calloc(img1->width * (img1->height+1), 3 * sizeof(uchar)));



  //rektifikace obrazku

cvPreWarpImage(line_count,img1,buffer_1,delkyL,scanlines1);
cvPreWarpImage(line_count,img2,buffer_2,delkyR,scanlines2);


 cvFindRuns (line_count,buffer_1,buffer_2,delkyL,delkyR,runs1,runs2,num_runs1,num_runs2);



 cvDynamicCorrespondMulti(line_count,runs1,num_runs1,runs2,num_runs2,first_cor,second_cor);




         // alpha=0.5;
         //alpha=(Pan/100.f);

         cvMakeAlphaScanlines(scanlines1,scanlines2,scanlines_p,delky,line_count,alpha);
         cvMorphEpilinesMulti(line_count,buffer_1,delkyL,buffer_2,delkyR,buffer_c,delky,alpha,runs1,num_runs1,runs2,num_runs2,first_cor,second_cor);


         cvPostWarpImage(line_count, buffer_c, delky, stereo, scanlines_p);

         cvDeleteMoire(stereo); //cvDeleteMoire(stereo);cvDeleteMoire(stereo);cvDeleteMoire(stereo);cvDeleteMoire(stereo);

    cvNamedWindow("Final", 1);

         cvShowImage("Final",stereo); //zobrazeni mezilehleho pohledu

         //uvolneni pameti:
     if (scanlines1 != 0)   free (scanlines1); if (scanlines2 != 0)   free (scanlines2);
     if (scanlines_p != 0)   free (scanlines_p); if (delkyL != 0)   free (delkyL);
     if (delkyR != 0)   free (delkyR); if (delky  != 0)   free (delky);
     if (buffer_1 != 0)   free (buffer_1); if (buffer_2 != 0)   free (buffer_2);
     if (buffer_c != 0)   free (buffer_c); if (runs1 != 0)   free (runs1);
     if (runs2 != 0)   free (runs2); if (first_cor != 0)   free (first_cor);
     if (second_cor != 0)   free (second_cor); if (num_runs1 != 0)   free (num_runs1);
     if (num_runs2 != 0)   free (num_runs2);
     scanlines1= 0; scanlines2 = 0; scanlines_p = 0;delkyL  = 0; delkyR = 0; delky = 0;
     runs1  = 0; runs2   = 0; buffer_1  = 0; buffer_2 = 0; buffer_c = 0; num_runs1 = 0;
     num_runs2 = 0; first_cor = 0; second_cor = 0;




         }


void __fastcall TForm1::FindCorBtnClick(TObject *Sender)
{   //CvSize ImgSize(img1->width, img1->height);


cvCvtColor(img1,gimg1,CV_BGR2GRAY);
cvCvtColor(img2,gimg2,CV_BGR2GRAY);


 cvFindStereoCorrespondence(gimg1, gimg2, CV_DISPARITY_BIRCHFIELD, disp, 255, 25, 5, 12, 15, 25 );
 cvNamedWindow("Disparity Map", 1);
 cvShowImage("Disparity Map",disp);
 /*
for(1; i; i++){
PointList1->Items->Add(IntToStr(i)+". X: " + IntToStr(l_sp.x) + ", Y: " +(IntToStr(l_sp.y)));
PointList2->Items->Add(IntToStr(i)+". X: " + IntToStr(r_sp.x) + ", Y: " +(IntToStr(r_sp.y)));
                 }*/
}
//---------------------------------------------------------------------------

void __fastcall TForm1::PanoramaChange(TObject *Sender)
{
Pan=Form1->Panorama->Position;
alpha=1-(Pan/100.00001f);
if (alpha>=1) {alpha =1;};
Form1->AlphaValue->Caption= "Alpha: " + FloatToStr(1-alpha);
ReWarp(FMAT);
}
//---------------------------------------------------------------------------



//---------------------------------------------------------------------------


void __fastcall TForm1::InViewClick(TObject *Sender)
{
ReWarp(FMAT);
}
//---------------------------------------------------------------------------

void __fastcall TForm1::SaveBtnClick(TObject *Sender)
{
 cvSaveImage("stereo.jpg",stereo);
}
//---------------------------------------------------------------------------


//---------------------------------------------------------------------------

//---------------------------------------------------------------------------

void __fastcall TForm1::FCorClick(TObject *Sender)
{


 #define KDTREE_MAX 200
 #define PRAHOVACI_UROVEN 0.49

 struct feature* feat1, * feat2, * feat;
 int n1, n2, k, i, m = 0;
 struct feature** nbrs;
 struct kd_node* kd_root;
 double d0, d1;
 int test1[2000][2];
 int test2[2000][2];
 int tb=0; int t1=1; int tt=1;  int t2=1;  int t3=1; int t4=1;

n1 = sift_features( img1, &feat1 );
     
n2 = sift_features( img2, &feat2 );
	kd_root = kdtree_build( feat2, n2 );
	for( i = 0; i < n1; i++ )
	{
		feat = feat1 + i;
		k = kdtree_bbf_knn( kd_root, feat, 2, &nbrs, KDTREE_MAX );
		if( k == 2 )
		{
			d0 = descr_dist_sq( feat, nbrs[0] );
			d1 = descr_dist_sq( feat, nbrs[1] );
			if( d0 < d1 * PRAHOVACI_UROVEN )


				test1[i][1]=  cvRound( feat->x );
                                 test1[i][2]=  cvRound( feat->y);
                                 test2[i][1]=  cvRound( nbrs[0]->x );
                                 test2[i][2]=  cvRound( nbrs[0]->y);

			    	feat1[i].fwd_match = nbrs[0];

		}
		free( nbrs );
	}
//Filtr falesnych korespondenci
l_s->data.fl[0]=test1[0][1];  l_s->data.fl[1]=test1[0][2]; r_s->data.fl[0]=test2[0][1];  r_s->data.fl[1]=test2[0][2];
for( i = 1; i < n1; i++ )
	{  for(j = 0; j < m; j++){
        if (test1[i][1]!= l_s->data.i[j*2]||test1[i][2]!= l_s->data.i[j*2+1]){t1=1;}  else {t1=0; }      }//filtrace duplikaci
        if (test1[i][1]>=0&&test1[i][2]>=0&&test2[i][1]>=0&&test2[i][2]>=0) {t2=1;} else {t2=0;}          //filtrace zapornych
        if (test1[i][1]<(img1->width)&&test2[i][1]<(img1->width)&&test1[i][2]<(img1->height)&&test2[i][2]<(img1->height)) {t3=1;} else {t3=0;}     //filtrace mimo obraz
        if (test1[i][1]!=0) {t4=1;} else {t4=0;}                        //filtrace nulovych
                                   tt=t2*t1*t3*t4;
                                  //}
        if (tt==1){ 	l_s->data.fl[m*2] = test1[i][1];             //jestlize plati vse, zahrn tuto korespondenci do aktivnich
                        l_s->data.fl[m*2+1] = test1[i][2];
	        	r_s->data.fl[m*2] = test2[i][1];
                        r_s->data.fl[m*2+1]= test2[i][2]; m++;
                  }

        }


for( i = 0; i < m; i++ )
        {l_sp.x = l_s->data.fl[i*2];        //nacteni souradnic do promenne typu CvPoint
        l_sp.y = l_s->data.fl[i*2+1];
        cvCircle( imgc1, l_sp, 5, CV_RGB(0,255,0), 2, 3,0); //oznaceni
        r_sp.x = r_s->data.fl[i*2];        //nacteni souradnic do promenne typu CvPoint
        r_sp.y = r_s->data.fl[i*2+1];
        cvCircle( imgc2, r_sp, 5, CV_RGB(0,255,0), 2, 3,0);}  //oznaceni

 cvShowImage("obrazek1",imgc1);  //obnoveni obrazku
 cvShowImage("obrazek2",imgc2);  //obnoveni obrazku


for(int p=0; p<m; p++){
PointList1->Items->Add(IntToStr(p)+". X: " + FloatToStr(l_s->data.fl[p*2]) + ", Y: " +(FloatToStr(l_s->data.fl[p*2+1])));
PointList2->Items->Add(IntToStr(p)+". X: " + FloatToStr(r_s->data.fl[p*2]) + ", Y: " +(FloatToStr(r_s->data.fl[p*2+1])));
                 }


}
//---------------------------------------------------------------------------

void __fastcall TForm1::TestCorClick(TObject *Sender)
{
  //testovací souřadnice
      //pro pravý obrázek:
       r_s->data.fl[0]=313;      //x-ová souřadnice
       r_s->data.fl[1]=217;      //y-ová souřadnice

       r_s->data.fl[2]=359;
       r_s->data.fl[3]=383;

       r_s->data.fl[4]=454;
       r_s->data.fl[5]=355;

       r_s->data.fl[6]=263;
       r_s->data.fl[7]=334;

       r_s->data.fl[8]=264;
       r_s->data.fl[9]=123;

       r_s->data.fl[10]=301;
       r_s->data.fl[11]=50;

       r_s->data.fl[12]=454;
       r_s->data.fl[13]=202;

       r_s->data.fl[14]=613;
       r_s->data.fl[15]=46;

       r_s->data.fl[16]=98;
       r_s->data.fl[17]=233;

       //pro levý obrázek:
       l_s->data.fl[0]=342;
       l_s->data.fl[1]=221;

       l_s->data.fl[2]=381;
       l_s->data.fl[3]=384;

       l_s->data.fl[4]=466;
       l_s->data.fl[5]=355;

       l_s->data.fl[6]=270;
       l_s->data.fl[7]=336;

       l_s->data.fl[8]=284;
       l_s->data.fl[9]=129;

       l_s->data.fl[10]=307;
       l_s->data.fl[11]=58;

       l_s->data.fl[12]=464;
       l_s->data.fl[13]=208;

       l_s->data.fl[14]=628;
       l_s->data.fl[15]=73;

       l_s->data.fl[16]=98;
       l_s->data.fl[17]=233;
       for(int p=0; p<9; p++){
PointList1->Items->Add(IntToStr(p)+". X: " + FloatToStr(l_s->data.fl[p*2]) + ", Y: " +(FloatToStr(l_s->data.fl[p*2+1])));
PointList2->Items->Add(IntToStr(p)+". X: " + FloatToStr(r_s->data.fl[p*2]) + ", Y: " +(FloatToStr(r_s->data.fl[p*2+1])));
                 }
}
//---------------------------------------------------------------------------


