/**
********************************************************************************
*
*    @file      panorama.cxx
*
*    @brief     A program using OpenCV to stitch images together.
*
*    @version   1.0
*
*    @date      27/02/2017
*
*    @author    Franck Vidal
*
*
********************************************************************************
*/


//******************************************************************************
//    Includes
//******************************************************************************
#include <exception> // Header for catching exceptions
#include <iostream>  // Header to display text in the console
#include <sstream>  // Header to create stings using a stream
#include <vector>
#include <opencv2/opencv.hpp> // Main OpenCV header
#include <opencv2/features2d.hpp> // Header for the feature detectors


//******************************************************************************
//    Namespaces
//******************************************************************************
using namespace std;


//******************************************************************************
//    Global variables
//******************************************************************************
std::vector<cv::Mat> g_p_input_image_set;

//******************************************************************************
//    Function declaration
//******************************************************************************
cv::Mat autoCrop(const cv::Mat& anImage);


//******************************************************************************
//    Implementation
//******************************************************************************


//-----------------------------
int main(int argc, char** argv)
//-----------------------------
{
    try
    {
		//**********************************************************************
		// Process the command line arguments
		//**********************************************************************

		std::string input_filename_1, input_filename_2, output_filename;

            // Add your code here to check the number of command line arguments
            // If invalid, throw an error
		if (argc != 4)
		{
			// Create an error message
			std::string error_message;
			error_message = "Usage: ";
			error_message += argv[0];
			error_message += " <input_image_1>";
			error_message += " <input_image_2>";
			error_message += " [output_image]";

			// Throw an error
			throw error_message;
		}


		//**********************************************************************
		// Load the data
		//**********************************************************************

            // Add your code here
		input_filename_1 = argv[1];
		input_filename_2 = argv[2];
		cv::Mat image1 = cv::imread(input_filename_1, CV_LOAD_IMAGE_COLOR);
		cv::Mat image2 = cv::imread(input_filename_2, CV_LOAD_IMAGE_COLOR);
		g_p_input_image_set.push_back(image1);
		g_p_input_image_set.push_back(image2);


		// If either image didn't load, throw error
		if (g_p_input_image_set.size() != 2) {
			std::string error_message;
			error_message = "Could not open or find the images \"";
			error_message += input_filename_1;
			error_message += "\" \"";
			error_message += input_filename_2;
			error_message += "\".";
			// Throw an error
			throw error_message;
		}



        //**********************************************************************
		// Display every image
		//**********************************************************************

            // Add your code here
		cv::namedWindow(input_filename_1, cv::WINDOW_AUTOSIZE);
		cv::namedWindow(input_filename_2, cv::WINDOW_AUTOSIZE);

		cv::imshow(input_filename_1, image1);
		cv::imshow(input_filename_2, image2);
        //**********************************************************************
		// Find features
		//**********************************************************************

            // Add your code here
		cv::Mat total_image = g_p_input_image_set[0];
		cv::Mat current_image = g_p_input_image_set[1];

		// Create a feature detector
		cv::Ptr<cv::FeatureDetector> p_feature_detector;
		p_feature_detector = cv::ORB::create();

		// Detect the keypoints
		std::vector<cv::KeyPoint> p_total_image_keypoint_set;
		std::vector<cv::KeyPoint> p_current_image_keypoint_set;
		p_feature_detector->detect(total_image, p_total_image_keypoint_set);
		p_feature_detector->detect(current_image, p_current_image_keypoint_set);

        //**********************************************************************
        // Describe/Extract features
        //**********************************************************************

        // Add your code here
		// Calculate descriptors (feature vectors)
		cv::Ptr<cv::DescriptorExtractor> p_feature_extractor;
		p_feature_extractor = cv::ORB::create();

		// The descriptors
		cv::Mat total_image_descriptors;
		cv::Mat current_image_descriptors;
		// Extract the features corresponding to the keypoints and the pictures
		p_feature_extractor->compute(total_image, p_total_image_keypoint_set, total_image_descriptors);
		p_feature_extractor->compute(current_image, p_current_image_keypoint_set, current_image_descriptors);


        //**********************************************************************
        // Match the features
        //**********************************************************************

        // Add your code here
		// Match the features
		cv::Ptr<cv::DescriptorMatcher> p_feature_matcher;
		p_feature_matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
		std::vector< cv::DMatch > p_match_set;
		p_feature_matcher->match(total_image_descriptors, current_image_descriptors, p_match_set);




        //**********************************************************************
        // Compute some basic statitics (e.g. min, max) to filter the matches
        //**********************************************************************
        double min_distance(DBL_MAX);
        double max_distance(-DBL_MAX);

            // Add your code here
		for (int i = 0; i < total_image_descriptors.rows; i++)
		{
			double dist = p_match_set[i].distance;
			if (dist < min_distance) min_distance = dist;
			if (dist > max_distance) max_distance = dist;
		}




        //**********************************************************************
        // Only use "good" matches (i.e. whose distance is less than 8 * min_distance)
        //**********************************************************************

        std::vector<cv::DMatch> p_good_match_set;
        for (std::vector< cv::DMatch >::const_iterator ite(p_match_set.begin());
             ite != p_match_set.end();
             ++ite)
        {
            if (ite->distance < 8 * min_distance)
            {
                p_good_match_set.push_back(*ite);
            }
        }




        //**********************************************************************
        // Get the 2D points in the key points from the good matches
        //**********************************************************************

            // Add your code here
			// Store the 2D points in the keypoint lists from the good matches
		std::vector<cv::Point2f> p_total_image_point_set;
		std::vector<cv::Point2f> p_current_image_point_set;

		// Look at each good match
		for (std::vector< cv::DMatch >::const_iterator ite(p_good_match_set.begin()); ite != p_good_match_set.end(); ++ite)
		{
			// Get the keypoints from the good match
			cv::KeyPoint total_image_keypoint(p_total_image_keypoint_set[ite->queryIdx]);
			cv::KeyPoint current_image_keypoint(p_current_image_keypoint_set[ite->trainIdx]);
			// Add the corresponding 2D points
			p_total_image_point_set.push_back(total_image_keypoint.pt);
			p_current_image_point_set.push_back(current_image_keypoint.pt);
		}


        //**********************************************************************
        // Find the Homography Matrix
        //**********************************************************************

            // Add your code here
			// Find the Homography Matrix
		cv::Mat homography_matrix(cv::findHomography(p_total_image_point_set, p_current_image_point_set, CV_RANSAC));


        //**********************************************************************
        // Use the Homography Matrix to warp the images
        //**********************************************************************
        cv::Mat output;

            // Add your code here
			// Use the Homography Matrix to warp the images
		cv::warpPerspective(total_image, output, homography_matrix, cv::Size(total_image.cols + current_image.cols, total_image.rows + 1));
		cv::Mat half(output(cv::Rect(0, 0, current_image.cols, current_image.rows)));
		current_image.copyTo(half);
		cv::imshow("Result", output);


        //**********************************************************************
        // Remove black edges
        //**********************************************************************
       output = autoCrop(output);
        cv::imshow("autoCrop", output);

        // Write the output
        cv::imwrite(argv[argc - 1], output);

        // Wait
        cv::waitKey(0);
    }
    // An error occured
    catch (const std::exception& error)
    {
        // Display an error message in the console
        cerr << error.what() << endl;
    }
    catch (const std::string& error)
    {
        // Display an error message in the console
        cerr << error << endl;
    }
    catch (const char* error)
    {
        // Display an error message in the console
        cerr << error << endl;
    }

    // Exit the program
    return 0;
}


//--------------------------------------
cv::Mat autoCrop(const cv::Mat& anImage)
//--------------------------------------
{
    // Convert to grey scale
    cv::Mat grey_image;
    cv::cvtColor(anImage, grey_image, cv::COLOR_BGR2GRAY);

    // Convert to binary
    cv::Mat binary_image;
    cv::threshold(grey_image, binary_image, 1, 255, cv::THRESH_BINARY);

    // Find contours
    std::vector<std::vector<cv::Point> > p_contour_set;
    std::vector<cv::Vec4i> p_hierarchy_set;

    cv::findContours(binary_image, p_contour_set, p_hierarchy_set, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE, cv::Point(0, 0) );
    cv::Rect bounding_rectangle(cv::boundingRect(p_contour_set.front()));

    // Crop the input image using the bounding rectangle
    cv::Mat output(anImage(bounding_rectangle));

    return output;
}