/* -*- C++ -*- ------------------------------------------------------------
 
Copyright (c) 2007 Jesse Anders and Demian Nave http://cmldev.net/

The Configurable Math Library (CML) is distributed under the terms of the
Boost Software License, v1.0 (see cml/LICENSE for details).

 *-----------------------------------------------------------------------*/
/** @file
 *  @brief
 */

#ifndef matrix_misc_h
#define matrix_misc_h

#include <cml/mathlib/checking.h>

/* Miscellaneous matrix functions. */

namespace cml {

/** Set a (possibly non-square) matrix to represent an identity transform */
template < typename E, class A, class B, class L > void
identity_transform(matrix<E,A,B,L>& m)
{
    typedef matrix<E,A,B,L> matrix_type;
    typedef typename matrix_type::value_type value_type;

    for (size_t i = 0; i < m.rows(); ++i) {
        for (size_t j = 0; j < m.cols(); ++j) {
            m(i,j) = value_type((i == j) ? 1 : 0);
        }
    }
}

/** Trace of a square matrix */
template < class MatT > typename MatT::value_type
trace(const MatT& m)
{
    typedef typename MatT::value_type value_type;
    
    /* Checking */
    detail::CheckMatSquare(m);

    value_type t = value_type(0);
    for (size_t i = 0; i < m.rows(); ++i) {
        t += m(i,i);
    }
    return t;
}

/** Trace of the upper-left 3x3 part of a matrix */
template < class MatT > typename MatT::value_type
trace_3x3(const MatT& m)
{
    /* Checking */
    detail::CheckMatMin3x3(m);
    
    return m(0,0) + m(1,1) + m(2,2);
}

/** Trace of the upper-left 2x2 part of a matrix */
template < class MatT > typename MatT::value_type
trace_2x2(const MatT& m)
{
    /* Checking */
    detail::CheckMatMin2x2(m);
    
    return m(0,0) + m(1,1);
}

/** 3D skew-symmetric matrix */
template < typename E, class A, class B, class L, class VecT > void
matrix_skew_symmetric(matrix<E,A,B,L>& m, const VecT& v)
{
    /* Checking */
    detail::CheckMatMin3x3(m);
    detail::CheckVec3(v);

    m.zero();

    m.set_basis_element(1,2, v[0]);
    m.set_basis_element(2,1,-v[0]);
    m.set_basis_element(2,0, v[1]);
    m.set_basis_element(0,2,-v[1]);
    m.set_basis_element(0,1, v[2]);
    m.set_basis_element(1,0,-v[2]);
}

/** 2D skew-symmetric matrix */
template < typename E, class A, class B, class L > void
matrix_skew_symmetric_2D(matrix<E,A,B,L>& m, E s)
{
    /* Checking */
    detail::CheckMatMin2x2(m);

    m.zero();

    m.set_basis_element(0,1, s);
    m.set_basis_element(1,0,-s);
}

/* @todo: Clean this up, and implement SRT as well */

/** Invert a matrix consisting of a 3D rotation and translation */
template < typename E, class A, class B, class L > void
matrix_invert_RT_only(matrix<E,A,B,L>& m)
{
    typedef vector< E, fixed<3> > vector_type;

    vector_type x, y, z;
    matrix_get_basis_vectors(m,x,y,z);
    matrix_set_transposed_basis_vectors(m,x,y,z);
    
    vector_type p = matrix_get_translation(m);
    matrix_set_translation(m,-dot(p,x),-dot(p,y),-dot(p,z));
}

/** Invert a matrix consisting of a 2D rotation and ranslation */
template < typename E, class A, class B, class L > void
matrix_invert_RT_only_2D(matrix<E,A,B,L>& m)
{
    typedef vector< E, fixed<2> > vector_type;

    vector_type x, y;
    matrix_get_basis_vectors_2D(m,x,y);
    matrix_set_transposed_basis_vectors_2D(m,x,y);
    
    vector_type p = matrix_get_translation_2D(m);
    matrix_set_translation_2D(m,-dot(p,x),-dot(p,y));
}

} // namespace cml

#endif