#include <iostream>
#include <vector>
#include <opencv2/core/core.hpp>
#include <opencv2/video/tracking.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

int main(void)
{
  // Load the prev image
  cv::Mat prev = cv::imread("blocks1.png", 1);
  if(prev.empty()) return -1; 
  // Load the next image
  cv::Mat next = cv::imread("blocks2.png", 1);
  if(next.empty()) return -1; 

  std::vector<cv::Point2f> prev_pts;
  std::vector<cv::Point2f> next_pts;
  
  // Initialize
  cv::Size flowSize(30,30);
  cv::Point2f center = cv::Point(prev.cols/2., prev.rows/2.);
  for(int i=0; i<flowSize.width; ++i) {
    for(int j=0; j<flowSize.width; ++j) {
      cv::Point2f p(i*float(prev.cols)/(flowSize.width-1), 
                    j*float(prev.rows)/(flowSize.height-1));
      prev_pts.push_back((p-center)*0.9f+center);
    }
  }

  cv::Mat status, error;

  // Calc optical flows.
  cv::calcOpticalFlowPyrLK(prev, next, prev_pts, next_pts, status, error);

  // Show results.
  // If you need necessary, please remove the comment.
  /*
  std::vector<cv::Point2f>::const_iterator p = prev_pts.begin();
  std::vector<cv::Point2f>::const_iterator n = next_pts.begin();
  for(; n!=next_pts.end(); ++n,++p) {
    cv::line(prev, *p, *n, cv::Scalar(150,0,0),2);
  }
  
  cv::namedWindow("optical flow", CV_WINDOW_AUTOSIZE|CV_WINDOW_FREERATIO);
  cv::imshow("optical flow", prev);
  cv::waitKey(0);
  */

  return 0;

  // The heap error occurs after the end of main function.
}