#include <stdio.h>
#include <iostream>

#include "i3d/image3d.h"
#include "i3d/vector3d.h"
#include "i3d/morphology.h"
#include "pasteImage.h"

using namespace std;
using namespace i3d;


void PatternSearch(Image3d<GRAY8> const &in,
						 Image3d<GRAY8> const &pattern,
						 Vector3d<int> const &sFrom,
						 Vector3d<int> const &sTo,
						 Vector3d<int> &pos)
{
	//time-saver :-)
	const i3d::Vector3d<size_t> centre=pattern.GetSize()/(size_t)2;

	//zatim nejlepsi shoda
	long bestSAVD=999999999999;

	//pro vhodny podprostor vstupniho prostoru hledej dany pattern
	for (size_t y=sFrom.y-centre.y; y < sTo.y-centre.y; ++y)
		for (size_t x=sFrom.x-centre.x; x < sTo.x-centre.x; ++x)
		{
			//spocitej aktualni shodu
			long SAVD=0;
			for (size_t yy=0; yy < pattern.GetSizeY(); yy+=2)
				for (size_t xx=0; xx < pattern.GetSizeX(); xx+=2)
				{
					float tmp=(float)in.GetVoxel(x+xx,y+yy,0) - (float)pattern.GetVoxel(xx,yy,0);
					SAVD+=(long)fabsf(tmp);
				}

			//zapamatuj si pozici zatim nejlepsi shody
			if (SAVD < bestSAVD)
			{
				bestSAVD=SAVD;
				pos.x=x+centre.x;
				pos.y=y+centre.y;
			}
		}
}


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

/*
	//nacteni vstupniho obrazu
	Image3d<GRAY8> ref;
	ref.ReadImageBMP("v1-gray/reference.bmp");

	//nacteni vzoru
	Image3d<GRAY8> lp,rp;
	lp.ReadImageBMP("v1-gray/leftTemplate.bmp");
	rp.ReadImageBMP("v1-gray/rightTemplate.bmp");

	//urceni referencni pozice vzoru uvnitr ref. obrazu
	Vector3d<int> ref_lpos,ref_rpos;
	PatternSearch(ref,lp,
					Vector3d<int>(180,320,0),
					Vector3d<int>(220,360,1),
					ref_lpos);
	PatternSearch(ref,rp,
					Vector3d<int>(1105,325,0),
					Vector3d<int>(1145,365,1),
					ref_rpos);

	cout << "L feature at " << ref_lpos << endl;
	cout << "R feature at " << ref_rpos << endl;
*/
	/*
	L feature at (199,345,0)
	R feature at (1127,345,0)
	*/

	Image3d<GRAY8> maskW;
	maskW.ReadImageBMP("v1-gray/playgroundMaskWeighted.bmp");

/*
	//projedeme celou sekvenci a registrujeme...
	for (int ff=1; ff <= 1000; ++ff)
	{
		char fn[100];
		sprintf(fn,"v1-gray/%08d.bmp",ff);

		Image3d<GRAY8> img;
		img.ReadImageBMP(fn);

		//urceni referencni pozice vzoru uvnitr test. obrazu
		Vector3d<int> lpos,rpos;
		PatternSearch(img,lp,
						Vector3d<int>(150,285,0),
						Vector3d<int>(250,405,1),
						lpos);
		PatternSearch(img,rp,
						Vector3d<int>(1080,285,0),
						Vector3d<int>(1180,405,1),
						rpos);

		cout << ff << ": L feature at " << lpos
			  << ",\t shift=" << ref_lpos-lpos << endl;
		cout << ff << ": R feature at " << rpos
			  << ",\t shift=" << ref_rpos-rpos << endl;

		Vector3d<int> shift;
		shift =ref_lpos-lpos;
		shift+=ref_rpos-rpos;
		shift/=2;
		cout << ff << ": shift=" << shift << endl;

		Image3d<GRAY8> img2;
		img2.CopyMetaData(ref);
		img2.GetVoxelData()=0;
		PasteImage(img2,img,shift);

		sprintf(fn,"v1-registered/%08d-both.bmp",ff);
		img2.SaveImageBMP(fn);

		sprintf(fn,"v1-diff/%08d-both.bmp",ff);
		GRAY8* rr=ref.GetFirstVoxelAddr();
		GRAY8* const rrS=rr+ref.GetImageSize();
		GRAY8* tt=img2.GetFirstVoxelAddr();
		for (; rr != rrS; ++rr, ++tt)
			*tt=(*rr < *tt)? 0 : *rr-*tt;
		img2.SaveImageBMP(fn);
	}
*/

	//projedeme celou sekvenci a hledame brouka pres vzdalenost od okraje
	for (int ff=1; ff <= 100; ++ff)
	{
		char fn[100];
		sprintf(fn,"v1-diff/%08d-both.bmp",ff);

		//tohle je vstupni diff obrazek
		Image3d<GRAY8> img;
		img.ReadImageBMP(fn);

		//tohle je vystupni detekcni obrazek
		Image3d<GRAY8> img2;
		img2.CopyMetaData(ref);

		//pro vypocitani geom. stredu z masky brouka
		Vector3d<size_t> geomCentreVec(0);
		long geomCentreMax=0;

		GRAY8* ii=img.GetFirstVoxelAddr();
		GRAY8* oo=img2.GetFirstVoxelAddr();
		GRAY8* mmW=maskW.GetFirstVoxelAddr();
		for (size_t qq=0; qq < img.GetImageSize(); ++qq, ++ii, ++oo, ++mmW)
		{
			*oo=0;
			if (*mmW > 0)
			{
				//uvnitr masky, ma cenu vubec neco pocitat...
				long sum=0;
				for (int y=-2; y <= 2; ++y)
				for (int x=-2; x <= 2; ++x)
					sum+=*(ii+x+y*img.GetSizeX());

				sum*= *mmW;
				if (*ii < 40) sum=0;

				*oo=sum / 3400; //6400 = 25*256

				if (sum > geomCentreMax)
				{
					//geom. stred
					geomCentreVec=img.GetPos(qq);
					geomCentreMax=sum;
				}
			}
		}

		//zahodime dost daleke pixely
		const float length=20*20;
		oo=img2.GetFirstVoxelAddr();
		for (size_t qq=0; qq < img2.GetImageSize(); ++qq, ++oo)
		{
			//urci x,y,z aktualni pozici
			Vector3d<size_t> pos=img2.GetPos(qq);
			if (i3d::Norm2(pos-geomCentreVec) > length) *oo=0;
		}

		//zaplnime diry
		ClosingO(img2,img,5);

		sprintf(fn,"v1-segres/%08d-both.bmp",ff);
		img.SaveImageBMP(fn);

/*
		//jen pro vizualizaci
		sprintf(fn,"v1-registered/%08d-both.bmp",ff);
		img.ReadImageBMP(fn);

		//jeste vlozi kolecko s domnelym geom. stredem brouka
		cout << ff << ": centre=" << geomCentreVec << " with maxVal=" << (float)geomCentreMax << endl;
		if (geomCentreMax > 0)
		{
			for (int y=-1; y <= 1; ++y)
			for (int x=-1; x <= 1; ++x)
				img.SetVoxel(geomCentreVec.x+x,geomCentreVec.y+y,0,255);
		}

		sprintf(fn,"v1-overlay/%08d-both.bmp",ff);
		img.SaveImageBMP(fn);
*/
	}

	return(0);
}