// @(#)root/smatrix:$Id$
// Authors: T. Glebe, L. Moneta    2005

#ifndef ROOT_Math_SMatrix_icc
#define ROOT_Math_SMatrix_icc
// ********************************************************************
//
// source:
//
// type:      source code
//
// created:   21. Mar 2001
//
// author:    Thorsten Glebe
//            HERA-B Collaboration
//            Max-Planck-Institut fuer Kernphysik
//            Saupfercheckweg 1
//            69117 Heidelberg
//            Germany
//            E-mail: T.Glebe@mpi-hd.mpg.de
//
// Description: SMatrix implementation file
//
// changes:
// 21 Mar 2001 (TG) creation
// 26 Mar 2001 (TG) place_in_row(), place_in_col() added
// 03 Apr 2001 (TG) invert() added
// 07 Apr 2001 (TG) CTOR from SVertex (dyadic product) added
// 09 Apr 2001 (TG) CTOR from array added
// 25 Mai 2001 (TG) row(), col() added
// 11 Jul 2001 (TG) added #include Functions.hh
// 11 Jan 2002 (TG) added operator==(), operator!=()
// 14 Jan 2002 (TG) added more operator==(), operator!=(), operator>(), operator<()
//
// ********************************************************************

#ifndef ROOT_Math_SMatrix
#error "Do not use SMatrix.icc directly. #include \"Math/SMatrix.h\" instead."
#endif // ROOT_Math_SMatrix

#include <iostream>
#include <iomanip>
#include <assert.h>
//#ifndef ROOT_Math_Dsinv
//#include "Math/Dsinv.h"
//#endif
//#include "Math/Dsinv_array.h"
//#include "Math/Dsfact.h"

#include "Math/Dfact.h"
#include "Math/Dinv.h"
#include "Math/Functions.h"
#include "Math/HelperOps.h"
#include "Math/StaticCheck.h"







namespace ROOT {

namespace Math {



//==============================================================================
// Constructors
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>::SMatrix() {
   // operator=(0);   // if operator=(T ) is defined
   for(unsigned int i=0; i<R::kSize; ++i) fRep.Array()[i] = 0;
}

//identity
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>::SMatrix( SMatrixIdentity ) {
   for(unsigned int i=0; i<R::kSize; ++i)
      fRep.Array()[i] = 0;
   if (D1 <= D2) {
      for(unsigned int i=0; i<D1; ++i)
         fRep[i*D2+i] = 1;
   }
   else {
      for(unsigned int i=0; i<D2; ++i)
         fRep[i*D2+i] = 1;
   }
}

template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>::SMatrix(const SMatrix<T,D1,D2,R>& rhs) {
   fRep = rhs.fRep;
}


template <class T, unsigned int D1, unsigned int D2, class R>
template <class R2>
SMatrix<T,D1,D2,R>::SMatrix(const SMatrix<T,D1,D2,R2>& rhs) {
   operator=(rhs);
}


template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
SMatrix<T,D1,D2,R>::SMatrix(const Expr<A,T,D1,D2,R2>& rhs) {
   operator=(rhs);
}


//=============================================================================
// New Constructors from STL interfaces
//=============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <class InputIterator>
SMatrix<T,D1,D2,R>::SMatrix(InputIterator ibegin, InputIterator iend, bool triang, bool lower) {
   // assume iterator size == matrix size
   for(unsigned int i=0; i<R::kSize; ++i) fRep.Array()[i] = 0;
   AssignItr<T,D1,D2,R>::Evaluate(*this,ibegin,iend,triang,lower);
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class InputIterator>
SMatrix<T,D1,D2,R>::SMatrix(InputIterator ibegin, unsigned int size, bool triang, bool lower) {
   // assume iterator size <= matrix size (no check needed in AssignItr)
   assert( size <= R::kSize);
   for(unsigned int i=0; i<R::kSize; ++i) fRep.Array()[i] = 0;
   AssignItr<T,D1,D2,R>::Evaluate(*this,ibegin,ibegin+size,triang,lower,false);
}


//==============================================================================
// Assignment and operator= for scalar types for matrices of size 1
// compiles only for matrices of size 1
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>::SMatrix(const T&  rhs) {
   STATIC_CHECK( kSize == 1,SMatrix_NOT_of_size_1 );
   fRep[0] = rhs;
}

template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator=(const T&  rhs) {
   STATIC_CHECK( kSize == 1,SMatrix_NOT_of_size_1 );
   fRep[0] = rhs;
   return *this;
}

//=============================================================================
//=============================================================================

template <class T, unsigned int D1, unsigned int D2, class R>
template <class M>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator=(const M&  rhs) {
   fRep = rhs.fRep;
   return *this;
}

template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator=(const SMatrix<T,D1,D2,R>&  rhs) {
   fRep = rhs.fRep;
   return *this;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator=(const Expr<A,T,D1,D2,R2>&  rhs) {

   Assign<T,D1,D2,A,R,R2>::Evaluate(*this, rhs);
   return *this;
}

//=============================================================================
// assign from an identity
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator= ( SMatrixIdentity ) {
   for(unsigned int i=0; i<R::kSize; ++i)
      fRep.Array()[i] = 0;
   if (D1 <= D2) {
      for(unsigned int i=0; i<D1; ++i)
         fRep[i*D2+i] = 1;
   }
   else {
      for(unsigned int i=0; i<D2; ++i)
         fRep[i*D2+i] = 1;
   }
   return *this;
}



//=============================================================================
// operator+=
//=============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator+=(const T&  rhs) {
   // self-addition with a scalar value
   for(unsigned int i=0; i<R::kSize; ++i) {
      fRep.Array()[i] += rhs;
   }
   return *this;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator+=(const SMatrix<T,D1,D2,R2>&  rhs) {
   // self-addition with another matrix (any representation)
   // use operator+= of the representation object
   fRep += rhs.fRep;
   return *this;
}


template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator+=(const Expr<A,T,D1,D2,R2>&  rhs) {
   // self-addition with an expression
   PlusEquals<T,D1,D2,A,R,R2>::Evaluate(*this, rhs);
   return *this;
}


//==============================================================================
// operator-=
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator-=(const T&  rhs) {
   // self-subtraction with a scalar value
   for(unsigned int i=0; i<R::kSize; ++i) {
      fRep.Array()[i] -= rhs;
   }
   return *this;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator-=(const SMatrix<T,D1,D2,R2>&  rhs) {
   // self-subtraction with another matrix (any representation)
   // use operator-= of the representation object
   fRep -= rhs.fRep;
   return *this;
}


template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator-=(const Expr<A,T,D1,D2,R2>&  rhs) {
   // self-subtraction with an expression
   MinusEquals<T,D1,D2,A,R,R2>::Evaluate(*this, rhs);
   return *this;
}

//==============================================================================
// operator*=
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator*=(const T&  rhs) {
   // case of multiplication with a scalar
   for(unsigned int i=0; i<R::kSize; ++i) {
      fRep.Array()[i] *= rhs;
   }
   return  *this;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator*=(const SMatrix<T,D1,D2,R2>&  rhs) {
   // self-multiplication with another matrix (will work only for square matrices)
   // a temporary is needed and will be created automatically to store intermediate result
   return operator=(*this * rhs);
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator*=(const Expr<A,T,D1,D2,R2>&  rhs) {
   // self-multiplication with an expression (will work only for square matrices)
   // a temporary is needed and will be created automatically to store intermediate result
   return operator=(*this * rhs);
}


//==============================================================================
// operator/= (only for scalar values)
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::operator/=(const T&  rhs) {
   // division with a scalar
   for(unsigned int i=0; i<R::kSize; ++i) {
      fRep.Array()[i] /= rhs;
   }
   return  *this;
}

//==============================================================================
// operator== (element wise comparison)
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
bool SMatrix<T,D1,D2,R>::operator==(const T& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<R::kSize; ++i) {
      rc = rc && (fRep.Array()[i] == rhs);
   }
   return rc;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class R2>
bool SMatrix<T,D1,D2,R>::operator==(const SMatrix<T,D1,D2,R2>& rhs) const {
   return fRep == rhs.fRep;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
bool SMatrix<T,D1,D2,R>::operator==(const Expr<A,T,D1,D2,R2>& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<D1*D2; ++i) {
      rc = rc && (fRep[i] == rhs.apply(i));
   }
   return rc;
}

//==============================================================================
// operator!= (element wise comparison)
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
inline bool SMatrix<T,D1,D2,R>::operator!=(const T& rhs) const {
   return !operator==(rhs);
}

template <class T, unsigned int D1, unsigned int D2, class R>
inline bool SMatrix<T,D1,D2,R>::operator!=(const SMatrix<T,D1,D2,R>& rhs) const {
   return !operator==(rhs);
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
inline bool SMatrix<T,D1,D2,R>::operator!=(const Expr<A,T,D1,D2,R2>& rhs) const {
   return !operator==(rhs);
}


//==============================================================================
// operator> (element wise comparison)
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
bool SMatrix<T,D1,D2,R>::operator>(const T& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<D1*D2; ++i) {
      rc = rc && (fRep[i] > rhs);
   }
   return rc;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class R2>
bool SMatrix<T,D1,D2,R>::operator>(const SMatrix<T,D1,D2,R2>& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<D1*D2; ++i) {
      rc = rc && (fRep[i] > rhs.fRep[i]);
   }
   return rc;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
bool SMatrix<T,D1,D2,R>::operator>(const Expr<A,T,D1,D2,R2>& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<D1*D2; ++i) {
      rc = rc && (fRep[i] > rhs.apply(i));
   }
   return rc;
}

//==============================================================================
// operator< (element wise comparison)
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
bool SMatrix<T,D1,D2,R>::operator<(const T& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<D1*D2; ++i) {
      rc = rc && (fRep[i] < rhs);
   }
   return rc;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class R2>
bool SMatrix<T,D1,D2,R>::operator<(const SMatrix<T,D1,D2,R2>& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<D1*D2; ++i) {
      rc = rc && (fRep[i] < rhs.fRep[i]);
   }
   return rc;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, class R2>
bool SMatrix<T,D1,D2,R>::operator<(const Expr<A,T,D1,D2,R2>& rhs) const {
   bool rc = true;
   for(unsigned int i=0; i<D1*D2; ++i) {
      rc = rc && (fRep[i] < rhs.apply(i));
   }
   return rc;
}


//==============================================================================
// invert
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
inline bool SMatrix<T,D1,D2,R>::Invert() {
   STATIC_CHECK( D1 == D2,SMatrix_not_square);
   return Inverter<D1,D2>::Dinv((*this).fRep);
}

// invert returning a new matrix
template <class T, unsigned int D1, unsigned int D2, class R>
inline SMatrix<T,D1,D2,R> SMatrix<T,D1,D2,R>::Inverse(int & ifail) const {
   SMatrix<T,D1,D2,R> tmp(*this);
   bool ok = tmp.Invert();
   ifail = 0;
   if (!ok) ifail = 1;
   return tmp;
}

// fast inversion
template <class T, unsigned int D1, unsigned int D2, class R>
inline bool SMatrix<T,D1,D2,R>::InvertFast() {
   STATIC_CHECK( D1 == D2,SMatrix_not_square);
   return FastInverter<D1,D2>::Dinv((*this).fRep);
}

// fast inversion returning a new matrix
template <class T, unsigned int D1, unsigned int D2, class R>
inline SMatrix<T,D1,D2,R> SMatrix<T,D1,D2,R>::InverseFast(int & ifail) const {
   SMatrix<T,D1,D2,R> tmp(*this);
   bool ok = tmp.InvertFast();
   ifail = 0;
   if (!ok) ifail = 1;
   return tmp;
}

// Choleski inversion for symmetric and positive defined matrices
template <class T, unsigned int D1, unsigned int D2, class R>
inline bool SMatrix<T,D1,D2, R>::InvertChol() {
   STATIC_CHECK( D1 == D2,SMatrix_not_square);
   return CholInverter<D1>::Dinv((*this).fRep);
}

template <class T, unsigned int D1, unsigned int D2, class R>
inline SMatrix<T,D1,D2, R>  SMatrix<T,D1,D2, R>::InverseChol(int &ifail) const {
   SMatrix<T,D1,D2,R > tmp(*this);
   bool ok = tmp.InvertChol();
   ifail = 0;
   if (!ok) ifail = 1;
   return tmp;
}



//==============================================================================
// det
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
inline bool SMatrix<T,D1,D2,R>::Det(T& det) {
   STATIC_CHECK( D1 == D2,SMatrix_not_square);
   //  return Dfact<SMatrix<T,D1,D1>, D1, D1>(*this,det);
   //return Dfact<R, D1, D1>(fRep, det);
   return Determinant<D1,D2>::Dfact(fRep, det);
}
template <class T, unsigned int D1, unsigned int D2, class R>
inline bool SMatrix<T,D1,D2,R>::Det2(T& det) const {
   SMatrix<T,D1,D2,R> tmp(*this);
   return tmp.Det(det);
}


//==============================================================================
// place_in_row
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <unsigned int D>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::Place_in_row(const SVector<T,D>& rhs,
                                                     unsigned int row,
                                                     unsigned int col) {

   assert(col+D <= D2);

   for(unsigned int i=row*D2+col, j=0; j<D; ++i, ++j) {
      fRep[i] = rhs.apply(j);
   }
   return *this;
}

//==============================================================================
// place_in_row
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, unsigned int D>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::Place_in_row(const VecExpr<A,T,D>& rhs,
                                                     unsigned int row,
                                                     unsigned int col) {

   assert(col+D <= D2);

   for(unsigned int i=row*D2+col, j=0; j<D; ++i, ++j) {
      fRep[i] = rhs.apply(j);
   }
   return *this;
}

//==============================================================================
// place_in_col
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <unsigned int D>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::Place_in_col(const SVector<T,D>& rhs,
                                                     unsigned int row,
                                                     unsigned int col) {

   assert(row+D <= D1);

   for(unsigned int i=row*D2+col, j=0; j<D; i+=D2, ++j) {
      fRep[i] = rhs.apply(j);
   }
   return *this;
}

//==============================================================================
// place_in_col
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, unsigned int D>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::Place_in_col(const VecExpr<A,T,D>& rhs,
                                                     unsigned int row,
                                                     unsigned int col) {

   assert(row+D <= D1);

   for(unsigned int i=row*D2+col, j=0; j<D; i+=D2, ++j) {
      fRep[i] = rhs.apply(j);
   }
   return *this;
}

//==============================================================================
// place_at
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <unsigned int D3, unsigned int D4, class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::Place_at(const SMatrix<T,D3,D4,R2>& rhs,
                                                 unsigned int row,
                                                 unsigned int col) {
   PlaceMatrix<T,D1,D2,D3,D4,R,R2>::Evaluate(*this,rhs,row,col);
   return *this;
}

//==============================================================================
// place_at
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <class A, unsigned int D3, unsigned int D4, class R2>
SMatrix<T,D1,D2,R>& SMatrix<T,D1,D2,R>::Place_at(const Expr<A,T,D3,D4,R2>& rhs,
                                                 unsigned int row,
                                                 unsigned int col) {
   PlaceExpr<T,D1,D2,D3,D4,A,R,R2>::Evaluate(*this,rhs,row,col);
   return *this;
}

//==============================================================================
// row
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SVector<T,D2> SMatrix<T,D1,D2,R>::Row(unsigned int therow) const {

   const unsigned int offset = therow*D2;

   /*static*/  SVector<T,D2> tmp;
   for(unsigned int i=0; i<D2; ++i) {
      tmp[i] = fRep[offset+i];
   }
   return tmp;
}

//==============================================================================
// col
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
SVector<T,D1> SMatrix<T,D1,D2,R>::Col(unsigned int thecol) const {

   /*static */ SVector<T,D1> tmp;
   for(unsigned int i=0; i<D1; ++i) {
      tmp[i] = fRep[thecol+i*D2];
   }
   return tmp;
}

//==============================================================================
// print
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
std::ostream& SMatrix<T,D1,D2,R>::Print(std::ostream& os) const {
   const std::ios_base::fmtflags prevFmt = os.setf(std::ios::right,std::ios::adjustfield);
   //  os.setf(ios::fixed);

   os << "[ ";
   for (unsigned int i=0; i < D1; ++i) {
      for (unsigned int j=0; j < D2; ++j) {
         os << std::setw(12) << fRep[i*D2+j];
         if ((!((j+1)%12)) && (j < D2-1))
            os << std::endl << "         ...";
      }
      if (i != D1 - 1)
         os << std::endl  << "  ";
   }
   os << " ]";

   if (prevFmt != os.flags() ) os.setf(prevFmt, std::ios::adjustfield);
   return os;
}

//==============================================================================
// Access functions
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
inline T SMatrix<T,D1,D2,R>::apply(unsigned int i) const { return fRep[i]; }

template <class T, unsigned int D1, unsigned int D2, class R>
inline const T* SMatrix<T,D1,D2,R>::Array() const { return fRep.Array(); }

template <class T, unsigned int D1, unsigned int D2, class R>
inline T* SMatrix<T,D1,D2,R>::Array() { return fRep.Array(); }

//==============================================================================
// Operators
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
inline const T& SMatrix<T,D1,D2,R>::operator()(unsigned int i, unsigned int j) const {
   return fRep(i,j);
}

template <class T, unsigned int D1, unsigned int D2, class R>
inline T& SMatrix<T,D1,D2,R>::operator()(unsigned int i, unsigned int j) {
   return fRep(i,j);
}


//==============================================================================
// Element access with At()
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
inline const T& SMatrix<T,D1,D2,R>::At(unsigned int i, unsigned int j) const {
   assert(i < D1);
   assert(j < D2);
   return fRep(i,j);
}

template <class T, unsigned int D1, unsigned int D2, class R>
inline T& SMatrix<T,D1,D2,R>::At(unsigned int i, unsigned int j) {
   assert(i < D1);
   assert(j < D2);
   return fRep(i,j);
}

//==============================================================================
// STL interface
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
inline T * SMatrix<T,D1,D2,R>::begin() {
   return fRep.Array();
}

template <class T, unsigned int D1, unsigned int D2, class R>
inline T * SMatrix<T,D1,D2,R>::end() {
   return fRep.Array() + R::kSize;
}

template <class T, unsigned int D1, unsigned int D2, class R>
inline const T * SMatrix<T,D1,D2,R>::begin() const {
   return fRep.Array();
}

template <class T, unsigned int D1, unsigned int D2, class R>
inline const T * SMatrix<T,D1,D2,R>::end() const {
   return fRep.Array() + R::kSize;
}


template <class T, unsigned int D1, unsigned int D2, class R>
template <class InputIterator>
void SMatrix<T,D1,D2,R>::SetElements(InputIterator ibegin, InputIterator iend, bool triang, bool lower) {
   // assume iterator size == matrix size when filling full matrix
   AssignItr<T,D1,D2,R>::Evaluate(*this,ibegin,iend,triang,lower);
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class InputIterator>
void SMatrix<T,D1,D2,R>::SetElements(InputIterator ibegin, unsigned int size, bool triang, bool lower) {
   // assume iterator size <= matrix size  (no check to be done in AssignItr)
   assert( size <= R::kSize);
   AssignItr<T,D1,D2,R>::Evaluate(*this,ibegin,ibegin+size,triang,lower,false);
}



//==============================================================================
// SubMatrices and slices of columns and rows
//==============================================================================
template <class T, unsigned int D1, unsigned int D2, class R>
template <class SubVector>
SubVector SMatrix<T,D1,D2,R>::SubRow(unsigned int therow, unsigned int col0 ) const {

   const unsigned int offset = therow*D2 + col0;

   STATIC_CHECK( SubVector::kSize <= D2,SVector_dimension_too_small);
   assert(col0 + SubVector::kSize <= D2);

   SubVector tmp;
   for(unsigned int i=0; i<SubVector::kSize; ++i) {
      tmp[i] = fRep[offset+i];
   }
   return tmp;
}

template <class T, unsigned int D1, unsigned int D2, class R>
template <class SubVector>
SubVector SMatrix<T,D1,D2,R>::SubCol(unsigned int thecol, unsigned int row0 ) const {

   const unsigned int offset = thecol + row0*D1;

   STATIC_CHECK( SubVector::kSize <= D1,SVector_dimension_too_small);
   assert(row0 + SubVector::kSize <= D1);

   SubVector tmp;
   for(unsigned int i=0; i<SubVector::kSize; ++i) {
      tmp[i] = fRep[offset+i*D1];
   }
   return tmp;
}

// submatrix
template <class T, unsigned int D1, unsigned int D2, class R>
template <class SubMatrix>
SubMatrix SMatrix<T,D1,D2,R>::Sub(unsigned int row0, unsigned int col0) const {

   SubMatrix tmp;
   RetrieveMatrix<T,SubMatrix::kRows, SubMatrix::kCols, D1, D2, typename SubMatrix::rep_type, R>::Evaluate
      (tmp,*this,row0,col0);
   return tmp;
}

//diagonal
template <class T, unsigned int D1, unsigned int D2, class R>
SVector<T,D1> SMatrix<T,D1,D2,R>::Diagonal( ) const {

   // only for squared matrices
   STATIC_CHECK( D1 == D2,SMatrix_NOT_square );

   SVector<T,D1> tmp;
   for(unsigned int i=0; i<D1; ++i) {
      tmp[i] = fRep[ i*D2 + i];
   }
   return tmp;
}

//set diagonal
template <class T, unsigned int D1, unsigned int D2, class R>
template <class Vector>
void SMatrix<T,D1,D2,R>::SetDiagonal( const Vector & v) {

   // check size that size of vector is correct
   STATIC_CHECK( ( ( D1 <= D2) && Vector::kSize == D1 ) ||
                 ( ( D2 < D1 ) && Vector::kSize == D2 ), SVector_size_NOT_correct );


   for(unsigned int i=0; i<Vector::kSize; ++i) {
      fRep[ i*D2 + i] = v[i];
   }
}

// matrix trace
template <class T, unsigned int D1, unsigned int D2, class R>
T SMatrix<T,D1,D2,R>::Trace( ) const {
   unsigned int diagSize = D1;
   if (D2 < D1) diagSize = D2;
   T trace = 0;
   for(unsigned int i=0; i< diagSize; ++i) {
      trace += fRep[ i*D2 + i] ;
   }
   return trace;
}

//upper block
template <class T, unsigned int D1, unsigned int D2, class R>
#ifndef UNSUPPORTED_TEMPLATE_EXPRESSION
SVector<T, D1 * (D2 +1)/2 >  SMatrix<T,D1,D2,R>::UpperBlock( ) const {
#else
template <class SubVector>
SubVector SMatrix<T,D1,D2,R>::UpperBlock( ) const {
#endif
   // only for squared matrices
   STATIC_CHECK( D1 == D2,SMatrix_NOT_square );

#ifndef UNSUPPORTED_TEMPLATE_EXPRESSION
   SVector<T, D1 * (D2 +1)/2 >  tmp;
#else
   // N must be equal D1 *(D1 +1)/2
   STATIC_CHECK( SubVector::kSize == D1*(D1+1)/2,SVector_Wrong_Size );
   SubVector tmp;
#endif

   int k = 0;
   for(unsigned int i=0; i<D1; ++i) {
      for(unsigned int j=i; j<D2; ++j) {
         tmp[k] = fRep[ i*D2 + j];
         k++;
      }
   }
   return tmp;
}

//lower block
template <class T, unsigned int D1, unsigned int D2, class R>
#ifndef UNSUPPORTED_TEMPLATE_EXPRESSION
SVector<T, D1 * (D2 +1)/2 >  SMatrix<T,D1,D2,R>::LowerBlock( ) const {
#else
template <class SubVector>
SubVector SMatrix<T,D1,D2,R>::LowerBlock( ) const {
#endif

   // only for squared matrices
   STATIC_CHECK( D1 == D2,SMatrix_NOT_square );

#ifndef UNSUPPORTED_TEMPLATE_EXPRESSION
   SVector<T, D1 * (D2 +1)/2 >  tmp;
#else
   // N must be equal D1 *(D1 +1)/2
   STATIC_CHECK( SubVector::kSize == D1*(D1+1)/2,SVector_Wrong_Size );
   SubVector tmp;
#endif

   int k = 0;
   for(unsigned int i=0; i<D1; ++i) {
      for(unsigned int j=0; j<=i; ++j) {
         tmp[k] = fRep[ i*D2 + j];
         k++;
      }
   }
   return tmp;
}

/// construct from upper/lower block

//lower block
template <class T, unsigned int D1, unsigned int D2, class R>
#ifndef UNSUPPORTED_TEMPLATE_EXPRESSION
SMatrix<T,D1,D2,R>::SMatrix(const SVector<T, D1*(D2+1)/2 >  & v, bool lower ) {
#else
template <unsigned int N>
SMatrix<T,D1,D2,R>::SMatrix(const SVector<T, N >  & v, bool lower ) {
#endif

   // only for squared matrices
   STATIC_CHECK( D1 == D2,SMatrix_NOT_square );

#ifdef UNSUPPORTED_TEMPLATE_EXPRESSION
   STATIC_CHECK( N == D1*(D1+1)/2,SVector_Wrong_Size );
#endif

   int k = 0;
   if (lower) {
      // case of lower block
      for(unsigned int i=0; i<D1; ++i) {
         for(unsigned int j=0; j<=i; ++j) {
            fRep[ i*D2 + j] = v[k];
            if ( i != j) fRep[ j*D2 + i] = v[k];
            k++;
         }
      }
   } else {
      // case of upper block
      for(unsigned int i=0; i<D1; ++i) {
         for(unsigned int j=i; j<D2; ++j) {
            fRep[ i*D2 + j] = v[k];
            if ( i != j) fRep[ j*D2 + i] = v[k];
            k++;
         }
      }
   }
}


template <class T, unsigned int D1, unsigned int D2, class R>
bool SMatrix<T,D1,D2,R>::IsInUse( const T * p) const {
   return p == fRep.Array();
}




  }  // namespace Math

}  // namespace ROOT



#endif
