/*
 * Copyright (C) 2011-2015, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Geometric Regression Library (GeoRegression).
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

package georegression.transform.se;

import georegression.struct.so.Quaternion_F64;
import org.ejml.alg.fixed.FixedOps3;
import org.ejml.data.DenseMatrix64F;
import org.ejml.data.FixedMatrix3x3_64F;
import org.ejml.factory.DecompositionFactory;
import org.ejml.interfaces.decomposition.SingularValueDecomposition;
import org.ejml.ops.CommonOps;
import org.ejml.ops.ConvertMatrixType;

import java.util.List;

/**
 * 
Finds the average of a set of {@link Quaternion_F64 quaternions} by using a modification of the
 * method proposed in [1].  It works by finding the average rotation matrix then finds the best fit matrix
 * in SO-3, see page 200 of [2].
 *
 * 
[1] MLA Markley, F. Landis, et al. "Quaternion averaging." (2007)
 * 
[2] "An Invitation to 3-D Vision, From Images to Geometric Models" 1st Ed. 2004. Springer.
 *
 * @author Peter Abeles
 */
public class AverageRotationMatrix_F64 {

	DenseMatrix64F M = new DenseMatrix64F(3,3);
	FixedMatrix3x3_64F F = new FixedMatrix3x3_64F();

	SingularValueDecomposition svd = DecompositionFactory.svd(3,3,true,true,true);

	public boolean process(List list , DenseMatrix64F average ) {

		if( list.isEmpty() )
			throw new IllegalArgumentException("Input list is empty");
		if( average == null )
			throw new IllegalArgumentException("average is null");

		M.zero();

		for (int i = 0; i < list.size(); i++) {
			DenseMatrix64F m = list.get(i);

			// unroll to make it faster.  M = M + m
			// row 0
			M.data[0]  += m.data[0]; M.data[1]  += m.data[1]; M.data[2]  += m.data[2];
			// row 1
			M.data[3]  += m.data[3]; M.data[4]  += m.data[4]; M.data[5]  += m.data[5];
			// row 2
			M.data[6]  += m.data[6]; M.data[7]  += m.data[7]; M.data[8]  += m.data[8];
		}

		CommonOps.divide(M,list.size());

		if( !svd.decompose(M) )
			return false;

		CommonOps.multTransB(svd.getU(null,false),svd.getV(null,false),average);

		// determinant should be +1
		/**/double det = CommonOps.det(average);

		if( det < 0 )
			CommonOps.scale(-1,average);

		return true;
	}

	public boolean process(List list , FixedMatrix3x3_64F average ) {

		if( list.isEmpty() )
			throw new IllegalArgumentException("Input list is empty");
		if( average == null )
			throw new IllegalArgumentException("average is null");

		FixedOps3.fill(F,0);

		for (int i = 0; i < list.size(); i++) {
			FixedMatrix3x3_64F m = list.get(i);

			// unroll to make it faster.  M = M + m
			// row 0
			F.a11  += m.a11; F.a12  += m.a12; F.a13  += m.a13;
			// row 1
			F.a21  += m.a21; F.a22  += m.a22; F.a23  += m.a23;
			// row 2
			F.a31  += m.a31; F.a32  += m.a32; F.a33  += m.a33;
		}

		FixedOps3.divide(F,list.size());

		ConvertMatrixType.convert(F,M);
		if( !svd.decompose(M) )
			return false;

		CommonOps.multTransB(svd.getU(null,false),svd.getV(null,false),M);

		// determinant should be +1
		/**/double det = CommonOps.det(M);

		if( det < 0 )
			CommonOps.scale(-1,M);

		ConvertMatrixType.convert(M,average);

		return true;
	}
}