/*# ---------------------------------------------------------------------
#
# Copyright (c) CREATIS (Centre de Recherche en Acquisition et Traitement de l'Image
#                        pour la Sant�)
# Authors : Eduardo Davila, Frederic Cervenansky, Claire Mouton
# Previous Authors : Laurent Guigues, Jean-Pierre Roux
# CreaTools website : www.creatis.insa-lyon.fr/site/fr/creatools_accueil
#
#  This software is governed by the CeCILL-B license under French law and
#  abiding by the rules of distribution of free software. You can  use,
#  modify and/ or redistribute the software under the terms of the CeCILL-B
#  license as circulated by CEA, CNRS and INRIA at the following URL
#  http://www.cecill.info/licences/Licence_CeCILL-B_V1-en.html
#  or in the file LICENSE.txt.
#
#  As a counterpart to the access to the source code and  rights to copy,
#  modify and redistribute granted by the license, users are provided only
#  with a limited warranty  and the software's author,  the holder of the
#  economic rights,  and the successive licensors  have only  limited
#  liability.
#
#  The fact that you are presently reading this means that you have had
#  knowledge of the CeCILL-B license and that you accept its terms.
# ------------------------------------------------------------------------ */

////////////////////////////////////////////////////////////////////////////////
// vmfuncs.h
// Creation : 02/02/2000
// Author   : Leonardo FLOREZ VALENCIA
//               l-florez@uniandes.edu.co
//               lflorez@creatis.insa-lyon.fr
// Copyright (C) 2000-2002 Leonardo FLOREZ VALENCIA
//
// 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 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.
////////////////////////////////////////////////////////////////////////////////

#ifndef GTMLIB__MATH__VMFUNCS__HXX
#define GTMLIB__MATH__VMFUNCS__HXX

#include <iostream>
#include <math.h>
#include <memory.h>
#include "mathdefs.h"

#include <creaSystem.h>

using namespace std;

namespace gtm
{
// -----------------------------------------------------------------------------
//
/// Functions for vectors.
//@{
// -----------------------------------------------------------------------------
    /** Stream out function for vectors.
     *
     *  @param o Output stream.
     *  @param v ANSI array.
     *  @param n Cardinality.
     *  @param col Should I print it in column format?
     */
    template< class T >
    inline
    void VectorPrint( std::ostream& o, T* v, uint32_t n, bool col )
    {
	uint32_t i;
	
	o << n << ": [";
	if( col ) o << endl;
	else      o << " ";
	for( uint32_t i = 0; i < n; i++ ) {
	    
	    o << v[ i ];
	    if( col ) o << endl;
	    else      o << " ";
	    
	} // rof
	o << "]" << endl;
	
    }
    
// -----------------------------------------------------------------------------
    /** Allocation function.
     *
     *  @param n Cardinality.
     */
    template< class T >
    inline
    T* VectorAllocateMemory( uint32_t n )
    {
	T* v = new T[ n ];

	memset( v, 0, sizeof( T ) * n );
	return( v );

    }

// -----------------------------------------------------------------------------
    /** Frees vector's memory.
     *
     *  @param v Vector.
     */
    template< class T >
    inline
    void VectorFreeMemory( T* v )
    {
	if( v ) delete [] v;

    }

// -----------------------------------------------------------------------------
    /** Copy.
     *
     *  @param v Target vector.
     *  @param v_o Source vector.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    void VectorAssignMemory( T* v, T* v_o, uint32_t n )
    {
	memcpy( v, v_o, sizeof( T ) * n );

    }

// -----------------------------------------------------------------------------
    /** Allocation & copy.
     *
     *  @param v Vector to be copied.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    T* VectorCopyMemory( T* v, uint32_t n )
    {
	T* n_v = VectorAllocateMemory< T >( n );
	VectorAssignMemory< T >( n_v, v, n );
	return( n_v );

    }

// -----------------------------------------------------------------------------
    /** Scalar assignation.
     *
     *  @param v Vector.
     *  @param s Scalar value.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    void VectorAssignScalar( T* v, T s, uint32_t n )
    {
	uint32_t i;

	for( i = 0; i < n; v[ i ] = s, i++ );

    }

// -----------------------------------------------------------------------------
    /** Convert a vector in a matrix.
     *
     *  @param ma Matrix.
     *  @param v Vector.
     *  @param n Columns.
     *  @param m Rows.
     *  @param col Is it a column vector?
     */
    template< class T >
    inline
    void VectorMatrixCast( T** ma, T* v, uint32_t n, uint32_t m, bool col )
    {
	uint32_t i;

	for( i = 0; i < ( ( col )? m: n ); i++ )
	    ma[ ( col )? n: i ][ ( col )? i: m ] = v[ i ];

    }

// -----------------------------------------------------------------------------
    /** Performs a vectorial addition.
     *
     *  @param v Result.
     *  @param v_l Left operand.
     *  @param v_r Right operand.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    void VectorAdd( T* v, T* v_l, T* v_r, uint32_t n )
    {
	uint32_t i;

	for( i = 0; i < n; v[ i ] = v_l[ i ] + v_r[ i ], i++ );

    }

// -----------------------------------------------------------------------------
    /** Performs a vectorial substraction.
     *
     *  @param v Result.
     *  @param v_l Left operand.
     *  @param v_r Right operand.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    void VectorSubtract( T* v, T* v_l, T* v_r, uint32_t n )
    {
	uint32_t i;

	for( i = 0; i < n; v[ i ] = v_l[ i ] - v_r[ i ], i++ );

    }

// -----------------------------------------------------------------------------
    /** Performs a vectorial cross product.
     *
     *  @param v Result.
     *  @param v_l Left operand.
     *  @param v_r Right operand.
     */
    template< class T >
    inline
    void VectorCrossProduct( T* v, T* v_l, T* v_r )
    {
	v[ 0 ] = ( v_l[ 1 ] * v_r[ 2 ] ) - ( v_l[ 2 ] * v_r[ 1 ] );
	v[ 1 ] = ( v_l[ 2 ] * v_r[ 0 ] ) - ( v_l[ 0 ] * v_r[ 2 ] );
	v[ 2 ] = ( v_l[ 0 ] * v_r[ 1 ] ) - ( v_l[ 1 ] * v_r[ 0 ] );

    }

// -----------------------------------------------------------------------------
    /** Performs a vectorial dot product.
     *
     *  @param v_l Left operand.
     *  @param v_r Right operand.
     *  @param n Cardinality.
     *  @return Dot product.
     */
    template< class T >
    inline
    T VectorDotProduct( T* v_l, T* v_r, uint32_t n )
    {
	T ret;
	uint32_t i;

	for( i = 0, ret = ( T )0; i < n; ret = ret + ( v_l[ i ] * v_r[ i ] ), i++ );
	return( ret );

    }

// -----------------------------------------------------------------------------
    /** Performs a vector-scalar product.
     *
     *  @param v Result.
     *  @param v_l Left operand.
     *  @param s Scalar.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    void VectorScalarProduct( T* v, T* v_l, T s, uint32_t n )
    {
	uint32_t i;

	for( i = 0; i < n; v[ i ] = v_l[ i ] * s, i++ );

    }

// -----------------------------------------------------------------------------
    /** Calculates vectorial L2 norm.
     *
     *  @param v Vector.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    T VectorNorm( T* v, uint32_t n )
    {
	return( ( T )( sqrt( ( double )( VectorDotProduct< T >( v, v, n ) ) ) ) );

    }

// -----------------------------------------------------------------------------
    /** Calculates a normal vector, usign the L2 norm.
     *
     *  @param v Result.
     *  @param v_o Original vector.
     *  @param n Cardinality.
     */
    template< class T >
    inline
    void VectorNormalize( T* v, T* v_o, uint32_t n )
    {
	uint32_t i;
	T norm = VectorNorm< T >( v_o, n );

	norm = ( norm == ( T )0 )? ( T )1: norm;
	for( i = 0; i < n; v[ i ] = v_o[ i ] / norm, i++ );

    }

// -----------------------------------------------------------------------------
//@}
/// Functions for matrices.
//@{
// -----------------------------------------------------------------------------
    /** Stream out function for matrices.
     *
     *  @param o Output stream.
     *  @param ma Matrix.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixPrint( std::ostream& o, T** ma, uint32_t n, uint32_t m )
    {
	uint32_t i, j;

	o << n << " x " << m << endl;
	for( j = 0; j < m; j++ ) {

	    for( i = 0; i < n; o << ma[ i ][ j ] << " ", i++ );
	    o << endl;

	} // rof

    }

// -----------------------------------------------------------------------------
    /** Allocates memory for a matrix.
     *
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    T** MatrixAllocateMemory( uint32_t n, uint32_t m )
    {
	uint32_t i;
	T** ma = new T*[ n ];

	for( i = 0; i < n; i++ ) {

	    ma[ i ] = new T[ m ];
	    memset( ma[ i ], 0, sizeof( T ) * m );

	} // rof
	return( ma );

    }

// -----------------------------------------------------------------------------
    /** Copies.
     *
     *  @param ma Target matrix.
     *  @param ma_o Source matrix.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixAssignMemory( T** ma, T** ma_o, uint32_t n, uint32_t m )
    {
	uint32_t i;

	for( i = 0; i < n; i++ )
	    memcpy( ma[ i ], ma_o[ i ], sizeof( T ) * m );

    }

// -----------------------------------------------------------------------------
    /** Allocates & copies.
     *
     *  @param ma Matrix.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    T** MatrixCopyMemory( T** ma, uint32_t n, uint32_t m )
    {
	T** n_ma = MatrixAllocateMemory< T >( n, m );
	MatrixAssignMemory< T >( n_ma, ma, n, m );
	return( n_ma );
	
    }
    
// -----------------------------------------------------------------------------
    /** Scalar assignation.
     *
     *  @param ma Matrix.
     *  @param s Scalar.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixAssignScalar( T** ma, T s, uint32_t n, uint32_t m )
    {
	uint32_t i, j;

	for( i = 0; i < n; i++ )
	    for( j = 0; j < m; j++ )
		ma[ i ][ j ] = s;

    }

// -----------------------------------------------------------------------------
    /** Converts a matrix in a vector.
     *
     *  @param v Vector.
     *  @param ma Matrix.
     *  @param n Columns.
     *  @param m Rows.
     *  @param col Convert to a column vector?
     */
    template< class T >
    inline
    void MatrixVectorCast( T* v, T** ma, uint32_t n, uint32_t m, bool col )
    {
	uint32_t i;

	for( i = 0; i < ( ( col )? m: n ); i++ )
	    v[ i ] = ma[ ( col )? n: i ][ ( col )? i: m ];

    }

// -----------------------------------------------------------------------------
    /** Frees matrix memory.
     *
     *  @param ma Matrix.
     *  @param n Columns.
     */
    template< class T >
    inline
    void MatrixFreeMemory( T** ma, uint32_t n )
    {
	uint32_t i;

	if( ma ) {

	    for( i = 0; i < n; i++ )
		if( ma[ i ] ) delete [] ma[ i ];
	    delete [] ma;

	} // fi

    }

// -----------------------------------------------------------------------------
    /** Performs a matricial addition.
     *
     *  @param ma Result.
     *  @param ma_l Left operand.
     *  @param ma_r Right operand.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixAdd( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
    {
	uint32_t i, j;

	for( i = 0; i < n; i++ )
	    for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] + ma_r[ i ][ j ], j++ );
	
    }

// -----------------------------------------------------------------------------
    /** Performs a matricial substraction.
     *
     *  @param ma Result.
     *  @param ma_l Left operand.
     *  @param ma_r Right operand.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixSubtract( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m )
    {
	uint32_t i, j;

	for( i = 0; i < n; i++ )
	    for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] - ma_r[ i ][ j ], j++ );

    }

// -----------------------------------------------------------------------------
    /** Performs a matricial product.
     *
     *  @param ma Result.
     *  @param ma_l Left operand.
     *  @param ma_r Right operand.
     *  @param n Left columns.
     *  @param m Left rows/right columns.
     *  @param nr Right columns.
     */
    template< class T >
    inline
    void MatrixProduct( T** ma, T** ma_l, T** ma_r, uint32_t n, uint32_t m, uint32_t nr )
    {
	unsigned i, j, k;

	for( i = 0; i < nr; i++ )
	    for( j = 0; j < m; j++ )
		for(
		    k = 0, ma[ i ][ j ] = ( T )0;
		    k < n;
		    ma[ i ][ j ] = ma[ i ][ j ] + ( ma_l[ k ][ j ] * ma_r[ i ][ k ] ), k++
		    );

    }

// -----------------------------------------------------------------------------
    /** Performs a scalar-matrix product.
     *
     *  @param ma Result.
     *  @param ma_l Left operand.
     *  @param s Scalar.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixScalarProduct( T** ma, T** ma_l, T s, uint32_t n, uint32_t m )
    {
	uint32_t i, j;

	for( i = 0; i < n; i++ )
	    for( j = 0; j < m; ma[ i ][ j ] = ma_l[ i ][ j ] * s, j++ );

    }

// -----------------------------------------------------------------------------
    /** Gets the matrix cofactor.
     *
     *  @param ma Result.
     *  @param ma_o Matrix.
     *  @param i Column index.
     *  @param j Row index.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixCofactor( T** ma, T** ma_o, uint32_t i, uint32_t j, uint32_t n, uint32_t m )
    {
	uint32_t k, l;
	
	for( k = 0; k < i; k++ ) {
	    
	    for( l = 0;     l < j; l++ ) ma[ k ][ l ]     = ma_o[ k ][ l ];
	    for( l = j + 1; l < m; l++ ) ma[ k ][ l - 1 ] = ma_o[ k ][ l ];
	    
	} // rof
	
	for( k = i + 1; k < n; k++ ) {
	    
	    for( l = 0;     l < j; l++ ) ma[ k - 1 ][ l ]     = ma_o[ k ][ l ];
	    for( l = j + 1; l < m; l++ ) ma[ k - 1 ][ l - 1 ] = ma_o[ k ][ l ];
	    
	} // rof
	
    }
    
// -----------------------------------------------------------------------------
    /** Gets the matrix determinant (square matrices only).
     *
     *  @param ma Matrix.
     *  @param n Dimension.
     */
    template< class T >
    inline
    T MatrixDeterminant( T** ma, uint32_t n )
    {
	uint32_t k;
	T** tmp = NULL;
	T ret = ( T )0, c;
	
	//  All these cases are for speed-up calculations.
	if( n == 1 ) {

	    ret = ma[ 0 ][ 0 ];

	} else if( n == 2 ) {

	    ret =
		( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] ) -
		( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] );

	} else if( n == 3 ) {

	    ret =
		( ma[ 0 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 2 ][ 2 ] ) -
		( ma[ 0 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 1 ][ 2 ] ) -
		( ma[ 1 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 2 ][ 2 ] ) +
		( ma[ 1 ][ 0 ] * ma[ 2 ][ 1 ] * ma[ 0 ][ 2 ] ) +
		( ma[ 2 ][ 0 ] * ma[ 0 ][ 1 ] * ma[ 1 ][ 2 ] ) -
		( ma[ 2 ][ 0 ] * ma[ 1 ][ 1 ] * ma[ 0 ][ 2 ] );

	} else {
	    
	    tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
	    for( k = 0, c = ( T )1, ret = ( T )0; k < n; k++ ) {

		MatrixCofactor< T >( tmp, ma, k, 0, n, n );
		ret = ret + ( c * ( ma[ k ][ 0 ] * MatrixDeterminant< T >( tmp, n - 1 ) ) );
		c = c * ( T )( 0 - 1 );

	    } // rof
	    MatrixFreeMemory< T >( tmp, n - 1 );

	} // fi
	return( ret );

    }

// -----------------------------------------------------------------------------
    /** Calculates the inverse using the adjoint method (only square matrices).
     *
     *  @param ma Result.
     *  @param ma_o Matrix.
     *  @param n Dimension.
     */
    template< class T >
    inline
    void MatrixInverseAdjoint( T** ma, T** ma_o, uint32_t n )
    {
	uint32_t i, j;
	T** tmp;
	T c;

	tmp = MatrixAllocateMemory< T >( n - 1, n - 1 );
	for( i = 0; i < n; i++ ) {

	    for( j = 0; j < n; j++ ) {

		c = ( ( i + j ) % 2 == 0 )? ( T )1: ( T )( 0 - 1 );
		MatrixCofactor< T >( tmp, ma_o, i, j, n, n );
		ma[ j ][ i ] = c * MatrixDeterminant< T >( tmp, n - 1 );

	    } // rof

	} // rof
	MatrixFreeMemory< T >( tmp, n - 1 );

	c = ( T )1 / MatrixDeterminant< T >( ma_o, n );
	MatrixScalarProduct< T >( ma, ma, c, n, n );

    }

// -----------------------------------------------------------------------------
    /** Calculates the inverse using the gauss method (only square matrices).
     *
     *  @param ma Result.
     *  @param ma_o Matrix.
     *  @param n Dimension.
     *  @warning Adapted from a java-implementation: http://www.nauticom.net/www/jdtaft/JavaMatrix.htm
     */
    template< class T >
    inline
    void MatrixInverseGauss( T** ma, T** ma_o, uint32_t n )
    {
	uint32_t i, j, k, n2 = 2 * n;
	T** ma_a = MatrixAllocateMemory< T >( n2, n );
	T a, b;

	//  Augmented matrix initialization
	for( i = 0; i < n; i++ )
	    for( j = 0; j < n; ma_a[ i ][ j ] = ma_o[ i ][ j ], j++ );
	for( i = n; i < n2; i++ )
	    for( j = 0; j < n; ma_a[ i ][ j ] = ( i - n == j )? ( T )1: ( T )0, j++ );

	//  Reduction
	for( j = 0; j < n; j++ ) {

	    a = ma_a[ j ][ j ];
	    if( a != 0 ) for( i = 0; i < n2; ma_a[ i ][ j ] = ma_a[ i ][ j ] / a, i++ );
	    for( k = 0; k < n; k++ ) {

		if( ( k - j ) != 0 ) {

		    b = ma_a[ j ][ k ];
		    for(
			i = 0;
			i < n2;
			ma_a[ i ][ k ] = ma_a[ i ][ k ] - ( b * ma_a[ i ][ j ] ), i++
			);

		} // fi

	    } // rof

	} // rof

	//  Result assignation
	MatrixAssignMemory< T >( ma, ma_a, n, n );
	MatrixFreeMemory< T >( ma_a, n2 );

    }

// -----------------------------------------------------------------------------
    /** Gets the transpose matrix.
     *
     *  @param ma Matrix result.
     *  @param ma_o Matrix.
     *  @param n Columns.
     *  @param m Rows.
     */
    template< class T >
    inline
    void MatrixTranspose( T** ma, T** ma_o, uint32_t n, uint32_t m )
    {
	uint32_t i, j;

	for( i = 0; i < m; i++ )
	    for( j = 0; j < n; ma[ i ][ j ] = ma_o[ j ][ i ], j++ );
	
    }

// -----------------------------------------------------------------------------
    /** Load a square matrix with the identity.
     *
     *  @param ma Matrix.
     *  @param n Dimension.
     */
    template< class T >
    inline
    void MatrixLoadIdentity( T** ma, uint32_t n )
    {
	uint32_t i, j;

	for( i = 0; i < n; i++ )
	    for( j = 0; j < n; ma[ i ][ j ] = ( i == j )? ( T )1: ( T )0, j++ );

    }

}
//@}

#endif // GTMLIB__MATH__VMFUNCS__HXX

// EOF - vmfuncs.h