//////////////////////////////////////////////////////////////////////////
//
//  Copyright (c) 2013-2014, Image Engine Design Inc. All rights reserved.
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions are
//  met:
//
//      * Redistributions of source code must retain the above
//        copyright notice, this list of conditions and the following
//        disclaimer.
//
//      * Redistributions in binary form must reproduce the above
//        copyright notice, this list of conditions and the following
//        disclaimer in the documentation and/or other materials provided with
//        the distribution.
//
//      * Neither the name of John Haddon nor the names of
//        any other contributors to this software may be used to endorse or
//        promote products derived from this software without specific prior
//        written permission.
//
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
//  IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
//  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
//  PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
//  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
//  EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
//  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
//  PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
//  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
//  NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
//  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
//////////////////////////////////////////////////////////////////////////

#include "GafferScene/Constraint.h"

#include "Gaffer/StringPlug.h"

#include "IECoreScene/CurvesPrimitive.h"
#include "IECoreScene/MeshPrimitive.h"
#include "IECoreScene/PointsPrimitive.h"

#include <boost/format.hpp>

#include <algorithm>
#include <cassert>
#include <cmath>
#include <limits>

using namespace Imath;
using namespace IECore;
using namespace Gaffer;
using namespace GafferScene;

namespace
{

template< typename Indexer >
bool convexPolygon( const Indexer vertices, const int n )
{
	assert( n >= 3 );

	if( n == 3 )
	{
		return true;
	}

	int c[ 2 ] = { 0, 0 };

	for( int i = 0; i < n; ++i )
	{
		const int ip = ( i + n - 1 ) % n;
		const int in = ( i     + 1 ) % n;

		const Imath::V2f vi = vertices( i );

		const float Av = ( vi - vertices( ip ) ) % ( vertices( in ) - vi );

		// NOTE : colinear edges are ok

		if( Av == 0.f )
		{
			c[ 0 ] += 1;
			c[ 1 ] += 1;
		}
		else
		{
			c[ std::signbit( Av ) ? 0 : 1 ] += 1;
		}
	}

	return
		( c[ 0 ] == n ) ||
		( c[ 1 ] == n );
}

template< typename Indexer >
bool ptInPolygon( const Indexer vertices, const Imath::V2f v, const int n )
{
	bool result = false;
	for( int i = 0; i < n; ++i )
	{
		const Imath::V2f v0 = vertices( i );
		const Imath::V2f v1 = vertices( ( i + 1 ) % n );

		// NOTE: Algorithm 3.5, "Collision Detection in Interactive 3d Environments", Gino Van Den Bergen
		//       with modifications to include end points and points on axis aligned edges.

		if( v0 == v )
		{
			return true; // NOTE : end point
		}
		else if(
			( ( v0.y == v1.y ) && ( v.y == v0.y ) && ( ( v0.x >= v.x ) != ( v1.x >= v.x ) ) ) ||
			( ( v0.x == v1.x ) && ( v.x == v0.x ) && ( ( v0.y >= v.y ) != ( v1.y >= v.y ) ) ) )
		{
			return true; // NOTE : axis aligned edge
		}
		else if( ( v0.y >= v.y ) != ( v1.y >= v.y ) )
		{
			// Edge crosses horizontal line `y == v.y`

			if( ( v0.x >= v.x ) != ( v1.x >= v.x ) )
			{
				// Edge crosses vertical line `x == v.x`

				if( v0.x + ( v.y - v0.y ) * ( v1.x - v0.x ) / ( v1.y - v0.y ) >= v.x )
				{
					// Edge crosses the ray `y == v.y, x >= v.x`

					result = !result;
				}
			}
			else if( v0.x >= v.x )
			{
				// Edge crosses the ray `y == v.y, x >= v.x`

				result = !result;
			}
		}
	}

	return result;
}

template< typename PIndexer, typename UVIndexer >
Imath::V3f interpolateConvexPolygon( const PIndexer points, const UVIndexer uvs, const Imath::V2f uv, const int n )
{
	// NOTE : compute vertex and edge triangle areas
	//
	//        Av[ i ] is 2 * area of the triangle formed by the vertices v(i-1), v(i) and v(i+1)
	//        Ae[ i ] is 2 * area of the triangle formed by the vertices v(i), v(i+1) and uv

	std::unique_ptr< float[] > Av( new float[ n ] );
	std::unique_ptr< float[] > Ae( new float[ n ] );

	for( int i = 0; i < n; ++i )
	{
		const Imath::V2f vi = uvs( i );

		// NOTE : avoid zero length edges by skipping past adjacent duplicate uv vertices
		//        in both directions, c is the number of times the duplicate vertex occurs
		//        in a consecutive run including the current vertex (vi). It is important
		//        that only adjacent duplicated vertices are included. The computed vertex
		//        area is weighted by the reciprocal of c to average the influence of the
		//        positions corresponding to the duplicated uv vertices.

		float c = 1.f;

		Imath::V2f vp;
		for( int j = 1; j < n; ++j )
		{
			vp = uvs( ( i + n - j ) % n );
			if( vp != vi ) break;
			c += 1.f;
		}

		Imath::V2f vn;
		for( int j = 1; j < n; ++j )
		{
			vn = uvs( ( i + j ) % n );
			if( vn != vi ) break;
			c += 1.f;
		}

		Av[ i ] = ( vi - vp ) % ( vn - vi );
		Ae[ i ] = ( vn - vi ) % ( uv - vn );

		// NOTE : sign of vertex area depends on winding.

		if( std::signbit( Av[ i ] ) )
		{
			Av[ i ] = -( Av[ i ] );
			Ae[ i ] = -( Ae[ i ] );
		}

		// NOTE : clamp edge area to minimum of zero to prevent negative weights

		Ae[ i ] = std::max( Ae[ i ], 0.f );

		// NOTE : this clamp is done in two steps to prevent underflow to zero

		Av[ i ] = std::max( Av[ i ], c * std::numeric_limits< float >::min() );
		Av[ i ] /= c;

		// NOTE : uv is considered on an edge when the edge area is below threshold in which
		//        case lerp between average of all positions corresponding to end vertices

		if(
			( Ae[ i ] < std::sqrt( std::numeric_limits< float >::min() ) ) &&
			( ( ( uv - vi ) ^ ( uv - vn ) ) < std::numeric_limits< float >::min() ) )
		{
			const float l2 = ( vn - vi ).length2();
			const float t = std::min( std::max(
				( l2 > ( 2.f * std::numeric_limits< float >::min() ) )
					? std::sqrt( ( uv - vn ).length2() / l2 ) : 0.f, 0.f ), 1.f );

			Imath::V3f pv( 0.f );
			Imath::V3f pn( 0.f );

			float pvc = 0.f;
			float pnc = 0.f;

			for( int j = 0; j < n; ++j )
			{
				if( uvs( j ) == vi )
				{
					pv += points( j );
					pvc += 1.f;
				}
				if( uvs( j ) == vn )
				{
					pn += points( j );
					pnc += 1.f;
				}
			}

			assert( pvc != 0.f );
			assert( pnc != 0.f );

			return
				( pv / pvc ) * (       t ) +
				( pn / pnc ) * ( 1.0 - t );
		}
	}

	// NOTE : uv is not on any edges or coincident with any vertices use wachspress coordinates
	//        the factor of 2 in the denominator cancels out during normalisation

	float ws = 0.f;
	Imath::V3f p( 0.f );

	for( int i = 0; i < n; ++i )
	{
		const Imath::V2f vi = uvs( i );

		int ip = 0;
		for( int j = 1; j < n; ++j )
		{
			ip = ( i + n - j ) % n;
			if( uvs( ip ) != vi )
				break;
		}

		const float Ad = Ae[ ip ] * Ae[ i ];

		if( Ad != 0.f )
		{
			const float w = Av[ i ] / Ad;
			p += points( i ) * w;
			ws += w;
		}
	}

	if( ws != 0.f )
	{
		p /= ws;
	}

	return p;
}

template< typename PIndexer, typename UVIndexer >
Imath::V3f interpolateNonConvexPolygon( const PIndexer points, const UVIndexer uvs, const Imath::V2f uv, const int n )
{
	// NOTE : a simple triangle fan from any vertex will cover the non convex polygon
	//        the generated triangles may cover areas outside the polygon this is fine

	Imath::V3f p( 0.f );

	for( int i = 2; i < n; ++i )
	{
		const Imath::V2f tuv[ 3 ] =
		{
			uvs( 0 ),
			uvs( i - 1 ),
			uvs( i )
		};

		if( ptInPolygon( [ & tuv ]( const int ii ){ return tuv[ ii ]; }, uv, 3 ) )
		{
			const float w = ( tuv[ 2 ] - tuv[ 0 ] ) % ( tuv[ 1 ] - tuv[ 0 ] );

			if( w == 0.f )
			{
				// NOTE : uv triangle numerically has zero area so is effectively a line or point.
				//        position in 3d space is ambiguous unless the triangle in 3d space is a point.

				const Imath::V3f& vp = points( i - 1 );

				if(
					( ( points( 0 ) - vp ).length2() <= ( 2.f * std::numeric_limits< float >::min() ) ) &&
					( ( points( i ) - vp ).length2() <= ( 2.f * std::numeric_limits< float >::min() ) ) )
				{
					p = vp;

					break;
				}

				throw IECore::InvalidArgumentException( (
					boost::format(
						"Gaffer::Constraint : UV coordinates \"%s\" map to ambiguous point(s) in 3d space."
					) % ( uv ) ).str() );
			}

			// NOTE : ensure that the positive barycentric coordinates sum to a maximum of one.

			const float b0 = std::max( 0.f, ( ( tuv[ 1 ] - tuv[ 2 ] ) % ( uv - tuv[ 1 ] ) ) / w );
			const float b1 = std::max( 0.f, ( ( tuv[ 2 ] - tuv[ 0 ] ) % ( uv - tuv[ 2 ] ) ) / w );
			const float b2 = std::max( 0.f, ( ( tuv[ 0 ] - tuv[ 1 ] ) % ( uv - tuv[ 0 ] ) ) / w );

			const float bs = b0 + b1 + b2;

			if( bs != 0.f )
			{
				p =
					points(     0 ) * ( b0 / bs ) +
					points( i - 1 ) * ( b1 / bs ) +
					points( i     ) * ( b2 / bs );

				break;
			}
		}
	}

	return p;
}

void constructMatrix( Imath::M44f& m, const Imath::V3f& p, const Imath::V3f& x, const Imath::V3f& y, const Imath::V3f& z )
{
	m[ 0 ][ 0 ] = x[ 0 ];
	m[ 0 ][ 1 ] = x[ 1 ];
	m[ 0 ][ 2 ] = x[ 2 ];
	m[ 0 ][ 3 ] = 0.f;

	m[ 1 ][ 0 ] = y[ 0 ];
	m[ 1 ][ 1 ] = y[ 1 ];
	m[ 1 ][ 2 ] = y[ 2 ];
	m[ 1 ][ 3 ] = 0.f;

	m[ 2 ][ 0 ] = z[ 0 ];
	m[ 2 ][ 1 ] = z[ 1 ];
	m[ 2 ][ 2 ] = z[ 2 ];
	m[ 2 ][ 3 ] = 0.f;

	m[ 3 ][ 0 ] = p[ 0 ];
	m[ 3 ][ 1 ] = p[ 1 ];
	m[ 3 ][ 2 ] = p[ 2 ];
	m[ 3 ][ 3 ] = 1.f;
}

void constructLocalFrame( Imath::M44f& m, const Imath::V3f& p, const Imath::V3f& t, const Imath::V3f& b, const Imath::V3f& n )
{
	// NOTE : use tangent and bitangent when both have non zero length and are not colinear
	//        and either the normal has zero length or both the tangent and bitangent are
	//        not colinear with the normal, otherwise use tangent and normal when they both
	//        have non zero length and are not colinear, otherwise use bitangent and normal
	//        when they both have non zero length and are not colinear

	const Imath::V3f nt = t.normalized();
	const Imath::V3f nb = b.normalized();
	const Imath::V3f nn = n.normalized();
	const Imath::V3f bxt = nb % nt;
	const Imath::V3f txn = nt % nn;
	const Imath::V3f nxb = nn % nb;

	if(
		( bxt.length2() > ( 2.f * std::numeric_limits< float >::min() ) ) &&
		( ( n.length2() < ( 2.f * std::numeric_limits< float >::min() ) ) ||
			( ( std::fabs( nt ^ nn ) < 0.999f ) && ( std::fabs( nb ^ nn ) < 0.999f ) ) ) )
	{
		Imath::V3f y = bxt.normalized();

		// NOTE : ensure y axis of local frame points in same direction as normal

		if( n.dot( y ) < 0.f )
		{
			y *= -1.f;
		}

		constructMatrix( m, p, nt, y, ( nt % y ).normalized() );
	}
	else if( txn.length2() > ( 2.f * std::numeric_limits< float >::min() ) )
	{
		const Imath::V3f z = txn.normalized();
		constructMatrix( m, p, nt, ( z % nt ).normalized(), z );
	}
	else if( nxb.length2() > ( 2.f * std::numeric_limits< float >::min() ) )
	{
		const Imath::V3f x = nxb.normalized();
		constructMatrix( m, p, x, ( nb % x ).normalized(), nb );
	}
	else
	{
		m.translate( p );
	}
}

struct UVIndexer
{
	UVIndexer( const IECoreScene::MeshPrimitive& primitive, const std::string& uvSet, const bool throwOnError )
	: m_indices( nullptr )
	, m_view()
	{
		const IECoreScene::PrimitiveVariableMap::const_iterator it = primitive.variables.find( uvSet );

		if(
			( it == primitive.variables.end() ) ||
			( ( *it ).second.data->typeId() != IECore::V2fVectorDataTypeId ) )
		{
			if( throwOnError )
			{
				throw IECore::InvalidArgumentException( (
					boost::format(
						"Gaffer::Constraint : MeshPrimitive has no V2fVectorData primitive variable named \"%s\"."
					) % ( uvSet ) ).str() );
			}
			else
			{
				return;
			}
		}

		// NOTE : for vertex and varying interpolation we need to redirect through the primitive indices

		if(
			( ( *it ).second.interpolation == IECoreScene::PrimitiveVariable::Vertex ) ||
			( ( *it ).second.interpolation == IECoreScene::PrimitiveVariable::Varying ) )
		{
			m_indices = &( primitive.vertexIds()->readable() );
		}
		else if( ( *it ).second.interpolation != IECoreScene::PrimitiveVariable::FaceVarying )
		{
			if( throwOnError )
			{
				throw IECore::InvalidArgumentException( (
					boost::format(
						"Gaffer::Constraint : Primitive variable named \"%s\" has incorrect interpolation, must be either Vertex, Varying or FaceVarying"
					) % ( uvSet ) ).str() );
			}
			else
			{
				return;
			}
		}

		m_view = IECoreScene::PrimitiveVariable::IndexedView< Imath::V2f >( ( *it ).second );
	}

	bool valid() const
	{
		return static_cast< bool >( m_view );
	}

	const Imath::V2f& operator[]( const int i ) const
	{
		assert( valid() );
		const int index = ( m_indices != nullptr ) ? ( ( *m_indices )[ i ] ) : i;
		return ( *m_view )[ index ];
	}

private:

	const std::vector< int >* m_indices;
	std::optional<IECoreScene::PrimitiveVariable::IndexedView<Imath::V2f>> m_view;
};

void computePrimitiveVertexLocalFrame( const IECoreScene::Primitive& primitive, Imath::M44f& m, const int vertexId, const bool throwOnError )
{
	const IECore::V3fVectorData* const pdata = primitive.variableData< IECore::V3fVectorData >( "P" );
	if( pdata == nullptr )
	{
		if( throwOnError )
		{
			throw IECore::InvalidArgumentException( "Gaffer::Contraint : Primitive has no Vertex \"P\" primitive variable." );
		}
		else
		{
			return;
		}
	}
	const IECore::V3fVectorData::ValueType& points = pdata->readable();

	if( ( vertexId < 0 ) || ( vertexId >= static_cast< int >( points.size() ) ) )
	{
		if( throwOnError )
		{
			throw IECore::InvalidArgumentException( (
				boost::format(
					"Gaffer::Constraint : Vertex id \"%d\" is out of range."
				) % ( vertexId ) ).str() );
		}
		else
		{
			return;
		}
	}

	m.translate( points[ vertexId ] );
}

void computeMeshVertexLocalFrame( const IECoreScene::MeshPrimitive& primitive, Imath::M44f& m, const int vertexId, const std::string& uvSet, const bool throwOnError, const IECore::Canceller* const canceller )
{
	const IECore::V3fVectorData* const pdata = primitive.variableData< IECore::V3fVectorData >( "P" );

	if( pdata == nullptr )
	{
		if( throwOnError )
		{
			throw IECore::InvalidArgumentException( "Gaffer::Contraint : MeshPrimitive has no Vertex \"P\" primitive variable." );
		}
		else
		{
			return;
		}
	}

	const IECore::V3fVectorData::ValueType& points = pdata->readable();

	if( ( vertexId < 0 ) || ( vertexId >= static_cast< int >( points.size() ) ) )
	{
		if( throwOnError )
		{
			throw IECore::InvalidArgumentException( (
				boost::format(
					"Gaffer::Constraint : Vertex id \"%d\" is out of range."
				) % ( vertexId ) ).str() );
		}
		else
		{
			return;
		}
	}

	const IECore::IntVectorData::ValueType& indices = primitive.vertexIds()->readable();
	const IECore::IntVectorData::ValueType& faces = primitive.verticesPerFace()->readable();

	const UVIndexer uvs( primitive, uvSet, throwOnError );

	Imath::V3f t( 0.f );
	Imath::V3f b( 0.f );
	Imath::V3f n( 0.f );

	if( uvs.valid() )
	{
		int js = 0;
		for( int i = 0; i < static_cast< int >( faces.size() ); ++i )
		{
			const int ni = faces[ i ];

			// canceller support

			if( ( i % 100 ) == 0 )
			{
				IECore::Canceller::check( canceller );
			}

			// find matching face vertex index

			int jm = 0;
			for( ; jm < ni; ++jm )
			{
				if( indices[ js + jm ] == vertexId )
				{
					break;
				}
			}

			if( jm < ni )
			{
				// compute face normal, tangent and bitangent

				Imath::V3f ft( 0.f );
				Imath::V3f fb( 0.f );
				Imath::V3f fn( 0.f );

				float w = 0.f;

				for( int j = 0; j < ni; ++j )
				{
					const int iv = js + j;
					const int ip = js + ( ( j + ni - 1 ) % ni );
					const int in = js + ( ( j      + 1 ) % ni );

					const Imath::V3f pv = points[ indices[ iv ] ];
					const Imath::V3f v0 = points[ indices[ ip ] ] - pv;
					const Imath::V3f v2 = points[ indices[ in ] ] - pv;

					const Imath::V2f uv = uvs[ iv ];
					const Imath::V2f e0 = uvs[ ip ] - uv;
					const Imath::V2f e2 = uvs[ in ] - uv;

					ft += ( v0 * -e2.y + v2 * e0.y ).normalized();
					fb += ( v0 * -e2.x + v2 * e0.x ).normalized();
					fn += ( -v0 % v2 ).normalized();

					if( j == jm )
					{
						const float lv2 = v0.length2() * v2.length2();

						if( lv2 > ( 2.f * std::numeric_limits< float >::min() ) )
						{
							const float lv = std::sqrt( lv2 );

							if( lv != 0.f )
							{
								w = std::acos( std::min( std::max( ( v0 ^ v2 ) / lv, -1.f ), 1.f ) );
							}
						}
					}
				}

				// accumulate angle weighted normal, tangent and bitangent

				t += ft.normalized() * w;
				b += fb.normalized() * w;
				n += fn.normalized() * w;
			}

			js += ni;
		}
	}

	constructLocalFrame( m, points[ vertexId ], t, b, n );
}

void computeVertexLocalFrame( const IECore::Object& object, Imath::M44f& m, const int vertexId, const std::string& uvSet, const bool throwOnError, const IECore::Canceller* const canceller )
{
	switch( static_cast< IECoreScene::TypeId >( object.typeId() ) )
	{
		case IECoreScene::CurvesPrimitiveTypeId:
		case IECoreScene::PointsPrimitiveTypeId:
			computePrimitiveVertexLocalFrame( static_cast< const IECoreScene::Primitive& >( object ), m, vertexId, throwOnError );
			break;
		case IECoreScene::MeshPrimitiveTypeId:
			computeMeshVertexLocalFrame( static_cast< const IECoreScene::MeshPrimitive& >( object ), m, vertexId, uvSet, throwOnError, canceller );
			break;
		default:
			if( throwOnError )
			{
				throw IECore::InvalidArgumentException( (
					boost::format(
						"Gaffer::Constraint : Target primitive of type \"%s\" is not supported in Vertex target mode."
					) % ( object.typeName() ) ).str() );
			}
			break;
	}
}

void computeMeshUVLocalFrame( const IECoreScene::MeshPrimitive& primitive, Imath::M44f& m, const Imath::V2f& uv, const std::string& uvSet, const bool throwOnError, const IECore::Canceller* const canceller )
{
	const IECore::V3fVectorData* const pdata = primitive.variableData< IECore::V3fVectorData >( "P" );

	if( pdata == nullptr )
	{
		if( throwOnError )
		{
			throw IECore::InvalidArgumentException( "Gaffer::Contraint : MeshPrimitive has no Vertex \"P\" primitive variable." );
		}
		else
		{
			return;
		}
	}

	const IECore::V3fVectorData::ValueType& points = pdata->readable();
	const IECore::IntVectorData::ValueType& indices = primitive.vertexIds()->readable();
	const IECore::IntVectorData::ValueType& faces = primitive.verticesPerFace()->readable();

	const UVIndexer uvs( primitive, uvSet, throwOnError );
	if( ! uvs.valid() )
	{
		return;
	}

	int js = 0;
	for( int i = 0; i < static_cast< int >( faces.size() ); ++i )
	{
		const int ni = faces[ i ];

		// canceller support

		if( ( i % 100 ) == 0 )
		{
			IECore::Canceller::check( canceller );
		}

		// points and uv indexers

		const auto pIndexer = [ & points, & indices, js ]( const int ii )
		{
			return points[ indices[ js + ii ] ];
		};

		const auto uvIndexer = [ & uvs, js ]( const int ii )
		{
			return uvs[ js + ii ];
		};

		// determine if uv coordinate is inside face

		if( ptInPolygon( uvIndexer, uv, ni ) )
		{
			Imath::V3f fp( 0.f );
			Imath::V3f ft( 0.f );
			Imath::V3f fb( 0.f );
			Imath::V3f fn( 0.f );

			try
			{
				fp = ( convexPolygon( uvIndexer, ni ) )
					? interpolateConvexPolygon( pIndexer, uvIndexer, uv, ni )
					: interpolateNonConvexPolygon( pIndexer, uvIndexer, uv, ni );

				// compute face tangent and bitangent

				for( int j = 0; j < ni; ++j )
				{
					const int iv = js + j;
					const int ip = js + ( ( j + ni - 1 ) % ni );
					const int in = js + ( ( j      + 1 ) % ni );

					const Imath::V3f pv = points[ indices[ iv ] ];
					const Imath::V3f v0 = points[ indices[ ip ] ] - pv;
					const Imath::V3f v2 = points[ indices[ in ] ] - pv;

					const Imath::V2f uv = uvs[ iv ];
					const Imath::V2f e0 = uvs[ ip ] - uv;
					const Imath::V2f e2 = uvs[ in ] - uv;

					ft += ( v0 * -e2.y + v2 * e0.y ).normalized();
					fb += ( v0 * -e2.x + v2 * e0.x ).normalized();
					fn += ( -v0 % v2 ).normalized();
				}
			}
			catch( const IECore::Exception& e )
			{
				if( throwOnError )
				{
					throw;
				}
			}

			constructLocalFrame( m, fp, ft, fb, fn );

			return;
		}

		js += ni;
	}

	if( throwOnError )
	{
		throw IECore::InvalidArgumentException( (
			boost::format(
				"Gaffer::Constraint : UV coordinates \"%s\" are out of range."
			) % ( uv ) ).str() );
	}
}

void computeUVLocalFrame( const IECore::Object& object, Imath::M44f& m, const Imath::V2f& uv, const std::string& uvSet, const bool throwOnError, const IECore::Canceller* const canceller )
{
	switch( static_cast< IECoreScene::TypeId >( object.typeId() ) )
	{
		case IECoreScene::MeshPrimitiveTypeId:
			computeMeshUVLocalFrame( static_cast< const IECoreScene::MeshPrimitive& >( object ), m, uv, uvSet, throwOnError, canceller );
			break;
		default:
			if( throwOnError )
			{
				throw IECore::InvalidArgumentException( (
					boost::format(
						"Gaffer::Constraint : Target primitive of type \"%s\" is not supported in UV target mode."
					) % ( object.typeName() ) ).str() );
			}
			break;
	}
}

} // namespace

GAFFER_NODE_DEFINE_TYPE( Constraint );

size_t Constraint::g_firstPlugIndex = 0;

Constraint::Constraint( const std::string &name )
	:	SceneElementProcessor( name, IECore::PathMatcher::NoMatch )
{
	storeIndexOfNextChild( g_firstPlugIndex );
	addChild( new ScenePlug( "targetScene" ) );
	addChild( new StringPlug( "target" ) );
	addChild( new BoolPlug( "ignoreMissingTarget" ) );
	addChild( new IntPlug( "targetMode", Plug::In, Constraint::Origin, Constraint::Origin, Constraint::Vertex ) );
	addChild( new V2fPlug( "targetUV" ) );
	addChild( new IntPlug( "targetVertex", Plug::In, 0, 0 ) );
	addChild( new V3fPlug( "targetOffset" ) );

	// Pass through things we don't want to modify
	outPlug()->attributesPlug()->setInput( inPlug()->attributesPlug() );
	outPlug()->objectPlug()->setInput( inPlug()->objectPlug() );
}

Constraint::~Constraint()
{
}

ScenePlug *Constraint::targetScenePlug()
{
	return getChild<ScenePlug>( g_firstPlugIndex );
}

const ScenePlug *Constraint::targetScenePlug() const
{
	return getChild<ScenePlug>( g_firstPlugIndex );
}

Gaffer::StringPlug *Constraint::targetPlug()
{
	return getChild<Gaffer::StringPlug>( g_firstPlugIndex + 1 );
}

const Gaffer::StringPlug *Constraint::targetPlug() const
{
	return getChild<Gaffer::StringPlug>( g_firstPlugIndex + 1 );
}

Gaffer::BoolPlug *Constraint::ignoreMissingTargetPlug()
{
	return getChild<Gaffer::BoolPlug>( g_firstPlugIndex + 2 );
}

const Gaffer::BoolPlug *Constraint::ignoreMissingTargetPlug() const
{
	return getChild<Gaffer::BoolPlug>( g_firstPlugIndex + 2 );
}

Gaffer::IntPlug *Constraint::targetModePlug()
{
	return getChild<Gaffer::IntPlug>( g_firstPlugIndex + 3 );
}

const Gaffer::IntPlug *Constraint::targetModePlug() const
{
	return getChild<Gaffer::IntPlug>( g_firstPlugIndex + 3 );
}

Gaffer::V2fPlug *Constraint::targetUVPlug()
{
	return getChild<Gaffer::V2fPlug>( g_firstPlugIndex + 4 );
}

const Gaffer::V2fPlug *Constraint::targetUVPlug() const
{
	return getChild<Gaffer::V2fPlug>( g_firstPlugIndex + 4 );
}

Gaffer::IntPlug *Constraint::targetVertexPlug()
{
	return getChild<Gaffer::IntPlug>( g_firstPlugIndex + 5 );
}

const Gaffer::IntPlug *Constraint::targetVertexPlug() const
{
	return getChild<Gaffer::IntPlug>( g_firstPlugIndex + 5 );
}

Gaffer::V3fPlug *Constraint::targetOffsetPlug()
{
	return getChild<Gaffer::V3fPlug>( g_firstPlugIndex + 6 );
}

const Gaffer::V3fPlug *Constraint::targetOffsetPlug() const
{
	return getChild<Gaffer::V3fPlug>( g_firstPlugIndex + 6 );
}

void Constraint::affects( const Gaffer::Plug *input, AffectedPlugsContainer &outputs ) const
{
	SceneElementProcessor::affects( input, outputs );

	if(
		input == targetPlug() ||
		input == ignoreMissingTargetPlug() ||
		input == inPlug()->existsPlug() ||
		input == inPlug()->transformPlug() ||
		input == inPlug()->boundPlug() ||
		input == targetScenePlug()->existsPlug() ||
		input == targetScenePlug()->transformPlug() ||
		input == targetScenePlug()->boundPlug() ||
		input == targetScenePlug()->objectPlug() ||
		input == targetModePlug() ||
		input->parent<Plug>() == targetOffsetPlug() ||
		input->parent<Plug>() == targetUVPlug() ||
		input == targetVertexPlug() ||
		// TypeId comparison is necessary to avoid calling pure virtual
		// if we're called before being fully constructed.
		( typeId() != staticTypeId() && affectsConstraint( input ) )
	)
	{
		outputs.push_back( outPlug()->transformPlug() );
		outputs.push_back( outPlug()->boundPlug() );
	}
}

bool Constraint::processesTransform() const
{
	return true;
}

void Constraint::hashProcessedTransform( const ScenePath &path, const Gaffer::Context *context, IECore::MurmurHash &h ) const
{
	auto targetOpt = target();
	if( !targetOpt )
	{
		// Pass through input unchanged
		h = inPlug()->transformPlug()->hash();
		return;
	}

	ScenePath parentPath = path;
	parentPath.pop_back();
	h.append( inPlug()->fullTransformHash( parentPath ) );

	h.append( targetOpt->scene->fullTransformHash( targetOpt->path ) );

	const TargetMode targetMode = (TargetMode)targetModePlug()->getValue();
	h.append( targetMode );
	switch( targetMode )
	{
		case Constraint::BoundMin:
		case Constraint::BoundMax:
		case Constraint::BoundCenter:
			h.append( targetOpt->scene->boundHash( targetOpt->path ) );
			break;
		case Constraint::UV:
			h.append( targetOpt->scene->objectHash( targetOpt->path ) );
			ignoreMissingTargetPlug()->hash( h );
			targetUVPlug()->hash( h );
			break;
		case Constraint::Vertex:
			h.append( targetOpt->scene->objectHash( targetOpt->path ) );
			ignoreMissingTargetPlug()->hash( h );
			targetVertexPlug()->hash( h );
			break;
		default:
			break;
	}

	targetOffsetPlug()->hash( h );

	hashConstraint( context, h );
}

Imath::M44f Constraint::computeProcessedTransform( const ScenePath &path, const Gaffer::Context *context, const Imath::M44f &inputTransform ) const
{
	auto targetOpt = target();
	if( !targetOpt )
	{
		return inputTransform;
	}

	ScenePath parentPath = path;
	parentPath.pop_back();

	const M44f parentTransform = inPlug()->fullTransform( parentPath );
	const M44f fullInputTransform = inputTransform * parentTransform;

	M44f fullTargetTransform = targetOpt->scene->fullTransform( targetOpt->path );

	const TargetMode targetMode = (TargetMode)targetModePlug()->getValue();

	switch( targetMode )
	{
		case Constraint::BoundMin:
		{
			const Box3f targetBound = targetOpt->scene->bound( targetOpt->path );
			if( ! targetBound.isEmpty() )
			{
				fullTargetTransform.translate( targetBound.min );
			}
			break;
		}
		case Constraint::BoundMax:
		{
			const Box3f targetBound = targetOpt->scene->bound( targetOpt->path );
			if( ! targetBound.isEmpty() )
			{
				fullTargetTransform.translate( targetBound.max );
			}
			break;
		}
		case Constraint::BoundCenter:
		{
			const Box3f targetBound = targetOpt->scene->bound( targetOpt->path );
			if( ! targetBound.isEmpty() )
			{
				fullTargetTransform.translate( targetBound.center() );
			}
			break;
		}
		case Constraint::UV:
		{
			const IECore::ConstObjectPtr object = targetOpt->scene->object( targetOpt->path );
			Imath::M44f surfaceTransform;
			const Imath::V2f uv = targetUVPlug()->getValue();
			const bool throwOnError = !( ignoreMissingTargetPlug()->getValue() );
			computeUVLocalFrame( *object, surfaceTransform, uv, "uv", throwOnError, context->canceller() );
			fullTargetTransform = surfaceTransform * fullTargetTransform;
			break;
		}
		case Constraint::Vertex:
		{
			const IECore::ConstObjectPtr object = targetOpt->scene->object( targetOpt->path );
			Imath::M44f surfaceTransform;
			const int vertexId = targetVertexPlug()->getValue();
			const bool throwOnError = !( ignoreMissingTargetPlug()->getValue() );
			computeVertexLocalFrame( *object, surfaceTransform, vertexId, "uv", throwOnError, context->canceller() );
			fullTargetTransform = surfaceTransform * fullTargetTransform;
			break;
		}
		default:
			break;
	}

	fullTargetTransform.translate( targetOffsetPlug()->getValue() );

	const M44f fullConstrainedTransform = computeConstraint( fullTargetTransform, fullInputTransform, inputTransform );
	return fullConstrainedTransform * parentTransform.inverse();
}

std::optional<Constraint::Target> Constraint::target() const
{
	std::string targetPathAsString = targetPlug()->getValue();
	if( targetPathAsString == "" )
	{
		return std::nullopt;
	}

	ScenePath targetPath;
	ScenePlug::stringToPath( targetPathAsString, targetPath );

	const ScenePlug *targetScene = targetScenePlug();
	if( !targetScene->getInput() )
	{
		// Backwards compatibility for time when there was
		// no `targetScene` plug.
		targetScene = inPlug();
	}

	if( !targetScene->exists( targetPath ) )
	{
		if( ignoreMissingTargetPlug()->getValue() )
		{
			return std::nullopt;
		}
		else
		{
			throw IECore::Exception( boost::str(
				boost::format( "Constraint target does not exist: \"%s\".  Use 'ignoreMissingTarget' option if you want to just skip this constraint" ) % targetPathAsString ) );
		}
	}

	return Target( { targetPath, targetScene } );
}


/*****************************************************************************
*
* Copyright (c) 2003-2018 by The University of Queensland
* http://www.uq.edu.au
*
* Primary Business: Queensland, Australia
* Licensed under the Apache License, version 2.0
* http://www.apache.org/licenses/LICENSE-2.0
*
* Development until 2012 by Earth Systems Science Computational Center (ESSCC)
* Development 2012-2013 by School of Earth Sciences
* Development from 2014 by Centre for Geoscience Computing (GeoComp)
*
*****************************************************************************/

/****************************************************************************/

/* Paso: SystemMatrix: controls iterative linear system solvers  */

/****************************************************************************/

/* Copyrights by ACcESS Australia 2003/04                     */
/* Author: Lutz Gross, l.gross@uq.edu.au                      */

/****************************************************************************/

#include "Solver.h"
#include "Options.h"
#include "SystemMatrix.h"

#include <boost/math/special_functions/fpclassify.hpp>  // for isnan

#include <iostream>

namespace bm = boost::math;

namespace paso {

void Solver_free(SystemMatrix<double>* A)
{
    A->freePreconditioner();
}

///  calls the iterative solver
SolverResult Solver(SystemMatrix_ptr<double> A, double* x, double* b, Options* options,
                    Performance* pp)
{
    const real_t EPSILON = escript::DataTypes::real_t_eps();
    double norm2_of_b,tol,tolerance,time_iter,net_time_start;
    double *r=NULL,norm2_of_residual,last_norm2_of_residual,norm_max_of_b;
    double norm2_of_b_local,norm_max_of_b_local,norm2_of_residual_local;
    double norm_max_of_residual_local,norm_max_of_residual;
    double last_norm_max_of_residual;
#ifdef ESYS_MPI
    double loc_norm;
#endif
    dim_t i,totIter=0,cntIter,method;
    bool finalizeIteration;
    SolverResult errorCode = NoError;
    const dim_t numSol = A->getTotalNumCols();
    const dim_t numEqua = A->getTotalNumRows();
    double *x0=NULL;

    tolerance=options->tolerance;
    if (tolerance < 100.* EPSILON) {
        throw PasoException("Solver: Tolerance is too small.");
    }
    if (tolerance >1.) {
        throw PasoException("Solver: Tolerance must be less than one.");
    }
    method=Options::getSolver(options->method, PASO_PASO, options->symmetric, A->mpi_info);
    /* check matrix type */
    if ((A->type & MATRIX_FORMAT_CSC) || (A->type & MATRIX_FORMAT_OFFSET1) ) {
        throw PasoException("Solver: Iterative solver requires CSR format with unsymmetric storage scheme and index offset 0.");
    }
    if (A->col_block_size != A->row_block_size) {
        throw PasoException("Solver: Iterative solver requires row and column block sizes to be equal.");
    }
    if (A->getGlobalNumCols() != A->getGlobalNumRows()) {
        throw PasoException("Solver: Iterative solver requires a square matrix.");
    }
    time_iter=escript::gettime();
    /* this for testing only */
    if (method==PASO_NONLINEAR_GMRES) {
        LinearSystem* F = new LinearSystem(A, b, options);
        A->solvePreconditioner(x, b);
        errorCode = Solver_NewtonGMRES(F, x, options, pp);
        if (errorCode != NoError) {
            throw PasoException("Solver_NewtonGMRES: an error has occurred.");
        }
        delete F;
        return errorCode;
    }

    r = new double[numEqua];
    x0 = new double[numEqua];
    A->balance();
    options->num_level=0;
    options->num_inner_iter=0;

    /* ========================= */
    Performance_startMonitor(pp, PERFORMANCE_ALL);
    A->applyBalance(r, b, true);
    /* get the norm of the right hand side */
    norm2_of_b=0.;
    norm_max_of_b=0.;
    #pragma omp parallel private(norm2_of_b_local,norm_max_of_b_local)
    {
        norm2_of_b_local=0.;
        norm_max_of_b_local=0.;
        #pragma omp for private(i) schedule(static)
        for (i = 0; i < numEqua ; ++i) {
            norm2_of_b_local += r[i] * r[i];
            norm_max_of_b_local = std::max(std::abs(r[i]),norm_max_of_b_local);
        }
        #pragma omp critical
        {
            norm2_of_b += norm2_of_b_local;
            norm_max_of_b = std::max(norm_max_of_b_local,norm_max_of_b);
        }
    }
#ifdef ESYS_MPI
    /* TODO: use one call */
    loc_norm = norm2_of_b;
    MPI_Allreduce(&loc_norm,&norm2_of_b, 1, MPI_DOUBLE, MPI_SUM, A->mpi_info->comm);
    loc_norm = norm_max_of_b;
    MPI_Allreduce(&loc_norm,&norm_max_of_b, 1, MPI_DOUBLE, MPI_MAX, A->mpi_info->comm);
#endif
    norm2_of_b=sqrt(norm2_of_b);
    /* if norm2_of_b==0 we are ready: x=0 */
    if (bm::isnan(norm2_of_b) || bm::isnan(norm_max_of_b)) {
        throw PasoException("Solver: Matrix or right hand side contains undefined values.");
    } else if (norm2_of_b <= 0.) {
#pragma omp parallel for private(i) schedule(static)
        for (i = 0; i < numSol; i++) x[i]=0.;
        if (options->verbose)
            std::cout << "right hand side is identical to zero." << std::endl;
    } else {
        if (options->verbose) {
            std::cout << "Solver: l2/lmax-norm of right hand side is "
                << norm2_of_b << "/" << norm_max_of_b << "." << std::endl
                << "Solver: l2/lmax-stopping criterion is "
                << norm2_of_b*tolerance << "/" << norm_max_of_b*tolerance
                << "." << std::endl;
            switch (method) {
                case PASO_BICGSTAB:
                    std::cout << "Solver: Iterative method is BiCGStab.\n";
                break;
                case PASO_PCG:
                    std::cout << "Solver: Iterative method is PCG.\n";
                break;
                case PASO_TFQMR:
                    std::cout << "Solver: Iterative method is TFQMR.\n";
                break;
                case PASO_MINRES:
                    std::cout << "Solver: Iterative method is MINRES.\n";
                break;
                case PASO_PRES20:
                    std::cout << "Solver: Iterative method is PRES20.\n";
                break;
                case PASO_GMRES:
                    if (options->restart > 0) {
                        std::cout << "Solver: Iterative method is GMRES("
                            << options->truncation << ","
                            << options->restart << ")." << std::endl;
                    } else {
                        std::cout << "Solver: Iterative method is GMRES("
                            << options->truncation << ")." << std::endl;
                    }
                break;
            }
        }

        // construct the preconditioner
        Performance_startMonitor(pp, PERFORMANCE_PRECONDITIONER_INIT);
        A->setPreconditioner(options);
        Performance_stopMonitor(pp, PERFORMANCE_PRECONDITIONER_INIT);
        options->set_up_time=escript::gettime()-time_iter;
        // get an initial guess by evaluating the preconditioner
        A->solvePreconditioner(x, r);

        totIter = 1;
        finalizeIteration = false;
        last_norm2_of_residual=norm2_of_b;
        last_norm_max_of_residual=norm_max_of_b;
        net_time_start=escript::gettime();

        // main loop
        while (!finalizeIteration) {
            cntIter = options->iter_max - totIter;
            finalizeIteration = true;

            // Set initial residual
            if (totIter > 1) {
                // in the first iteration r = balance * b already
                A->applyBalance(r, b, true);
            }

            A->MatrixVector_CSR_OFFSET0(-1., x, 1., r);
            norm2_of_residual = 0;
            norm_max_of_residual = 0;
            #pragma omp parallel private(norm2_of_residual_local,norm_max_of_residual_local)
            {
                norm2_of_residual_local = 0;
                norm_max_of_residual_local = 0;
                #pragma omp for private(i) schedule(static)
                for (i = 0; i < numEqua; i++) {
                    norm2_of_residual_local+= r[i] * r[i];
                    norm_max_of_residual_local=std::max(std::abs(r[i]),norm_max_of_residual_local);
                }
                #pragma omp critical
                {
                    norm2_of_residual += norm2_of_residual_local;
                    norm_max_of_residual = std::max(norm_max_of_residual_local,norm_max_of_residual);
                }
            }
#ifdef ESYS_MPI
            // TODO: use one call
            loc_norm = norm2_of_residual;
            MPI_Allreduce(&loc_norm,&norm2_of_residual, 1, MPI_DOUBLE, MPI_SUM, A->mpi_info->comm);
            loc_norm = norm_max_of_residual;
            MPI_Allreduce(&loc_norm,&norm_max_of_residual, 1, MPI_DOUBLE, MPI_MAX, A->mpi_info->comm);
#endif
            norm2_of_residual =sqrt(norm2_of_residual);
            options->residual_norm=norm2_of_residual;

            if (options->verbose)
                std::cout << "Solver: Step " << totIter
                    << ": l2/lmax-norm of residual is "
                    << norm2_of_residual << "/" << norm_max_of_residual;

            if (totIter > 1 &&
                    norm2_of_residual >= last_norm2_of_residual &&
                    norm_max_of_residual >= last_norm_max_of_residual) {

                if (options->verbose) std::cout << " divergence!\n";
                throw PasoException("Solver: No improvement during iteration. Iterative solver gives up.");

            } else {
                if (norm2_of_residual>tolerance*norm2_of_b ||
                        norm_max_of_residual>tolerance*norm_max_of_b ) {

                    tol=tolerance*std::min(norm2_of_b,0.1*norm2_of_residual/norm_max_of_residual*norm_max_of_b);
                    if (options->verbose)
                        std::cout << " (new tolerance = " << tol << ").\n";

                    last_norm2_of_residual=norm2_of_residual;
                    last_norm_max_of_residual=norm_max_of_residual;

                    // call the solver
                    switch (method) {
                        case PASO_BICGSTAB:
                            errorCode = Solver_BiCGStab(A, r, x, &cntIter, &tol, pp);
                        break;
                        case PASO_PCG:
                            errorCode = Solver_PCG(A, r, x, &cntIter, &tol, pp);
                        break;
                        case PASO_TFQMR:
                            tol=tolerance*norm2_of_residual/norm2_of_b;
                            errorCode = Solver_TFQMR(A, r, x0, &cntIter, &tol, pp);
                            #pragma omp for private(i) schedule(static)
                            for (i = 0; i < numEqua; i++) {
                                x[i]+= x0[i];
                            }
                        break;
                        case PASO_MINRES:
                            //tol=tolerance*norm2_of_residual/norm2_of_b;
                            errorCode = Solver_MINRES(A, r, x, &cntIter, &tol, pp);
                        break;
                        case PASO_PRES20:
                            errorCode = Solver_GMRES(A, r, x, &cntIter, &tol, 5, 20, pp);
                        break;
                        case PASO_GMRES:
                            errorCode = Solver_GMRES(A, r, x, &cntIter, &tol, options->truncation, options->restart, pp);
                        break;
                    }

                    totIter += cntIter;

                    // error handling
                    if (errorCode == NoError) {
                        finalizeIteration = false;
                    } else if (errorCode == MaxIterReached) {
                        if (options->verbose)
                            std::cout << "Solver: Maximum number of "
                                "iterations reached." << std::endl;
                        break;
                    } else if (errorCode == InputError) {
                        if (options->verbose)
                            std::cout << "Solver: Internal error!\n";
                        break;
                    } else if (errorCode == NegativeNormError) {
                        if (options->verbose)
                            std::cout << "Solver: negative energy norm"
                               " (try other solver or preconditioner)!\n";
                        break;
                    } else if (errorCode == Breakdown) {
                        if (cntIter <= 1) {
                            if (options->verbose)
                                std::cout << "Solver: Uncurable break "
                                    "down!" << std::endl;
                            break;
                        } else {
                            if (options->verbose)
                                std::cout << "Solver: Breakdown at iter "
                                    << totIter << " (residual = "
                                    << tol << "). Restarting ...\n";
                            finalizeIteration = false;
                            errorCode = NoError;
                        }
                    } else {
                        if (options->verbose)
                            std::cout << "Solver: Generic error in solver!\n";
                        break;
                    }
                } else {
                    if (options->verbose)
                        std::cout << " convergence!" << std::endl;
                    options->converged = true;
                }
            }
        } // while
        options->net_time = escript::gettime()-net_time_start;
        options->num_iter = totIter;
        A->applyBalanceInPlace(x, false);
    }
    delete[] r;
    delete[] x0;
    options->time = escript::gettime()-time_iter;
    Performance_stopMonitor(pp, PERFORMANCE_ALL);
    return errorCode;
}

SolverResult Solver(SystemMatrix_ptr<cplx_t> A, cplx_t* x, cplx_t* b, Options* options,
                    Performance* pp)
{
    throw PasoException("Solver(): complex not implemented.");
}

void Solver_free(SystemMatrix<cplx_t>* A)
{
    throw PasoException("Solver_free(): complex not implemented.");
}

} // namespace paso


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory for Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */
#include "DavidsonDiagonalizer.h"
#include "IndirectPreconditionerEvaluator.h"
#include "IndirectSigmaVectorEvaluator.h"
#include <Utils/IO/Logger.h>
#include <Eigen/Eigenvalues>

namespace Scine {
namespace Utils {

template<class MatrixType, DavidsonBalancedType s>
DavidsonDiagonalizer<MatrixType, s>::DavidsonDiagonalizer(int eigenvaluesToCompute, int numberGuessVectors, int totalDimension)
  : eigenvaluesToCompute_(eigenvaluesToCompute), blockSize_(numberGuessVectors), subspaceDimension_(numberGuessVectors) {
  assert(eigenvaluesToCompute > 0 &&
         "Unintended behaviour: calculate negative amount of eigenvalues in Davidson diagonalizer.");
  if (eigenvaluesToCompute_ > blockSize_)
    throw InvalidDavidsonInputException();
  initialize(totalDimension);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::initialize(int dimension) {
  if (eigenvaluesToCompute_ > dimension) {
    eigenvaluesToCompute_ = dimension;
    Utils::Log::warning() << "Too many eigenvalues requested (" << eigenvaluesToCompute_ << "), they were set to "
                          << dimension << ".";
  }
  if (subspaceDimension_ > dimension) {
    subspaceDimension_ = eigenvaluesToCompute_;
    blockSize_ = subspaceDimension_;
    Utils::Log::warning() << "Initial subspace dimension too big (" << subspaceDimension_ << "), it was reduced to "
                          << eigenvaluesToCompute_ << ".";
  }
  assert(dimension >= blockSize_ && "Matrix dimension cannot be smaller than number of guess vectors!");
  guessVectors_ = Eigen::MatrixXd::Zero(dimension, subspaceDimension_);
  if (s == DavidsonBalancedType::standard) {
    guessVectors_.block(0, 0, subspaceDimension_, subspaceDimension_) =
        Eigen::MatrixXd::Identity(subspaceDimension_, subspaceDimension_);
  }
  else {
    srand(seed_);
    guessVectors_.block(0, 0, subspaceDimension_, subspaceDimension_) =
        Eigen::MatrixXd::Random(subspaceDimension_, subspaceDimension_);
  }
  maxDimension_ = dimension;
  maxIterations_ = dimension;
  sigmaVectorEvaluator_.reset();
  preconditionerEvaluator_.reset();
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setMaxIterations(int maxIterations) {
  maxIterations_ = maxIterations;
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setMatrixToDiagonalize(const MatrixType& matrix) {
  if (type_ == DavidsonDirectType::direct) { // if a direct method is in use, no matrix is needed.
    return;
  }
  matrixToDiagonalize_ = matrix;
  originalDiagonal_ = matrixToDiagonalize_.diagonal();
  initialize(matrixToDiagonalize_.cols());
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setGuess(const Eigen::MatrixXd& guessVectors) {
  guessVectors_ = guessVectors.leftCols(subspaceDimension_);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setDavidsonType(DavidsonDirectType type) {
  type_ = type;
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setSeed(unsigned seed) {
  seed_ = seed;
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setSigmaVectorEvaluator(std::unique_ptr<SigmaVectorEvaluator<MatrixType>>&& evaluator) {
  sigmaVectorEvaluator_ = std::move(evaluator);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::setPreconditionerEvaluator(std::unique_ptr<PreconditionerEvaluator>&& evaluator) {
  preconditionerEvaluator_ = std::move(evaluator);
}

template<class MatrixType, DavidsonBalancedType s>
EigenContainer DavidsonDiagonalizer<MatrixType, s>::solve() {
  checkEvaluators();

  // Set initial size of guess matrix
  subspaceDimension_ = blockSize_;
  for (int i = 0; i < maxIterations_; ++i) {
    performIteration();
    Log::info() << "Iteration number: " << i << "\tSubspace dimension: " << subspaceDimension_ << ".";
    if (converged_) { // if full matrix still calculate everything
      Log::info() << "Converged in " << i << " iterations, dimensionality " << subspaceDimension_ << ".";
      return eigenPairs_;
    }
  }
  throw DavidsonNotConvergedException();
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::performIteration() {
  // Orthogonalize the projection matrix
  if (s == DavidsonBalancedType::standard) {
    orthogonalizeSubspace(guessVectors_);
    guessVectors_.leftCols(subspaceDimension_).colwise().normalize();
  }

  const Eigen::MatrixXd& projector = guessVectors_.leftCols(subspaceDimension_);
  if (s == DavidsonBalancedType::balanced) {
    for (int i = basisOverlap_.cols(); i < subspaceDimension_; ++i) {
      basisOverlap_.conservativeResize(subspaceDimension_, subspaceDimension_);
      basisOverlap_.col(i) = projector.transpose() * projector.col(i);
      Eigen::VectorXd symmetricRow = basisOverlap_.col(i);
      basisOverlap_.row(i) = symmetricRow;
    }
  }

  Eigen::MatrixXd sigmaMatrix = sigmaVectorEvaluator_->evaluateSigmaVector(projector);
  Eigen::MatrixXd projectedMatrix = projector.transpose() * sigmaMatrix;

  Eigen::MatrixXd subspaceEVectors;
  Eigen::VectorXd subspaceEValues;

  if (s == DavidsonBalancedType::standard) {
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> subspaceDiagonalizer(projectedMatrix);
    subspaceEVectors = subspaceDiagonalizer.eigenvectors();
    subspaceEValues = subspaceDiagonalizer.eigenvalues();
  }
  else if (s == DavidsonBalancedType::balanced) {
    Eigen::GeneralizedSelfAdjointEigenSolver<Eigen::MatrixXd> subspaceDiagonalizer(projectedMatrix, basisOverlap_);
    subspaceEVectors = subspaceDiagonalizer.eigenvectors();
    subspaceEValues = subspaceDiagonalizer.eigenvalues();
  }

  // Check convergence:
  // Get a matrix with the first *eigenvaluesToCompute_* residual vectors as column.
  // basically, it calculated r_k = H*Psi_{k, approx} - h_k*Psi_{k, approx}
  // = (sigmaMatrix - h_k * bMatrix) * v_k
  // H: matrix to diagonalize
  // Psi_{k, approx} k-th approximated Ritz eigenvectors for the subspace
  // h_k: k-th approximated Ritz eigenvalue
  // sigmaMatrix: matrix with as columns the sigma vectors
  // bMatrix: matrix with as column the tentative vectors
  // v_k: k-th subspace eigenvector
  Eigen::MatrixXd residualVectors(sigmaMatrix.rows(), eigenvaluesToCompute_);
  for (int col = 0; col < eigenvaluesToCompute_; ++col) {
    residualVectors.col(col) = (sigmaMatrix - subspaceEValues(col) * projector) * subspaceEVectors.col(col);
  }

  Eigen::VectorXd residualNorms = residualVectors.colwise().norm();
  converged_ = true;
  for (int i = 0; i < residualNorms.size(); ++i) {
    if (residualNorms(i) > eigenvalueTol)
      converged_ = false;
  }
  if (converged_) {
    eigenPairs_ = {subspaceEValues.head(eigenvaluesToCompute_), projector * subspaceEVectors.leftCols(eigenvaluesToCompute_)};
    return;
  }
  expandSubspace(subspaceEValues, residualVectors, projector, residualNorms);
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::expandSubspace(const Eigen::VectorXd& subspaceEValues,
                                                         const Eigen::MatrixXd& residualVectors, const Eigen::MatrixXd& projector,
                                                         const Eigen::VectorXd& residualNorms) {
  // look at each residual vector
  for (int i = 0; i < eigenvaluesToCompute_; ++i) {
    if (residualNorms(i) > eigenvalueTol) {
      Eigen::VectorXd correctionVector =
          residualVectors.col(i).cwiseProduct(-preconditionerEvaluator_->evaluate(subspaceEValues(i)));
      if (s == DavidsonBalancedType::standard) {
        Eigen::VectorXd orthogonalizedCorrectionVector(guessVectors_.rows());
        orthogonalizedCorrectionVector = orthogonalizeToSubspace(correctionVector.normalized(), projector);

        if (orthogonalizedCorrectionVector.norm() / correctionVector.norm() > /*correctionTol*/ 1e-3) {
          if (subspaceDimension_ < maxDimension_) {
            ++subspaceDimension_;
            guessVectors_.conservativeResize(Eigen::NoChange, subspaceDimension_);
            guessVectors_.col(subspaceDimension_ - 1) = orthogonalizedCorrectionVector.normalized();
          }
        }
      }
      else {
        if (subspaceDimension_ < maxDimension_) {
          ++subspaceDimension_;
          guessVectors_.conservativeResize(Eigen::NoChange, subspaceDimension_);
          guessVectors_.col(subspaceDimension_ - 1) = correctionVector;
        }
      }
    }
  }
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::checkEvaluators() {
  if (!sigmaVectorEvaluator_ || !preconditionerEvaluator_) {
    switch (type_) {
      case DavidsonDirectType::standard:
        setSigmaVectorEvaluator(std::make_unique<IndirectSigmaVectorEvaluator<MatrixType>>(matrixToDiagonalize_));
        setPreconditionerEvaluator(std::make_unique<IndirectPreconditionerEvaluator>(originalDiagonal_));
        break;
      case DavidsonDirectType::direct:
        throw InvalidSigmaVectorEvaluator();
      default:
        throw InvalidDavidsonTypeException();
    }
  }
}

template<class MatrixType, DavidsonBalancedType s>
void DavidsonDiagonalizer<MatrixType, s>::orthogonalizeSubspace(Eigen::MatrixXd& trialSpace) {
  int spaceDimension = trialSpace.rows();
  // Calculate modified Gram-Schmidt QR decomposition
  Eigen::MatrixXd Q(spaceDimension, subspaceDimension_);
  Eigen::MatrixXd R(subspaceDimension_, subspaceDimension_);

  for (int i = 0; i < subspaceDimension_; ++i) {
    R(i, i) = trialSpace.col(i).norm();
    Q.col(i) = trialSpace.col(i) / R(i, i);
    for (int j = i + 1; j < subspaceDimension_; ++j) {
      R(i, j) = Q.col(i).transpose() * trialSpace.col(j);
      trialSpace.col(j) -= Q.col(i) * R(i, j);
    }
  }
}
template<class MatrixType, DavidsonBalancedType s>
Eigen::VectorXd DavidsonDiagonalizer<MatrixType, s>::orthogonalizeToSubspace(const Eigen::VectorXd& vector,
                                                                             const Eigen::MatrixXd& subspace) const {
  Eigen::VectorXd orthonormalizedVector = vector;
  for (int basis = 0; basis < subspace.cols(); ++basis) {
    orthonormalizedVector -= subspace.col(basis).dot(vector) * subspace.col(basis) / subspace.col(basis).norm();
  }
  return orthonormalizedVector;
}

template class DavidsonDiagonalizer<Eigen::MatrixXd>;
template class DavidsonDiagonalizer<Eigen::SparseMatrix<double>>;
template class DavidsonDiagonalizer<Eigen::MatrixXd, DavidsonBalancedType::balanced>;
template class DavidsonDiagonalizer<Eigen::SparseMatrix<double>, DavidsonBalancedType::balanced>;

} // namespace Utils
} // namespace Scine


/*=========================================================================
 *
 *   Program:   Bifurcation Analysis Library
 *   Module:    main.cpp
 *
 *   Copyright (C) 2009 Daniele Linaro
 *
 *   This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 *=========================================================================*/

#include <unistd.h>
#include <iostream>
#include <cstdlib>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
using boost::property_tree::ptree;

#include <nvector/nvector_serial.h>

#include "balCommon.h"
#include "balParameters.h"
#include "balLogger.h"
#include "balODESolver.h"
#include "balBifurcationDiagram.h"
#include "balBifurcationParameters.h"
#include "balPLL.h"
using namespace bal;

const realtype pi = 3.1415926535897931;

int main(int argc, char *argv[]) {
  if(argc == 1) {
    fprintf(stderr, "Usage: %s ConfigFile\n", argv[0]);
    exit(1);
  }
  ptree config;
  read_xml(argv[1], config);
  char name[100];

  int steps[PLL::npar];
  BifurcationParameters * bp = BifurcationParameters::Create();
  bp->SetNumber(PLL::npar);

  for(int i=0; i<PLL::npar; i++) {
    sprintf(name,"pll.%s.steps",PLL::parname[i]);
    steps[i] = config.get<int>(name);
    if(steps[i] == 1) {
      sprintf(name,"pll.%s.min",PLL::parname[i]);
      bp->SetIthParameter(i,config.get<double>(name));
    }
    else {
      sprintf(name,"pll.%s.min",PLL::parname[i]);
      bp->SetIthParameterLowerBound(i,config.get<double>(name));
      sprintf(name,"pll.%s.max",PLL::parname[i]);
      bp->SetIthParameterUpperBound(i,config.get<double>(name));
    }
  }

#ifdef WITHPHIERR
#ifdef EXTEND
  realtype x0[7] = {0.,0.,0.,0.,pi,0.,0.};
#else
  realtype x0[5] = {0.,0.,0.,0.,pi};
#endif
#else
#ifdef EXTEND
  realtype x0[6] = {0.,0.,0.,0.,0.,0.};
#else
  realtype x0[4] = {0.,0.,0.,0.};
#endif
#endif
  x0[1] = config.get<double>("pll.vdd.min");

  bp->SetNumberOfSteps(steps);
  PLL * pll = PLL::Create();
  pll->SetParameters(bp);
  BifurcationDiagram * bifd = BifurcationDiagram::Create();
  bifd->RestartFromX0(true);
  bifd->SetDynamicalSystem(pll);
  bifd->SetFilename((char *) config.get<std::string>("simulation.outputfile").c_str());
  if(config.get<bool>("simulation.trajectory"))
    bifd->GetODESolver()->SetIntegrationMode(BOTH);
  else
    bifd->GetODESolver()->SetIntegrationMode(EVENTS);
  bifd->GetODESolver()->SetTransientDuration(config.get<double>("simulation.ttran"));
  bifd->GetODESolver()->HaltAtEquilibrium(false);
  bifd->GetODESolver()->HaltAtCycle(false);
  bifd->GetODESolver()->SetFinalTime(config.get<double>("simulation.tout"));
  bifd->GetODESolver()->SetTimeStep(1e-11);
  bifd->GetODESolver()->SetMaxNumberOfIntersections((int) 1e7);
  bifd->GetODESolver()->SetX0(x0);
  bifd->GetODESolver()->SetRelativeTolerance(1e-10);
  bifd->SetNumberOfThreads(1);
  bifd->ComputeDiagram();
  bifd->Destroy();
  pll->Destroy();
  bp->Destroy();

  return EXIT_SUCCESS;
}


//
//  synthetic_data_helper.cpp
//  synthetic_data_helper
//
//  Created by Cristián Garay on 10/15/16.
//  Copyright © 2016 Cristian Garay. All rights reserved.
//

#define NPY_NO_DEPRECATED_API NPY_1_11_API_VERSION

#include <iostream>
#include <stdint.h>
#include <alloca.h>
#include <Eigen/Core>
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/python/ptr.hpp>
#include <Python.h>
#include <numpy/ndarrayobject.h>

using namespace Eigen;
using namespace std;
using namespace boost::python;

#if PY_VERSION_HEX >= 0x03000000
void *
#else
void
#endif
init_numpy(){
    //Py_Initialize;
    import_array();
}

struct double_to_python_float
{
    static PyObject* convert(double const& d)
      {
        return boost::python::incref(
          boost::python::object(d).ptr());
      }
};

//numpy scalar converters.
template <typename T, NPY_TYPES NumPyScalarType>
struct enable_numpy_scalar_converter
{
  enable_numpy_scalar_converter()
  {
    // Required NumPy call in order to use the NumPy C API within another
    // extension module.
    // import_array();
    init_numpy();

    boost::python::converter::registry::push_back(
      &convertible,
      &construct,
      boost::python::type_id<T>());
  }

  static void* convertible(PyObject* object)
  {
    // The object is convertible if all of the following are true:
    // - is a valid object.
    // - is a numpy array scalar.
    // - its descriptor type matches the type for this converter.
    return (
      object &&                                                    // Valid
      PyArray_CheckScalar(object) &&                               // Scalar
      PyArray_DescrFromScalar(object)->type_num == NumPyScalarType // Match
    )
      ? object // The Python object can be converted.
      : NULL;
  }

  static void construct(
    PyObject* object,
    boost::python::converter::rvalue_from_python_stage1_data* data)
  {
    // Obtain a handle to the memory block that the converter has allocated
    // for the C++ type.
    namespace python = boost::python;
    typedef python::converter::rvalue_from_python_storage<T> storage_type;
    void* storage = reinterpret_cast<storage_type*>(data)->storage.bytes;

    // Extract the array scalar type directly into the storage.
    PyArray_ScalarAsCtype(object, storage);

    // Set convertible to indicate success.
    data->convertible = storage;
  }
};

dict create_synthetic_data(dict& model, numeric::array& starts, numeric::array& lengths, numeric::array& resources){
    //TODO: check if parameters are null.
    //TODO: check that dicts have the required members.
    //TODO: check that all parameters have the right sizes.
    //TODO: i'm not sending any error messages.

    numeric::array learns = extract<numeric::array>(model["learns"]);
    int num_resources = len(learns);

    numeric::array forgets = extract<numeric::array>(model["forgets"]);
    numeric::array guesses = extract<numeric::array>(model["guesses"]);

    numeric::array slips = extract<numeric::array>(model["slips"]);
    int num_subparts = len(slips);

    Vector2d initial_distn;
    double prior = extract<double>(model["prior"]);
    initial_distn << 1-prior, prior;

    MatrixXd As(2, 2*num_resources);
    for (int n=0; n<num_resources; n++) {
        double learn = extract<double>(learns[n]);
        double forget = extract<double>(forgets[n]);
        As.col(2*n) << 1-learn, learn;
        As.col(2*n+1) << forget, 1-forget;
    }

    int num_sequences = len(starts);

    int64_t bigT = 0;
    for (int k=0; k<num_sequences; k++) {
        bigT += extract<int64_t>(lengths[k]); //extract this as int??
    }

    //// outputs
    int all_stateseqs[1][bigT]; //used to be int8_t
    int all_data[num_subparts][bigT]; //used to be int8_t
    all_data[0][0] = 0;
    dict result;

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        int64_t sequence_start = extract<int64_t>(starts[sequence_index]) - 1; //should i extract these as ints?
        int64_t T = extract<int64_t>(lengths[sequence_index]);

        Vector2d nextstate_distr = initial_distn;

        for (int t=0; t<T; t++) {
            all_stateseqs[0][sequence_start + t] = nextstate_distr(0) < ((double) rand()) / ((double) RAND_MAX); //always all_stateseqs[0]?
            for (int n=0; n<num_subparts; n++) {
                all_data[n][sequence_start+t] = ((all_stateseqs[0][sequence_start + t]) ? extract<double>(slips[n]) : (1-extract<double>(guesses[n]))) < (((double) rand()) / ((double) RAND_MAX));
            }

            nextstate_distr = As.col(2*(extract<int64_t>(resources[sequence_start + t])-1)+all_stateseqs[0][sequence_start + t]); //extract int is right??
        }
    }

    //wrapping results in numpy objects.
    npy_intp all_stateseqs_dims[2] = {1, bigT}; //just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_stateseqs_pyObj = PyArray_SimpleNewFromData(2, all_stateseqs_dims, NPY_INT, all_stateseqs); //this should be NPY_INT8.
    boost::python::handle<> all_stateseqs_handle( all_stateseqs_pyObj );
    boost::python::numeric::array all_stateseqs_handle_arr( all_stateseqs_handle );

    npy_intp all_data_dims[2] = {num_subparts, bigT}; //just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_data_pyObj = PyArray_SimpleNewFromData(2, all_data_dims, NPY_INT, all_data); //this should be NPY_INT8.
    boost::python::handle<> all_data_handle( all_data_pyObj );
    boost::python::numeric::array all_data_arr( all_data_handle );

    result["stateseqs"] = all_stateseqs_handle_arr;
    result["data"] = all_data_arr;
    return(result);

}

BOOST_PYTHON_MODULE(synthetic_data_helper){
    //import_array();
    init_numpy();
    /*if(PyArray_API == NULL)
	{
	    import_array();
	}*/
    numeric::array::set_module_and_type("numpy", "ndarray");
    to_python_converter<double, double_to_python_float>();
    enable_numpy_scalar_converter<boost::int8_t, NPY_INT8>();
    enable_numpy_scalar_converter<boost::int16_t, NPY_INT16>();
    enable_numpy_scalar_converter<boost::int32_t, NPY_INT32>();
    enable_numpy_scalar_converter<boost::int64_t, NPY_INT64>();

    def("create_synthetic_data", create_synthetic_data);

}


/// @file DeconvolverEntropy.tcc
/// @brief Class for a deconvolver based on entropy or min L1 norm
/// @details This concrete class defines a deconvolver used to estimate an
/// image from a residual image, psf optionally using a weights image.
/// @ingroup Deconvolver
///
/// @copyright (c) 2007 CSIRO
/// Australia Telescope National Facility (ATNF)
/// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
/// PO Box 76, Epping NSW 1710, Australia
/// atnf-enquiries@csiro.au
///
/// This file is part of the ASKAP software distribution.
///
/// The ASKAP software distribution 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 2 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 this program; if not, write to the Free Software
/// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
///
/// @author Tim Cornwell <tim.cornwell@csiro.au>
///

#include <string>

#include <casacore/casa/aips.h>
#include <casacore/casa/Arrays/Array.h>
#include <casacore/casa/Arrays/ArrayMath.h>
#include <boost/shared_ptr.hpp>
#include <askap/AskapLogging.h>
ASKAP_LOGGER(decentropylogger, ".deconvolution.entropy");

#include <deconvolution/DeconvolverEntropy.h>
#include <deconvolution/EntropyBase.h>
#include <deconvolution/EntropyI.h>
#include <deconvolution/Emptiness.h>

using namespace casa;

namespace askap {

    namespace synthesis {

        /// @brief Class for a deconvolver based on the Entropy algorithm of Cornwell and Evans
        /// @details This base class defines a deconvolver used to estimate an
        /// image from a residual image, psf optionally using a weights image.
        /// The template argument T is the type, and FT is the transform
        /// e.g. DeconvolverEntropy<Double, DComplex>
        /// @ingroup Deconvolver

        template<class T, class FT>
        DeconvolverEntropy<T, FT>::~DeconvolverEntropy()
        {
        };

        template<class T, class FT>
        DeconvolverEntropy<T, FT>::DeconvolverEntropy(Vector<Array<T> >& dirty, Vector<Array<T> >& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf)
        {
            if (this->itsNumberDirtyTerms > 1) {
                throw(AskapError("Entropy deconvolver cannot perform multi-term deconvolutions"));
            }
        };

        template<class T, class FT>
        DeconvolverEntropy<T, FT>::DeconvolverEntropy(Array<T>& dirty, Array<T>& psf)
                : DeconvolverBase<T, FT>::DeconvolverBase(dirty, psf)
        {
        };
        template<class T, class FT>
        void DeconvolverEntropy<T, FT>::finalise()
        {
            // Find residuals for current model model
            this->updateResiduals(this->itsModel);
        }

        template<class T, class FT>
        void DeconvolverEntropy<T, FT>::initialise()
        {
            DeconvolverBase<T, FT>::initialise();

            // Set the initial value of Q
            T Q;
            Q = sum(this->psf()(this->psf() > T(0.1)));
            ASKAPLOG_INFO_STR(decentropylogger, "Initial value of Q = " << Q << " pixels");

            this->itsEntropy->setQ(Q);
        }

        // This is basically the algorithm described in the Cornwell-Evans paper of 1985, with
        // some improvements as suggested by Bob Sault.
        template<class T, class FT>
        bool DeconvolverEntropy<T, FT>::deconvolve()
        {
            this->initialise();

            ASKAPLOG_INFO_STR(decentropylogger, "Performing Entropy deconvolution for "
                                  << this->control()->targetIter() << " iterations");

            ASKAPLOG_INFO_STR(decentropylogger, "Target rms fit = "
                                  << this->control()->targetObjectiveFunction() << " Jy/beam");

            uInt numberPixels = this->itsModel.shape().product();
            T targetChisq = square(this->control()->targetObjectiveFunction()) * numberPixels;
            T chisq;

            Array<T> trialModel(this->model().shape());
            trialModel.set(T(0.0));

            this->updateResiduals(this->model());

            T lipschitz(10.0);

            this->model() = this->dirty().copy() / lipschitz;

            Array<T> step(this->model().shape());
            step.set(T(0.0));

            T absPeakVal(max(abs(this->dirty())));
            T peakSidelobe(0.1);
            T aFit = max(peakSidelobe * absPeakVal, rms(this->dirty())) / lipschitz;
            ASKAPLOG_INFO_STR(decentropylogger, "Scaling = " << aFit << " Jy/pixel");
            this->itsEntropy->setScale(aFit);

            this->model().set(aFit);

            this->updateResiduals(this->model());

            do {
                // Find the current fit
                chisq = sum(square(this->dirty()));

                Matrix<T> GDG(this->itsEntropy->formGDGStep(this->model(), this->dirty(), step));

                // Check to see if Alpha and Beta need to be initialised. If so then we need to
                // do so and recalculate the gradients and step
                if (this->itsEntropy->initialiseAlphaBeta(GDG)) {
                    GDG = this->itsEntropy->formGDGStep(this->model(), this->dirty(), step);
                }

                T flux = sum(this->model());
                this->itsEntropy->changeAlphaBeta(GDG, targetChisq, chisq, this->control()->targetFlux(), flux);

                // Now find the normalised gradient - we will use this to limit the step taken
                T length(this->itsEntropy->formLength(GDG));
                if (length <= T(0.0)) {
                    length = GDG(F, F);
                }
                T normGrad = GDG(J, J) / length;

                //        relaxMin();

                // We limit the step to less than the tolerance e.g. 0.1 so that the
                // quadratic approximation in the Newton-Raphson is still valid.
                T scale = 1.0;
                T scalem = 1.0;
                if (normGrad > 0.0) {
                    scalem = this->control()->tolerance() / normGrad;
                }
                scale = min(T(1.0), scalem);

                // OK - now we take the proposed step and evaluate the
                // gradient there.
                trialModel = this->model() + scale * step;

                // Calculate residual for this new trial image
                this->updateResiduals(trialModel);
                chisq = sum(square(this->dirty()));

                // Form the scalar Gradient . Step at this new location. Ideally this should be
                // zero. Once we know the value of the gradient initially and for the trial image, we
                // can determine the optimal step by interpolating the gradient to zero. Ideally the
                // step should be O(1) times the original step
                T eps = 1.0;
                T gradDotStep0 = GDG(J, J);
                T gradDotStep1(this->itsEntropy->formGDS(this->model(), this->dirty(), step));

                if (gradDotStep0 != gradDotStep1) {
                    eps = gradDotStep0 / (gradDotStep0 - gradDotStep1);
                }
                if (scale != T(0.0)) eps = min(eps, T(scalem / scale));
                if (eps <= T(0.0)) {
                    eps = T(1.0);
                }

                // Step to optimum point
                this->model() = this->model() + scale * eps * step;

                // Recalculate residual for the new image
                this->updateResiduals(this->model());

                chisq = sum(square(this->dirty()));

                // readjust beam volume
                //      itsQ = itsQ*(T(1.0)/max(T(0.5), min(T(2.0),eps))+T(1.0))/T(2.0);

                flux = sum(this->model());
                this->itsEntropy->changeAlphaBeta(GDG, targetChisq, chisq, this->control()->targetFlux(), flux);

                absPeakVal = max(abs(this->dirty()));

                T lipschitz(10.0);

                aFit = max(peakSidelobe * absPeakVal, rms(this->dirty())) / lipschitz;
                ASKAPLOG_INFO_STR(decentropylogger, "Scaling = " << aFit << " Jy/pixel");
                this->itsEntropy->setScale(aFit);

                this->state()->setPeakResidual(absPeakVal);
                this->state()->setObjectiveFunction(sqrt(chisq / numberPixels));
                this->state()->setTotalFlux(flux);

                this->monitor()->monitor(*(this->state()));
                this->state()->incIter();
            } while (!this->control()->terminate(*(this->state())));

            ASKAPLOG_INFO_STR(decentropylogger, "Performed Entropy deconvolution for "
                                  << this->state()->currentIter() << " iterations");

            ASKAPLOG_INFO_STR(decentropylogger, this->control()->terminationString());

            this->finalise();

            return True;
        }

        template<class T, class FT>
        void DeconvolverEntropy<T, FT>::configure(const LOFAR::ParameterSet& parset)
        {
            DeconvolverBase<T, FT>::configure(parset);

            String algorithm(parset.getString("algorithm", "Emptiness"));
            this->control()->setAlgorithm(algorithm);

            if (algorithm == "EntropyI") {
                ASKAPLOG_INFO_STR(decentropylogger, "Maximising information entropy of model image");
                itsEntropy = boost::shared_ptr<EntropyBase<T> >(new EntropyI<T>());
                itsEntropy->setTolerance(parset.getFloat("tolerance", 0.3));
            } else {
                ASKAPLOG_INFO_STR(decentropylogger, "Maximising emptiness (negative L1 norm) of model image");
                itsEntropy = boost::shared_ptr<EntropyBase<T> >(new Emptiness<T>());
                itsEntropy->setTolerance(parset.getFloat("tolerance", 0.3));
            }
        }

    } // namespace synthesis

} // namespace askap