/* -----------------------------------------------------------------------------

Copyright (c) 2006 Simon Brown                          si@sjbrown.co.uk
Copyright (c) 2006 Ignacio Castano                      icastano@nvidia.com

Permission is hereby granted, free of charge, to any person obtaining
a copy of this software and associated documentation files (the 
"Software"), to	deal in the Software without restriction, including
without limitation the rights to use, copy, modify, merge, publish,
distribute, sublicense, and/or sell copies of the Software, and to 
permit persons to whom the Software is furnished to do so, subject to 
the following conditions:

The above copyright notice and this permission notice shall be included
in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 
CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, 
TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE 
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

-------------------------------------------------------------------------- */

#include "weightedclusterfit.h"
#include "colourset.h"
#include "colourblock.h"
#include <cfloat>


namespace squish {

	WeightedClusterFit::WeightedClusterFit()
	{
	}

	void WeightedClusterFit::SetColourSet( ColourSet const* colours, int flags )
	{
		ColourFit::SetColourSet( colours, flags );

		// initialise the best error
#if SQUISH_USE_SIMD
		m_besterror = VEC4_CONST( FLT_MAX );
		Vec3 metric = m_metric.GetVec3();
#else
		m_besterror = FLT_MAX;
		Vec3 metric = m_metric;
#endif

		// cache some values
		int const count = m_colours->GetCount();
		Vec3 const* values = m_colours->GetPoints();

		// get the covariance matrix
		Sym3x3 covariance = ComputeWeightedCovariance( count, values, m_colours->GetWeights(), metric );

		// compute the principle component
		Vec3 principle = ComputePrincipleComponent( covariance );

		// build the list of values
		float dps[16];
		for( int i = 0; i < count; ++i )
		{
			dps[i] = Dot( values[i], principle );
			m_order[i] = i;
		}

		// stable sort
		for( int i = 0; i < count; ++i )
		{
			for( int j = i; j > 0 && dps[j] < dps[j - 1]; --j )
			{
				std::swap( dps[j], dps[j - 1] );
				std::swap( m_order[j], m_order[j - 1] );
			}
		}

		// weight all the points
#if SQUISH_USE_SIMD
		Vec4 const* unweighted = m_colours->GetPointsSimd();
		Vec4 const* weights = m_colours->GetWeightsSimd();
		m_xxsum = VEC4_CONST( 0.0f );
		m_xsum = VEC4_CONST( 0.0f );
#else
		Vec3 const* unweighted = m_colours->GetPoints();
		float const* weights = m_colours->GetWeights();
		m_xxsum = Vec3( 0.0f );
		m_xsum = Vec3( 0.0f );
		m_wsum = 0.0f;	
#endif

		for( int i = 0; i < count; ++i )
		{
			int p = m_order[i];
			m_weighted[i] = weights[p] * unweighted[p];
			m_xxsum += m_weighted[i] * m_weighted[i];
			m_xsum += m_weighted[i];
#if !SQUISH_USE_SIMD		
			m_weights[i] = weights[p];
			m_wsum += m_weights[i];
#endif
		}
	}


	void WeightedClusterFit::SetMetric(float r, float g, float b)
	{
#if SQUISH_USE_SIMD
		m_metric = Vec4(r, g, b, 0);
#else
		m_metric = Vec3(r, g, b);
#endif
		m_metricSqr = m_metric * m_metric;
	}

	float WeightedClusterFit::GetBestError() const
	{
#if SQUISH_USE_SIMD
		Vec4 x = m_xxsum * m_metricSqr;
		Vec4 error = m_besterror + x.SplatX() + x.SplatY() + x.SplatZ();
		return error.GetVec3().X();
#else
		return m_besterror + Dot(m_xxsum, m_metricSqr);
#endif

	}

#if SQUISH_USE_SIMD

	void WeightedClusterFit::Compress3( void* block )
	{
		int const count = m_colours->GetCount();
		Vec4 const one = VEC4_CONST(1.0f);
		Vec4 const zero = VEC4_CONST(0.0f);
		Vec4 const half(0.5f, 0.5f, 0.5f, 0.25f);
		Vec4 const two = VEC4_CONST(2.0);
		Vec4 const grid( 31.0f, 63.0f, 31.0f, 0.0f );
		Vec4 const gridrcp( 1.0f/31.0f, 1.0f/63.0f, 1.0f/31.0f, 0.0f );

		// declare variables
		Vec4 beststart = VEC4_CONST( 0.0f );
		Vec4 bestend = VEC4_CONST( 0.0f );
		Vec4 besterror = VEC4_CONST( FLT_MAX );

		Vec4 x0 = zero;

		int b0 = 0, b1 = 0;

		// check all possible clusters for this total order
		for( int c0 = 0; c0 <= count; c0++)
		{	
			Vec4 x1 = zero;

			for( int c1 = 0; c1 <= count-c0; c1++)
			{
				Vec4 const x2 = m_xsum - x1 - x0;

				//Vec3 const alphax_sum = x0 + x1 * 0.5f;
				//float const alpha2_sum = w0 + w1 * 0.25f;
				Vec4 const alphax_sum = MultiplyAdd(x1, half, x0); // alphax_sum, alpha2_sum
				Vec4 const alpha2_sum = alphax_sum.SplatW();

				//Vec3 const betax_sum = x2 + x1 * 0.5f;
				//float const beta2_sum = w2 + w1 * 0.25f;
				Vec4 const betax_sum = MultiplyAdd(x1, half, x2); // betax_sum, beta2_sum
				Vec4 const beta2_sum = betax_sum.SplatW();

				//float const alphabeta_sum = w1 * 0.25f;
				Vec4 const alphabeta_sum = (x1 * half).SplatW(); // alphabeta_sum

				// float const factor = 1.0f / (alpha2_sum * beta2_sum - alphabeta_sum * alphabeta_sum);
				Vec4 const factor = Reciprocal( NegativeMultiplySubtract(alphabeta_sum, alphabeta_sum, alpha2_sum*beta2_sum) );

				Vec4 a = NegativeMultiplySubtract(betax_sum, alphabeta_sum, alphax_sum*beta2_sum) * factor;
				Vec4 b = NegativeMultiplySubtract(alphax_sum, alphabeta_sum, betax_sum*alpha2_sum) * factor;

				// clamp to the grid
				a = Min( one, Max( zero, a ) );
				b = Min( one, Max( zero, b ) );
				a = Truncate( MultiplyAdd( grid, a, half ) ) * gridrcp;
				b = Truncate( MultiplyAdd( grid, b, half ) ) * gridrcp;

				// compute the error (we skip the constant xxsum)
				Vec4 e1 = MultiplyAdd( a*a, alpha2_sum, b*b*beta2_sum );
				Vec4 e2 = NegativeMultiplySubtract( a, alphax_sum, a*b*alphabeta_sum );
				Vec4 e3 = NegativeMultiplySubtract( b, betax_sum, e2 );
				Vec4 e4 = MultiplyAdd( two, e3, e1 );

				// apply the metric to the error term
				Vec4 e5 = e4 * m_metricSqr;
				Vec4 error = e5.SplatX() + e5.SplatY() + e5.SplatZ();

				// keep the solution if it wins
				if( CompareAnyLessThan( error, besterror ) )
				{
					besterror = error;
					beststart = a;
					bestend = b;
					b0 = c0;
					b1 = c1;
				}

				x1 += m_weighted[c0+c1];
			}

			x0 += m_weighted[c0];
		}

		// save the block if necessary
		if( CompareAnyLessThan( besterror, m_besterror ) )
		{
			// compute indices from cluster sizes.
			u8 bestindices[16];
			{
				int i = 0;
				for(; i < b0; i++) {
					bestindices[i] = 0;
				}
				for(; i < b0+b1; i++) {
					bestindices[i] = 2;
				}
				for(; i < count; i++) {
					bestindices[i] = 1;
				}
			}

			// remap the indices
			u8 ordered[16];
			for( int i = 0; i < count; ++i )
				ordered[m_order[i]] = bestindices[i];

			m_colours->RemapIndices( ordered, bestindices );


			// save the block
			WriteColourBlock3( beststart.GetVec3(), bestend.GetVec3(), bestindices, block );

			// save the error
			m_besterror = besterror;
		}
	}

	void WeightedClusterFit::Compress4( void* block )
	{
		int const count = m_colours->GetCount();
		Vec4 const one = VEC4_CONST(1.0f);
		Vec4 const zero = VEC4_CONST(0.0f);
		Vec4 const half = VEC4_CONST(0.5f);
		Vec4 const two = VEC4_CONST(2.0);
		Vec4 const onethird( 1.0f/3.0f, 1.0f/3.0f, 1.0f/3.0f, 1.0f/9.0f );
		Vec4 const twothirds( 2.0f/3.0f, 2.0f/3.0f, 2.0f/3.0f, 4.0f/9.0f );
		Vec4 const twonineths = VEC4_CONST( 2.0f/9.0f );
		Vec4 const grid( 31.0f, 63.0f, 31.0f, 0.0f );
		Vec4 const gridrcp( 1.0f/31.0f, 1.0f/63.0f, 1.0f/31.0f, 0.0f );

		// declare variables
		Vec4 beststart = VEC4_CONST( 0.0f );
		Vec4 bestend = VEC4_CONST( 0.0f );
		Vec4 besterror = VEC4_CONST( FLT_MAX );

		Vec4 x0 = zero;
		int b0 = 0, b1 = 0, b2 = 0;

		// check all possible clusters for this total order
		for( int c0 = 0; c0 <= count; c0++)
		{	
			Vec4 x1 = zero;

			for( int c1 = 0; c1 <= count-c0; c1++)
			{	
				Vec4 x2 = zero;

				for( int c2 = 0; c2 <= count-c0-c1; c2++)
				{
					Vec4 const x3 = m_xsum - x2 - x1 - x0;

					//Vec3 const alphax_sum = x0 + x1 * (2.0f / 3.0f) + x2 * (1.0f / 3.0f);
					//float const alpha2_sum = w0 + w1 * (4.0f/9.0f) + w2 * (1.0f/9.0f);
					Vec4 const alphax_sum = MultiplyAdd(x2, onethird, MultiplyAdd(x1, twothirds, x0)); // alphax_sum, alpha2_sum
					Vec4 const alpha2_sum = alphax_sum.SplatW();

					//Vec3 const betax_sum = x3 + x2 * (2.0f / 3.0f) + x1 * (1.0f / 3.0f);
					//float const beta2_sum = w3 + w2 * (4.0f/9.0f) + w1 * (1.0f/9.0f);
					Vec4 const betax_sum = MultiplyAdd(x2, twothirds, MultiplyAdd(x1, onethird, x3)); // betax_sum, beta2_sum
					Vec4 const beta2_sum = betax_sum.SplatW();

					//float const alphabeta_sum = (w1 + w2) * (2.0f/9.0f);
					Vec4 const alphabeta_sum = twonineths*( x1 + x2 ).SplatW(); // alphabeta_sum

					// float const factor = 1.0f / (alpha2_sum * beta2_sum - alphabeta_sum * alphabeta_sum);
					Vec4 const factor = Reciprocal( NegativeMultiplySubtract(alphabeta_sum, alphabeta_sum, alpha2_sum*beta2_sum) );

					Vec4 a = NegativeMultiplySubtract(betax_sum, alphabeta_sum, alphax_sum*beta2_sum) * factor;
					Vec4 b = NegativeMultiplySubtract(alphax_sum, alphabeta_sum, betax_sum*alpha2_sum) * factor;

					// clamp to the grid
					a = Min( one, Max( zero, a ) );
					b = Min( one, Max( zero, b ) );
					a = Truncate( MultiplyAdd( grid, a, half ) ) * gridrcp;
					b = Truncate( MultiplyAdd( grid, b, half ) ) * gridrcp;

					// compute the error (we skip the constant xxsum)
					Vec4 e1 = MultiplyAdd( a*a, alpha2_sum, b*b*beta2_sum );
					Vec4 e2 = NegativeMultiplySubtract( a, alphax_sum, a*b*alphabeta_sum );
					Vec4 e3 = NegativeMultiplySubtract( b, betax_sum, e2 );
					Vec4 e4 = MultiplyAdd( two, e3, e1 );

					// apply the metric to the error term
					Vec4 e5 = e4 * m_metricSqr;
					Vec4 error = e5.SplatX() + e5.SplatY() + e5.SplatZ();

					// keep the solution if it wins
					if( CompareAnyLessThan( error, besterror ) )
					{
						besterror = error;
						beststart = a;
						bestend = b;
						b0 = c0;
						b1 = c1;
						b2 = c2;
					}

					x2 += m_weighted[c0+c1+c2];
				}

				x1 += m_weighted[c0+c1];
			}

			x0 += m_weighted[c0];
		}

		// save the block if necessary
		if( CompareAnyLessThan( besterror, m_besterror ) )
		{
			// compute indices from cluster sizes.
			u8 bestindices[16];
			{
				int i = 0;
				for(; i < b0; i++) {
					bestindices[i] = 0;
				}
				for(; i < b0+b1; i++) {
					bestindices[i] = 2;
				}
				for(; i < b0+b1+b2; i++) {
					bestindices[i] = 3;
				}
				for(; i < count; i++) {
					bestindices[i] = 1;
				}
			}

			// remap the indices
			u8 ordered[16];
			for( int i = 0; i < count; ++i )
				ordered[m_order[i]] = bestindices[i];

			m_colours->RemapIndices( ordered, bestindices );

			// save the block
			WriteColourBlock4( beststart.GetVec3(), bestend.GetVec3(), bestindices, block );

			// save the error
			m_besterror = besterror;
		}
	}

#else

	void WeightedClusterFit::Compress3( void* block )
	{
		int const count = m_colours->GetCount();
		Vec3 const one( 1.0f );
		Vec3 const zero( 0.0f );
		Vec3 const half( 0.5f );
		Vec3 const grid( 31.0f, 63.0f, 31.0f );
		Vec3 const gridrcp( 1.0f/31.0f, 1.0f/63.0f, 1.0f/31.0f );

		// declare variables
		Vec3 beststart( 0.0f );
		Vec3 bestend( 0.0f );
		float besterror = FLT_MAX;

		Vec3 x0(0.0f);
		float w0 = 0.0f;

		int b0 = 0, b1 = 0;

		// check all possible clusters for this total order
		for( int c0 = 0; c0 <= count; c0++)
		{	
			Vec3 x1(0.0f);
			float w1 = 0.0f;

			for( int c1 = 0; c1 <= count-c0; c1++)
			{	
				float w2 = m_wsum - w0 - w1;

				// These factors could be entirely precomputed.
				float const alpha2_sum = w0 + w1 * 0.25f;
				float const beta2_sum = w2 + w1 * 0.25f;
				float const alphabeta_sum = w1 * 0.25f;
				float const factor = 1.0f / (alpha2_sum * beta2_sum - alphabeta_sum * alphabeta_sum);

				Vec3 const alphax_sum = x0 + x1 * 0.5f;
				Vec3 const betax_sum = m_xsum - alphax_sum;

				Vec3 a = (alphax_sum*beta2_sum - betax_sum*alphabeta_sum) * factor;
				Vec3 b = (betax_sum*alpha2_sum - alphax_sum*alphabeta_sum) * factor;

				// clamp to the grid
				a = Min( one, Max( zero, a ) );
				b = Min( one, Max( zero, b ) );
				a = Floor( grid*a + half )*gridrcp;
				b = Floor( grid*b + half )*gridrcp;

				// compute the error
				Vec3 e1 = a*a*alpha2_sum + b*b*beta2_sum + 2.0f*( a*b*alphabeta_sum - a*alphax_sum - b*betax_sum );

				// apply the metric to the error term
				float error = Dot( e1, m_metricSqr );

				// keep the solution if it wins
				if( error < besterror )
				{
					besterror = error;
					beststart = a;
					bestend = b;
					b0 = c0;
					b1 = c1;
				}

				x1 += m_weighted[c0+c1];
				w1 += m_weights[c0+c1];
			}

			x0 += m_weighted[c0];
			w0 += m_weights[c0];
		}

		// save the block if necessary
		if( besterror < m_besterror )
		{
			// compute indices from cluster sizes.
			u8 bestindices[16];
			{
				int i = 0;
				for(; i < b0; i++) {
					bestindices[i] = 0;
				}
				for(; i < b0+b1; i++) {
					bestindices[i] = 2;
				}
				for(; i < count; i++) {
					bestindices[i] = 1;
				}
			}

			// remap the indices
			u8 ordered[16];
			for( int i = 0; i < count; ++i )
				ordered[m_order[i]] = bestindices[i];

			m_colours->RemapIndices( ordered, bestindices );

			// save the block
			WriteColourBlock3( beststart, bestend, bestindices, block );

			// save the error
			m_besterror = besterror;
		}
	}

	void WeightedClusterFit::Compress4( void* block )
	{
		int const count = m_colours->GetCount();
		Vec3 const one( 1.0f );
		Vec3 const zero( 0.0f );
		Vec3 const half( 0.5f );
		Vec3 const grid( 31.0f, 63.0f, 31.0f );
		Vec3 const gridrcp( 1.0f/31.0f, 1.0f/63.0f, 1.0f/31.0f );

		// declare variables
		Vec3 beststart( 0.0f );
		Vec3 bestend( 0.0f );
		float besterror = FLT_MAX;

		Vec3 x0(0.0f);
		float w0 = 0.0f;
		int b0 = 0, b1 = 0, b2 = 0;

		// check all possible clusters for this total order
		for( int c0 = 0; c0 <= count; c0++)
		{	
			Vec3 x1(0.0f);
			float w1 = 0.0f;

			for( int c1 = 0; c1 <= count-c0; c1++)
			{	
				Vec3 x2(0.0f);
				float w2 = 0.0f;

				for( int c2 = 0; c2 <= count-c0-c1; c2++)
				{
					float w3 = m_wsum - w0 - w1 - w2;

					float const alpha2_sum = w0 + w1 * (4.0f/9.0f) + w2 * (1.0f/9.0f);
					float const beta2_sum = w3 + w2 * (4.0f/9.0f) + w1 * (1.0f/9.0f);
					float const alphabeta_sum = (w1 + w2) * (2.0f/9.0f);
					float const factor = 1.0f / (alpha2_sum * beta2_sum - alphabeta_sum * alphabeta_sum);

					Vec3 const alphax_sum = x0 + x1 * (2.0f / 3.0f) + x2 * (1.0f / 3.0f);
					Vec3 const betax_sum = m_xsum - alphax_sum;

					Vec3 a = ( alphax_sum*beta2_sum - betax_sum*alphabeta_sum )*factor;
					Vec3 b = ( betax_sum*alpha2_sum - alphax_sum*alphabeta_sum )*factor;

					// clamp to the grid
					a = Min( one, Max( zero, a ) );
					b = Min( one, Max( zero, b ) );
					a = Floor( grid*a + half )*gridrcp;
					b = Floor( grid*b + half )*gridrcp;

					// compute the error
					Vec3 e1 = a*a*alpha2_sum + b*b*beta2_sum + 2.0f*( a*b*alphabeta_sum - a*alphax_sum - b*betax_sum );

					// apply the metric to the error term
					float error = Dot( e1, m_metricSqr );

					// keep the solution if it wins
					if( error < besterror )
					{
						besterror = error;
						beststart = a;
						bestend = b;
						b0 = c0;
						b1 = c1;
						b2 = c2;
					}

					x2 += m_weighted[c0+c1+c2];
					w2 += m_weights[c0+c1+c2];
				}

				x1 += m_weighted[c0+c1];
				w1 += m_weights[c0+c1];
			}

			x0 += m_weighted[c0];
			w0 += m_weights[c0];
		}

		// save the block if necessary
		if( besterror < m_besterror )
		{
			// compute indices from cluster sizes.
			u8 bestindices[16];
			{
				int i = 0;
				for(; i < b0; i++) {
					bestindices[i] = 0;
				}
				for(; i < b0+b1; i++) {
					bestindices[i] = 2;
				}
				for(; i < b0+b1+b2; i++) {
					bestindices[i] = 3;
				}
				for(; i < count; i++) {
					bestindices[i] = 1;
				}
			}

			// remap the indices
			u8 ordered[16];
			for( int i = 0; i < count; ++i )
				ordered[m_order[i]] = bestindices[i];

			m_colours->RemapIndices( ordered, bestindices );

			// save the block
			WriteColourBlock4( beststart, bestend, bestindices, block );

			// save the error
			m_besterror = besterror;
		}
	}

#endif

} // namespace squish