#include "DSP2D.h"

DSP2D::DSP2D(unsigned int width, unsigned int height) {
	data=NULL;
	resize(width,height);
}

DSP2D::~DSP2D() { free(data); }

unsigned int DSP2D::width () { return(w); }

unsigned int DSP2D::height() { return(h); }

void DSP2D::getMinMax(int &min, int &max) {
	if ((w>0) && (h>0)) {
		min=(int)data[0];
		max=(int)data[0];
		for (unsigned int i=0;i<top;++i) {
			if ((int)data[i]<min) min=(int)data[i];
			if ((int)data[i]>max) max=(int)data[i];
		}
	}
}

void DSP2D::resize(unsigned int width, unsigned int height) {
	if (data!=NULL) free(data);
	w=width;
	h=height;
	if ((w>0) && (h>0)) {
		top=w*h;
		data=(double*)malloc(top*sizeof(double));
	} else {
		top=0;
		data=NULL;
	}
}

double& DSP2D::operator[](unsigned int i) {
	return(data[i]);
}

void DSP2D::set(int x, int y, double v) {
	if ((x>=0) && (y>=0) && (x<(int)w) && (y<(int)h)) data[x+w*y]=v;
}

double DSP2D::get(int x, int y) {
	if ((x>=0) && (y>=0) && (x<(int)w) && (y<(int)h)) return(data[x+w*y]);
	return(0);
}

void DSP2D::resetPixelCounter() {
	pixelCounter=(unsigned int)-1;
}

double DSP2D::getNextPixel() {
	return(data[++pixelCounter]);
}

void   DSP2D::setNextPixel(double v) {
	data[++pixelCounter]=v;
}

void DSP2D::operator +=(double v) {
	for (unsigned int i=0;i<w*h;++i) data[i]+=v;
}

void DSP2D::operator *=(double v) {
	for (unsigned int i=0;i<w*h;++i) data[i]*=v;
}

KERNEL2D::KERNEL2D(KERNEL_MANAGER *m) {
	morph_on=false;
	rank_on =false;	
	morph_select=0.0;
	rank_scale  =0.0;
	morph_select1=0; morph_weight1=0.0;
	morph_select2=0; morph_weight2=0.0;
	manager=m;
	ch=new CACHE2D();
	name="";
	manager->checkin(name,this);
}

KERNEL2D::KERNEL2D(const char *n, KERNEL_MANAGER *m) {
	rank_on =false;
	morph_on=false;	
	morph_select=0.0;
	rank_scale  =0.0;
	morph_select1=0; morph_weight1=0.0;
	morph_select2=0; morph_weight2=0.0;
	manager=m;
	ch=new CACHE2D();
	name=n;
	manager->checkin(name,this);
}

KERNEL2D::~KERNEL2D() {
	delete ch;
	for (map<set<KFactor*>,double>::iterator p=FDescriptor.begin();p!=FDescriptor.end();++p)
		for (set<KFactor*>::iterator q=p->first.begin();q!=p->first.end();++q)
			delete *q;

}

double KERNEL2D::getFilter(DSP2D &source, int x, int y) {
	long pos=x+source.w*y;
	double v;
	if ((x>=0) && (y>=0) && (x<(int)source.w) && (y<(int)source.h) && (ch->get(source,pos,v))) return(v);
	if (morph_on) v=getMorph(source,x,y); else
	if (rank_on ) v=getRank (source,x,y); else
		      v=getConv (source,x,y);
	if ((x>=0) && (y>=0) && (x<(int)source.w) && (y<(int)source.h)) ch->set(source,pos,v);
	return(v);
}

void KERNEL2D::filter(DSP2D &source, DSP2D &target, int x, int y) {
	target.set(x,y,getFilter(source,x,y));
}

void KERNEL2D::filter(DSP2D &source, DSP2D &target) {
	for (int x=0;x<(int)source.w;++x) for (int y=0;y<(int)source.h;++y)
		filter(source,target,x,y);
}

double KERNEL2D::getConv(DSP2D &source, int x, int y) {
	double r=0;
	for (map<set<KFactor*>,double>::iterator p=FDescriptor.begin();p!=FDescriptor.end();++p) if (p->first.size()>0) {
		double prod=p->second;
		for (set<KFactor*>::iterator q=p->first.begin();q!=p->first.end();++q)
			prod *= (*q)->get(source,x,y);
		r+=prod;
	}	
	return(r);
}

double KERNEL2D::getMorph(DSP2D &source, int x, int y) {
	vector<double> result;
	for (map<set<KFactor*>,double>::iterator p=FDescriptor.begin();p!=FDescriptor.end();++p) if (p->first.size()>0) {
		double prod=p->second;
		for (set<KFactor*>::iterator q=p->first.begin();q!=p->first.end();++q)
			prod *= (*q)->get(source,x,y);
		result.insert(result.end(),prod);
	}
	double selection=result[0];
	if (morph_select==1.0) {
		for (vector<double>::iterator p=result.begin();p!=result.end();++p) if (selection<(*p)) selection=(*p);
	} else if (morph_select==0.0) {
		for (vector<double>::iterator p=result.begin();p!=result.end();++p) if (selection>(*p)) selection=(*p);
	} else {
		sort(result.begin(),result.end());
		if (morph_weight2==0) selection=result[morph_select1];
		else selection=result[morph_select1]*morph_weight1+result[morph_select2]*morph_weight2;
	}
	return(selection);
}

double KERNEL2D::getRank(DSP2D &source, int x, int y) {
	vector<double> result;
	for (map<set<KFactor*>,double>::iterator p=FDescriptor.begin();p!=FDescriptor.end();++p) if (p->first.size()>0) {
		double prod=p->second;
		for (set<KFactor*>::iterator q=p->first.begin();q!=p->first.end();++q)
			prod *= (*q)->get(source,x,y);
		result.insert(result.end(),prod);

	}
	double v=0;
	int n=0;
	if (result.size()>0) v=result[0];
	for (unsigned int i=0;i<result.size();++i) if (result[i]>v) { v=result[i]; ++n; }
	v=((double)n)*rank_scale;
	return(v);
}

void KERNEL2D::convolve(DSP2D &source, DSP2D &target, int x, int y) {
	target.set(x,y,getConv(source,x,y));
}

void KERNEL2D::convolve(DSP2D &source, DSP2D &target) {
	target.resize(source.w,source.h);
	for (int x=0;x<(int)source.w;++x) for (int y=0;y<(int)source.h;++y)
		convolve(source,target,x,y);
}

void KERNEL2D::morph(DSP2D &source, DSP2D &target, int x, int y) {
	target.set(x,y,getMorph(source,x,y));
}

void KERNEL2D::morph(DSP2D &source, DSP2D &target) {
	target.resize(source.w,source.h);
	for (int x=0;x<(int)source.w;++x) for (int y=0;y<(int)source.h;++y)
		morph(source,target,x,y);
}

void KERNEL2D::rank(DSP2D &source, DSP2D &target, int x, int y) {
	target.set(x,y,getRank(source,x,y));
}

void KERNEL2D::rank(DSP2D &source, DSP2D &target) {
	target.resize(source.w,source.h);
	for (int x=0;x<(int)source.w;++x) for (int y=0;y<(int)source.h;++y)
		rank(source,target,x,y);
}

string KERNEL2D::getName() {
	return(name);
}

void KERNEL2D::insert(int x, int y) {
	buffer.ins(x,y);
}

void KERNEL2D::weight(double v) {
	set<KFactor*> fdset;
	KFactor *fkt=new KFactor("");
	*fkt=buffer;
	fdset.insert(fkt);
	FDescriptor.insert(map<set<KFactor*>,double>::value_type(fdset,v));
	buffer.clear();
}

void KERNEL2D::clear() {
	FDescriptor.clear();
	morph_on=false;
	morph_select=0.0;
	morph_select1=0; morph_weight1=0.0;
	morph_select2=0; morph_weight2=0.0;
}

void KERNEL2D::load(const char* filename) {
	ifstream ifs(filename,ios::in);
	if (ifs) {
		clear();
		while (!ifs.eof()) {
			char command=0;
			ifs >> command;

			if (command=='V') {
				double weight;
				ifs >> weight;
				KFactor *kf=new KFactor("");
				while ((ifs.peek()!='\n') && (!ifs.eof())) {
					int x,y;
					ifs >> x;
					ifs >> y;
					kf->ins(x,y);
				}
				set<KFactor*> kfset;
				kfset.insert(kf);
				FDescriptor.insert(map<set<KFactor*>,double>::value_type(kfset,weight));
			}
			if (command=='K') {
				string name;
				double weight;
				ifs >> weight;
				ifs >> name;
				KFactor *kf=new KFactor(name);
				while ((ifs.peek()!='\n') && (!ifs.eof())) {
					int x,y;
					ifs >> x;
					ifs >> y;
					kf->ins(x,y);
				}
				set<KFactor*> kfset;
				kfset.insert(kf);
				FDescriptor.insert(map<set<KFactor*>,double>::value_type(kfset,weight));
			}
			if (command=='P') {
				double weight;
				ifs >> weight;
				int x,y;
				set<KFactor*> kfset;
				KFactor *kf=new KFactor("");
				while ((ifs.peek()!='\n') && (!ifs.eof())) {
					string token;
					ifs >> token;
					if (sscanf(token.c_str(),"%d",&x)==1) {
						ifs >> token;
						if (sscanf(token.c_str(),"%d",&y)==1) kf->ins(x,y);
					} else {
						if (kf->size()>0) kfset.insert(kf); else delete kf;
						kf=new KFactor(token);
					}
				}
				if (kf->size()>0) kfset.insert(kf);
				FDescriptor.insert(map<set<KFactor*>,double>::value_type(kfset,weight));
			}
			if (command=='N') {
				manager->checkout(name);
				ifs >> name;
				manager->checkin(name,this);
			}
			if (command=='M') {
				rank_on =false;
				morph_on=true;
				ifs >> morph_select;
			}
			if (command=='R') {
				morph_on=false;
				rank_on =true;
				ifs >> rank_scale;
			}
		}
		ifs.close();
		clean();
		if (morph_on) calcSel();
	}
}

void KERNEL2D::save(const char* filename) {
	ofstream ofs(filename,ios::out);
	if (ofs) {
		ofs << "N" << name << endl;
		for (map<set<KFactor*>,double>::iterator p=FDescriptor.begin();p!=FDescriptor.end();++p) {
			ofs << "P " << p->second;
			for (set<KFactor*>::iterator q=p->first.begin();q!=p->first.end();++q)
				(*q)->save(ofs);
			ofs << endl;
		}
		if (morph_on) ofs << "M " << morph_select << endl; 
		if (rank_on ) ofs << "R " << rank_scale   << endl;
		ofs.close();
	}
}

void KERNEL2D::calcSel() {
	double pos=1.0+((double)(FDescriptor.size()-1))*morph_select;
	if (pos==(int)pos) { morph_select1=(int)pos-1; morph_weight1=1.0;
			     morph_select2=(int)pos;   morph_weight2=0.0; }
	else { morph_select1=(int)pos-1; morph_weight1=1.0-pos+(double)((int)pos);
	       morph_select2=(int)pos;   morph_weight2=1.0-morph_weight1;
	}
}

void KERNEL_MANAGER::clearCaches() {
	for (map<string,KERNEL2D*>::iterator p=kernels.begin();p!=kernels.end();++p)
		p->second->clearCache();
}

void KERNEL_MANAGER::checkin(string &name, KERNEL2D* k) {
	kernels.insert(map<string,KERNEL2D*>::value_type(name,k));
	changed=true;
}

void KERNEL_MANAGER::checkout(string &name) {
	kernels.erase(name);
	changed=true;
}

void KERNEL_MANAGER::resolveAll() {
	if (changed) {
		changed=false;
		for (map<string,KERNEL2D*>::iterator p=kernels.begin();p!=kernels.end();++p)
			p->second->resolve();
	}
}

void KERNEL_MANAGER::deleteAll() {
	changed=true;
	for (map<string,KERNEL2D*>::iterator p=kernels.begin();p!=kernels.end();++p)
		delete p->second;
	kernels.clear();
}

KERNEL2D* KERNEL_MANAGER::resolve(const string name) {
	if (kernels.count(name)>0) return(kernels[name]);
	return(NULL);
}

void KERNEL2D::resolve() {
	for (map<set<KFactor*>,double>::iterator p=FDescriptor.begin();p!=FDescriptor.end();++p)
	for (set<KFactor*>::iterator q=p->first.begin();q!=p->first.end();++q)
		(*q)->res(manager);
}

void KERNEL2D::clearCache() {
	if (ch!=NULL) ch->clear();
}

void KERNEL2D::clean() {
	if (!morph_on) {
		set<set<KFactor*> >toDelete;
		map<set<KFactor*>,double> fd=FDescriptor;
		FDescriptor.clear();
		for (map<set<KFactor*>,double>::iterator p=fd.begin();p!=fd.end();++p)
			if (p->second!=0.0) FDescriptor.insert(*p);
			else  toDelete.insert(p->first);
		for (set<set<KFactor*> >::iterator p=toDelete.begin();p!=toDelete.end();++p)
			for (set<KFactor*>::iterator q=p->begin();q!=p->end();++q)
				delete *q;
	}
}

KFactor::KFactor() {
	name="";
	krn=NULL;
}

KFactor::KFactor(string n) {
	name=n;
	krn=NULL;
}

void KFactor::ins(int x, int y) {
	volterra.insert(make_pair(x,y));
}

void KFactor::clear() {
	volterra.clear();
}

double KFactor::get(DSP2D &source, int x, int y) {
	double prod=1.0;
	if (krn==NULL)
	for (multiset<pair<int,int> >::iterator p=volterra.begin();p!=volterra.end();++p)
		prod*=source.get(x+p->first,y+p->second);
	 else
	for (multiset<pair<int,int> >::iterator p=volterra.begin();p!=volterra.end();++p)
		prod*=krn->getFilter(source,x+p->first,y+p->second);
	return(prod);
}

void KFactor::res(KERNEL_MANAGER *m) {
	if (name!="") {
		krn=m->resolve(name);
	} else {
		krn=NULL;
	}
}

void KFactor::save(ofstream &ofs) {
	ofs << " " << name;
	for (multiset<pair<int,int> >::iterator p=volterra.begin();p!=volterra.end();++p)
		ofs << " " << p->first << " " << p->second;
}

int KFactor::size() {
	return(volterra.size());
}

void CACHE2D::set(DSP2D &source, long pos, double v) {
	if (cdata.count(&source)==0) {
		double *d=(double*)malloc(sizeof(double)*source.top);
		bool   *b=(bool  *)malloc(sizeof(bool  )*source.top);
		for (long i=0;i<(long)source.top;++i) b[i]=false;
		cdata.insert(map<DSP2D*,pair<bool*,double*> >::value_type(&source,make_pair(b,d)));
	}
	if ((pos>0) && (pos<(long)source.top)) {
		pair<bool*,double*> *p=&cdata[&source];
		p->first[pos]=true;
		p->second[pos]=v;
	}
}

bool CACHE2D::get(DSP2D &source, long pos, double &v) {
	if (cdata.count(&source)!=0) {
		if ((pos>0) && (pos<(long)source.top)) {
			pair<bool*,double*> *p=&cdata[&source];
			v=p->second[pos];
			return(p->first[pos]);
		}
	}
	return(false);
}

CACHE2D::CACHE2D() {
}

CACHE2D::~CACHE2D() {
	clear();
}

void CACHE2D::clear() {
	for (map<DSP2D*,pair<bool*,double*> >::iterator p=cdata.begin();p!=cdata.end();++p) {
		delete p->second.first;
		delete p->second.second;
	}
	cdata.clear();
}