// Opletal Tomáš
// Bakalářská práce
// VUT Brno

// Tato aplikace se využívá pro bezkontaktní měření rychlosti pohybu měřeného objektu v reálném čase.
// Využívá kameru DMK 33UP5000 od firmy TheImagingSource a objektiv 4:1, od firmy meopta.
// Tento kód je rozdělen do dvou částí

// První částí je získávání snímku z kamery. To je realizováno pomocí oficiální oficiální C++ knihovny od výrobce této kamery. 
// URL: https://www.theimagingsource.com/support/documentation/ic-imaging-control-cpp/
// Z této knihovny je dále využit cvičný příklad, který je ve složce ZeroCopyThirdParty pro nakládání se snimky z kamery.
// Jedná se v podstatě o třídu Listener, se třemi metodami.
// Metoda frameReceived() se zavolá vždy, když se je k dispozici aktuální snímek z kamery. Proto v ní je veškere zpracování obrazu. 
// Algoritmus pro zpracovani obrazu je dílem autora.
// Metoda sinkConnected() a sinkDisconnected() se volaji pouze jednou a to při přepnutí z a do live módu.


#include <tisudshl.H>
#include <string>
#include <opencv2/opencv.hpp>


using namespace DShowLib;
using namespace cv;

const int frequency = 30;  // Konstatní hodnota frekvence, je nastavitelná v závislosti na typu kamery

class Listener: public DShowLib::FrameNotificationSinkListener
	{
	private:
		std::clock_t start;   // Proměnná určená pro měření času
		cv::Mat m_output;     // Proměnná určená pro uložení aktuálního snímku z kamery
		cv::Mat help_Variable;// Proměnná určena pro uložení aktuálního snímku a použití v následujicí iteraci
		cv::Mat result;       // Promenná určená pro uložení výsledku funkce matchTemplate
		cv::Mat image1;       // Promenná určená pro uložení oříznutého snímku, který je uložen v proměnné help_Variable
		cv::Mat image2;       // Promenná určená pro uložení oříznutého snímku, který je uložen v proměnné m_output
		cv::Mat templ;	      // Proměnná určená pro uložení šablony, získáne z proměnné image1
		cv::Point s = { 200,400 }; // Bod, označující levý horní rok obdelníku, který ořízneme z snímků image1 a image2
		cv::Point b = { 540,40 }; // Bod, označující levý horní rok obdelníku, který ořízneme z snímků image1. 
		double minVal = 0; double maxVal = 0; cv::Point minLoc = { 0, 0 }; cv::Point maxLoc = { 0,0 }; // Proměnné, které jsou argumentem funkce minMaxLoc
		const double transfer_Constant = 4.8 / 1000000; // Konstatní proměnná, do které uložíme konstantu, která označuje převod mezi rychlosti v digitální pdoobě a skutečnou rychlostí měřeného objektu
		double time = 1.0 / frequency; // Proměnná, ve které je uložen čas mezi dvěmi snímky. Je závislý na frekvenci f
		double speed = 0.0;      // Proměnná, ve které je uložena skutečná rychlost měřeného objektu
		double duration = 0.0; // Promenná, ve které je uložen hodnota času vykonání jedné iterace programu
		int width = 600;       // Proměnná, ve které je uložena šířka obdelníku, který získáme z proměnn= m_output a help_Variable
		int height = 100;      // Proměnná, ve které je uložena výška obdelníku, který získáme z proměnn= m_output a help_Variable
		int size = 60;         // Proměnná, ve které je uložena velikost šablony, získavaná z proměnné image1
		int feed = 0;          // Proměnná, ve které je uložen celkový posun mezi dvěmi snímky
		int feedX = 0;		   // Proměnná, ve které je uložen posun mezi dvěmi snímky v x souradnici
		int feedY = 0;		   // Proměnná, ve které je uložen posun mezi dvěmi snímky v y souradnici
		bool first = 0;        // Pomopcná proměnná, sloužící jako indikátor, zda se má vykonat větev if nebo else. Větev if se vykoná pouze jednou
		size_t i = 0;          // Pomocná proměnná sloužící k testovacím účelům

	public:
		// Tato metoda se provede pouze jednou v případě, že kamera se přepne do live módu pomocí příkazu grabber.StartLive(). 
		virtual void sinkConnected(const DShowLib::FrameTypeInfo& frameType)
			{
			// Alokace paměti pro proměnné typu Mat, do kterých se ukládají snímky z kamery
			m_output.create(frameType.dim.cy, frameType.dim.cx, CV_8UC1);
			result.create(height - size + 1, width - size + 1, CV_32FC1);
			help_Variable.create(frameType.dim.cy, frameType.dim.cx, CV_8UC1);
			}
		// Tato metoda se provede pouze jednou v případě, že kamera se kamera ukončí live mód pomocí příkazu grabber.stopLive()
		virtual void sinkDisconnected()
			{
			// Odalokace paměti
			m_output.release();
			result.release();
			help_Variable.release();
			}
		// Tato metoda je zavolaná vždy, když tzv. sink obdrží snímek z kamery. Proto se v ní nachází celý program zpracování obrazu
		virtual void frameReceived(DShowLib::IFrame& frame)
			{

			if(first == 0)  // Pro zpracování obrazu je potřeba dvou snímku. Proto v první iteraci pouze získám aktuální snímek a ten uložím. Tato větev se provede pouze jednou
				{
				DShowLib::FrameTypeInfo ft = frame.getFrameType();
				// Vytvoření dočasné proměnné typu Mat, která ukazuje na paměť, kde je uložen aktuální snímek
				cv::Mat input(ft.dim.cy, ft.dim.cx, CV_8UC1, frame.getPtr());

				// Jelikož snímky z kamery přicházejí invertované kolem x osy, do pamětí se ukládá invertovaný snímek vhodný pro další zpracování
				cv::flip(input, help_Variable, 0);
				first = true;
				}
			else
				{
				start = std::clock(); // Čas začátku aktuální iterace ukládám do proměnné start
				DShowLib::FrameTypeInfo ft = frame.getFrameType();
				cv::Rect r(s.x, s.y, width, height); // Definice velikosti a umištění levého horního bodu obdelníku, který se ořízné z proměnných help_Variable a m_output a uloží do proměnných image1 a image2
				cv::Rect k(b.x, b.y, size, size);   // Definice velikosti a umištění levého horního bodu obdelníku, který se ořízné z proměnné image1, reprezentující šablonu
				// Menší testy, zdali vše sedí
				assert(ft.dim.cx == m_output.cols);
				assert(ft.dim.cy == m_output.rows);

				// Vytvoření dočasné proměnné typu Mat, která ukazuje na paměť, kde je uložen aktuální snímek
				cv::Mat input(ft.dim.cy, ft.dim.cx, CV_8UC1, frame.getPtr());

				// Jelikož snímky z kamery přicházejí invertované kolem x osy, do pamětí se ukládá invertovaný snímek vhodný pro další zpracování
				cv::flip(input, m_output, 0);
				// Následuje zpracování obrazu
				image1 = help_Variable(r);
				image2 = m_output(r);
				templ = image1(k);
				matchTemplate(image2, templ, result, TM_CCOEFF_NORMED);
				normalize(result, result, 0, 1, NORM_MINMAX, -1, Mat());
				minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, Mat());
				feedY = maxLoc.y - b.y;
				feedX = maxLoc.x - b.x;
				feed = sqrt(pow(feedX, 2) + pow(feedY, 2));             // Získání posuvu mezi dvěmi snímky v pixelech
				speed = (feed / time) * transfer_Constant;
				help_Variable = m_output.clone();  // Do této proměnné zkopirují aktuální snímek pro využití v další iteraci

				duration = (std::clock() - start) / (double)CLOCKS_PER_SEC; // Zjištění, jak dlouho trvala jedna iterace 
				if(i % 10 == 0)                     // Tisk výsledku do konzole. Pro testovací účely není potřeba tisknout výsledky s frekvenci 30 hz, dle potřeby je možno modifikovat
					{
					std::cout << duration << std::endl;
					std::cout << "Feed in the Y axis is  " << feedY << std::endl;
					std::cout << "Speed is   " << speed << "m/s" << std::endl;
					}
				i++;
				}

			}
	};



int main()
	{

	DShowLib::InitLibrary();				// Inicializace knihovny
	atexit(DShowLib::ExitLibrary);

	DShowLib::Grabber grabber;				// Hlavní třídá této knihovny. Více info je v dokumentaci k této knihovně 
	const long int exposureValue = -14;		// Tato constantní proměnná se využívá pro uchování hodnoty doby expozice. Lze nastavit od -14 do +3, kde toto číslo vyznačuje mocninu čísla dva

	// Pokud zařízení nelze nalést, ukonči program. Stává se pokud kamera není připojená
	if(!grabber.openDev("DMK 33UP5000"))
		{
		std::cout << "Device cant be found" << std::endl;
		return 1;
		}
	// Tato metoda nastaví formát připojenné kamery. Rozsah možných hodnot je v dokumentaci ke kameře.
	if(!grabber.setVideoFormat("Y800 (1280x720)"))
		{
		std::cout << "Cant set video format" << std::endl;
		return 2;
		}
	// Nastavení snímkovací frekvence kamery
	grabber.setFPS(frequency);
	// Pomocí této třídy lze manipulovat s dobou expozice. Více info je v dokumentaci
	tIVCDSwitchPropertyPtr autoexposure = grabber.getVCDPropertyInterface<IVCDSwitchProperty>(VCDID_Exposure, VCDElement_Value);
	if(autoexposure != NULL)
		{
		autoexposure->setSwitch(false);
		}
	// Touto metodou se nastavuje doba expozice kamery
	tIVCDRangePropertyPtr exposure = grabber.getVCDPropertyInterface<IVCDRangeProperty>(VCDID_Exposure, VCDElement_Value);
	exposure->setValue(exposureValue);
	// Pro kontrolu se vytisknou hodnoty parametrů snímkovací frekvence, rozlišení a doby expozice do konzole 
	std::cout << "Snimkovaci frekvence je " << grabber.getFPS() << std::endl;
	std::cout << "Rozliseni kamery je " << grabber.getAcqSizeMaxX() << " x " << grabber.getAcqSizeMaxY() << std::endl;
	std::cout << "Doba expozice je " << exposure->getValue() << std::endl;

	// Pro minimální datový tok se zakáže filtr překrytí
	grabber.setOverlayBitmapPathPosition(DShowLib::ePP_NONE);

	Listener listener; // vytovření instance třídy Listener s názvem listener
	DShowLib::tFrameNotificationSinkPtr pSink = DShowLib::FrameNotificationSink::create(listener); // vytvoření FrameNotificationSink
	if(pSink == NULL) // Lednoduchá kontrola, zdali se povedlo vytvořit sink
		{
		std::cout << "Sink could not be choosen" << std::endl;
		}
	// Jednodcuhá kontrola, zdali se povedlo připojit vytvořeny sink ke grabberu
	if(!grabber.setSinkType(pSink))
		{
		std::cout << "Cant be changed sink destination" << std::endl;
		}

	// Vytvoření OpenCV okna
	cv::namedWindow("display", cv::WINDOW_NORMAL);
	cv::setWindowTitle("display", "Press any key to exit");

	// Přepnutí kamery do live módu, v případě chyby, vytisknutí chyby do konzole
	if(!grabber.startLive(false))
		{
		std::cout << grabber.getLastError().toString().c_str();
		return 3;
		}
	// Čekání na jakékoliv stisknutí klávesy pro ukončení

	cv::waitKey();

	// Vypnutí live módu
	grabber.stopLive();

	return 0;
	}
