/*
 *  psychlops_figure_gabor.cpp
 *  Psychlops Standard Library (Universal)
 *
 *  Last Modified 2006/01/19 by Kenchi HOSOKAWA
 *  (C) 2006 Kenchi HOSOKAWA, Kazushi MARUYA and Takao SATO
 */


#include <math.h>
#include <vector>
#include <algorithm>

#include "../../../platform/gl/psychlops_g_GL_h.h"

#include "../../../psychlops_core.h"
#include "psychlops_figure_gabor.h"


namespace Psychlops {


	double bellcurve(const double x, const double sigma) {
		return exp( -(x*x) / (2.0*sigma*sigma) );
	}


namespace StimulusTemplate {

	Gabor::~Gabor() {
	}
	Gabor& Gabor::setContrast(double cont) {
		contrast = cont;
		return *this;
	}
	Gabor& Gabor::setWavelength(Length length) {
		wavelength = length;
		return *this;
	}
	Gabor& Gabor::setOrientation(Angle orient) {
		orientation = orient;
		return *this;
	}
	Gabor& Gabor::setPhase(Angle phs) {
		phase = phs;
		return *this;
	}
	const Point Gabor::getDatum() const {
		Point p(left_+width_/2.0, top_+height_/2.0);
		return p;
	}

}

namespace Figures {

	int substructImages(Image &result, const Image &s1, const Image &s2, const double factor)
	{
		if(s1.getHeight()!=s2.getHeight()) throw new Exception("substructImages");
		if(s1.getWidth()!=s2.getWidth()) throw new Exception("substructImages");
		result.release();
		result.set(s1.getWidth(), s1.getHeight());
		Color c1, c2, r1;
		double difference = 0, sum = 0;
		bool error = false, serror = false;
		for(int y=0; y<s1.getHeight(); y++) {
			for(int x=0; x<s1.getWidth(); x++) {
				c1 = s1.getPix(x,y);
				c2 = s2.getPix(x,y);
				difference = c1.getR()-c2.getR();
				sum += difference!=0.0 ? 1 : 0;
				error = difference>=2.0/255.0;
				serror =  serror || error;
				result.pix_raw(x,y,Color(
					error ? 1 : 0,
					difference>0 ? factor*difference : 0,
					difference<0 ? factor*difference : 0,
					//c2.getR()-c1.getR()> 0 ? factor*(c2.getR()-c1.getR()) : 0,//.5 + factor * (c1.getR() - c2.getR()),
					//c1.getR()-c2.getR()> 0 ? factor*(c1.getR()-c2.getR()) : 0,//.5 + factor * (c1.getG() - c2.getG()),
					0,//.5 + factor * (c1.getB() - c2.getB()),
					1
				));
				//if(c1.getR()-c2.getR()>0.01) std::cout << c1.getR()-c2.getR() << " " << c1.getR() << " " <<  c2.getR() << std::endl;
			}
		}
		return serror ? -sum : sum;
	}

	void drawImageCoordinateTuner(Image &img)
	{
		const int width=256, height=256;
		img.release();
		img.set(width, height);
		for(int y=0; y<height; y++) {
			for(int x=0; x<width; x++) {
				img.pix_raw(x,y,
				  Color(abs(x-127.5)/255.0, 0.0, 0.0)
				);
			}
		}
	}
	void drawGrating(Image &img, int width , int height, double wavelength, double contrast, double orientation, double phase)
	{
		img.release();
		img.set(width, height);
		double xp, yp, r, freq = (1.0/wavelength)*2*PI;
		for(int y=0; y<height; y++) {
			yp = y-height/2.0;
			for(int x=0; x<width; x++) {
				xp = x-width/2.0;
				r = sqrt(xp*xp+yp*yp);
				img.pix_raw(x,y,
				  Color(
					0.5+0.5 * contrast* cos(freq* (sin(orientation)*xp-cos(orientation)*yp) + phase)
				  )
				);
			}
		}
	}
	void drawGaussian(Image &img, double sigma, double factor)
	{
		img.release();
		img.set(sigma*8, sigma*8);
		const int width = img.getWidth(), height = img.getHeight();
		double xp, yp, r, r2;
		for(int y=0; y<height; y++) {
			yp = y-height/2.0;
			for(int x=0; x<width; x++) {
				xp = x-width/2.0;
				r = sqrt(xp*xp+yp*yp);
				r2 = -(r*r) / (2.0*sigma*sigma);
				img.pix_raw(x,y,Color(factor*(exp(r2))));
			}
		}
	}
	void drawGabor(Image &img, double sigma, double wavelength, double contrast, double orientation, double phase)
	{
		img.release();
		img.set(sigma*8, sigma*8);
		const int width = img.getWidth(), height = img.getHeight();
		double xp, yp, r, freq = (1.0/wavelength)*2*PI;
		for(int y=0; y<height; y++) {
			yp = y-height/2.0;
			for(int x=0; x<width; x++) {
				xp = x-width/2.0;
				r = sqrt(xp*xp+yp*yp);
				img.pix_raw(x,y,
				  Color(0.996078 * (
					0.5+0.5 * contrast* cos(freq* (sin(orientation)*xp-cos(orientation)*yp) + phase)
					* exp( -(r*r) / (2.0*sigma*sigma) ))
				  ));
			}
		}
	}




	GaborBase::GaborBase()
	{
		set(0,0);
		contrast = 1.0;
		wavelength = 10;
		orientation = 0;
		phase = 0;
	}

	GaborBase& GaborBase::setSigma(double sigma)
	{
		return setSigma(sigma*pixel);
	}
	GaborBase& GaborBase::setSigma(Length sigma)
	{
		Rectangle::set(sigma*8.0, sigma*8.0);
		return *this;
	}
	GaborBase& GaborBase::setWave(double wavelen, double cont, double orient, double phs)
	{
		return setWave(wavelen*pixel, cont, orient*degree, phs*degree);
	}
	GaborBase& GaborBase::setWave(Length wavelen, double cont, Angle orient, Angle phs)
	{
		contrast = cont;
		wavelength = wavelen;
		orientation = orient;
		phase = phs;
		return *this;
	}


	const bool ImageGabor::Key::operator <(Key rhs) const
	{
		for(int i=0; i<5; i++)
		{
			if(data[i]!=rhs.data[i]) return data[i]<rhs.data[i];
		}
		return false;
	}
	const bool ImageGabor::Key::operator >(Key rhs) const
	{
		for(int i=0; i<5; i++)
		{
			if(data[i]!=rhs.data[i]) return data[i]>rhs.data[i];
		}
		return false;
	}


	ImageGabor::ImageGabor() : GaborBase()
	{
	}

	ImageGabor& ImageGabor::cache(DrawableWithCache &target)
	{
		Image* tmp = new Image();
		Figures::drawGabor(*tmp, this->getWidth()/8.0, 1.0/wavelength, contrast, orientation, phase);
		tmp->cache(target);
		Key key;
		key.data[0] = this->getWidth();
		key.data[1] = wavelength;
		key.data[2] = contrast;
		key.data[3] = orientation;
		key.data[4] = phase;
		cache_list[key] = tmp;
		return *this;
	}

	ImageGabor& ImageGabor::draw(Drawable &target)
	{
		Key key;
		key.data[0] = this->getWidth();
		key.data[1] = wavelength;
		key.data[2] = contrast;
		key.data[3] = orientation;
		key.data[4] = phase;
		if(cache_list.count(key)!=0) {
			cache_list[key]->centering(this->getCenter()).draw();
		} else {
			Figures::drawGabor(normal_, this->getWidth()/8.0, 1.0/wavelength, contrast, orientation, phase);
			normal_.centering(this->getCenter()).draw();
		}
		return *this;
	}

	void ImageGabor::to(Image &dest, Canvas &media)
	{
		Figures::drawGabor(dest, this->getWidth()/8.0, 1.0/wavelength, contrast, orientation, phase);

	}




}

	QuickGabor::Instance::Instance() : instantiated(false) {
		referenced_count_ = 0;
	}
	QuickGabor::Instance::~Instance() {
	}
	void QuickGabor::Instance::release() {
		referenced_count_--;
		if(referenced_count_<=0) {
			if(instantiated) {
				delete envelope_;
				delete [] carrier_;
			}
			instantiated = false;
		}
	}
	void QuickGabor::Instance::set(double sigma) {
		sigma_ = sigma;
		size_ = (int)(sigma_*6.0);
		setEnvelope();
		setCarrier();
		instantiated = true;
	}
	void QuickGabor::Instance::setEnvelope() {
		Color col;
		int particle=0;
		envelope_ = new Image;
		envelope_->set(size_, size_, Image::RGBA, Image::FLOAT);
		int halfsize = size_/2;

		if(halfsize*2 != size_) particle = 1;
		for(int y=-halfsize; y<halfsize+particle; y++) {
			for(int x=-halfsize; x<halfsize+particle; x++) {
				//col.set( 0.5, 0.5, 0.5, 1-bellcurve(sqrt(x*x+y*y), sigma_) );
				envelope_->pix(x+halfsize, y+halfsize, Color::gray);
				envelope_->alpha(x+halfsize, y+halfsize, 1-bellcurve(::sqrt((double)(x*x+y*y)), sigma_));
			}
		}
		envelope_->cache();
	}
	void QuickGabor::Instance::setCarrier() {
		double height, halfwidth;
		carrier_ = new Rectangle[size_];
		for(int x=1; x<size_-1; x++) {
			halfwidth = size_/2-1;
			height = 2*sqrt(halfwidth*halfwidth-(x-halfwidth)*(x-halfwidth));
			carrier_[x].set(1.1,height).shift(x-halfwidth-0.5,-height/2);
		}
	}
	QuickGabor::Instance& QuickGabor::Instance::draw(int x, int y, Length wavelength, Angle orientation, Angle phase, double contrast, Drawable &target) {
		Color col;
		int limit = size_/2 -1;
		glPushMatrix();
//		glEnable(GL_LINE_SMOOTH);
//		glHint(GL_LINE_SMOOTH_HINT, GL_NICEST);
		glTranslated(x+limit,y+limit,0);
		glRotated(orientation,0,0,1);
		for(int i=1; i<size_-3; i++) {
			col.set( contrast *
						sin( ((double)i/(double)wavelength)*2*PI + phase.at_radian() )
					/2.0+0.5);
			carrier_[i].draw(col, target);
		}
		glPopMatrix();
		envelope_->draw(x,y,target);
		return *this;
	}



	std::vector<QuickGabor::Instance *> QuickGabor::instance_;
	int QuickGabor::setInstance(double sigma) {
		Instance *ins;
		int index = 0;
		bool found =false;
		if(instance_.empty()) {
//			instance_ = std::vector<Instance>();
		} else {
			for(std::vector<Instance *>::iterator iter=instance_.begin(); iter!=instance_.end(); iter++, index++) {
				if((*iter)->sigma_ == sigma) {
					(*iter)->referenced_count_++;
					found = true;
					break;
				}
			}
		}
		if(found==true) {
			return index;
		} else {
			ins = new Instance;
			ins->set(sigma);
			instance_.push_back( ins );
			instance_.back()->referenced_count_++;
			return index;
		}
		return -1;
	}

	void QuickGabor::release() {
		if(sigma_index_>=0 && sigma_index_<instance_.size()) instance_[sigma_index_]->release();
	}



	QuickGabor::QuickGabor() {
	}
	QuickGabor::QuickGabor(double length, double sigma, double cont, double orient, double phs) {
		set(length, sigma, cont, orient, phs);
	}
	QuickGabor::QuickGabor(Length length, Length sigma, double cont, Angle orient, Angle phs) {
		set(length, sigma, cont, orient, phs);
	}
	QuickGabor::~QuickGabor() {
		release();
	}
	QuickGabor & QuickGabor::set(double length, double sigma, double cont, double orient, double phs) {
		return set(length*pixel, sigma*pixel, cont, orient*degree, phs*degree);
	}
	QuickGabor & QuickGabor::set(Length length, Length sigma, double cont, Angle orient, Angle phs) {
		sigma_ = sigma;
		contrast = cont;
		wavelength = length;
		orientation = orient;
		phase = phs;
		left_ = 0.0;
		top_ = 0.0;

		width_ = (int)(sigma_*6.0);
		height_ = width_;

		sigma_index_ = setInstance(sigma_);

		return *this;
	}
	QuickGabor & QuickGabor::setDatum(const Point &po) {
		return centering(po);
	}
	QuickGabor & QuickGabor::shift(const double x, const double y, const double z) {
		left_ += x;
		top_ += y;
		return *this;
	}
	QuickGabor & QuickGabor::centering(const double x, const double y, const double z) {
		left_ = x - width_/2.0;
		top_ = y - height_/2.0;
		return *this;
	}
	QuickGabor & QuickGabor::centering(const Point &po) {
		return centering(po.x, po.y);
	}
	QuickGabor & QuickGabor::centering(const Drawable &target) {
		return centering(Display::getCenter());
	}

	QuickGabor & QuickGabor::draw(Drawable &target) {
		instance_[sigma_index_]->draw((int)left_, (int)top_, wavelength, orientation, phase, contrast, target);
		return *this;
	}
	QuickGabor & QuickGabor::draw(int x, int y, Drawable &target) {
		instance_[sigma_index_]->draw(x, y, wavelength, orientation, phase, contrast, target);
		return *this;
	}

}

