/* Applied Video Analysis of Sequences (AVSA)
 *
 *	LAB3: Kalman Filter
 */

//system libraries C/C++
#include <stdio.h>
#include <iostream>
#include <sstream>

//opencv libraries
#include <opencv2/opencv.hpp>
#include <opencv2/video/background_segm.hpp>

//Header ShowManyImages
#include "ShowManyImages.hpp"
#include "ObservationGenerator.hpp"
#include "KalmanWrapper.hpp"
#include "Visualiser.hpp"

//namespaces
using namespace cv; //avoid using 'cv' to declare OpenCV functions and variables (cv::Mat or Mat)
using namespace std;

int main(int argc, char ** argv)
{

	if (argc < 2)
	{
		cout << "At least one path has to be passed as an input argument. Ending program." << endl;
		return 0;
	}

	double t, acum_t; // variables for execution time
	int t_freq = getTickFrequency();

	// Loop over all videos
	int num_videos = argc - 1;
	for (int i = 1; i <= num_videos; i++)
	{

		// Check whether video exists
		VideoCapture cap;
		cap.open(argv[i]);
		if (!cap.isOpened())
		{
			cout << "Could not open video file " << argv[i] << endl;
			return -1;
		}

		ObservationGenerator observation_generator;
		KalmanWrapper kalman_wrapper("constant_velocity");
//		KalmanWrapper kalman_wrapper("constant_acceleration");
		kalman_wrapper.setParams(argv[i]);

		Mat img; // current Frame
		Mat frame; // copy of current Frame (passed to algorithms)
		vector<Point> observations;
		vector<Point> predictions;
		vector<Point> corrected_measurements;

		int it = 1; // Frame counter
		acum_t = 0; // Time accumulator

		bool initial_observation_made = false;

		// do the Background substraction once before to initialize the bkgs model
		// and the Visualiser class (which depends on the amount of rows of the frames)
		cap >> img;
		img.copyTo(frame);
		observation_generator.doBackGroundSubstraction(frame);
		Visualiser visualiser(frame.rows);

		// Main loop
		for (;;) {

			Mat observation = Mat::zeros(2, 1, CV_32F); // current observation [X, Y]^T of biggest blob
			Mat prediction = Mat::zeros(2, 1, CV_32F); // prediction of kalman filter
			Mat corrected_measurement = Mat::zeros(2, 1, CV_32F); // correction of the observation given observation

			//get frame
			cap >> img;
			img.copyTo(frame);

			// Check if we achieved the end of  the file (e.g. img.data is empty)
			if (!img.data)
				break;

			// Time measurement
			t = (double)getTickCount();

			/** ALGORITHMS START **/

			// 1. Generate observation, i.e. the center of the biggest blob
			int result_code = observation_generator.generateObservation(frame, observation);

			// if an initial observation was made initialise the X and Y based on observation
			if (!initial_observation_made && result_code == 1) {
				initial_observation_made = true;
				kalman_wrapper.setInitialXY(observation);
			}


			// 2. Do Kalman filtering once we had an inital observation
			if (initial_observation_made) {
				kalman_wrapper.generatePrediction(prediction);
				predictions.push_back(Point(prediction.at<float>(0,0),
											prediction.at<float>(1,0)));
				observations.push_back(Point(observation));

				if (result_code == 1) {
					// Observation made, we can correct the observation
					kalman_wrapper.generateCorrection(observation, corrected_measurement);
					corrected_measurements.push_back(Point(corrected_measurement.at<float>(0,0),
														   corrected_measurement.at<float>(1,0)));
				}
			}


			/** ALGORITHMS END **/


			// Time measurement
			t = (double)getTickCount() - t;
			acum_t += t;

			// Comment: We tried to encapsule the visualisation into a class (Visualiser), but we were running into problems with passing
			// OpenCV matrices to other functions which we were unable to resolve after hours of trying. Therefore, the code
			// remains in the main function.
			// Maybe for the next cohort, instead of having the first AVSA slots for the c++ basics, make it specifically
			// about OpenCV and c++, as most of us knew the c++ basics but are unable to handle the OpenCV specifics. Like in
			// this case.

			// SHOW RESULTS
			// Markers and text size dependent on height of frame
			double marker_size = 0.05 * frame.rows;
			double marker_thickness = 0.01 * frame.rows;
			double padding_x = 5;
			double padding_y = 0.083 * frame.rows;
			double text_size = 0.0035 * frame.rows;
			double text_thickness = 0.009 * frame.rows;
			Mat obs_frame;
			Mat pred_frame;
			Mat corrected_meas_frame;
			Mat combined_frame;
			Mat fgmask = observation_generator.getFgMask();
			frame.copyTo(obs_frame);
			frame.copyTo(pred_frame);
			frame.copyTo(corrected_meas_frame);
			frame.copyTo(combined_frame);

			for (int i = 0; i < predictions.size(); i++) {
				drawMarker(obs_frame, observations[i], Scalar(46, 204, 113), MARKER_SQUARE, marker_size, marker_thickness);
				drawMarker(pred_frame, predictions[i], Scalar(241, 196, 15), MARKER_CROSS, marker_size, marker_thickness);
				drawMarker(combined_frame, predictions[i], Scalar(241, 196, 15), MARKER_CROSS, marker_size, marker_thickness);
			}

			for (int i = 0; i < corrected_measurements.size(); i++){
				drawMarker(corrected_meas_frame, corrected_measurements[i], Scalar(52, 152, 219), MARKER_SQUARE, marker_size, marker_thickness);
				drawMarker(combined_frame, corrected_measurements[i], Scalar(52, 152, 219), MARKER_SQUARE, marker_size, marker_thickness);
			}

			putText(frame, "Frame", Point(padding_x,padding_y), FONT_HERSHEY_SIMPLEX, text_size, Scalar(255, 255, 255), text_thickness);
			putText(fgmask, "Foreground Mask", Point(padding_x,padding_y), FONT_HERSHEY_SIMPLEX, text_size, Scalar(255, 255, 255), text_thickness);
			putText(obs_frame, "Measurement", Point(padding_x,padding_y), FONT_HERSHEY_SIMPLEX, text_size, Scalar(46, 204, 113), text_thickness);
			putText(pred_frame, "Prediction", Point(padding_x,padding_y), FONT_HERSHEY_SIMPLEX, text_size, Scalar(241, 196, 15), text_thickness);
			putText(corrected_meas_frame, "Corrected Measurement", Point(padding_x,padding_y), FONT_HERSHEY_SIMPLEX, text_size, Scalar(52, 152, 219), text_thickness);
			putText(combined_frame, "Prediction", Point(padding_x,padding_y), FONT_HERSHEY_SIMPLEX, text_size, Scalar(241, 196, 15), text_thickness);
			putText(combined_frame, "Corrected Measurement", Point(padding_x,2*padding_y), FONT_HERSHEY_SIMPLEX, text_size, Scalar(52, 152, 219), text_thickness);


			// Add bounding box of biggest detected blob to the frame
			cvBlob biggest_blob = observation_generator.getBlobMask();


			Point p1 = Point(biggest_blob.x, biggest_blob.y);
			Point p2 = Point(biggest_blob.x+biggest_blob.w, biggest_blob.y+biggest_blob.h);
			rectangle(frame, p1, p2, Scalar(15,196,241), 3, 8, 0);

			ShowManyImages(argv[i], 6, frame, fgmask, obs_frame, pred_frame, corrected_meas_frame, combined_frame);

//			Mat fgmask = observation_generator.getFgMask();
//			cvBlob biggest_blob = observation_generator.getBlobMask();
//			visualiser.plot(argv[i], frame, fgmask, biggest_blob, observations, predictions, corrected_measurement);

			// exit if ESC key is pressed
			if(waitKey(30) == 27) break;
			it++;


		}
		cout << it-1 << " frames processed in " << 1000*acum_t/t_freq << " milliseconds."<< endl;

		// Release all resources
		cap.release();
		destroyAllWindows();
		waitKey(0); // (should stop till any key is pressed .. doesn't!!!!!)

	}
	return 0;
}