/////////////////////////////////////////////////////////////////////////////
//
// Image class for personale functions
//
// Copyright © 1998
//
// IMM, Department of Mathematical Modelling
// Technical University of Denmark, Building 321
// DK-2800 Lyngby, Denmark
// http://www.imm.dtu.dk
//
// author: Søren Falch
//
// Disclaimer:
//
// No guarantees of performance accompany this software,
// nor is any responsibility assumed on the part of the author(s).
// The software has been tested extensively and every effort has been
// made to insure its reliability.
//
// This software is provided by IMM and the contributor(s) ``as is'' and
// any express or implied warranties, including, but not limited to, the
// implied warranties of merchantability and fitness for a particular purpose
// are disclaimed.  In no event shall IMM or the contributor(s) be liable
// for any direct, indirect, incidental, special, exemplary, or consequential
// damages (including, but not limited to, procurement of substitute goods
// or services; loss of use, data, or profits; or business interruption)
// however caused and on any theory of liability, whether in contract, strict
// liability, or tort (including negligence or otherwise) arising in any way
// out of the use of this software, even if advised of the possibility of
// such damage.
//
// This software is partly based on the Microsoft Vision Software Developers Kit VisSDK
//
/////////////////////////////////////////////////////////////////////////////

//#include "stdafx.h"
#include <complex>

/////////////////////////////////////////////////////////////////////////////
//	Calculates the Fast Fourier Transform (FFT) on image
//	and returnes result in a Magnitude image and Phase image
//
//	author:	Søren Falch 26/5-99
template <class TPixel>
//void CDImageFFT<TPixel>::FFT(CDImageFFT<std::complex<float> > &imageDest)
void CDImageFFT<TPixel>::FFT(CDImageFFT<float> &imageMagn, CDImageFFT<float> &imagePhase)
{
	CDImageFFT<float> Tmp(MemoryShape());
	Tmp.SetRect(Rect());

//	CDImageFFT<std::complex<float> > Tmp2(MemoryShape());
//	CDImageFFT<float> Tmp2(MemoryShape());

	IplImage *iplSrcImage = Image2IplImage();
	IplImage *iplDestImage = Tmp.Image2IplImage();

	// Call The IPL function
	iplRealFft2D(iplSrcImage, iplDestImage, IPL_FFT_Forw);

	// Convert from IPL Image format to VisSDK format.
//	RCPack2DToComplex(Tmp2);
//	Tmp.RCPack2DToMagnPhase(imageMagn, imagePhase);
	Tmp.MagPhase(imageMagn, imagePhase);
	CString Msg;
	Msg.Format("DIVA: FFT to Magnitude and Phase images");
	AddToHistory(Msg);
}

/////////////////////////////////////////////////////////////////////////////
//	Calculates the Fast Fourier Transform (FFT) on image
//	and returnes result in a RCPack2D image
//
//	author:	Søren Falch 26/5-99
template <class TPixel>
//void CDImageFFT<TPixel>::FFT(CDImageFFT<std::complex<float> > &imageDest)
void CDImageFFT<TPixel>::FFT(CDImageFFT<float> &imageRCPack2D)
{
	CDImageFFT<float> Tmp(MemoryShape());
	Tmp.SetRect(Rect());

//	CDImageFFT<std::complex<float> > Tmp2(MemoryShape());
	CDImageFFT<float> Tmp2(MemoryShape());

	IplImage *iplSrcImage = Image2IplImage();
	IplImage *iplDestImage = imageRCPack2D.Image2IplImage();

	// Call The IPL function
	iplRealFft2D(iplSrcImage, iplDestImage, IPL_FFT_Forw);

	CString Msg;
	Msg.Format("DIVA: FFT to RCPack2D image");
	AddToHistory(Msg);
}


/////////////////////////////////////////////////////////////////////////////
//	Calculates the Inverse Fast Fourier Transform (FFT) on image
//	
//	author:	Søren Falch 26/5-99
template <class TPixel>
void CDImageFFT<TPixel>::IFFT()
{
	CDImageFFT<float> Tmp(MemoryShape());
	Tmp.SetRect(Rect());

	IplImage *iplSrcImage = Image2IplImage();
	IplImage *iplDestImage = Tmp.Image2IplImage();

	// Call The IPL function Inverse FFT
	iplCcsFft2D(iplSrcImage, iplDestImage, IPL_FFT_Inv | IPL_FFT_UseFloat); 

	// Convert from IPL Image format to VisSDK format.

	CString Msg;
	Msg.Format("DIVA: Inverse Fast Fourier Transform");
	AddToHistory(Msg);

}

/////////////////////////////////////////////////////////////////////////////
//	Performs Optical transform on image (gray scale only). 
//
//	author:	Søren Falch 14/1-99
template <class TPixel>
void CDImageFFT<TPixel>::QuadFlip()
{
	const int conj=1;
	CDImageFFT<TPixel> Tmp(*this);
	Allocate(MemoryShape());

	int x,y;
	const int w2 = Width()/2;
	const int h2 = Height()/2;



	// Copy to Quadrant 4
	for (x=w2-1; x>=0; x--) {
		for (y=h2-1; y>=0; y--) {
			Pixel(x+w2, y+h2)    = Tmp.Pixel(x,y)*conj;
		}
	}
}

/////////////////////////////////////////////////////////////////////////////
//	Multiplicatively applies a mask (window). 
//	This is to correct for the frequency characteristics of the wincorresponds to freq
//
//	author:	Søren Falch 14/1-99
template <class TPixel>
void CDImageFFT<TPixel>::Taper()
{

}

/////////////////////////////////////////////////////////////////////////////
//	Converts RCPack2D format to complex<float> format
//	This is to correct for the frequency characteristics of the wincorresponds to freq
//
//	author:	Søren Falch 14/1-99

template <class TPixel>
void CDImageFFT<TPixel>::RCPack2DToComplex(CDImageFFT<std::complex<float> > &Tmp)
//void CDImageFFT<TPixel>::RCPack2DToComplex(CDImageFFT<float> &Tmp)
{
	// k is the row (y) counter
	// l is the column (x) counter
	int k,l;

	// First column is special
//	(Tmp.Pixel(0,0)).real()=Pixel(0,0);
//	(Tmp.Pixel(0,0)).imag()=0;
	Tmp.Pixel(0,0)=std::complex<float>(Pixel(0,0),0);
	for (k=1; k<Height()-1; k+=2) {
		Tmp.Pixel(0,k)=std::complex<float>(Pixel(0,k), Pixel(0,k+1));
	}
	if (Height()%2==0) // "If K is even"
	{
		Tmp.Pixel(0,Height()-1)=std::complex<float>(Pixel(0,Height()-1), 0);
	
		// "If K is even", there is one more row to process
	
		k=Height()-1;
		for (l=1; l<Width()-1; l+=2) {
			Tmp.Pixel(l,k)=std::complex<float>(Pixel(l,k), Pixel(l+1,k));
		}
		Tmp.Pixel(Width()-1,k)=std::complex<float>(Pixel(Height()-1,k), 0);
	}

	if (Width()%2==0) // "If L is even", there is one more column to process
	{
		l=Width()-1;
		for (k=1; k<Height()-1; k+=2) {
			Tmp.Pixel(l,k)=std::complex<float>(Pixel(l,k), Pixel(l,k+1));
		}
		Tmp.Pixel(l,Height()-1)=std::complex<float>(Pixel(l,Height()-1),0);
	}

	// Do body
	for (k=1; k<Height(); k++) {
		for (l=1; l<Width()-1; l+=2) {
			Tmp.Pixel(l/2,k)= std::complex<float>(Pixel(l,k), Pixel(l+1,k));
			
			Tmp.Pixel(Width()-l/2,k) = std::complex<float>(Pixel(l,k), Pixel(l+1,k));
		}
	}
}
/////////////////////////////////////////////////////////////////////////////
//	Converts RCPack2D format to complex<float> format
//	This is to correct for the frequency characteristics of the wincorresponds to freq
//
//	author:	Søren Falch 14/1-99

template <class TPixel>
std::complex<float> CDImageFFT<TPixel>::RCPack2D(const int X, const int Y)
{
	// k is the row (y) counter
	// l is the column (x) counter

	assert(X>0 && Y>0 && X<Width() && Y<Height());

	// Swap coordinates for optical FFT (DC at Pixel(Height()/2, Width()/2))
	// 1st and 3rd Quadrant are swaped, as are 2nd and 4th Quadrants

	int x, y;
	// Swap horizontally
	if (X<Width()/2) 		
		x=X+Width()/2;	
	else 
		x=X-Width()/2;

	// Swap vertically
	if (Y<Height()/2) 
		y=Y+Height()/2;
	else 
		y=Y-Height()/2;


	// Look up in RCPack2D data
	if (x==0 || (x==Width()/2 && !(Width()%2))) {		// Special: First column, (and last if L is Even)
		if (y==0)
			return std::complex<float>(Pixel(0,0),0);
		else if (y == Height()/2)
				if (Height()%2) 
					return std::complex<float>(Pixel(x, Height()-1),0);						// for Odd K
				else
					return std::complex<float>(Pixel(x, Height()-2),Pixel(x, Height()-1));	// for Even K
		else if (y < Height()/2)
			return std::complex<float>(Pixel(x,y*2-1), Pixel(x,y*2));
		else if (y > Height()/2)
			return std::complex<float>(Pixel(x, (Height()-y)*2-1), -Pixel(x, (Height()-y)*2-1));
		else
			return std::complex<float>(0,0);			// If every thing else fails
	}

	if (x>Width()/2) {
		int xm = Width()-x; int ym = (y==0 ? 0 : Height()-y);
		return std::complex<float>(Pixel(xm*2-1,ym), -Pixel(xm*2,ym));
	} else if (x<Width()/2)
		return std::complex<float>(Pixel(x*2-1,y), Pixel(x*2,y));

	return std::complex<float>(0,0);	// If every thing else fails
}
/////////////////////////////////////////////////////////////////////////////
//	Converts RCPack2D format to complex<float> format
//	This is to correct for the frequency characteristics of the wincorresponds to freq
//
//	author:	Søren Falch 14/1-99

template <class TPixel>
void CDImageFFT<TPixel>::MagPhase(CDImageFFT<float> &imgMagn, CDImageFFT<float> &imgPhase)
{
	int x,y;
	for (x=Width()-1; x>0; x--) {
		for (y=Height()-1; y>0; y--) {
			std::complex<float> c = RCPack2D(x,y);
			imgMagn.Pixel(x,y) = Magn(c.real(), c.imag());
			imgPhase.Pixel(x,y) = Phase(c.real(), c.imag());
		}
	}
}


/////////////////////////////////////////////////////////////////////////////
//	Converts RCPack2D format to complex<float> format
//	This is to correct for the frequency characteristics of the wincorresponds to freq
//
//	author:	Søren Falch 14/1-99

template <class TPixel>
void CDImageFFT<TPixel>::RCPack2DToMagnPhase(CDImageFFT<float> &imgMagn, CDImageFFT<float> &imgPhase)
{
	
	


	int K=Height()-1; // K is the number of Rows-1
	int L=Width()-1;  // L is the number of Columns-1
	int k,l;

	// First column is special
	imgMagn.Pixel(0,0)=Pixel(0,0);
	for (k=1; k<Height()-1; k+=2) {
		imgMagn.Pixel(0,k/2+1)=imgMagn.Pixel(0,K-(k/2+1))=
			Magn(Pixel(0,k), Pixel(0,k+1));

		imgPhase.Pixel(0,K-(k/2+1)) =  Phase(Pixel(0,k), Pixel(0,k+1));
		imgPhase.Pixel(0,k/2+1)		= -Phase(Pixel(0,k), Pixel(0,k+1));

		if (Height()%2==0) // "If K is even"
		{
			// "If K is even", then (K/2,0) has no imaginary value
			imgMagn.Pixel(0,Height()-1)=Magn(Pixel(0,Height()-1), 0);
			imgPhase.Pixel(0,Height()-1)=Phase(Pixel(0,Height()-1), 0);

//			imgMagn.Pixel(0,Height()-1)=Magn(Pixel(0,Height()-1), 0);
//			imgPhase.Pixel(0,Height()-1)=Phase(Pixel(0,Height()-1), 0);
		}
	}

	if (Width()%2==0) // "If L is even", last column is special too
	{
		l=Width()-1;

		// (0,L/2) has no imaginary value
		imgMagn.Pixel(l/2,0)=Pixel(l,0);
		imgPhase.Pixel(l/2,0)=0;
//		imgMagn.Pixel(l/2+1,0)=Pixel(l,0);
//		imgPhase.Pixel(l/2+1,0)=0;

		for (k=1; k<Height()-1; k+=2) {
			imgMagn.Pixel(l,k/2+1)			=
			imgMagn.Pixel(l,K-(k/2+1))		=Magn(Pixel(l,k), Pixel(l,k+1));
			imgPhase.Pixel(l,k/2+1)			=Phase(Pixel(l,k), Pixel(l,k+1));
			imgPhase.Pixel(l,K-(k/2+1))		=-Phase(Pixel(l,k), Pixel(l,k+1));

		}
		// "If K is even", then (K/2,L/2) has no imaginary value
		if (Height()%2==0) {
			imgMagn.Pixel(0,Height()-1)=		Pixel(Width()-1,Height()-1);
			imgPhase.Pixel(0,Height()-1)=		0;
		}
	
		imgMagn.Pixel(l,Height()-1)=Pixel(l,Height()-1);
		imgPhase.Pixel(l,Height()-1)=0;
	}

	// Do body (i.e. all pixels, EXCEPT: corner pixels and first/last column)
	for (k=0; k<Height(); k++) {
		for (l=1; l<Width()-1; l+=2) {
			imgMagn.Pixel(l/2,k)		= Magn(Pixel(l,k), Pixel(l+1,k)); 
//			imgMagn.Pixel(Width()-l/2,k)= Magn(Pixel(l,k), Pixel(l+1,k));
			imgPhase.Pixel(l/2,k)		= Phase(Pixel(l,k), Pixel(l+1,k));
//			imgPhase.Pixel(Width()-l/2,k)= Phase(Pixel(l,k), Pixel(l+1,k));
		}
	}
}
/////////////////////////////////////////////////////////////////////////////
//	Private function to calculate the magnitude of a (real, imaginary) set
//
//	author:	Søren Falch 28/5-99

template <class TPixel>
inline double CDImageFFT<TPixel>::Magn(const TPixel Real, const TPixel Imag) {
	return sqrt(Real*Real+Imag*Imag);
} // Magn

/////////////////////////////////////////////////////////////////////////////
//	Private function to calculate the phase of a (real, imaginary) set
//
//	author:	Søren Falch 28/5-99

template <class TPixel>
inline double CDImageFFT<TPixel>::Phase(const TPixel Real, const TPixel Imag) {
	const double pi = 3.1415926535897932384626433832795;
	if (Real==0)
		if (Imag > 0) 
			return pi/2;
		else
			return -pi/2;

	return atan2(Imag,Real);
} // Phase

/////////////////////////////////////////////////////////////////////////////
//	Applies a Bartlett (triangular) window to the image
//	"Out" is the resulting output image. 
//	The dimensions of "Out" should be less or equal to that of the current image
//	"Out" must be quadratic (Width()==Height())
//
//	OffsetX and OffsetY is the offset of the upper left corner of the window,
//	The values must be such that the window is fully contained in the image
//
//
//	author:	Søren Falch 2/7-99

template <class TPixel>
void CDImageFFT<TPixel>::BartlettWindow(CDImageFFT<float> &Out,
										const int OffsetX, const int OffsetY) {

	if (Out.Width()+OffsetX > Width() || Out.Height()+OffsetY > Height())
		return;	// Window does not fit in image

	int Cx = OffsetX+Width()/2;
	int Cy = OffsetY+Height()/2;

	for (y=OffsetY; y<OffsetY+Height(); y++) {
		for (x=OffsetX, x<OffsetX+Width();x++) {
			Out.Pixel(x,y)= Pixel(x,y)*(1-EucDist(x,y,Cx,Cy)/Out.Width());
		}
	}
}

/////////////////////////////////////////////////////////////////////////////
//	Applies a Hanning (Tukey or raised cosine) window to the image
//	"Out" is the resulting output image. 
//	The dimensions of "Out" should be less or equal to that of the current image
//	"Out" must be quadratic (Width()==Height())
//
//	OffsetX and OffsetY is the offset of the upper left corner of the window,
//	The values must be such that the window is fully contained in the image
//
//
//	author:	Søren Falch 2/7-99

template <class TPixel>
void CDImageFFT<TPixel>::HanningWindow(CDImageFFT<float> &Out,
										const int OffsetX, const int OffsetY) {

	const float pi = 3.1415926535897932384626433832795;

	if (Out.Width()+OffsetX > Width() || Out.Height()+OffsetY > Height())
		return;	// Window does not fit in image

	int Cx = OffsetX+Width()/2;
	int Cy = OffsetY+Height()/2;

	for (y=OffsetY; y<OffsetY+Height(); y++) {
		for (x=OffsetX, x<OffsetX+Width();x++) {
			Out.Pixel(x,y)= Pixel(x,y)*((1+cos(EucDist(x,y,Cx,Cy)*pi/Out.Width()))/2);
		}
	}
}

/////////////////////////////////////////////////////////////////////////////
//	Applies a Hamming window to the image
//	"Out" is the resulting output image. 
//	The dimensions of "Out" should be less or equal to that of the current image
//	"Out" must be quadratic (Width()==Height())
//
//	OffsetX and OffsetY is the offset of the upper left corner of the window,
//	The values must be such that the window is fully contained in the image
//
//
//	author:	Søren Falch 2/7-99

template <class TPixel>
void CDImageFFT<TPixel>::HammingWindow(CDImageFFT<float> &Out,
										const int OffsetX, const int OffsetY) {

	const float pi = 3.1415926535897932384626433832795;

	if (Out.Width()+OffsetX > Width() || Out.Height()+OffsetY > Height())
		return;	// Window does not fit in image

	int Cx = OffsetX+Width()/2;
	int Cy = OffsetY+Height()/2;

	for (y=OffsetY; y<OffsetY+Height(); y++) {
		for (x=OffsetX, x<OffsetX+Width();x++) {
			Out.Pixel(x,y)= Pixel(x,y)*((0.54+0.46*cos(EucDist(x,y,Cx,Cy)*pi/Out.Width())));
		}
	}
}

/////////////////////////////////////////////////////////////////////////////
//	Returns Euclidian distance from (x1,y1) to (x2,y2)
//
//	author:	Søren Falch 2/7-99


template <class TPixel>
inline double CDImageFFT<TPixel>::EucDist(int x1,int y1, int x2, int y2)
{
	return sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
}

/////////////////////////////////////////////////////////////////////////////
//	Generates correlation map from two FFT images.
//	Both Images must be in the special RCPack2D format.
//	Input must be two FFT images of 
//	author:	Søren Falch 3/7-99


template <class TPixel>
void CDImageFFT<TPixel>::FFTCrossCorrelate(CDImageFFT<TPixel> &Img, 
										   CDImageFFT<float> &ImgOut)
{
	CDImageFFT<float> Tmp1(MemoryShape());
	Tmp1.SetRect(Rect());
	CDImageFFT<float> Tmp2(MemoryShape());
	Tmp2.SetRect(Rect());

	IplImage *iplSrcImage1  = Image2IplImage();
	IplImage *iplDestImage1 = Tmp1.Image2IplImage();
	IplImage *iplSrcImage2  = Img.Image2IplImage();
	IplImage *iplDestImage2 = Tmp2.Image2IplImage();

	// Call The IPL function
	iplRealFft2D(iplSrcImage1, iplDestImage1, IPL_FFT_Forw);
	iplRealFft2D(iplSrcImage2, iplDestImage2, IPL_FFT_Forw);

	// Call the Cross correlation function
	Tmp1.FFTCC(Tmp2, ImgOut);
	
	CString Msg;
	Msg.Format("DIVA: FFT Cross Correlation");
	AddToHistory(Msg);


}

/////////////////////////////////////////////////////////////////////////////
//	Takes two RCPack2D images, makes pixelwise multiplication and then does 
//	Inverse FFT on the result.
//
//	author:	Søren Falch 8/7-99

template <class TPixel>
void CDImageFFT<TPixel>::FFTCC(CDImageFFT<float> &Img, 
							   CDImageFFT<float> &ImgOut)
{

	if (Img.Width() != Width() || Img.Height() != Height())
		throw CVisError("Images must have same dimensions for cross correlation to be calculated",eviserrorImageShapeMismatch,"FFTCrossCorrelate()","DImageFFT.inl", __LINE__);

	CDImageFFT<float> Tmp(MemoryShape());
	Tmp.SetRect(Rect());

	IplImage *iplImage1 = Image2IplImage();
	IplImage *iplImage2 = Img.Image2IplImage();
	IplImage *iplImageTmp = Tmp.Image2IplImage();
	IplImage *iplImageOut = ImgOut.Image2IplImage();

// !! removed by rf
//	Pixel(0,0)		= 0; // Force DC values to 0
//	Img.Pixel(0,0)	= 0; // 


	// Pixelwise multiplication
	iplMpyRCPack2D (iplImage1, iplImage2, iplImageTmp);

	// Inverse FFT
	iplCcsFft2D(iplImageTmp, iplImageOut, IPL_FFT_Inv | IPL_FFT_UseFloat); 

	CString Msg;
	Msg.Format("DIVA: FFT Cross correlation");
	ImgOut.AddToHistory(Msg);

}

/////////////////////////////////////////////////////////////////////////////
//	Pads image to a size of 2^N times 2^M for maximum FFT performance
//	
//	
//	author:	Søren Falch 3/7-99
//

template <class TPixel>
void CDImageFFT<TPixel>::FFTPad(CDImageFFT<TPixel> &imageOut, int Size, int Window, int Position)
{
	int cx, cy;
	if (Size==edivaFFTMinRectangle) {
		int NewWidth=1;
		int NewHeight=1;
		
		// Find new 
		while ((NewWidth<<1) <= Width())
			NewWidth<<=1;

		while ((NewHeight<<1) <= Height())
			NewHeight<<=1;

		cx = (Width()-NewWidth)/2;	// Default is to cut out in the center of image
		cy = (Height()-NewHeight)/2;

		imageOut.Deallocate();
		imageOut.Allocate(NewWidth, NewHeight);
		
		int x,y;
		// To be optimized using memcpy()
		for (x=imageOut.Left(); x<imageOut.Right(); x++) {
			for (y=imageOut.Top(); y<imageOut.Bottom(); y++) {
				imageOut.Pixel(x,y) = Pixel(x+cx, y+cy);
			}
		}

		
		imageOut.CopyPixelsTo(*this,evisnormalizeCopyBytesSameType);
		
		// update history
		CString str;
		str.Format("DIVA: Image padded to new size of (%dx%d)",Width(),Height());
		AddToHistory(str);

	}
}

/*
// Class Instantiation
CDImageFFT<byte>;
CDImageFFT<int>;
CDImageFFT<short>;
CDImageFFT<unsigned short>;
CDImageFFT<float>;
CDImageFFT<double>;
CDImageFFT<std::complex<float> >;
*/