//*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                                License Agreement
//                       For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
//   * Redistribution's of source code must retain the above copyright notice,
//     this list of conditions and the following disclaimer.
//
//   * Redistribution's in binary form must reproduce the above copyright notice,
//     this list of conditions and the following disclaimer in the documentation
//     and/or other materials provided with the distribution.
//
//   * The name of Intel Corporation may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// the use of this software, even if advised of the possibility of such damage.
//
//M*/
#include "precomp.hpp"
#include <stdio.h>
#include <iostream>
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/contrib/hybridtracker.hpp"

using namespace cv;

CvFeatureTracker::CvFeatureTracker(CvFeatureTrackerParams _params) :
	params(_params)
{
	switch (params.feature_type)
	{
	case CvFeatureTrackerParams::SIFT:
        dd = Algorithm::create<Feature2D>("Feature2D.SIFT");
        if( dd.empty() )
            CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SIFT support");
        dd->set("nOctaveLayers", 5);
        dd->set("contrastThreshold", 0.04);
        dd->set("edgeThreshold", 10.7);
	case CvFeatureTrackerParams::SURF:
        dd = Algorithm::create<Feature2D>("Feature2D.SURF");
        if( dd.empty() )
            CV_Error(CV_StsNotImplemented, "OpenCV has been compiled without SURF support");
        dd->set("hessianThreshold", 400);
        dd->set("nOctaves", 3);
        dd->set("nOctaveLayers", 4);
    default:
        CV_Error(CV_StsBadArg, "Unknown feature type");    
	}

	matcher = new BFMatcher(NORM_L2);
}

CvFeatureTracker::~CvFeatureTracker()
{
}

void CvFeatureTracker::newTrackingWindow(Mat image, Rect selection)
{
	image.copyTo(prev_image);
	cvtColor(prev_image, prev_image_bw, CV_BGR2GRAY);
	prev_trackwindow = selection;
	prev_center.x = selection.x;
	prev_center.y = selection.y;
	ittr = 0;
}

Rect CvFeatureTracker::updateTrackingWindow(Mat image)
{
	if(params.feature_type == CvFeatureTrackerParams::OPTICAL_FLOW)
		return updateTrackingWindowWithFlow(image);
	else
		return updateTrackingWindowWithSIFT(image);
}

Rect CvFeatureTracker::updateTrackingWindowWithSIFT(Mat image)
{
	ittr++;
	vector<KeyPoint> prev_keypoints, curr_keypoints;
	vector<Point2f> prev_keys, curr_keys;
	Mat prev_desc, curr_desc;

	Rect window = prev_trackwindow;
	Mat mask = Mat::zeros(image.size(), CV_8UC1);
	rectangle(mask, Point(window.x, window.y), Point(window.x + window.width,
			window.y + window.height), Scalar(255), CV_FILLED);

	dd->operator()(prev_image, mask, prev_keypoints, prev_desc);

	window.x -= params.window_size;
	window.y -= params.window_size;
	window.width += params.window_size;
	window.height += params.window_size;
	rectangle(mask, Point(window.x, window.y), Point(window.x + window.width,
			window.y + window.height), Scalar(255), CV_FILLED);

	dd->operator()(image, mask, curr_keypoints, curr_desc);

	if (prev_keypoints.size() > 4 && curr_keypoints.size() > 4)
	{
		//descriptor->compute(prev_image, prev_keypoints, prev_desc);
		//descriptor->compute(image, curr_keypoints, curr_desc);

		matcher->match(prev_desc, curr_desc, matches);

		for (int i = 0; i < (int)matches.size(); i++)
		{
			prev_keys.push_back(prev_keypoints[matches[i].queryIdx].pt);
			curr_keys.push_back(curr_keypoints[matches[i].trainIdx].pt);
		}

		Mat T = findHomography(prev_keys, curr_keys, CV_LMEDS);

		prev_trackwindow.x += cvRound(T.at<double> (0, 2));
		prev_trackwindow.y += cvRound(T.at<double> (1, 2));
	}

	prev_center.x = prev_trackwindow.x;
	prev_center.y = prev_trackwindow.y;
	prev_image = image;
	return prev_trackwindow;
}

Rect CvFeatureTracker::updateTrackingWindowWithFlow(Mat image)
{
	ittr++;
	Size subPixWinSize(10,10), winSize(31,31);
	Mat image_bw;
	TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03);
	vector<uchar> status;
	vector<float> err;

	cvtColor(image, image_bw, CV_BGR2GRAY);
	cvtColor(prev_image, prev_image_bw, CV_BGR2GRAY);

	if (ittr == 1)
	{
		Mat mask = Mat::zeros(image.size(), CV_8UC1);
		rectangle(mask, Point(prev_trackwindow.x, prev_trackwindow.y), Point(
				prev_trackwindow.x + prev_trackwindow.width, prev_trackwindow.y
						+ prev_trackwindow.height), Scalar(255), CV_FILLED);
		goodFeaturesToTrack(image_bw, features[1], 500, 0.01, 20, mask, 3, 0, 0.04);
		cornerSubPix(image_bw, features[1], subPixWinSize, Size(-1, -1), termcrit);
	}
	else
	{
		calcOpticalFlowPyrLK(prev_image_bw, image_bw, features[0], features[1],
				status, err, winSize, 3, termcrit);

		Point2f feature0_center(0, 0);
		Point2f feature1_center(0, 0);
		int goodtracks = 0;
		for (int i = 0; i < (int)features[1].size(); i++)
		{
			if (status[i] == 1)
			{
				feature0_center.x += features[0][i].x;
				feature0_center.y += features[0][i].y;
				feature1_center.x += features[1][i].x;
				feature1_center.y += features[1][i].y;
				goodtracks++;
			}
		}

		feature0_center.x /= goodtracks;
		feature0_center.y /= goodtracks;
		feature1_center.x /= goodtracks;
		feature1_center.y /= goodtracks;

		prev_center.x += (feature1_center.x - feature0_center.x);
		prev_center.y += (feature1_center.y - feature0_center.y);

		prev_trackwindow.x = (int)prev_center.x;
		prev_trackwindow.y = (int)prev_center.y;
	}

	swap(features[0], features[1]);
	image.copyTo(prev_image);
	return prev_trackwindow;
}

void CvFeatureTracker::setTrackingWindow(Rect _window)
{
	prev_trackwindow = _window;
}

Rect CvFeatureTracker::getTrackingWindow()
{
	return prev_trackwindow;
}

Point2f CvFeatureTracker::getTrackingCenter()
{
	Point2f center(0, 0);
	center.x = (float)(prev_center.x + prev_trackwindow.width/2.0);
	center.y = (float)(prev_center.y + prev_trackwindow.height/2.0);
	return center;
}