/*
 * Visualiser.cpp
 *
 *  Created on: Apr 23, 2020
 *      Author: lukas
 */

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

using namespace cv;
using namespace std;

/**
 * Creates a Visualiser class.
 *
 * Given a number of rows of a frame the size of the texts and markers
 * are set so the roughly fit the screen (we are not aiming for perfection here)
 *
 * @param rows int, amount of pixel along the y-axis in the frame
 */
Visualiser::Visualiser(int rows) {
	marker_size = 0.05 * rows;
	marker_thickness = 0.01 * rows;
	padding_x = 5;
	padding_y = 0.083 * rows;
	text_size = 0.0035 * rows;
	text_thickness = 0.009 * rows;

}

Visualiser::~Visualiser() {
	// TODO Auto-generated destructor stub
}

void Visualiser::plot(String name, Mat frame, Mat fgmask, const cvBlob &biggest_blob,
		  	  	      const vector<Point> &observations, const vector<Point> &predictions, const vector<Point> &corrected_measurements) {
	Mat obs_frame;
	Mat pred_frame;
	Mat corrected_meas_frame;
	Mat combined_frame;
	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

	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(name, 6, frame, fgmask, obs_frame, pred_frame, corrected_meas_frame, combined_frame);
}