#include <iostream>  
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>  
#include <opencv2/highgui/highgui.hpp> 
#include <opencv2/imgproc/imgproc.hpp>  // Gaussian Blur
#include <opencv2/ml/ml.hpp>


using namespace cv;
using namespace std;


void duan_OpticalFlow(Mat &frame, Mat & result);
bool
 
 addNewPoints();
bool acceptTrackedPoint(int i);


Mat curgray;    // 當前圖片
Mat pregray;    // 預測圖片
vector<Point2f> point[2];   // point0為特徵點的原來位置，point1為特徵點的新位置
vector<Point2f> initPoint;  // 初始化跟蹤點的位置
vector<Point2f> features;   // 檢測的特徵
int maxCount = 500;         // 檢測的最大特徵數
double qLevel = 0.01
 
;   // 特徵檢測的等級
double minDist = 10.0;  // 兩特徵點之間的最小距離
vector<uchar> status;   // 跟蹤特徵的狀態，特徵的流發現為1，否則為0
vector<float> err;

int main()
{

    Mat matSrc;
    Mat matRst;

    VideoCapture cap(0);
    //int totalFrameNumber = cap.get(CV_CAP_PROP_FRAME_COUNT);

    // perform the tracking process
    printf
 
("Start the tracking process, press ESC to quit.\n");
    while(cap.isOpened()) {
        // get frame from the video
        cap >> matSrc;
        if (!matSrc.empty())
        {
            duan_OpticalFlow(matSrc, matRst);

        }
        else
        {
            cout << "Error : Get picture is empty!" << endl;
        }
        if (waitKey(1) == 27) break;
    }


    waitKey(0);

    return 0;

}



void duan_OpticalFlow(Mat &frame, Mat & result)
{
    cvtColor(frame, curgray, CV_BGR2GRAY);
    frame.copyTo(result);

    if (addNewPoints())
    {
        goodFeaturesToTrack(curgray, features, maxCount, qLevel, minDist);
        point[0].insert(point[0].end(), features.begin(), features.end());
        initPoint.insert(initPoint.end(), features.begin(), features.end());
    }


    if (pregray.empty())
    {
        curgray.copyTo(pregray);
    }

    calcOpticalFlowPyrLK(pregray, curgray, point[0], point[1], status, err);


    int k = 0;
    for (size_t i = 0; i<point[1].size(); i++)
    {
        if (acceptTrackedPoint(i))
        {
            initPoint[k] = initPoint[i];
            point[1][k++] = point[1][i];
        }
    }


    point[1].resize(k);
    initPoint.resize(k);

    for (size_t i = 0; i<point[1].size(); i++)
    {
        line(result, initPoint[i], point[1][i], Scalar(0, 0, 255));
        circle(result, point[1][i], 3, Scalar(0, 255, 0), -1);
    }


    swap(point[1], point[0]);
    swap(pregray, curgray);


    imshow("Optical Flow Demo", result);
    //waitKey(50);
}


bool addNewPoints()
{
    return point[0].size() <= 10;
}


bool acceptTrackedPoint(int i)
{
    return status[i] && ((abs(point[0][i].x - point[1][i].x) + abs(point[0][i].y - point[1][i].y)) > 2);//檢測是否為移動點
}