/* -*- mia-c++  -*-
 *
 * This file is part of MIA - a toolbox for medical image analysis 
 * Copyright (c) Leipzig, Madrid 1999-2017 Gert Wollny
 *
 * MIA is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with MIA; if not, see <http://www.gnu.org/licenses/>.
 *
 */

#include <mia/core/filter.hh>
#include <mia/core/msgstream.hh>
#include <mia/2d/cost.hh>

#include <numeric>
#include <limits>

NS_BEGIN(ssd_2dimage_cost)

NS_MIA_USE; 
using namespace std; 
using namespace boost; 



class CSSDCost: public C2DImageCost {
public: 	

private: 
	virtual double do_value(const C2DImage& a, const C2DImage& b) const; 
	virtual void do_evaluate_force(const C2DImage& a, const C2DImage& b, float scale, C2DFVectorfield& force) const; 
};

struct FEvalSSD : public TFilter<double> {
	
	template <typename T, typename R>
	struct SQD {
		double operator ()(T a, R b) const {
			double d = (double)a - (double)b; 
			return d * d;
		}
	}; 
	
	template <typename  T, typename  R>
	FEvalSSD::result_type operator () (const T2DImage<T>& a, const T2DImage<R>& b) const {
		return inner_product(a.begin(), a.end(), b.begin(), 0.0,  plus<double>(), SQD<T,R>()); 
	}
}; 

double CSSDCost::do_value(const C2DImage& a, const C2DImage& b) const
{
	FEvalSSD essd; 
	return filter(essd, a, b); 
}

struct FEvalForce: public TFilter<int> {
	FEvalForce(C2DFVectorfield& force, float scale):
		m_force(force),
		m_scale(scale)
		{
		}
	template <typename T, typename R> 
	int operator ()( const T2DImage<T>& a, const T2DImage<R>& b) const {
		
		C2DFImage help(a.get_size()); 
		
		typename T2DImage<T>::const_iterator ai = a.begin(); 
		typename T2DImage<T>::const_iterator ae = a.end(); 
		typename T2DImage<R>::const_iterator bi = b.begin(); 
		C2DFImage::iterator hi = help.begin(); 
		while (ai != ae) {
			*hi = ((float)*ai - (float)*bi) * m_scale; 
			++hi; ++ai; ++bi; 
		}
		
		m_force = get_gradient(help); 

		return 0; 
	}
private: 
	mutable C2DFVectorfield& m_force; 
	float m_scale; 

}; 
		

void CSSDCost::do_evaluate_force(const C2DImage& a, const C2DImage& b, float scale, C2DFVectorfield& force) const
{
	assert(a.get_size() == b.get_size()); 
	assert(a.get_size() == force.get_size()); 
	FEvalForce ef(force, scale); 
	filter(ef, a, b); 
}


class C2DSSDCostPlugin: public C2DImageCostPlugin {
public: 
	C2DSSDCostPlugin();
	virtual C2DSSDCostPlugin::ProductPtr do_create()const;
	virtual const string do_get_descr()const; 
private: 

	float m_min; 
	float m_max; 
};


C2DSSDCostPlugin::C2DSSDCostPlugin():
	C2DImageCostPlugin("ssddf")
{
}

C2DSSDCostPlugin::ProductPtr C2DSSDCostPlugin::do_create()const
{
	return C2DSSDCostPlugin::ProductPtr(new CSSDCost()); 
}


const string C2DSSDCostPlugin::do_get_descr()const
{
	return "2D image cost: sum of squared intensity differences, alternative formulation ";
}

extern "C" EXPORT CPluginBase *get_plugin_interface()
{
	return new C2DSSDCostPlugin(); 
}

NS_END
