// Listing 15.2. Nauka modelu tła, aby móc identyfikować piksele pierwszego planu
#include <opencv2/opencv.hpp>
#include <iostream>
#include <cstdlib>
#include <fstream>

using namespace std;

// globalny magazyn
//
// zmiennoprzecinkowe trzykanałowe obrazy
//
cv::Mat image;
cv::Mat IavgF, IdiffF, IprevF, IhiF, IlowF;
cv::Mat tmp, tmp2, mask;

// zmiennoprzecinkowe jednokanałowe obrazy
//
vector<cv::Mat> Igray(3);
vector<cv::Mat> Ilow(3);
vector<cv::Mat> Ihi(3);

// jednokanałowy obraz bajtowy
//
cv::Mat Imaskt;

// progi
//
float high_thresh = 20.0;  // skalowanie progów w backgroundDiff()
float low_thresh = 28.0;

// // liczba poznanych obrazów do późniejszego uśredniania
//
float Icount;

// I jest tylko przykładowym obrazem na potrzeby alokacji
// (stanowi wzór rozmiaru)
//
void AllocateImages( const cv::Mat& I ) {
	cv::Size sz = I.size();
	IavgF = cv::Mat::zeros(sz, CV_32FC3 );
	IdiffF = cv::Mat::zeros(sz, CV_32FC3 );
	IprevF = cv::Mat::zeros(sz, CV_32FC3 );
	IhiF = cv::Mat::zeros(sz, CV_32FC3 );
	IlowF = cv::Mat::zeros(sz, CV_32FC3 );
	Icount = 0.00001; // ochrona przed dzieleniem przez zero
	tmp = cv::Mat::zeros( sz, CV_32FC3 );
	tmp2 = cv::Mat::zeros( sz, CV_32FC3 );
	Imaskt = cv::Mat( sz, CV_32FC1 );
}

// poznawanie danych statystycznych tła dla kolejnej klatki
// I jest próbką koloru tła, 3 kanały, 8u
//
void accumulateBackground( cv::Mat& I ){
	static int first = 1; // niebezpieczne wątkowo
	I.convertTo( tmp, CV_32F ); // konwersja na typ zmiennoprzecinkowy
	if( !first ){
		IavgF += tmp;
		cv::absdiff( tmp, IprevF, tmp2 );
		IdiffF += tmp2;
		Icount += 1.0;
	}
	first = 0;
	IprevF = tmp;
}

void setHighThreshold( float scale ) {
	IhiF = IavgF + (IdiffF * scale);
	cv::split( IhiF, Ihi );
}

void setLowThreshold( float scale ) {
	IlowF = IavgF - (IdiffF * scale);
	cv::split( IlowF, Ilow );
}

void createModelsfromStats() {
	IavgF *= (1.0/Icount);
	IdiffF *= (1.0/Icount);
	
	// różnica zawsze powinna mieć jakąś wartość
	//
	IdiffF += cv::Scalar( 1.0, 1.0, 1.0 );
	setHighThreshold( high_thresh);
	setLowThreshold( low_thresh);
}


// tworzy binarną maskę 0,255, w której 255 oznacza piksel pierwszego planu
// I jest obrazem wejściowym, 3 kanały, 8u
// Imask to obraz maski, który ma zostać utworzony, 1 kanał, 8u
//
void backgroundDiff(
		cv::Mat& I,
		cv::Mat& Imask) {
	
	I.convertTo( tmp, CV_32F ); // na typ zmiennoprzecinkowy
	cv::split( tmp, Igray );
	
	// kanał 1
	//
	cv::inRange( Igray[0], Ilow[0], Ihi[0], Imask );

	// kanał 2
	//
	cv::inRange( Igray[1], Ilow[1], Ihi[1], Imaskt );
	Imask = cv::min( Imask, Imaskt );

	// kanał 3
	//
	cv::inRange( Igray[2], Ilow[2], Ihi[2], Imaskt );
	Imask = cv::min( Imask, Imaskt );

	// na koniec odwracamy wyniki
	//
	Imask = 255 - Imask;
}

///////////////////
void help(char** argv ) {
	cout << "\n"
	<< "Szkoli model tła na <#frames to train on> klatkach filmu wejściowego i uruchamia ten model.\n"
	<< argv[0] <<" <#frames to train on> <avi_path/filename>\n"
	<< "Na przykład:\n"
	<< argv[0] << " 50 ../tree.avi\n"
	<< "A lub a do regulacji progów, esc, q lub Q do kończenia pracy"
	<< endl;
}

void showForgroundInRed( char** argv, const cv::Mat &img) {
		cv::Mat rawImage;
		cv::split( img, Igray );
		Igray[2] = cv::max( mask, Igray[2] );
		cv::merge( Igray, rawImage );
		cv::imshow( argv[0], rawImage );
		cv::imshow("Segmentation", mask);
}

void adjustThresholds(char** argv, cv::Mat &img) {
	int key = 1;
	while((key = cv::waitKey()) != 27 && key != 'Q' && key != 'q')  // Esc or Q or q to exit
	{
		if(key == 'L') { low_thresh += 0.2;}
		if(key == 'l') { low_thresh -= 0.2;}	
		if(key == 'H') { high_thresh += 0.2;}
		if(key == 'h') { high_thresh -= 0.2;}
		cout << "H or h, L or l, esq or q to quit;  high_thresh = " << high_thresh << ", " << "low_thresh = " << low_thresh << endl;
		setHighThreshold(high_thresh);
		setLowThreshold(low_thresh);
		backgroundDiff(img, mask);
		showForgroundInRed(argv, img);
	}
}

////////////////////////////////////////////////////////////////
int main( int argc, char** argv) {
	cv::namedWindow( argv[0], cv::WINDOW_AUTOSIZE );
	cv::VideoCapture cap;
	if((argc < 3)|| !cap.open(argv[2])) {
		cerr << "Nie udało się uruchomić programu." << endl;
		help(argv);
		cap.open(0);
		return -1;
	}
	int number_to_train_on = atoi( argv[1] );

	// PIERWSZA PĘTLA PRZETWARZANIA (SZKOLENIE):
	//
	int frame_count = 0;
	int key;
	bool first_frame = true;
	cout << "Liczba klatek szkoleniowych = " << number_to_train_on << endl; //db
	while(1) {
		cout << "frame#: " << frame_count << endl;
		cap >> image;
		if( !image.data ) exit(1); // Something went wrong, abort
		if(frame_count == 0) { AllocateImages(image);}
		accumulateBackground( image );
		cv::imshow( argv[0], image );
		frame_count++;
		if( (key = cv::waitKey(7)) == 27 || key == 'q' || key == 'Q' || frame_count >= number_to_train_on) break; // przerwanie programu spacją, esc lub q
	}

	// zakończyliśmy szkolenie, teraz tworzymy modele
	//
	cout << "Creating the background model" << endl;
	createModelsfromStats();
	cout << "Done!  Hit any key to continue into single step. Hit 'a' or 'A' to adjust thresholds, esq, 'q' or 'Q' to quit\n" << endl;
	
	// DRUGA PĘTLA PRZETWARZANIA (TESTOWANIE):
	//
	cv::namedWindow("Segmentation", cv::WINDOW_AUTOSIZE ); // For the mask image
	while((key = cv::waitKey()) != 27 || key == 'q' || key == 'Q'  ) { // esc, q lub Q zamykają program
		cap >> image;
		if( !image.data ) exit(0);
		cout <<  frame_count++ << endl;
		backgroundDiff( image, mask );
		cv::imshow("Segmentation", mask);

		// prosta wizualizacja zapisu w kanale czerwonym
		//
		showForgroundInRed( argv, image);
		if(key == 'a') {
			cout << "In adjust thresholds, 'H' or 'h' == high thresh up or down; 'L' or 'l' for low thresh up or down." << endl;
			cout << " esq, 'q' or 'Q' to quit " << endl;
			adjustThresholds(argv, image);
			cout << "Done with adjustThreshold, back to frame stepping, esq, q or Q to quit." << endl;
		}
	}
	exit(0);
}
