// Header
class Complex
{

public:

	Real32 R, I;

	Complex()
	{
	}

	~Complex()
	{
	}

	Complex(const Real32 &real, const Real32 &image)
	{
		R = real;
		I = image;
	}

	// Operator
	Complex operator -() const { Complex c(-R, -I); return c; }
	Complex operator !() const { Complex c( R, -I); return c; }

	Complex operator +(const Complex &l) const { Complex c(R + l.R, I + l.I); return c; }
	Complex operator -(const Complex &l) const { Complex c(R - l.R, I - l.I); return c; }
	Complex operator *(const Complex &l) const { Complex c(R*l.R - I*l.I, R*l.I + I*l.R); return c; }

	Complex operator *(const Real32 &l) const { Complex c(R * l, I * l); return c; }
	Complex operator /(const Real32 &l) const { Complex c(R / l, I / l); return c; }

	inline Real32 Norm() const
	{
		return Sqrt32(R*R + I*I);
	}

	inline Complex Exponent() const
	{
		Complex c( Cos32(I), -Sin32(I) );
		return c * Exp32(R);
	}

};


class PhaseOnlyCorrelation
{

public:
	static void Fourier1D(std::vector<Complex> &array, SInt32 size, SInt32 step, SInt32 offset, Binary inverse = false);
	static void Run      (Image &output, const Image &image0, const Image &image1);

};


// Implementation
void PhaseOnlyCorrelation::Fourier1D(std::vector<Complex> &array, SInt32 size, SInt32 step, SInt32 offset, Binary inverse = false)
{

	SInt32  j, jnh, jp, jx, k, ne, n_half, n_half2;
	Complex uj(0.0f, 1.0f);
	Complex tmp, arg = uj * (-2.0f * Pi32 / (Real32)size);

	if(inverse)
	{
		arg = -arg;
	}

	for(j = 0; j < size; ++j)
	{
		jp = j;
		jx = 0;
		for(k = 1; k < size; k <<= 1)
		{
			jx = (jx << 1) | (jp & 1);
			jp = jp >> 1;
		}
		if(j < jx)
		{
			Swap(array[offset + j * step], array[offset + jx * step]);
		}
	}

	n_half = 2;
	for(ne = size >> 1; ne >= 1; ne >>= 1)
	{
		n_half2 = n_half >> 1;
		for(jp = 0; jp < size; jp = jp + n_half)
		{
			jx = 0;
			for(j = jp; j < jp + n_half2; ++j)
			{
				jnh = j + n_half2;
				tmp = array[offset + jnh * step] * (arg*(Real32)jx).Exponent();

				array[offset + jnh * step] = array[offset + j * step] - tmp;
				array[offset +   j * step] = array[offset + j * step] + tmp;

				jx = jx + ne;
			}
		}
		n_half <<= 1;
	}

	if(!inverse)
	{
		for(j = 0; j < size; ++j)
		{
			array[offset + j * step] = array[offset + j * step] / (Real32)size;
		}
	}
}


void PhaseOnlyCorrelation::Run(Image &output, const Image &image0, const Image &image1)
{

	SInt32 w = image0.Width;
	SInt32 h = image0.Height;

	CieRgb color;

	// Allocate
	std::vector<Complex> array0(w * h);
	std::vector<Complex> array1(w * h);

	// Copy
	for(SInt32 v = 0; v < h; ++v)
	{
		for(SInt32 u = 0; u < w; ++u)
		{
			image0.GetColor(color, u, v);
			array0[v*w + u].R = color.Average();
			array0[v*w + u].I = 0.0f;
		}
	}

	for(SInt32 v = 0; v < h; ++v)
	{
		for(SInt32 u = 0; u < w; ++u)
		{
			image1.GetColor(color, u, v);
			array1[v*w + u].R = color.Average();
			array1[v*w + u].I = 0.0f;
		}
	}

	// 2D Fourier
	for(SInt32 v = 0; v < h; ++v)
	{
		Fourier1D(array0, w, 1, v*w);
		Fourier1D(array1, w, 1, v*w);
	}

	for(SInt32 u = 0; u < w; ++u)
	{
		Fourier1D(array0, h, w, u);
		Fourier1D(array1, h, w, u);
	}
		
	// Phase Only Image
	for(SInt32 v = 0; v < h; ++v)
	{
		for(SInt32 u = 0; u < w; ++u)
		{
			// Fourier(image0) x Conjugate( Fourier(image1) )
			array0[v*w + u] 
				= (  array0[v*w + u]  / array0[v*w + u].Norm())
				* ((!array1[v*w + u]) / array1[v*w + u].Norm());
		}
	}

	// 2D Inverse Fourier
	for(SInt32 v = 0; v < h; ++v)
	{
		Fourier1D(array0, w, 1, v*w, true);
	}

	for(SInt32 u = 0; u < w; ++u)
	{
		Fourier1D(array0, h, w, u, true);
	}

	// Re-Ordering
	for(SInt32 v = 0; v < (h >> 1); ++v)
	{
		for(SInt32 u = 0; u < (w >> 1); ++u)
		{
			Swap( array0[v*w + u], array0[(v+(h>>1))*w + (u+(w>>1))] );
		}
		for(SInt32 u = (w >> 1); u < w; ++u)
		{
			Swap( array0[v*w + u], array0[(v+(h>>1))*w + (u-(w>>1))] );
		}
	}

	// Result
	Real32 size;
	for(SInt32 v = 0; v < h; ++v)
	{
		for(SInt32 u = 0; u < w; ++u)
		{
			size = array0[v*w + u].Norm() / (w * h);
			output.SetColor(CieRgb(size, size, size), u, v);
		}
	}

	// Dispose
	array0.clear();
	array1.clear();

}