#include "rnd_generators.h"
#include "cell_movement.h"

template <class MT>
void MultiplyWithRadialGaussian(i3d::Image3d<MT> &img,
				const float mean,
				const float sigma)
{
	if (sigma < 0.f)
		throw ERROR_REPORT("Can't live with negative sigma of Gaussian");

	//time-savers:
	const float sigma2=sigma*sigma;
	const float xRes2=img.GetResolution().GetX() * img.GetResolution().GetX();
	const float yRes2=img.GetResolution().GetY() * img.GetResolution().GetY();
	const float zRes2=img.GetResolution().GetZ() * img.GetResolution().GetZ();

	//image centre
	const int xC=(int)img.GetSizeX()/2;
	const int yC=(int)img.GetSizeY()/2;
	const int zC=(int)img.GetSizeZ()/2;
	//DEBUG_REPORT("Using (" << xC << "," << yC << "," << zC << ") for image centre");

	MT *m=img.GetFirstVoxelAddr();

	for (int z=0; z < (signed)img.GetSizeZ(); ++z) {
	  const float dz=(z-zC)*(z-zC)/zRes2;

	  for (int y=0; y < (signed)img.GetSizeY(); ++y) {
	    const float dy=(y-yC)*(y-yC)/yRes2;

	    for (int x=0; x < (signed)img.GetSizeX(); ++x, ++m) {
		float dx=(x-xC)*(x-xC)/xRes2;
		dx=sqrtf(dx + dy + dz +0.000001); //the distance to centre in microns
		*m *= expf(-0.5f * (mean-dx)*(mean-dx)/sigma2 );
	    }
	  }
	}
}


template <class MT>
void MultiplyWithAngularGaussian(i3d::Image3d<MT> &img,
				 i3d::Vector3d<float> const &v,
				 const float sigma)
{
	if (sigma < 0.f)
		throw ERROR_REPORT("Can't live with negative sigma of Gaussian");

	if (i3d::Norm2(v) > 0.001) {
		//time-savers:
		const float sigma2=sigma*sigma;
		const float xRes=img.GetResolution().GetX();
		const float yRes=img.GetResolution().GetY();
		const float zRes=img.GetResolution().GetZ();

		//normalized vector to measure angle against to
		const float vx=v.x/i3d::Norm(v);
		const float vy=v.y/i3d::Norm(v);
		const float vz=v.z/i3d::Norm(v);

		//image centre
		const int xC=(int)img.GetSizeX()/2;
		const int yC=(int)img.GetSizeY()/2;
		const int zC=(int)img.GetSizeZ()/2;
		//DEBUG_REPORT("Using (" << xC << "," << yC << "," << zC << ") for image centre");

		MT *m=img.GetFirstVoxelAddr();

		for (int z=0; z < (signed)img.GetSizeZ(); ++z) {
		  const float dz=(z-zC)/zRes;
		  const float dz2=dz*dz;

		  for (int y=0; y < (signed)img.GetSizeY(); ++y) {
		    const float dy=(y-yC)/yRes;
		    const float dy2=dy*dy;

		    for (int x=0; x < (signed)img.GetSizeX(); ++x, ++m) {
			float dx=(x-xC)/xRes;
			float dx2=dx*dx;

			dx2=sqrtf(dx2 + dy2 + dz2 +0.000001); //size of the examined vector in microns
			dx=acosf((vx*dx + vy*dy + vz*dz)/dx2); //the angle between vectors

			*m *= expf(-0.5f * dx*dx/sigma2 );

			//error avoidance hack...
			if (!std::isfinite(*m))
			{
				*m=0;
				DEBUG_REPORT("avoiding non-finite value, please debug me!");
			}
		    }
		  }
		}
	} else DEBUG_REPORT("Can't live with (nearly) zero direction vector, changing nothing.");
}


template <class MT>
void MultiplyWithSliceDrivenGaussian(i3d::Image3d<MT> &img,
				const float sigma)
{
	if (sigma < 0.f)
		throw ERROR_REPORT("Can't live with negative sigma of Gaussian");

	//time-saver ;-)
	const float sigma2=sigma*sigma;

	//z-coordinate of image centre
	const int zC=(int)img.GetSizeZ()/2;

	//running pointer
	MT *m=img.GetFirstVoxelAddr();

	for (int z=0; z < (signed)img.GetSizeZ(); ++z) {
		const float Gauss=expf(-0.5f * (float)((z-zC)*(z-zC))/sigma2 );
		for (int xy=0; xy < (signed)img.GetSliceSize(); ++xy, ++m)
			*m *= Gauss;
	}
}


template <class MT>
void ReadVectorFromPreferenceMap(i3d::Vector3d<float> &v,
				 i3d::Image3d<MT> const &img)
{
	//compute the sum of intensities over the whole image
	float sum=0.f;

	//pointer to maximum non-zero vector in the MovsMask
	const MT *validIndex=NULL;
	float maxIndex=0.f;

	const MT *m=img.GetFirstVoxelAddr();
	MT const *mL=img.GetFirstVoxelAddr()+img.GetImageSize();
	while (m != mL) {
		sum+= *m;
		if (*m > maxIndex) { validIndex=m; maxIndex=*m; }
		++m;
	}

	//consistency check...
	if (validIndex == NULL) {
		//no valid translations at all, problem :(
		throw ERROR_REPORT("No translations available when there ought to be.");
	}

	if (sum >= 0.01) {
		//"choose" some vector
		float rnd=GetRandomUniform(0.001f,sum-0.001f);
		//note: interval cannot be empty
		//note: 0.001f prevents from rnd==0, which does not guarantee correct result

		//find out what it looks like
		m=img.GetFirstVoxelAddr() -1;
		while ((rnd > 0) && (m != mL)) { ++m; if (m != mL) rnd-= *m; }
	} else {
		//sum too small to do reliable search
		REPORT("Sum too small, ignoring preferences and using the \"maximum\" "
			<< "valid vector. sum=" << sum);
		m=validIndex;
	}

	if (m == mL) {
		REPORT("Strange, didn't expect to occur here. Is MovsMask valid? sum=" << sum);
		//img.SaveImage("MovsMask.ics");
		v.x=v.y=v.z=0.f; //return safe value
	} else {
		//restore coordinate from pointer
		i3d::Vector3d<size_t> tmpV=img.GetPos(m-img.GetFirstVoxelAddr());

		//subtract centre position and turn into microns
		v.x=((float)tmpV.x-(float)(img.GetSizeX()/2)) /img.GetResolution().GetX();
		v.y=((float)tmpV.y-(float)(img.GetSizeY()/2)) /img.GetResolution().GetY();
		v.z=((float)tmpV.z-(float)(img.GetSizeZ()/2)) /img.GetResolution().GetZ();

		//image centre
		/*
		DEBUG_REPORT("Using (" << (float)(img.GetSizeX()/2) << ","
			<< img.GetSizeY()/2 << "," << (float)(img.GetSizeZ()/2)
			<< ") for image centre");
		*/
	}

	DEBUG_REPORT("returning with vector " << v);
}



//
// explicit instantiations
//
template void MultiplyWithRadialGaussian(i3d::Image3d<float> &img,
				const float mean,const float sigma);
template void MultiplyWithAngularGaussian(i3d::Image3d<float> &img,
				i3d::Vector3d<float> const &v,const float sigma);
template void MultiplyWithSliceDrivenGaussian(i3d::Image3d<float> &img, const float sigma);
template void ReadVectorFromPreferenceMap(i3d::Vector3d<float> &v,
				 i3d::Image3d<float> const &img);
