/*
 * Copyright (c) 2015-2017, Sumio Morioka
 * All rights reserved.
 *
 * This source code was originally written by Dr.Sumio Morioka for use in the 2015-2017 issues of 
 * "the Interface magazine", published by CQ publishing Co.Ltd in Japan (http://www.cqpub.co.jp).
 * The author has no responsibility on any results caused by using this code.
 *
 * - Author: Dr.Sumio Morioka (http://www002.upp.so-net.ne.jp/morioka, FB:Sumio Morioka)
 *
 *
 * IMPORTANT NOTICE:
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions 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.
 *   * Neither the name of the copyright holder nor the
 *     names of its contributors may 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 COPYRIGHT HOLDER 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.
 */

#include <time.h>
#include <pthread.h>

#include "cv.h"
#include "highgui.h"
//using namespace cv;		// comment in if preferable
#include <opencv2/opencv.hpp>		// added; OpenCV 3.1
#include <opencv2/tracking.hpp>		// added; OpenCV 3.1

#include "opencv_func.h"
#include "websocket_flags.h"

//////////////////////////////////////////////////////////////////////////// >>
// ADD YOUR ORIGINAL CODE HERE
// any functions/global variables
//////////////////////////////////////////////////////////////////////////// <<

void opencv_func_init(void *)
{
	if (wantPreview != 0) {
		cv::namedWindow(WINNAME, CV_WINDOW_AUTOSIZE);
	}

//////////////////////////////////////////////////////////////////////////// >>
// ADD YOUR ORIGINAL CODE HERE
// preparation for image processing
	pthread_mutex_init(&tracker_mutex_G, NULL);		// init mutex
#ifdef IMG_COLOR
	tracker_image_G.create(cv::Size(DEFAULT_WIDTH, DEFAULT_HEIGHT), CV_8UC3);
#endif
#ifdef IMG_GRAY
	tracker_image_G.create(cv::Size(DEFAULT_WIDTH, DEFAULT_HEIGHT), CV_8UC1);
#endif

	tracking_status_G	= TRACKING_NONE;
	tracking_moved_G	= 0;
//////////////////////////////////////////////////////////////////////////// <<
}


void opencv_func_run(IMG_PROCESSING_PARAM *param)
{
	cv::cvtColor(*(param->srcImage), *(param->srcImage), CV_BGR2RGB);	// modify color

//////////////////////////////////////////////////////////////////////////// >>
// ADD YOUR ORIGINAL CODE HERE
// image processing main part
	{
		int src_x	= (*(param->srcImage)).cols;
		int src_y	= (*(param->srcImage)).rows;

		time_t		t;
		struct tm	*t_st;
		char strbuf[256];

		// change image orientation
		cv::Mat	rotmat;
		rotmat.create(cv::Size(3, 2), CV_32FC1);
		rotmat	= cv::getRotationMatrix2D(cv::Point(src_x / 2, src_y / 2), 180, 1);					// center, angle, scale
		cv::warpAffine(*(param->srcImage), *(param->dstImage), rotmat, cv::Size(src_x, src_y));		// rotate

		// update tracking status
		pthread_mutex_lock(&tracker_mutex_G);	// lock
#ifdef IMG_COLOR
		memcpy(tracker_image_G.data, (*(param->dstImage)).data, DEFAULT_WIDTH * DEFAULT_HEIGHT * 3);	// set image
#endif
#ifdef IMG_GRAY
		memcpy(tracker_image_G.data, (*(param->dstImage)).data, DEFAULT_WIDTH * DEFAULT_HEIGHT);		// set image
#endif

		if (tracking_status_G == TRACKING_PREP) {	// start tracking
			tracking_status_G	= TRACKING_RUN;

//			tracker_G	= cv::Tracker::create("KCF");
//			tracker_G	= cv::Tracker::create("BOOSTING");
//			tracker_G	= cv::Tracker::create("MIL");
			tracker_G	= cv::Tracker::create("TLD");
//			tracker_G	= cv::Tracker::create("MEDIANFLOW");

			tracker_G->init(tracker_image_G, tracking_roi_G);	// IMPORTANT; do not refer *(param->dstImage)

			tracking_x_G	= tracking_roi_G.x;
			tracking_y_G	= tracking_roi_G.y;
			tracking_w_G	= tracking_roi_G.width;
			tracking_h_G	= tracking_roi_G.height;

			tracking_moved_G	= 1;	// (location change)

			cv::rectangle(*(param->dstImage), cv::Point(tracking_roi_G.x, tracking_roi_G.y), cv::Point(tracking_roi_G.x + tracking_roi_G.width, tracking_roi_G.y + tracking_roi_G.height), CV_RGB(255, 0, 0), 1, CV_AA);

			fprintf(stderr, "tracking start (%d %d %d %d)\n", tracking_x_G, tracking_y_G, tracking_w_G, tracking_h_G);
		}
		else if (tracking_status_G == TRACKING_RUN) {
			if (tracker_G->update(tracker_image_G, tracking_roi_G) == true) {	// IMPORTANT; do not refer *(param->dstImage)
				int	x, y, w, h;

				x	= tracking_roi_G.x;
				y	= tracking_roi_G.y;
				w	= tracking_roi_G.width;
				h	= tracking_roi_G.height;

				if (x != tracking_x_G || y != tracking_y_G || w != tracking_w_G || h != tracking_h_G) {
					tracking_x_G	= x;
					tracking_y_G	= y;
					tracking_w_G	= w;
					tracking_h_G	= h;

					tracking_moved_G	= 1;	// (location change)

					fprintf(stderr, "tracking running (%d %d %d %d)\n", tracking_x_G, tracking_y_G, tracking_w_G, tracking_h_G);
				}
//				else {
//					tracking_moved_G	= 0;	do not clear here
//				}

				cv::rectangle(*(param->dstImage), cv::Point(tracking_roi_G.x, tracking_roi_G.y), cv::Point(tracking_roi_G.x + tracking_roi_G.width, tracking_roi_G.y + tracking_roi_G.height), CV_RGB(0, 0, 255), 1, CV_AA);
			}
			else {
				tracker_G->clear();
				tracking_status_G	= TRACKING_NONE;
				tracking_moved_G	= 2;		// NOTE; force stop

				fprintf(stderr, "tracking lost\n");
			}
		}
		pthread_mutex_unlock(&tracker_mutex_G);	// unlock

		// write info on image
//		time(&t);
//		t_st	= localtime(&t);
//
//		sprintf(strbuf, "%d/%d/%d %02d:%02d:%02d", 
//						t_st->tm_year + 1900, t_st->tm_mon + 1, t_st->tm_mday,
//						t_st->tm_hour, t_st->tm_min, t_st->tm_sec);

		int	v1	= receive_serial16(0x0D);
		sprintf(strbuf, "Direction %d.%d",
					v1 / 100, v1 % 100
				);

//		cv::putText(*(param->dstImage), strbuf, cv::Point(20,20), cv::FONT_HERSHEY_PLAIN, 1.0, cv::Scalar(255,255,255), 1, CV_AA);
		cv::putText(*(param->dstImage), strbuf, cv::Point(10,10), cv::FONT_HERSHEY_PLAIN, 0.5, cv::Scalar(255,255,255), 1, CV_AA);
	}
//////////////////////////////////////////////////////////////////////////// <<
}


void opencv_func_stop(void *)
{
	if (wantPreview != 0) {
		cv::destroyWindow(WINNAME);
	}

//////////////////////////////////////////////////////////////////////////// >>
// ADD YOUR ORIGINAL CODE HERE
// release resources for program termination
//////////////////////////////////////////////////////////////////////////// <<
}

// end of file
