#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <iostream>
#include <string>

using namespace cv;
using namespace std;

void show(const char* windowname, Mat image)
{
    namedWindow(windowname, CV_WINDOW_AUTOSIZE);
    imshow(windowname, image);
}

//TRACKBAR FUNCTIONS - BEGIN
/*
int pfs=0;
int pfs_max=63;
int pfc=0;
int pfc_max=63;
int sws=0;
int sws_max=63;
//int md=0;
//int md_max=10;
int nod=0;
int nod_max=128;
int tt=0;
int tt_max=50;
int ur=0;
int ur_max=20;
char* threshWin = "Threshold Window";

int a=31;
int b=31;
int c=35;
int d=0;
int e=48;
int f=20;
int g=15;

void onPreFilterSizeTrackbar(int, void*)
{
cout << "new pfs " << pfs << endl;
if((pfs%2)!=0 && pfs>=5)
a = pfs;
}
void onPreFilterCapTrackbar(int, void*)
{
cout << "new pfc " << pfc << endl;
if((pfc%2)!=0 && pfc>=5)
b = pfc;
}
void onSADWindowSizeTrackbar(int, void*)
{
cout << "new sws " << sws << endl;
if((sws%2)!=0 && sws>=5)
c = sws;
if(sws < 5)
c = 5;
}
/*
void onMinDisparityTrackbar(int, void*)
{
cout << "new md " << md << endl;
d = md;
}
*/
/*
void onNumberOfDisparitiesTrackbar(int, void*)
{
cout << "new nod " << nod << endl;
if((nod%16)==0 && nod>0)
e = nod;
if(nod==0)
e=16;
}
void onTextureThresholdTrackbar(int, void*)
{
cout << "new tt " << tt << endl;
f = tt;
}
void onUniquenessRatioTrackbar(int, void*)
{
cout << "new ur " << ur << endl;
g = ur;
}
*/
//TRACKBAR FUNCTIONS - END

int main()
{
    Mat image1, image2;

    Mat camMat1 = (Mat_<double>(3,3) << 793.1338, 0, 337.2309, 0, 792.0555, 256.9991, 0, 0, 1);
    Mat camMat2 = (Mat_<double>(3,3) << 799.1271, 0, 319.8581, 0, 797.2460, 243.4638, 0, 0, 1);
    Mat dispCoeffs1 = (Mat_<double>(1,5) << 0.0033, -0.1320, -0.0019, 0.0026, 0);
    Mat dispCoeffs2 = (Mat_<double>(1,5) << -0.0109, -0.0188, -0.0014, -0.0055, 0);
    Mat RotMat = (Mat_<double>(3,3) << 0.9998, -0.0023, 0.0221, 0.0022, 1, 0.0031, -0.0221, -0.0031, 0.9998);
    Mat TransMat = (Mat_<double>(3,1) << 374.2306, -1.8319, 5.5745);

    //Rectify
    Mat R1, R2, P1, P2, Q;
    stereoRectify(camMat1, dispCoeffs1, camMat2, dispCoeffs2, Size(640,480), RotMat, TransMat, R1, R2, P1, P2, Q, CV_CALIB_ZERO_DISPARITY, 1, Size(640,480));

    //Define the mapping to the done
    Mat rx1, ry1;
    Mat rx2, ry2;
    initUndistortRectifyMap(camMat1, dispCoeffs1, R1, P1, Size(640,480), CV_16SC2, rx1, ry1);
    initUndistortRectifyMap(camMat2, dispCoeffs2, R2, P2, Size(640,480), CV_16SC2, rx2, ry2);

    //SET THE BM STATE VARIABLES BEGIN - DONE GLOBALLY
    StereoBM bm;
    bm.state->preFilterSize = 31;
    bm.state->preFilterCap = 63;
    bm.state->SADWindowSize = 9;
    bm.state->minDisparity = -128;
    //bm.state->disp12MaxDiff = 2;
    bm.state->numberOfDisparities = 128;
    bm.state->textureThreshold = 50;
    bm.state->uniquenessRatio = 15;
    bm.state->speckleWindowSize = 100;
    bm.state->speckleRange = 16;
    //SET THE BM STATE VARIABLES END

    /*
    namedWindow(threshWin);
    createTrackbar("Pre Filter Size", threshWin, &pfs, pfs_max, onPreFilterSizeTrackbar);
    createTrackbar("Pre Filter Cap", threshWin, &pfc, pfc_max, onPreFilterCapTrackbar);
    createTrackbar("SAD Window Size", threshWin, &sws, sws_max, onSADWindowSizeTrackbar);
    //createTrackbar("Minimum Disparity", threshWin, &md, md_max, onMinDisparityTrackbar);
    createTrackbar("Number Of Disparities", threshWin, &nod, nod_max, onNumberOfDisparitiesTrackbar);
    createTrackbar("Texture Threshold", threshWin, &tt, tt_max, onTextureThresholdTrackbar);
    createTrackbar("Uniqueness Ratio", threshWin, &ur, ur_max, onUniquenessRatioTrackbar);
    */

    VideoCapture cap3 = VideoCapture(0);
    VideoCapture cap4 = VideoCapture(1);

    //cap3.set(CV_CAP_PROP_FRAME_WIDTH, 320);
    //cap3.set(CV_CAP_PROP_FRAME_HEIGHT, 240);
    //cap4.set(CV_CAP_PROP_FRAME_WIDTH, 320);
    //cap4.set(CV_CAP_PROP_FRAME_HEIGHT, 240);

    cap3 >> image1;
    cap4 >> image2;
    Size imageSize = image1.size();

    Mat gray_image1;
    Mat gray_image2;
    Mat frame1r;
    //frame1r.create(image1.size(), CV_8U);
    Mat frame2r;
    //frame2r.create(image2.size(), CV_8U);
    Mat frame1rf;
    Mat frame2rf;
    //Mat disp(image1.size(), CV_16S);
    //Mat vdisp(image1.size(), CV_8U);
    Mat disp, vdisp;
    //Mat image3d(image1.size(), CV_32FC3);
    Mat image3d;
    Mat rectified_pair;
    rectified_pair.create(imageSize.height, (imageSize.width)*2, CV_8UC3);

    //Actually do the mapping -- based on the mapping definition

    while(1)
    {
        /*
        bm.state->preFilterSize = a;
        bm.state->preFilterCap = b;
        bm.state->SADWindowSize = c;
        //bm.state->minDisparity = d;
        bm.state->disp12MaxDiff = 2;
        bm.state->numberOfDisparities = e;
        bm.state->textureThreshold = f;
        bm.state->uniquenessRatio = g;
        bm.state->speckleWindowSize = 50;
        bm.state->speckleRange = 32;
        */

        bm.state->preFilterSize = 31;
        bm.state->preFilterCap = 63;
        bm.state->SADWindowSize = 21;
        bm.state->minDisparity = -128;
        //bm.state->disp12MaxDiff = 2;
        bm.state->numberOfDisparities = 64;
        bm.state->textureThreshold = 20;
        bm.state->uniquenessRatio = 10;
        bm.state->speckleWindowSize = 100;
        bm.state->speckleRange = 32;

        cvtColor(image1, gray_image1, CV_BGR2GRAY);
        cvtColor(image2, gray_image2, CV_BGR2GRAY);
        remap(gray_image1, frame1r, rx1, ry1, CV_INTER_LINEAR);
        remap(gray_image2, frame2r, rx2, ry2, CV_INTER_LINEAR);
        bm(frame1r, frame2r, disp);
        normalize(disp, vdisp, 0, 255, NORM_MINMAX, CV_8U);
        //convertScaleAbs(vdisp, vdisp, 1, 0);
        disp.convertTo(vdisp, CV_8U, 255/(64*16.));
        show("disparity", vdisp);
        //reprojectImageTo3D(disp, image3d, Q, true);
        //show("depth map", image3d);

        //display image side by side for rectified window
        //copy frame1r to the left side
        cvtColor(frame1r, frame1rf, CV_GRAY2BGR);
        frame1rf.copyTo(rectified_pair(Rect(0,0,imageSize.width, imageSize.height)));
        //copy frame2r to the right side
        cvtColor(frame2r, frame2rf, CV_GRAY2BGR);
        frame2rf.copyTo(rectified_pair(Rect(imageSize.width,0,imageSize.width, imageSize.height)));
        for(int i=0; i<imageSize.height; i+=32)
            line(rectified_pair, Point(0,i), Point((imageSize.width)*2, i), CV_RGB(0,255,0));
        show("rectified", rectified_pair);

        cap3 >> image1;
        cap4 >> image2;
        if(waitKey(15) == 27)
            break;
    }

    return 0;
}