#include <stdio.h>
#include <opencv.hpp>
#include <nonfree/nonfree.hpp>
#include <vector>

using namespace std;
using namespace cv;

Mat TwoInOneOut(Mat Left, Mat Right);

int main()
{
VideoCapture stream1(1);
VideoCapture stream2(2);

if (!stream1.isOpened()) { //check if video device has been initialised
cout << "cannot open camera 1";
return 0;
}

if (!stream2.isOpened()) { //check if video device has been initialised
cout << "cannot open camera 2";
return 0;
}

Mat H;
int mode=0;

while (true) {
Mat cameraFrame1;
stream1.read(cameraFrame1); //get one frame from cam1 (left)


Mat cameraFrame2;
stream2.read(cameraFrame2); //get one frame from cam2 (right)

if(mode == 0)
{
imshow("Left", cameraFrame1);
imshow("Right", cameraFrame2);
}

Mat Left(cameraFrame1.rows, cameraFrame1.cols, CV_8U);
Mat Right(cameraFrame2.rows, cameraFrame2.cols, CV_8U);
cvtColor(cameraFrame1, Left, CV_RGB2GRAY, CV_8U);
cvtColor(cameraFrame2, Right, CV_RGB2GRAY, CV_8U);

if (waitKey(33) == 'p')
{
printf("Homography Matrix Processing\n");
H = TwoInOneOut(Left, Right);
mode=1;
destroyWindow("Left");
destroyWindow("Right");
}

if(waitKey(33) == 'r')
{
printf("normal mode\n");
destroyWindow("Processing");
mode=0;
}

if(H.cols == 3 && H.rows == 3)
{
Mat WarpImg( Left.rows*2, Left.cols*2, cameraFrame1.depth() );
warpPerspective(cameraFrame2, WarpImg, H, Size(WarpImg.cols, WarpImg.rows));
Mat tempWarpImg = WarpImg(Rect(0,0,Left.cols,Left.rows));
cameraFrame1.copyTo(tempWarpImg);

if(mode ==1)
imshow("Processing", WarpImg );

}



if (waitKey(33) == 27)
break;
}

destroyAllWindows();


}


Mat TwoInOneOut(Mat Left, Mat Right)
{
Mat H;

if(Left.channels() != 1 || Right.channels() != 1)
{
printf("Channel Error\n");
return H;
}

/////////////////
//Detect the keypoints using SURF Detector
int minHessian = 300; //1500;
SurfFeatureDetector detector( minHessian );
SurfDescriptorExtractor extractor;

/////////////////
//A
std::vector<KeyPoint> kp_Left;
detector.detect( Left, kp_Left );
Mat des_Left;
extractor.compute( Left, kp_Left, des_Left );

/////////////////
//B
std::vector<KeyPoint> kp_Right;
detector.detect( Right, kp_Right );
Mat des_Right;
extractor.compute( Right, kp_Right, des_Right );

/////////////////
//Match
std::vector<vector<DMatch > > matches;
FlannBasedMatcher matcher;
matcher.knnMatch(des_Left, des_Right, matches, 2);
std::vector<DMatch > good_matches;
good_matches.reserve(matches.size());

for (size_t i = 0; i < matches.size(); ++i)
{
if (matches[i].size() < 2)
continue;

const DMatch &m1 = matches[i][0];
const DMatch &m2 = matches[i][1];

if(m1.distance <= 0.7 * m2.distance)
good_matches.push_back(m1);
}

//Draw only "good" matches
Mat img_matches;
drawMatches( Left, kp_Left, Right, kp_Right, good_matches,
img_matches, Scalar::all(-1), Scalar::all(-1),
vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
imshow("Match", img_matches);

/////////////////
//Find H
if(good_matches.size() > 20 )
{
std::vector< Point2f > LeftMatchPT;
std::vector< Point2f > RightMatchPT;

for( unsigned int i = 0; i < good_matches.size(); i++ )
{
//-- Get the keypoints from the good matches
LeftMatchPT.push_back( kp_Left[ good_matches[i].queryIdx ].pt );
RightMatchPT.push_back( kp_Right[ good_matches[i].trainIdx ].pt );
}

H = findHomography( RightMatchPT, LeftMatchPT, CV_RANSAC );
}


return H;

return img_matches;
}