/*
 *  psychlops_FFTW_bridge.cpp
 *  Psychlops Standard Library (Universal)
 *
 *  Last Modified 2010/03/05 by Kenchi HOSOKAWA
 *  (C) 2010 Kenchi HOSOKAWA, Kazushi MARUYA, Takao SATO
 */


#include "../../../psychlops_core.h"
#include "psychlops_fftw_bridge.h"
#include "fftw3.h"



namespace Psychlops {

	double sigmoid(double x, double d, double a) { return 1 / ( 1 + pow(Math::E, -(a*(x-d))) ); }
	double log2NormalDistibution(double log_x, double octave_mu, double octave_sigma) { return Math::normalDistibution(log(log_x)*Math::LOG2E, octave_mu, octave_sigma); }
	double cumulativeLog2NormalDistibution(double log_x, double octave_mu, double octave_sigma) { return Math::cumulativeNormalDistibution(log(log_x)*Math::LOG2E, octave_mu, octave_sigma); }
	enum { REAL, IMAGINARY };




	FFT2::FFT2()
	{
		construct_default();
	}
	FFT2::FFT2(int wid, int hei)
	{
		construct_default();
		set(wid, hei);
	}
	FFT2::FFT2(const Image &source)
	{
		construct_default();
		set(source);
	}
	FFT2::FFT2(const Matrix &source)
	{
		construct_default();
		set(source);
	}
	FFT2::FFT2(const Matrix &reali, const Matrix &imagi)
	{
		width_=0; height_=0;
		set(reali, imagi);
	}
	FFT2::~FFT2()
	{
		release();
	}
	void FFT2::construct_default()
	{
		width_=0; height_=0;
		img_spc=0; frq_spc=0;
	}
	void FFT2::release()
	{
		if(img_spc!=0) { free(img_spc); img_spc = 0; }
		if(frq_spc!=0) { free(frq_spc); frq_spc = 0; }
		width_=0; height_=0;
	}
	void FFT2::set(int wid, int hei)
	{
		if(img_spc==0 || frq_spc==0 || (width_*height_ != wid*hei) ) {
			release();
			img_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei);
			frq_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei);
		}
		width_  = wid;
		height_ = hei;
		x_zero = wid + 1;
		y_zero = hei + 1;
		left = (wid)/2;
		top = (hei)/2;
	}
	void FFT2::set(const Image &source)
	{
		set(source.getWidth(), source.getHeight());
		for(int y=0; y<height_; y++) {
			for(int x=0; x<width_; x++) {
				img_spc[x + width_*y][REAL] = source.getPix(x, y).getR();
				img_spc[x + width_*y][IMAGINARY] = 0.0;
			}
		}
	}
	void FFT2::set(const Matrix &source)
	{
		set(source.getCols(), source.getRows());
		for(int y=0; y<height_; y++) {
			for(int x=0; x<width_; x++) {
				img_spc[x + width_*y][REAL] = source(y+1, x+1);
				img_spc[x + width_*y][IMAGINARY] = 0.0;
			}
		}
	}
	void FFT2::set(const Matrix &reali, const Matrix &imagi)
	{
		set(reali.getCols(), reali.getRows());
		for(int y=0; y<height_; y++) {
			for(int x=0; x<width_; x++) {
				img_spc[x + width_*y][REAL] = reali(y+1, x+1);
				img_spc[x + width_*y][IMAGINARY] = imagi(y+1, x+1);
			}
		}
	}
	void FFT2::setSpectrum(const Matrix &reali, const Matrix &imagi)
	{
		set(reali.getCols(), reali.getRows());
		for(int y=0; y<height_; y++) {
			for(int x=0; x<width_; x++) {
				frq_spc[x + width_*y][REAL] = reali(y+1, x+1);
				frq_spc[x + width_*y][IMAGINARY] = imagi(y+1, x+1);
			}
		}
	}

	int FFT2::freqX(int x) const
	{
		return x<0 ? x_zero+x : x;
	}
	int FFT2::freqY(int y) const
	{
		return y<0 ? y_zero+y : y;
	}

	double FFT2::pix(int x, int y, double l)
	{
		return img_spc[x + width_*y][REAL] = l;
	}
	double FFT2::pix(int x, int y, const Color &c)
	{
		return img_spc[x + width_*y][REAL] = c.getR();
	}
	double FFT2::getPix(int x, int y)
	{
		return img_spc[x + width_*y][REAL];
	}

	double FFT2::getDC()
	{
		return sqrt(frq_spc[0][REAL]*frq_spc[0][REAL]+frq_spc[0][IMAGINARY]*frq_spc[0][IMAGINARY]);
	}
	double FFT2::setDC(double l)
	{
		frq_spc[0][REAL] = l;
		frq_spc[0][IMAGINARY] = 0;
		return l;
	}


	void FFT2::fft()
	{
		fftw_plan plan = fftw_plan_dft_2d(height_, width_, img_spc, frq_spc, FFTW_FORWARD, FFTW_ESTIMATE);
		fftw_execute(plan);
		normalizeFFT();
	}
	void FFT2::ifft()
	{
		fftw_plan plan = fftw_plan_dft_2d(height_, width_, frq_spc, img_spc, FFTW_BACKWARD, FFTW_ESTIMATE);
		fftw_execute(plan);
	}
	void FFT2::normalizeFFT()
	{
		double num = height_*width_;
		for (int i=0; i<num; i++) {
			frq_spc[i][REAL] /= num;
			frq_spc[i][IMAGINARY] /= num;
		}
	}
	void FFT2::getImage(Image &target)
	{
		double rl, im;
		Color col;
		if( !target.hasInstance() || (target.getWidth()!=width_) || (target.getHeight()!=height_) ) {
			target.release();
			target.set(width_, height_);
		}
		for (int y=0; y<height_; y++) {
			for (int x=0; x<width_; x++) {
				rl = img_spc[x + width_*y][REAL];
				im = img_spc[x + width_*y][IMAGINARY];
				col.set(sqrt(rl*rl+im*im));
				target.pix(x, y, col);
			}
		}
	}
	void FFT2::getSpectrum(Image &target, double gamma)
	{
		const int bottom = height_%2==0 ? top : top+1;
		const int right = width_%2==0 ? left : left+1;
		double rl, im;
		Color col;
		if( !target.hasInstance() || (target.getWidth()!=width_) || (target.getHeight()!=height_) ) {
			target.release();
			target.set(width_, height_);
		}
		int h, v;
		for (int y=0; y<height_; y++) {
			v = (y<bottom ? y+top : y-bottom);
			for (int x=0; x<right; x++) {
				h = x+left;
				rl = frq_spc[x + width_*y][REAL];
				im = frq_spc[x + width_*y][IMAGINARY];
				col.set(pow( sqrt(rl*rl+im*im), gamma ));
				target.pix(h, v, col);
			}
			for (int x=right; x<width_; x++) {
				h = x-right;
				rl = frq_spc[x + width_*y][REAL];
				im = frq_spc[x + width_*y][IMAGINARY];
				col.set(pow( sqrt(rl*rl+im*im), gamma ));
				target.pix(h, v, col);
			}
		}
	}
	void FFT2::getImage(Matrix &target)
	{
		double rl, im;
		int h, v;
		if( (target.getCols()!=width_) || (target.getRows()!=height_) ) {
			target.release();
			target.set(height_, width_);
		}
		for (int y=0; y<height_; y++) {
			v = y+1;
			for (int x=0; x<width_; x++) {
				rl = img_spc[x + width_*y][REAL];
				im = img_spc[x + width_*y][IMAGINARY];
				target(v, x+1) = sqrt(rl*rl+im*im);
			}
		}
	}
	void FFT2::getSpectrum(Matrix &target, double gamma)
	{
		const int bottom = height_%2==0 ? top : top+1;
		const int right = width_%2==0 ? left : left+1;
		double rl, im;
		Color col;
		if( (target.getCols()!=width_) || (target.getRows()!=height_) ) {
			target.release();
			target.set(height_, width_);
		}
		int h, v;
		for (int y=0; y<height_; y++) {
			v = (y<bottom ? y+top : y-bottom) + 1;
			for (int x=0; x<right; x++) {
				h = x+left + 1;
				rl = frq_spc[x + width_*y][REAL];
				im = frq_spc[x + width_*y][IMAGINARY];
				target(v, h) = pow( sqrt(rl*rl+im*im), gamma );
			}
			for (int x=right; x<width_; x++) {
				h = x-right + 1;
				rl = frq_spc[x + width_*y][REAL];
				im = frq_spc[x + width_*y][IMAGINARY];
				target(v, h) = pow( sqrt(rl*rl+im*im), gamma );
			}
		}
		/*for (int y=0; y<height_; y++) {
			v = y+1;
			for (int x=0; x<width_; x++) {
				rl = frq_spc[x + width_*y][REAL];
				im = frq_spc[x + width_*y][IMAGINARY];
				col.set(rl);
				target(v, x+1) = pow( sqrt(rl*rl+im*im), gamma );
			}
		}*/
	}
	void FFT2::getImage(Matrix &reali, Matrix &imagi)
	{
		if( (reali.getCols()!=width_) || (reali.getRows()!=height_) ) { reali.release(); reali.set(height_, width_); }
		if( (imagi.getCols()!=width_) || (imagi.getRows()!=height_) ) { imagi.release(); imagi.set(height_, width_); }
		for (int y=0; y<height_; y++) {
			for (int x=0; x<width_; x++) {
				reali(y+1, x+1) = img_spc[x + width_*y][REAL];
				imagi(y+1, x+1) = img_spc[x + width_*y][IMAGINARY];
			}
		}
	}
	void FFT2::getSpectrum(Matrix &reali, Matrix &imagi)
	{
		const int bottom = height_%2==0 ? top : top+1;
		const int right = width_%2==0 ? left : left+1;
		if( (reali.getCols()!=width_) || (reali.getRows()!=height_) ) { reali.release(); reali.set(height_, width_); }
		if( (imagi.getCols()!=width_) || (imagi.getRows()!=height_) ) { imagi.release(); imagi.set(height_, width_); }
		int h, v;
		for (int y=0; y<height_; y++) {
			v = (y<bottom ? y+top : y-bottom) + 1;
			for (int x=0; x<width_; x++) {
				reali(y+1, x+1) = frq_spc[x + width_*y][REAL];
				imagi(y+1, x+1) = frq_spc[x + width_*y][IMAGINARY];
			}
		}
	}
	void FFT2::filtering(const Matrix &filter)
	{
		const int bottom = height_%2==0 ? top : top+1;
		const int right = width_%2==0 ? left : left+1;
		int h, v;
		double factor;
		//std::cout << width_ << ":" << left << " " << height_ << ":" << top << "*" << bottom << std::endl;
		for (int y=0; y<height_; y++) {
			v = (y<bottom ? y+top : y-bottom) + 1;
			//std::cout << v << ":" << y << "  ";
			if(v<=0 || v>height_) exit(v);
			for (int x=0; x<right; x++) {
				h = x+left + 1;
				//std::cout << h << ":" << x << "  ";
				factor = filter(v, h);
				frq_spc[x + width_*y][REAL] *= factor;
				frq_spc[x + width_*y][IMAGINARY] *= factor;
			}
			for (int x=right; x<width_; x++) {
				h = x-right + 1;
				//std::cout << h << ":" << x << "  ";
				factor = filter(v, h);
				frq_spc[x + width_*y][REAL] *= factor;
				frq_spc[x + width_*y][IMAGINARY] *= factor;
			}
			//std::cout << std::endl;
		}
	}
	void FFT2::filtering(double cutoff_lambda1, double cutoff_lambda2, double slope)
	{
		Matrix filter = makeFilter(height_, width_, cutoff_lambda1, cutoff_lambda2, slope);
		filtering(filter);
	}

	double gaussian(const double x, const double sigma) { return exp( -(x*x) / (2.0*sigma*sigma) ); }
	void FFT2::getSpectrumExec(FFT2 &tmp, Image &result, double gamma)
	{
		double xx,yy;
		for (int y=0; y<tmp.height_; y++) {
			yy = (double)(y-tmp.top)/tmp.height_;
			for (int x=0; x<tmp.width_; x++) {
				xx = (double)(x-tmp.left)/tmp.width_;
				tmp.img_spc[x + tmp.width_*y][REAL] = (tmp.img_spc[x + tmp.width_*y][REAL]-.5) * gaussian(sqrt(xx*xx+yy*yy), 0.125) + 0.5;
			}
		}
		tmp.fft();
		tmp.getSpectrum(result, gamma);
	}
	void FFT2::getSpectrum(const Image &source, Image &result, double gamma)
	{
		FFT2 tmp(source);
		getSpectrumExec(tmp, result, gamma);
	}
	void FFT2::getSpectrum(const Matrix &source, Image &result, double gamma)
	{
		FFT2 tmp(source);
		getSpectrumExec(tmp, result, gamma);
	}
	void FFT2::getRawSpectrumExec(FFT2 &tmp, Image &reali, Image &imagi, double gamma)
	{
		tmp.fft();
		tmp.getSpectrum(reali, imagi, gamma);
	}
	void FFT2::getSpectrum(const Image &source, Image &reali, Image &imagi, double gamma)
	{
		FFT2 tmp(source);
		getRawSpectrumExec(tmp, reali, imagi, gamma);
	}
	void FFT2::getSpectrum(const Matrix &source, Image &reali, Image &imagi, double gamma)
	{
		FFT2 tmp(source);
		getRawSpectrumExec(tmp, reali, imagi, gamma);
	}

	void FFT2::filterImageExec(FFT2 &tmp, Image &result, const Matrix &carnel)
	{
		tmp.fft();
		tmp.filtering(carnel);
		tmp.ifft();
		tmp.getImage(result);
	}
	void FFT2::filterImage(const Image &source, Image &result, const Matrix &carnel)
	{
		FFT2 tmp(source);
		filterImageExec(tmp, result, carnel);
	}
	void FFT2::filterImage(const Matrix &source, Image &result, const Matrix &carnel)
	{
		FFT2 tmp(source);
		filterImageExec(tmp, result, carnel);
	}
	void FFT2::filterImage(const Image &source, Image &result, double cutoff_lambda1, double cutoff_lambda2, double slope)
	{
		FFT2 tmp(source);
		Matrix carnel = makeFilter(tmp.height_, tmp.width_, cutoff_lambda1, cutoff_lambda2, slope);
		filterImageExec(tmp, result, carnel);
	}
	void FFT2::filterImage(const Matrix &source, Image &result, double cutoff_lambda1, double cutoff_lambda2, double slope)
	{
		FFT2 tmp(source);
		Matrix carnel = makeFilter(tmp.height_, tmp.width_, cutoff_lambda1, cutoff_lambda2, slope);
		filterImageExec(tmp, result, carnel);
	}

	Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double slope)
	{
		Matrix filter(height, width);

		const double originY = (height)/2+1;
		const double originX = (width )/2+1;
		const double cutoffL = cutoff_lambda1;
		const double cutoffH = cutoff_lambda2;
		const double half = (cutoffL + cutoffH)/2.0;
		double modulus = 0.0;
		double sslope = slope<=0.0 ? 0.0000000000000001 : slope;
		double value;

		if(cutoffL<=cutoffH) {
			for(int y=1; y<=height; y++) {
				for(int x=1; x<=width; x++) {
					modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
					if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope);
					else if(modulus<=cutoffH) filter(y, x) = 1.0;
					else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope);
					//if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		} else {
			for(int y=1; y<=height; y++) {
				for(int x=1; x<=width; x++) {
					modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
					if(modulus<cutoffH) value = 1.0;
					else if(modulus<half) value = (pow(1.0/(modulus-cutoffH+1.0), 1.0/sslope));
					else if(modulus<cutoffL) value = (pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope));
					else value = 1.0;
					filter(y, x) = value;
					//if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		}
		filter(originY, originX) = 1.0;
		return filter;
	}
	Matrix FFT2::makeFilterSector(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double theta1, double theta2, double half_power_width, double half_power_width_ori)
	{
		Matrix filter(height, width);

		const double originY = (height)/2+1;
		const double originX = (width )/2+1;
		const double cutoffL = cutoff_lambda1;
		const double cutoffH = cutoff_lambda2;
		const double half = (cutoffL + cutoffH)/2;
		double modulus = 0.0;
		double sslope = half_power_width<=0.0 ? 0.0000000001 : half_power_width;
		double oslope = half_power_width<=0.0 ? 0.0000000001 : half_power_width_ori;
		const double cutoffOL = 0, cutoffOH = Math::mod(theta2-theta1, PI), halfO = Math::mod(cutoffOH + (PI-cutoffOH)/2, PI);
		double yy, xx, phy, sec_factor;


		if(cutoffL<=cutoffH) {
			for(int y=1; y<=height; y++) {
				yy = y-originY;
				for(int x=1; x<=width; x++) {
					xx = x-originX;
					phy = Math::mod(atan2(yy, xx) - theta1, PI);
					if(phy<cutoffOH) sec_factor = 1.0;
					else if(phy<halfO) sec_factor = pow(1.0/(-cutoffOH+phy+1.0), 1.0/oslope);
					else sec_factor = pow(1.0/(PI-phy+1.0), 1.0/oslope);
					//if(isinf(sec_factor)) { sec_factor = 1.0; }
					//if(isnan(sec_factor)) { sec_factor = 0.0; }
					modulus = sqrt(yy*yy + xx*xx);
					if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope) * sec_factor;
					else if(modulus<cutoffH) filter(y, x) = 1.0 * sec_factor;
					else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope) * sec_factor;
					//if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		} else {
			for(int y=1; y<=height; y++) {
				yy = y-originY;
				for(int x=1; x<=width; x++) {
					xx = x-originX;
					phy = Math::mod(atan2(yy, xx) - theta1, PI);
					if(phy<cutoffOH) sec_factor = 1.0;
					else if(phy<halfO) sec_factor = pow(1.0/(-cutoffOH+phy+1.0), 1.0/oslope);
					else sec_factor = pow(1.0/(PI-phy+1.0), 1.0/oslope);

					modulus = sqrt(yy*yy + xx*xx);
					if(modulus<cutoffH) filter(y, x) = 1.0 * sec_factor;
					else if(modulus<half) filter(y, x) = (pow(1.0/(modulus-cutoffH+1.0), 1.0/sslope)) * sec_factor;
					else if(modulus<cutoffL) filter(y, x) = (pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope)) * sec_factor;
					else filter(y, x) = 1.0 * sec_factor;
					//if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		}

		filter(originY, originX) = 1.0;
		return filter;
	}

	Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double cutoff_lambda3, double cutoff_lambda4, double theta1, double theta2, double slope)
	{
		Matrix filter(height, width);

		const double originY = (height)/2+1;
		const double originX = (width )/2+1;
		double modulus = 0.0, phyL = 0.0, phyH = 0.0;
		double sslope = slope<=0.0 ? 0.0000000001 : slope;
		double cutoffL, cutoffH, half;
		Point p;

		if(cutoff_lambda1<=cutoff_lambda2) {
			for(int y=1; y<=height; y++) {
				for(int x=1; x<=width; x++) {
					phyL = atan2(y-originY, x-originX);
					phyH = Math::mod(phyL - theta2, 2*PI);
					phyL = Math::mod(phyL - theta1, 2*PI);
					modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
					cutoffL = cutoff_lambda1*cutoff_lambda3 / p.set(cutoff_lambda3*cos(phyL), cutoff_lambda1*sin(phyL)).length();
					cutoffH = cutoff_lambda2*cutoff_lambda4 / p.set(cutoff_lambda4*cos(phyH), cutoff_lambda2*sin(phyH)).length();
					if(modulus<cutoffL) filter(y, x) = pow(1.0/(cutoffL-modulus+1.0), 1.0/sslope);
					else if(modulus<cutoffH) filter(y, x) = 1.0;
					else filter(y, x) = pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope);
					//if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		} else {
			for(int y=1; y<=height; y++) {
				for(int x=1; x<=width; x++) {
					phyL = atan2(y-originY, x-originX);
					phyH = Math::mod(phyL - theta2, 2*PI);
					phyL = Math::mod(phyL - theta1, 2*PI);
					modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
					cutoffL = cutoff_lambda1*cutoff_lambda3 / p.set(cutoff_lambda3*cos(phyL), cutoff_lambda1*sin(phyL)).length();
					cutoffH = cutoff_lambda2*cutoff_lambda4 / p.set(cutoff_lambda4*cos(phyH), cutoff_lambda2*sin(phyH)).length();
					half = (cutoffL + cutoffH)/2;
					if(modulus<cutoffH) filter(y, x) = 1.0;
					else if(modulus<half) filter(y, x) = (pow(1.0/(-cutoffH+modulus+1.0), 1.0/sslope));
					else if(modulus<cutoffL) filter(y, x) = (pow(1.0/(+modulus-cutoffL+1.0), 1.0/sslope));
					else filter(y, x) = 1.0;
					//if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		}
		filter(originY, originX) = 1.0;
		return filter;
	}


	void FFT2::copyBoxSpectrum(FFT2 &source, int s_left, int s_top, FFT2 &target, int t_left, int t_top, int width, int height)
	{
		int t_width = target.width_, s_width = source.width_;
		int sv, sh, tv, th;
		double factor;
		//std::cout << width_ << ":" << left << " " << height_ << ":" << top << "*" << bottom << std::endl;
		for (int y=0; y<height; y++) {
			sv = s_top + y;
			tv = t_top + y;
			if(sv<0 || sv>=source.height_) throw Exception("FFT2::copyBoxSpectrum: sv :");
			if(tv<0 || tv>=target.height_) throw Exception("FFT2::copyBoxSpectrum: tv :");
			for (int x=0; x<width; x++) {
				//std::cout << h << ":" << x << "  ";
				target.frq_spc[(t_left+x) + t_width*tv][REAL] = source.frq_spc[(s_left+x) + s_width*sv][REAL];
				target.frq_spc[(t_left+x) + t_width*tv][IMAGINARY] = source.frq_spc[(s_left+x) + s_width*sv][IMAGINARY];
			}
			//std::cout << std::endl;
		}
	}


	void FFT2::resizeImage(const Image &source, Image &result, int new_width, int new_height)
	{
		FFT2 src;
		FFT2 rst;
		Matrix source_buf[4];
		Matrix result_buf[4];

		//src.getSpectrum(buf_r, buf_i);

		const int s_width = source.getWidth();
		const int s_height = source.getHeight();
		const int cp_width = Math::min(source.getWidth(), new_width);
		const int cp_height = Math::min(source.getHeight(), new_height);

		int s_right_b = 0, s_left_b, s_bottom_b = 0, s_top_b;
		int t_right_b = 0, t_left_b, t_bottom_b = 0, t_top_b;
		int right_length, left_length, top_length, bottom_length;


		if(s_width>=new_width) {
			right_length = cp_width - originFor(cp_width);
			left_length = originFor(cp_width);

		} else {
			right_length = cp_width - originFor(cp_width);
			left_length = originFor(cp_width);
		}
		s_left_b = s_width - left_length;
		t_left_b = new_width - left_length;

		if(s_width>=new_width) {
			bottom_length = cp_width - originFor(cp_width);
			top_length = originFor(cp_width);

		} else {
			bottom_length = cp_height - originFor(cp_height);
			top_length = originFor(cp_height);
		}
		s_top_b = s_height - top_length;
		t_top_b = new_height - top_length;

		int itr;
		switch (source.getComponentKind()) {
			case Image::GRAY:
				source.to(source_buf[0]);
				itr = 1;
				break;
			case Image::RGB:
				source.to(source_buf[0], source_buf[1], source_buf[2]);
				itr = 3;
				break;
			case Image::RGBA:
				source.to(source_buf[0], source_buf[1], source_buf[2], source_buf[3]);
				itr = 4;
				break;
			default:
				throw Exception("FFT2::resizeImage");
		}
		rst.set(new_width, new_height);
		for(int i=0; i<itr; i++) {
			src.set(source_buf[i]);
			src.fft();
			std::cout << src.width_ << " " << src.height_ << std::endl;
			copyBoxSpectrum(src, s_right_b, s_top_b, rst, t_right_b, t_top_b, right_length, top_length);
			copyBoxSpectrum(src, s_left_b, s_top_b, rst, t_left_b, t_top_b, left_length, top_length);
			copyBoxSpectrum(src, s_right_b, s_bottom_b, rst, t_right_b, t_bottom_b, right_length, bottom_length);
			copyBoxSpectrum(src, s_left_b, s_bottom_b, rst, t_left_b, t_bottom_b, left_length, bottom_length);
			rst.ifft();
			rst.getImage(result_buf[i]);
			result_buf[i] = result_buf[i];
		}
		switch (source.getComponentKind()) {
			case Image::GRAY:
				result.from(result_buf[0]);
				break;
			case Image::RGB:
				result.from(result_buf[0], result_buf[1], result_buf[2]);
				break;
			case Image::RGBA:
				result.from(result_buf[0], result_buf[1], result_buf[2], result_buf[3]);
				break;
			default:
				throw Exception("FFT2::resizeImage");
		}
	}


	int FFT2::originFor(int size)
	{
		return (size)/2+1;
	}

/*
	Matrix FFT2::makeFilter(int height, int width, double cutoff_lambda1, double cutoff_lambda2, double slope)
	{
		Matrix filter(height, width);

		double originY = (height-1)/2+1;
		double originX = (width -1)/2+1;
		double modulus = 0.0;
		double cutoffL = Math::log2(cutoff_lambda1);
		double cutoffH = Math::log2(cutoff_lambda2);

		if(cutoffL<cutoffH) {
			for(int y=1; y<=height; y++) {
				for(int x=1; x<=width; x++) {
					modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
					if(modulus!=0) modulus = Math::log2(modulus);
					if(modulus<cutoffL) filter(y, x) = 1.0-slope*(cutoffL-modulus);
					else if(modulus>cutoffH) filter(y, x) = 1.0-slope*(-cutoffH+modulus);
					else
						filter(y, x) = 1.0;
					//filter(y, x) = sigmoid(-modulus, -frequency, 1);
					if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		} else {
			for(int y=1; y<=height; y++) {
				for(int x=1; x<=width; x++) {
					modulus = sqrt((y-originY) * (y-originY) + (x-originX) * (x-originX));
					if(modulus!=0) modulus = Math::log2(modulus);
					if(modulus<cutoffL) filter(y, x) = slope*(cutoffL-modulus);
					else if(modulus>cutoffH) filter(y, x) = slope*(-cutoffH+modulus);
					else
						filter(y, x) = 0.0;
					//filter(y, x) = sigmoid(-modulus, -frequency, 1);
					if(filter(y,x)<0)filter(y,x)=0;
				}
			}
		}
		filter(originY, originX) = 1.0;
		return filter;
	}
	*/


	void FFT2::makeNoise()
	{
		double num = height_*width_;
		for (int i=0; i<num; i++) {
			frq_spc[i][REAL] = random(0.0,1.0);
			frq_spc[i][IMAGINARY] = 0;
		}
	}


	FFTW3D::FFTW3D()
	{
		img_spc = 0;
		frq_spc = 0;
	}
	FFTW3D::FFTW3D(int wid, int hei, int dep)
	{
		set(wid, hei, dep);
	}
	FFTW3D::FFTW3D(const Image *source, int framsize)
	{
		set(source, framsize);
	}
	FFTW3D::FFTW3D(const Matrix *source, int framsize)
	{
		set(source, framsize);
	}
	FFTW3D::~FFTW3D()
	{
		if(img_spc==0) free(img_spc);
		if(frq_spc==0) free(frq_spc);
	}
	void FFTW3D::set(int wid, int hei, int dep)
	{
		width_  = wid;
		height_ = hei;
		depth_  = dep;
		image_size_ = width_*height_;
		img_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei * dep);
		frq_spc = (fftw_complex*)malloc(sizeof(fftw_complex) * wid * hei * dep);
		x_zero = wid + 1;
		y_zero = hei + 1;
		z_zero = dep + 1;
		left = wid/2;
		top = hei/2;
		front = dep/2;
	}
	void FFTW3D::set(const Image *source, int framsize)
	{
		set(source[0].getWidth(), source[0].getHeight(), framsize);
		for(int z=0; z<depth_; z++) {
			for(int y=0; y<height_; y++) {
				for(int x=0; x<width_; x++) {
					img_spc[x + width_*y + image_size_*z][REAL] = source[z].getPix(x, y).getR();
					img_spc[x + width_*y + image_size_*z][IMAGINARY] = 0.0;
				}
			}
		}
	}
	void FFTW3D::set(const Matrix *source, int framsize)
	{
		set(source[0].getCols(), source[0].getRows(), framsize);
		for(int z=0; z<depth_; z++) {
			for(int y=0; y<height_; y++) {
				for(int x=0; x<width_; x++) {
					img_spc[x + width_*y + image_size_*z][REAL] = source[z](y+1, x+1);
					img_spc[x + width_*y + image_size_*z][IMAGINARY] = 0.0;
				}
			}
		}
	}

	int FFTW3D::freqX(int x) const
	{
		return 0;//imgs[0].freqX(x);
	}
	int FFTW3D::freqY(int y) const
	{
		return 0;//imgs[0].freqY(y);
	}
	int FFTW3D::freqT(int t) const
	{
		return 0;//t<0 ? t_zero+t : t;
	}
	double FFTW3D::pix(int x, int y, int z, double l)
	{
		return img_spc[x + width_*y + image_size_*z][REAL] = l;
	}
	double FFTW3D::pix(int x, int y, int z, const Color &c)
	{
		return img_spc[x + width_*y + image_size_*z][REAL] = c.getR();
	}
	double FFTW3D::getPix(int x, int y, int z)
	{
		return img_spc[x + width_*y + image_size_*z][REAL];
	}

	double FFTW3D::getDC()
	{
		return sqrt(frq_spc[0][REAL]*frq_spc[0][REAL]+frq_spc[0][IMAGINARY]*frq_spc[0][IMAGINARY]);
	}
	double FFTW3D::setDC(double l)
	{
		frq_spc[0][REAL] = l;
		frq_spc[0][IMAGINARY] = 0;
		return l;
	}


	void FFTW3D::fft()
	{
		fftw_plan plan = fftw_plan_dft_3d(depth_, height_, width_, img_spc, frq_spc, FFTW_FORWARD, FFTW_ESTIMATE);
		fftw_execute(plan);
		normalizeFFT();
	}
	void FFTW3D::ifft()
	{
		fftw_plan plan = fftw_plan_dft_3d(depth_, height_, width_, frq_spc, img_spc, FFTW_BACKWARD, FFTW_ESTIMATE);
		fftw_execute(plan);
	}
	void FFTW3D::normalizeFFT()
	{
		double num = depth_*height_*width_;
		for (int i=0; i<num; i++) {
			frq_spc[i][REAL] /= num;
			frq_spc[i][IMAGINARY] /= num;
		}
	}


	void FFTW3D::drawCloudImage(Drawable &target)
	{
		double rl, im;
		Color col;
		Point p;
		for (int z=0; z<depth_; z++) {
			p.z = z;
			for (int y=0; y<height_; y++) {
				p.y = y;
				for (int x=0; x<width_; x++) {
					p.x = x;
					rl = img_spc[x + width_*y + image_size_*z][REAL];
					//im = img_spc[x + width_*y + image_size_*z][IMAGINARY];
					col.set(rl);
//					target.pix(p, col);
				}
			}
		}
	}
	void FFTW3D::drawCloudSpectrum(Point c, double gamma, Drawable &target)
	{
		double rl, im;
		Color col;
		int h, v, d;
		Point p;
		for (int z=0; z<depth_; z++) {
			d = z<front ? z+front : z-front;
			p.z = d;
			for (int y=0; y<height_; y++) {
				v = y<top ? y+top : y-top;
				p.y = v;
				for (int x=0; x<left; x++) {
					h = x+left;
					p.z = h;
					rl = frq_spc[x + width_*y + image_size_*z][REAL];
					im = frq_spc[x + width_*y + image_size_*z][IMAGINARY];
					col.set(pow( sqrt(rl*rl+im*im), gamma ));
//					target.pix(p, col);
				}
				for (int x=left; x<width_; x++) {
					h = x-left;
					p.z = h;
					rl = frq_spc[x + width_*y + image_size_*z][REAL];
					im = frq_spc[x + width_*y + image_size_*z][IMAGINARY];
					col.set(pow( sqrt(rl*rl+im*im), gamma ));
//					target.pix(p, col);
				}
			}
		}
	}

}	/*	<- namespace Psycholops 	*/
