#include <vector>
#include <iostream>
#include <algorithm>
#include <opencv/highgui.h>
#include <opencv/cv.h>
#include <boost/numeric/ublas/matrix.hpp>
#include <opencv2/nonfree/nonfree.hpp>
using namespace boost::numeric::ublas;

// Homography matrix
cv::Mat H;

// Point of images save in cv::Mat
cv::Mat srcPoints;
cv::Mat disPoints;

std::vector<IplImage*> images;

int globalMaxX, globalMaxY, globalMinX, globalMinY;
// Multiplication width convert to boost::matrix
vector<double> matrixMultiplication(const cv::Mat* H, int x, int y)
{
        matrix<double> m(3,3);
        vector<double> pt(3);

        pt(0) = x; pt(1) = y; pt(2) = 1;

        // if there isn't matrix of homography return original point
        if (H == 0) return pt;

        // Copy to new matrix m
        for (unsigned i = 0; i < m.size1(); ++ i)
                for (unsigned j = 0; j < m.size2(); ++ j)
                        m(i, j) = (*H).at<double>(i, j);
       // Multiplication from boost            
       pt = prod(m, pt);
       // Normalization homogeneous coordinates
       pt = pt/pt(2);
       return pt;
}

// The size of image after applying maxtrix of homography
void ImageSizePoint(const IplImage* img, const cv::Mat* H, int* minX, int* minY, int* maxX, int* maxY)
{
        vector<double> x = matrixMultiplication(H, img->width, img->height);
        *maxX = x(0);   *maxY = x(1);
        *minX = x(0);   *minY = x(1);

        x = matrixMultiplication(H, img->width, 0);
        *maxX = std::max(*maxX,(int)x(0));
        *maxY = std::max(*maxY,(int)x(1));
        *minX = std::min(*minX,(int)x(0));
        *minY = std::min(*minY,(int)x(1));

        x = matrixMultiplication(H, img->height, 0);
        *maxX = std::max(*maxX,(int)x(0));
        *maxY = std::max(*maxY,(int)x(1));
        *minX = std::min(*minX,(int)x(0));
        *minY = std::min(*minY,(int)x(1));

        x = matrixMultiplication(H, 0, 0);
        *maxX = std::max(*maxX,(int)x(0));
        *maxY = std::max(*maxY,(int)x(1));
        *minX = std::min(*minX,(int)x(0));
        *minY = std::min(*minY,(int)x(1));
}
void flannFindPairs( const CvSeq*, const CvSeq* objectDescriptors,
const CvSeq*, const CvSeq* imageDescriptors, std::vector<int>& ptpairs )
{
        int length = (int)(objectDescriptors->elem_size/sizeof(float));

        cv::Mat m_object(objectDescriptors->total, length, CV_32F);
        cv::Mat m_image(imageDescriptors->total, length, CV_32F);

        // copy descriptors
        CvSeqReader obj_reader;
        float* obj_ptr = m_object.ptr<float>(0);
        cvStartReadSeq( objectDescriptors, &obj_reader );
        for(int i = 0; i < objectDescriptors->total; i++ )
        {
                const float* descriptor = (const float*)obj_reader.ptr;
                CV_NEXT_SEQ_ELEM( obj_reader.seq->elem_size, obj_reader );
                memcpy(obj_ptr, descriptor, length*sizeof(float));
                obj_ptr += length;
        }
        CvSeqReader img_reader;
        float* img_ptr = m_image.ptr<float>(0);
        cvStartReadSeq( imageDescriptors, &img_reader );
        for(int i = 0; i < imageDescriptors->total; i++ )
        {
                const float* descriptor = (const float*)img_reader.ptr;
                CV_NEXT_SEQ_ELEM( img_reader.seq->elem_size, img_reader );
                memcpy(img_ptr, descriptor, length*sizeof(float));
                img_ptr += length;
        }
       // find nearest neighbors using FLANN
       cv::Mat m_indices(objectDescriptors->total, 2, CV_32S);
       cv::Mat m_dists(objectDescriptors->total, 2, CV_32F);
       cv::flann::Index flann_index(m_image, cv::flann::KDTreeIndexParams(4));  // using 4 randomized kdtrees
       flann_index.knnSearch(m_object, m_indices, m_dists, 2, cv::flann::SearchParams(64) ); // maximum number of leafs checked
       int* indices_ptr = m_indices.ptr<int>(0);
       float* dists_ptr = m_dists.ptr<float>(0);
       for (int i=0;i<m_indices.rows;++i)
       {
                if (dists_ptr[2*i]<0.6*dists_ptr[2*i+1])
                {
                        ptpairs.push_back(i);
                        ptpairs.push_back(indices_ptr[2*i]);
                }
       }
}

// Remapping 1st image on 2nd 
int remapping(IplImage *img1, IplImage *img2, IplImage **result)
{
        // features points
        CvSeq *keyPoints1, *keyPoints2;
        CvSeq *desPoints1, *desPoints2;
        // for cvExtractSURF
        CvMemStorage *storage = cvCreateMemStorage();
        CvSURFParams params = cvSURFParams(500,1);

        std::vector<int> ptPairs;

        std::cout << "Remapping...\n";


        cvExtractSURF(img1, 0, &keyPoints1, &desPoints1, storage, params); 
        cvExtractSURF(img2, 0, &keyPoints2, &desPoints2, storage, params); 

        flannFindPairs(keyPoints1, desPoints1, keyPoints2, desPoints2, ptPairs);
        
        int n = (int)(ptPairs.size()/2);
        if (n < 4) return 0;

        std::vector<CvPoint2D32f> pt1, pt2;   
        pt1.resize(n);  pt2.resize(n);

        for(int j = 0; j < n; ++j) {
                pt1[j] = ((CvSURFPoint*)cvGetSeqElem(keyPoints1,ptPairs[j*2]))->pt;
                pt2[j] = ((CvSURFPoint*)cvGetSeqElem(keyPoints2,ptPairs[j*2+1]))->pt;
        }

        srcPoints = cv::Mat(1, n, CV_32FC2, &pt1[0]);
        disPoints = cv::Mat(1, n, CV_32FC2, &pt2[0]);
        // Calculating matrix of homography
        H = cv::findHomography(srcPoints, disPoints, CV_RANSAC, 5);

        std::cout << "Perspective transformation H between two images:\n";
      //    std::cout << "H = \n" << H << "\n";

        int maxX, maxY, minX, minY;
        int maxX2, maxY2, minX2, minY2;
        // Define image's size
        ImageSizePoint(img1, &H, &minX, &minY, &maxX, &maxY);
          std::cout << " 1 = \n" ;
        ImageSizePoint(img2, 0, &minX2, &minY2, &maxX2, &maxY2);
          std::cout << " 2 = \n" ;
        // Select the most minimal
        minX = std::min(minX, minX2); minY = std::min(minY, minY2);
        // if the image is out of the window border             // the move images
        if (minX < 0) minX = std::abs(minX);
        else    minX = 0;
        if (minY < 0) minY = std::abs(minY);
        else    minY = 0;
        globalMaxX = std::max(maxX, maxX2);
        globalMaxY = std::max(maxY, maxY2);
        // Create final image 
                 std::cout << " 3 = \n" ;
 
        *result = cvCreateImage(cvSize(  std::max(maxX,maxX2) + minX,
                                        std::max(maxY,maxY2) + minY)
                                        ,img2->depth,img2->nChannels);

                                                 std::cout << " 4 = \n" ;
 
        // H^(-1)
        H=H.inv();

        CvScalar s;
        // Display original 2nd image
        for (int i = 0; i < img2->height; ++i)
                for (int j = 0; j < img2->width; ++j)
                {
                        s = cvGet2D(img2, i, j);
                        cvSet2D(*result, i+minY, j+minX, s);
                }

        vector<double> x;
        // Remapping 1st image
        for (int i = -minY; i < ((*result)->height - minY); ++i)
                for (int j = -minX; j < ((*result)->width - minX); ++j)
                {
                        // Compute point's coordinates at remapping image
                        // x = H^(-1)*x'
                        x = matrixMultiplication(&H, j, i);

                        // If does computed point exists at 1st image
                        // note: x(0)=j' x(1)=i'
                        if (    x(1)>=0 && x(1)<img1->height &&
                                x(0)>=0 && x(0)<img1->width)
                        {
                                s = cvGet2D(img1, x(1), x(0));
                                cvSet2D(*result, i+minY, j+minX, s);
                        }
                }
//      cvNamedWindow("win");
//      cvShowImage("win", result);
        return 0;
}


int main(int argc, char *argv[])
{
        if (argc < 3) return -1;
        int  compression = 100;             //

        for (int i = 1; i < argc; ++i) {
                cvNamedWindow(argv[i]);
                images.push_back(cvLoadImage(argv[i]));
                cvShowImage(argv[i], images.at(i-1));
        }

        cv::initModule_nonfree();
        IplImage *result;
        
        std::cout << "Make a panorama from images:\n";
        for (int i = 0; i < images.size() - 1; ++i)
        {
                if (i > 0) images.at(i) = result;
                remapping(images.at(i), images.at(i+1), &result);
        }

        cvNamedWindow("win");
        cvShowImage("win", result);
        
        cvWaitKey();
      	  cvSaveImage(".ready.jpg", result, &compression);

        for (int i = 0; i < images.size(); ++i)
                cvReleaseImage(&images.at(i));
      
        cvReleaseImage(&result);
        cvDestroyAllWindows();

        return 0;
}