/*
 *
 *  February 21, 2000
 *  Incorporated ISOcxx compatibility
 *  - W. E. Brown
 *
 */


#ifdef ISOCXX__ISOCXX
#  include <ISOcxx/ISOcxx.h>
#endif


#include "LinearAlgebra/Matrix.h"
#include "LinearAlgebra/MLD22.h"
#include "LinearAlgebra/MLC22.h"   // Needed only for cloneComplex
#include "LinearAlgebra/MLDGen.h"  // For the arithmetic function table.
#include "ZMtools/pretendToUse.h"    // Needed for pretendToUse()

#ifdef __BORLAND_CPP__
#include <string.h>
#include <iomanip.h>
#else
#include <cstring>
#include <ZMutility/iomanip>
using namespace std;
#endif // __BORLAND_CPP__

// ===
// === Begin MLD22.icc file ==============================
// === This is temporarily put here for diagnostics ======
// ===

/* inline */ MLD* MLD22::EmptyClone() const
{
  return new MLD22();
}

// ??? REMOVE /* inline */ const Type_info& MLD22::typeID()
// ??? REMOVE {
// ??? REMOVE   return typeid( *this );
// ??? REMOVE }

/* inline */ int MLD22::vtblIndex() const
{
  return FPCL_INDEX_D_M22;
}

// ===
// === End MLD22.icc file ================================
// === This is temporarily put here for diagnostics ======
// ===

// ===
// === Declaration function ==============================
// ===

MatrixD& MatrixD::declareM22()
{
  // ??? REMOVE if( typeid( *_pml ) != typeid( MLD22 ) ) {
  // ??? REMOVE   if( --( _pml->_rc ) == 0 ) delete _pml;
  // ??? REMOVE   _pml = new MLD22();
  // ??? REMOVE }
  // ??? REMOVE return *this;
  // For a 2x2 data model, there is no difference from .makeM22()
  return makeM22();
}


MatrixD& MatrixD::makeM22()
{
  static MLD22* pNewMLD;

  if( typeid( *_pml ) != typeid( MLD22 ) ) {
    if( ( _pml->_r == 2 ) && ( _pml->_c == 2 ) ) {
      pNewMLD = new MLD22();

      pNewMLD->_m[0][0] = _pml->value(0,0);
      pNewMLD->_m[0][1] = _pml->value(0,1);
      pNewMLD->_m[1][0] = _pml->value(1,0);
      pNewMLD->_m[1][1] = _pml->value(1,1);

      if( --( _pml->_rc ) == 0 ) delete _pml;
      _pml = pNewMLD;
    }
    else { 
      ZMthrow ( ZMxlaDimensionMismatch(
               "MatrixD::makeM22 "
               "Dimensions are not correct") );
    }
  }
  return *this;
}

// ??? REMOVE MatrixD& MatrixD::redeclareM22( const Float8& initval )
// ??? REMOVE {
// ??? REMOVE   if( --( _pml->_rc ) == 0 ) delete _pml;
// ??? REMOVE   _pml = new MLD22( initval );
// ??? REMOVE   return *this;
// ??? REMOVE }
// ??? REMOVE
// ??? REMOVE
// ??? REMOVE MatrixD& MatrixD::redeclareM22( const Float8* initval )
// ??? REMOVE {
// ??? REMOVE   // This code is an exact copy of the one with Float8& argument.
// ??? REMOVE   if( --( _pml->_rc ) == 0 ) delete _pml;
// ??? REMOVE   _pml = new MLD22( initval );
// ??? REMOVE   return *this;
// ??? REMOVE }
// ??? REMOVE

// ===
// === Two-way virtual table entries =====================
// ===

MLD* SumM22M22( const MLD* argx, const MLD* argy )
{
  static MLD22*  pNew;
  static Float8* px;
  static Float8* py;
  static Float8* pz;

  pNew = new MLD22( 0.0 );
  pz = pNew->_pdi;
  px = argx->_pdi;
  py = argy->_pdi;
  while( pz <= pNew->_pdf ) *(pz++) = *(px++) + *(py++);

  return pNew;
}

MLD* DiffM22M22( const MLD* argx, const MLD* argy )
{
  static MLD22*  pNew;
  static Float8* px;
  static Float8* py;
  static Float8* pz;

  pNew = new MLD22( 0.0 );
  pz = pNew->_pdi;
  px = argx->_pdi;
  py = argy->_pdi;
  while( pz <= pNew->_pdf ) *(pz++) = *(px++) - *(py++);

  return pNew;
}

MLD* ProdM22M22( const MLD* argx, const MLD* argy )
{
  static MLD22* pNew;
  static const MLD22* x;
  static const MLD22* y;

#ifndef NO_DYNAMIC_CAST
  x = dynamic_cast<const MLD22*>( argx );
  y = dynamic_cast<const MLD22*>( argy );
#endif
#ifdef NO_DYNAMIC_CAST
  x = (MLD22*)( argx );
  y = (MLD22*)( argy );
#endif

  pNew = new MLD22( 0.0 );

  pNew->_m[0][0] = x->_m[0][0] * y->_m[0][0] + x->_m[0][1] * y->_m[1][0];
  pNew->_m[0][1] = x->_m[0][0] * y->_m[0][1] + x->_m[0][1] * y->_m[1][1];
  pNew->_m[1][0] = x->_m[1][0] * y->_m[0][0] + x->_m[1][1] * y->_m[1][0];
  pNew->_m[1][1] = x->_m[1][0] * y->_m[0][1] + x->_m[1][1] * y->_m[1][1];

  return pNew;
}


MLD* SumGenM22( const MLD* argx, const MLD* argy )
{
  static MLDGeneric* pNew;
  static Float8* px;
  static Float8* py;
  static Float8* pz;

  if( ( argx->_r != 2 ) ||
      ( argx->_c != 2 )    )
  {
     return 0;
  }

  pNew = new MLDGeneric( 2, 2, 0.0 );

  pz = pNew->_pdi;
  px = argx->_pdi;
  py = argy->_pdi;
  while( pz <= pNew->_pdf ) *(pz++) = *(px++) + *(py++);

  return pNew;
}


MLD* SumM22Gen( const MLD* argy, const MLD* argx )
{
  // This is a direct copy of SumGenM22
  static MLDGeneric* pNew;
  static Float8* px;
  static Float8* py;
  static Float8* pz;

  if( ( argx->_r != 2 ) ||
     ( argx->_c != 2 )    )
  {
     return 0;
  }

  pNew = new MLDGeneric( 2, 2, 0.0 );

  pz = pNew->_pdi;
  px = argx->_pdi;
  py = argy->_pdi;
  while( pz <= pNew->_pdf ) *(pz++) = *(px++) + *(py++);

  return pNew;
}

MLD* DiffGenM22( const MLD* argx, const MLD* argy )
{
  static MLDGeneric* pNew;
  static Float8* px;
  static Float8* py;
  static Float8* pz;

  if( ( argx->_r != 2 ) ||
      ( argx->_c != 2 )    )
  {
    return 0;
  }

  pNew = new MLDGeneric( 2, 2, 0.0 );

  pz = pNew->_pdi;
  px = argx->_pdi;
  py = argy->_pdi;
  while( pz <= pNew->_pdf ) *(pz++) = *(px++) - *(py++);

  return pNew;
}


MLD* DiffM22Gen( const MLD* argy, const MLD* argx )
{
  // This is a direct copy of DiffGenM22
  // except interchanging x and y at the last.
  static MLDGeneric* pNew;
  static Float8* px;
  static Float8* py;
  static Float8* pz;

  if( ( argx->_r != 2 ) ||
      ( argx->_c != 2 )    )
  {
     return 0;
  }

  pNew = new MLDGeneric( 2, 2, 0.0 );

  pz = pNew->_pdi;
  px = argx->_pdi;
  py = argy->_pdi;
  while( pz <= pNew->_pdf ) *(pz++) = *(py++) - *(px++);

  return pNew;
}

MLD* ProdGenM22( const MLD* argx, const MLD* argy )
{
  static MLDGeneric* pNew;
  static const MLDGeneric* x;
  static const MLD22* y;
  static int i;
  static int r;

  if( argx->_c != 2 )
  {
    return 0;
  }

  r = argx->_r;
#ifndef NO_DYNAMIC_CAST
  x = dynamic_cast<const MLDGeneric*>( argx );
  y = dynamic_cast<const MLD22*>( argy );
#endif
#ifdef NO_DYNAMIC_CAST
  x = (MLDGeneric*)( argx );
  y = (MLD22*)( argy );
#endif

  pNew = new MLDGeneric( r, 2, 0.0 );

  for( i = 0; i < r; i++ )  {
    pNew->_m[i][0] = x->_m[i][0] * y->_m[0][0] + x->_m[i][1] * y->_m[1][0];
    pNew->_m[i][1] = x->_m[i][0] * y->_m[0][1] + x->_m[i][1] * y->_m[1][1];
  }

  return pNew;
}

MLD* ProdM22Gen( const MLD* argy, const MLD* argx )
{
  static MLDGeneric* pNew;
  static const MLDGeneric* x;
  static const MLD22* y;
  static int j;
  static int c;

  if( argx->_r!= 2 )
  {
    return 0;
  }

  c = argx->_c;
#ifndef NO_DYNAMIC_CAST
  x = dynamic_cast<const MLDGeneric*>( argx );
  y = dynamic_cast<const MLD22*>( argy );
#endif
#ifdef NO_DYNAMIC_CAST
  x = (MLDGeneric*)( argx );
  y = (MLD22*)( argy );
#endif

  pNew = new MLDGeneric( 2, c, 0.0 );

  for( j = 0; j < c; j++ )  {
    pNew->_m[0][j] = y->_m[0][0] * x->_m[0][j] + y->_m[0][1] * x->_m[1][j];
    pNew->_m[1][j] = y->_m[1][0] * x->_m[0][j] + y->_m[1][1] * x->_m[1][j];
  }

  return pNew;
}


int MLD22::initOps()
{
  SumVtbl[ FPCL_INDEX_D_M22 ][ FPCL_INDEX_D_M22 ] = SumM22M22;
  SumVtbl[ FPCL_INDEX_D_GEN ][ FPCL_INDEX_D_M22 ] = SumGenM22;
  SumVtbl[ FPCL_INDEX_D_M22 ][ FPCL_INDEX_D_GEN ] = SumM22Gen;

  DiffVtbl[ FPCL_INDEX_D_M22 ][ FPCL_INDEX_D_M22 ] = DiffM22M22;
  DiffVtbl[ FPCL_INDEX_D_GEN ][ FPCL_INDEX_D_M22 ] = DiffGenM22;
  DiffVtbl[ FPCL_INDEX_D_M22 ][ FPCL_INDEX_D_GEN ] = DiffM22Gen;

  ProdVtbl[ FPCL_INDEX_D_M22 ][ FPCL_INDEX_D_M22 ] = ProdM22M22;
  ProdVtbl[ FPCL_INDEX_D_GEN ][ FPCL_INDEX_D_M22 ] = ProdGenM22;
  ProdVtbl[ FPCL_INDEX_D_M22 ][ FPCL_INDEX_D_GEN ] = ProdM22Gen;

  return 1;
}



// ===
// === MLD22   constructor ===============================
// ===

MLD22::MLD22( const Float8& initval )
: MLD( 2, 2 )
{
  // Initialize the entries into the two-way virtual function table
  // This should be done once and once only.
  static int opsDone = initOps();
  
  // pretend to use the variable in order to avoid unused variable
  // warnings at compile time
  pretendToUse(opsDone);   

  _m[0][0] = _m[1][1] = initval;
  _m[0][1] = _m[1][0] = 0.0;

  _pdi = _m[0];
  _pdf = &( _m[1][1] );
  _size = 4*sizeof( Float8 );
}


MLD22::MLD22( const Float8* initval )
: MLD( 2, 2 )
{
  // Initialize the entries into the two-way virtual function table
  // This should be done once and once only.
  static int opsDone = initOps();
  
  // pretend to use the variable in order to avoid unused variable
  // warnings at compile time
  pretendToUse(opsDone);   

  _pdi = _m[0];
  _pdf = &( _m[1][1] );
  _size = 4*sizeof( Float8 );

  _m[0][0] = initval[0];  // ??? Use memcpy here.
  _m[0][1] = initval[1];
  _m[1][0] = initval[2];
  _m[1][1] = initval[3];
}


void MLD22::loadFrom( const MLD* x )
{
  static Float8* p;
  if( ( _r == x->_r ) && ( _c == x->_c ) ) {
    p = _pdi;
    *(p++) = x->value(0,0);
    *(p++) = x->value(0,1);
    *(p++) = x->value(1,0);
    *p     = x->value(1,1);
  }

  // Dimensions were not correct.
  else { 
    ZMthrow ( ZMxlaDimensionMismatch(
              "MLD22::loadFrom( MLD* ) "
              "Dimensions do not match") );
  }
}


bool MLD22::setValue( int i, int j, const Float8& x )
{
  // Normally, the bounds check should be done
  // at a higher level.
  #if defined(ZM_MLD_DEBUG) || defined(ZM_MLD_TESTS)
  if( ( i < 0 ) || ( i > 1 ) ||
      ( j < 0 ) || ( j > 1 )
    ) { 
    ZMthrow ( ZMxlaInvalidIndex(
              "MLD22::setValue "
              "Indices out of bounds") );
    return 1;
  };
  #endif

  _m[i][j] = x;
  return 0;
}


Float8 MLD22::value( int i, int j ) const
{
  return _m[i][j];
}


MLD* MLD22::Clone() const
{
  static MLD*     q;
  // ??? REMOVE static Float8* py;
  // ??? REMOVE static Float8* px;
  q = new MLD22();
  // ??? REMOVE py = q->_pdi;
  // ??? REMOVE px =    _pdi;
  // ??? REMOVE while( px <= _pdf ) *(py++) = *(px++);
  ::memcpy( q->_pdi, _pdi, _size );
  return q;
}

MLC* MLD22::cloneComplex() const
{
  static MLC* q;
  static Complex8* pf;
  static Float8* pi;
  static int i;

  q = new MLC22();
  pi = _pdi;
  pf = q->_pdi;
  for( i = 0; i < 4; i++, pf++, pi++ ) {
    *pf = *pi;
  }

  return q;
}

// ===
// === Utility functions =================================
// ===

Float8 MLD22::determinant() const
{
  return ( _m[0][0]*_m[1][1] - _m[0][1]*_m[1][0] );
}

Float8 MLD22::trace() const
{
  return ( _m[0][0] + _m[1][1] );
}

MLD* MLD22::transpose() const
{
  static MLD22* ret;
  ret = new MLD22;
  ret->_m[0][0] = _m[0][0];
  ret->_m[1][1] = _m[1][1];
  ret->_m[0][1] = _m[1][0];
  ret->_m[1][0] = _m[0][1];
  return ret;
}


// ===
// === Inversion, solving linear equations ===============
// ===

MLD* MLD22::inverse() const
{
  static MLD22* ret;
  static Float8 det;

  det = _m[0][0]*_m[1][1] - _m[0][1]*_m[1][0];  // determinant

  // ??? Should the test be made: fabs(det) <= epsilon ???
  if( det == 0.0 ) { 
    ZMthrow ( ZMxlaSingularMatrix(
              "MLD22::inverse() "
              "Matrix is singular") );
    return 0;
  }

  ret = new MLD22;
  ret->_m[0][0] =   _m[1][1]/det;
  ret->_m[1][1] =   _m[0][0]/det;
  ret->_m[0][1] = - _m[0][1]/det;
  ret->_m[1][0] = - _m[1][0]/det;

  return ret;
}


// ===
// === Eigen-analysis ====================================
// ===

MatrixEigenData MLD22::eigen() const
{
  MatrixEigenData ret( 2 );
  MatrixC z( 2, 1 );
  MatrixD c( 3, 1 );

  // Solve characteristic polynomial ...
  c( 0, 0 ) = determinant();
  c( 1, 0 ) = - trace();
  c( 2, 0 ) = 1.0;
  MatrixEigenData::_polySolve( c, z );

  // Load eigenvalues ...
  ret._values( 0, 0 ) = z( 0, 0 );
  ret._values( 1, 1 ) = z( 1, 0 );

  // Load eigenvectors ...
  ret._vectors( 0, 0 ) = (Complex8) (- _m[0][1]);
  ret._vectors( 1, 0 ) = ((Complex8) _m[0][0]) - (Complex8)z( 0, 0 );
  ret._vectors( 0, 1 ) = (Complex8) (- _m[0][1]);
  ret._vectors( 1, 1 ) = ((Complex8)  _m[0][0]) - (Complex8)z( 1, 0 );

  // Normalize ...
  Float8 amp;
  for( int k = 0; k < 2; k++ ) {
    amp = norm( Complex8(ret._vectors( 0, k )) )
        + norm( Complex8(ret._vectors( 1, k )) );
    // Note: conversion must be forced for compilers like
    // g++ using templated version of Complex8.
    amp = ::sqrt( amp );
    ret._vectors( 0, k ) /= amp;
    ret._vectors( 1, k ) /= amp;
  }

  return ret;
}


// ===
// === Row, column, and element operations ===============
// ===

void MLD22::switchRows( int i, int j )
{
  static Float8 temp;
  temp     = _m[i][0];
  _m[i][0] = _m[j][0];
  _m[j][0] = temp;
  temp     = _m[i][1];
  _m[i][1] = _m[j][1];
  _m[j][1] = temp;
}


void MLD22::switchColumns( int i, int j )
{
  static Float8 temp;
    temp = _m[0][i];
    _m[0][i] = _m[0][j];
    _m[0][j] = temp;
    temp = _m[1][i];
    _m[1][i] = _m[1][j];
    _m[1][j] = temp;
}


// ===
// === Remaining MLDVirtF.h functions ====================
// ===

void MLD22::opPlusEqFlt  ( const Float8& x )
{
  _m[0][0] += x;
  _m[1][1] += x;
}

void MLD22::opSubEqFlt   ( const Float8& x )
{
  _m[0][0] -= x;
  _m[1][1] -= x;
}

void MLD22::opMultEqMLD  ( const MLD* x )
{
  static Float8 temp[2];

  temp[0] = _m[0][0];
  temp[1] = _m[0][1];
  ( _m[0][0] = temp[0] * x->value( 0, 0 ) )
            += temp[1] * x->value( 1, 0 );
  ( _m[0][1] = temp[0] * x->value( 0, 1 ) )
            += temp[1] * x->value( 1, 1 );

  temp[0] = _m[1][0];
  temp[1] = _m[1][1];
  ( _m[1][0] = temp[0] * x->value( 0, 0 ) )
            += temp[1] * x->value( 1, 0 );
  ( _m[1][1] = temp[0] * x->value( 0, 1 ) )
            += temp[1] * x->value( 1, 1 );
}

MLD* MLD22::add( const Float8& x ) const
{
  static MLD22*   ret;

#ifdef NO_DYNAMIC_CAST
  ret = (MLD22*) ( this->Clone() );
#else
  ret = dynamic_cast<MLD22*>( this->Clone() );
#endif

  ret->_m[0][0] += x;
  ret->_m[1][1] += x;
  return ret;
}

MLD* MLD22::subtract( const Float8& x ) const
{
  static MLD22*   ret;

#ifdef NO_DYNAMIC_CAST
  ret = (MLD22*) ( this->Clone() );
#else
  ret = dynamic_cast<MLD22*>( this->Clone() );
#endif

  ret->_m[0][0] -= x;
  ret->_m[1][1] -= x;
  return ret;
}

void MLD22::readFrom( istream& is )
{
#include "LinearAlgebra/MLDREADFROM.ins"
}