#include <iostream>
#include <sstream>
#include <opencv2/opencv.hpp>

#include "kptracker.hpp"

#define INPUT     0                          // Number of input camera
#define TEMPLATE  "../data/rack01.tiff"
#define MASK      "../data/rack01_ROI.tiff"
//#define OUTPUT    "../data/rack01_outp.mp4"
#define OUT_IM    "../data/rack01_"
#define DELAY     1
#define TSCALE    1
#define SCALE     0.8
#define FPS       10
#define EIGEN_T   800
#define MAX_DISC  10
#define SENSITIV  1200000

using namespace cv;
using namespace std;

template<typename T>
int hamming_distance(T x, T y)
{
    int dist = 0;
    T val = x ^ y;

    // Count the number of bits set
    while (val != 0)
    {
        // A bit is set, so increment the count and clear the bit
        dist++;
        val &= val - 1;
    }

    // Return the number of differing bits
    return dist;
}

int main(int argc, char **argv) {
  
  // Read template image file
  Mat temp = imread(TEMPLATE, IMREAD_COLOR);
  
  // Read template mask
  Mat mask = imread(MASK, IMREAD_GRAYSCALE);
  
  if( temp.empty() || mask.empty() ){
    cerr << "Unable to read template image" << std::endl;
    return 1;
  }
  
  // Resize template and mask
  resize(temp, temp, Size(0,0), TSCALE, TSCALE, CV_INTER_LINEAR);
  resize(mask, mask, Size(0,0), TSCALE, TSCALE, CV_INTER_NN);
  
  // Open the input video file
  //VideoCapture input(VIDEO, CAP_FFMPEG);
  VideoCapture input(INPUT);
//   input.set(CAP_PROP_FRAME_WIDTH, 1280);
//   input.set(CAP_PROP_FRAME_HEIGHT, 720);
  
  if( !input.isOpened() ){
    cerr << "Unable to open video file" << std::endl;
    return 1;
  }
  
  // Get some video file properties
//   int frame_count = (int)input.get(CAP_PROP_FRAME_COUNT);
  
  // Output video
//   VideoWriter output;
//   int ex = input.get(CV_CAP_PROP_FOURCC);
//   Size S = Size(
//     input.get(CV_CAP_PROP_FRAME_WIDTH) * SCALE,
//     input.get(CV_CAP_PROP_FRAME_HEIGHT) * SCALE
//   );
//   output.open(OUTPUT, ex, FPS, S, true);
//   
//   if( !output.isOpened() ){
//     std::cerr << "Unable to open output video!" << std::endl;
//     return 1;
//   }
  
  // Create output window
  namedWindow("Out");
  
  // Create keypoint tracker and feed it with template image
  KpTracker tracker = KpTracker(temp, mask);
  std::cout << temp.size() << std::endl;
  
  // Template corner points
  Mat corners;
  corners.push_back(Point2f(0,          0));
  corners.push_back(Point2f(0,          temp.rows));
  corners.push_back(Point2f(temp.cols,  temp.rows));
  corners.push_back(Point2f(temp.cols,  0));
  
  // Some used variables
  std::vector<DMatch> matches;
  std::vector<Point> pts1, pts2;
  std::vector<KeyPoint> kp;
  Mat desc;
  Mat visualize;
  
  // Initial homography matrix
  Mat_<double> HH(3,3);
  HH << 1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1, 0;
        
  // Number of discarded homography estimation
  int n_discarded = 0;
  
  // Main loop
  Mat frame;
  Mat original;
  int key = 0;
  int imid = 0;
  while( 1 ) {
    // Get input frame, resize and convert to grayscale
    input >> frame;
    original = frame.clone();
    
    resize(frame, frame, Size(0,0), SCALE, SCALE, CV_INTER_LINEAR);
    
    // Detect and match keypoints
    tracker.DetectKeypoints(frame, noArray(), kp, desc);
    tracker.MatchKeypoints(desc, kp, pts1, pts2, matches);
    
    // Find homography
    Mat H;
    if( !pts1.empty() && !pts2.empty())
      H = findHomography(pts1, pts2, CV_RANSAC, 1);

    // If we found a homography show it
    if ( !H.empty() )
    {
      // Decide if the instanteneous matrix H is to be used
      // Calculate the eigen values of difference matrix
      Mat values;
      Mat dH  = H - HH;
      eigen(dH.t()*dH, values, noArray());
      
      // Find maximal eigenvalue
      double max = 0;
      for( int i = 0; i < dH.rows; i++ )
        if( max < values.at<double>(i)) max = values.at<double>(i);
        
      // Update homography if H is accepted (after MAX_DISC homography estimates
      // was discarded we accept one to get some estimation)
      if( max < EIGEN_T  ){
        HH = (2*HH + H) / 3;
        n_discarded = 0;
      }else if(n_discarded > MAX_DISC){
        HH = H;
        n_discarded = 0;
      }else{
        n_discarded++;
      }
      
      // Transform corner points
      Mat transformed;
      perspectiveTransform ( corners, transformed, HH );

      vector<vector<Point>> poly;
      poly.push_back ( transformed );

      // Draw transformed rectangle
      polylines ( frame, poly, true, Scalar ( 255,0,0 ), 2 );

      int xcenter = frame.cols / 2;
      int ycenter = frame.rows / 2;
      line( frame, Point(0, ycenter), Point(frame.cols, ycenter), 
            Scalar(0,0,255), 1);
      line( frame, Point(xcenter, 0), Point(xcenter, frame.rows), 
            Scalar(0,0,255), 1);

      // Calculate the rotation
      Mat_<double> A(2,2);
      A << HH.at<double>(0,0), HH.at<double>(0,1),
           HH.at<double>(1,0), HH.at<double>(1,1);
           
      Mat_<double> t(2,1);
      t << HH.at<double>(0,2), HH.at<double>(1,2);
      
      Mat_<double> v(2,1);
      v << HH.at<double>(2,0), HH.at<double>(2,1);
      
      A = A - t * v.t();
      Mat_<double> delta(2,1);
      delta << HH.at<double>(2,0) * SENSITIV,
               HH.at<double>(2,1) * SENSITIV;
      
      // Rotate the error vector
      Mat_<double> rot(2,2);
      rot << A(0,0), -A(1,0),
             A(1,0),  A(0,0);
      delta = rot * delta;

      circle( frame,
              Point(xcenter +  delta(0), ycenter + delta(1)),
              10, Scalar(0,0,255), 2);
    }

    // Draw some info into the frame
    std::ostringstream oss;
    oss << "NFeatures: T=" << tracker.mKp.size();
    oss << ", F=" << kp.size();
    oss << "; Matched=" << matches.size();
    putText(frame, oss.str(), Point(10,20), FONT_HERSHEY_PLAIN, 1.0,
            Scalar(0,0,0), 2);
    
    imshow("Out", frame);
    key = waitKey(DELAY);
    if( key == 'q' )
      break;
    if( key == ' ' ){
      std::string filename = std::string(OUT_IM);
      imwrite(filename + std::to_string(imid) + ".tiff", original);
      imid++;
    }
  }
  return 0;
}
