#ifndef MATHTOOLS_H__
#define MATHTOOLS_H__


#include "itkArray.h"
#include "itkArray2D.h"

#include <vnl/algo/vnl_svd.h>

template<class T>
itk::Array2D<T> 
HadamardProduct(const itk::Array2D<T> &A, const itk::Array2D<T> &B)
{
	if ( A.rows() != B.rows() || A.cols() != B.cols() )
	{
		std::cout << "Invalid matrix size detected..." << std::endl;
	}

	itk::Array2D<T>  Result(A.rows(),A.cols());
	for ( unsigned int r = 0; r < A.rows(); r++ )
	{
		for ( unsigned int c = 0; c < A.cols(); c++ )
		{
			Result[r][c] = A[r][c] * B[r][c];
		}
	}
	return Result;
}
	

template<class T>
itk::Array2D<T> 
Diag(const itk::Array2D<T> &A)
{
	if ( A.rows() != A.cols() )
		std::cout << "Invalid matrix dimension detected..." << std::endl;

	itk::Array2D<T>  Result(A.rows(), A.rows());
	Result.fill(0);
	for ( unsigned int i = 0; i < A.rows(); i++ )
		Result[i][i] = A[i][i];

	return Result;
}
	

template<class T>
T 
Trace(const vnl_diag_matrix<T> &A)
{
	if ( A.rows() != A.cols() )
	std::cout << "Invalid matrix dimension detected..." << std::endl;

	T result = 0;

	for ( unsigned int i = 0; i < A.rows(); i++ )
		result += A[i];

	return result;
}

#endif // MATHTOOLS_H__