// $Id: CovMatrix.cc,v 1.10 2002/10/22 17:50:14 sachs Exp $
// David Sachs - Fermilab - April 2002
//
// Basic Functions for CovMatrix class

#include "CovMatrixInternal.h"

namespace CovMatrices
{
  // Construct Uninitialized Matrix of size m
  CovMatrix::CovMatrix(int m) : n(m), M(0)
  {
    if ( m == 0 ) return;	// Empty CovMatrix Created
    if ( m > 0 )
    {
      M = new double[(n*(n+1))/2];
      return;
    }
    
    std::ostringstream msg;
    msg << " **** CovMatrix( int ) ****\n";
    msg << "A CovMatrix must have a size at least zero.\n";
    msg << "The value you supplied was " << m << std::endl;
    CovMatrixThrow(msg.str());
    exit(2);
  }

  // Copy Constructor
  CovMatrix::CovMatrix( const CovMatrix& C) : n(C.n), M(0)
  {
    if (!n) return;	// Copied Empty CovMatrix

    M = new double[(n*(n+1))/2];
    memcpy( M, C.M, ((n*(n+1))/2)*sizeof(double));
  }

  // Construct from vector<double>
  CovMatrix::CovMatrix( const std::vector<double>& V) : M(0)
  {
    int sz = V.size();
    for ( n = 1; ; n++)
    {
      if ((n*(n+1))/2 == sz)
      {
	M = new double[sz];
	memcpy(M, &V[0], V.size()*sizeof(double));
	return;
      }
      if ((n*(n+1))/2 > sz)
      {
	std::ostringstream msg;
	msg << " **** CovMatrix( const std::vector<double>& ) ****\n";
	msg << " Vector size must be of the form (n*(n+1))/2.\n";
	msg << " Supplied vector had size " << sz << std::endl;
	CovMatrixThrow(msg.str());
	exit(2);
      }
    }	// loop never completes
  }

  // Construct from BiVector
  CovMatrix::CovMatrix(const BiVector& B) : n(2), M(new double[3])
  {
    if (B.n > 0)
    {
      // Format 2x2 Product B'*B
      M[0] = 0.;
      M[1] = 0.;
      M[2] = 0.;

      for (int i = 0; i < B.n; i++)
      {
	M[0] += B.v0[i]*B.v0[i];
	M[1] += B.v0[i]*B.v1[i];
	M[2] += B.v1[i]*B.v1[i];
      }
      return;
    }
    else
    {
      delete[] M;	// Prevent memory leak
      M = 0;
      n = 0;

      std::ostringstream msg;
      msg << " **** CovMatrix( const BiVector& ) ****\n";
      msg << " Cannot Create CovMatrix from empty BiVector." << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
  }

  // Copy assignment
  CovMatrix& CovMatrix::operator= (const CovMatrix& C)
  {
    // Do nothing if assignment to self or
    // if copy of empty CovMatrix to Empty Covmatrixax
    if ( M == C.M ) return *this;

    // Must resize if size has changed
    if ( n != C.n )
    {
      delete[] M;
      M = 0;
      n = C.n;
      if (!n) return *this; // Matrix has been made empty
      M = new double[(n*(n+1))/2];
    }

    // Copy contents of source CovMatrix
    memcpy(M, C.M, ((n*(n+1))/2)*sizeof(double));
    return *this;
  }

  // Assignment from vector<double>
  CovMatrix& CovMatrix::operator= (const std::vector<double>& V)
  {
    int sz = V.size();
    if (!n || (sz != (n*(n+1))/2))	// Must resize
    {
      delete[] M;
      M = 0;
      for ( n = 1; ; n++)
      {
	if ((n*(n+1))/2 == sz)
	{
	  M = new double[sz];
	  break;
	}
	if ((n*(n+1))/2 > sz)
	{
	  n = 0;
	  std::ostringstream msg;
	  msg << " **** CovMatrix = const vector<double>& ****\n";
	  msg << " Vector size must be of the form (n*(n+1))/2.\n";
	  msg << " Supplied vector had size " << sz << std::endl;
          CovMatrixThrow(msg.str());
          exit(2);
	}
      }
    }
    memcpy( M, &V[0], sz*sizeof(double));	// Copy data
    return *this;
  }

  // Assignment from BiVector
  CovMatrix& CovMatrix::operator= (const BiVector& B)
  {
    if (B.n > 0)
    {
      if ( n != 2 )	// Must resize
      {
	delete[] M;

	// The following line is needed to prevent memory corruption
	// in case the reallocation of the array throws an exception
	M = 0;	// Do NOT delete this line.
	M = new double[3];
	n = 2;
      }

      // Format 2x2 Product B'*B
      M[0] = 0.;
      M[1] = 0.;
      M[2] = 0.;
      for (int i = 0; i < B.n; i++)
      {
	M[0] += B.v0[i]*B.v0[i];
	M[1] += B.v0[i]*B.v1[i];
	M[2] += B.v1[i]*B.v1[i];
      }
      return *this;
    }
    else
    {
      std::ostringstream msg;
      msg << " **** CovMatrix = const BiVector& ****\n";
      msg << " Cannot Assign CovMatrix from empty BiVector." << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
    return *this;  // *** UNUSED - Required by C++ standard
  }

  // Element Access
  double& CovMatrix::operator() (int i, int j)
  {
    if ( (i >= 0 ) && ( i < n ) && ( j >= 0 ) && ( j < n ))
    {
      return M[ i>j ? (i*(i+1))/2+j : (j*(j+1))/2+i ];
    }
    else
    {
      std::ostringstream msg;
      msg << " **** CovMatrix( int, int ) ****\n";
      msg << " Subscript out of range for Matrix size = " << n << std::endl;
      msg << " Requested subscript was [ " << i << " , " 
	<< j << " ]" << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
    return M[0];   //  *** UNUSED - Required by C++ standard ***
  }

  const double& CovMatrix::operator() (int i, int j) const
  {
    if ( (i >= 0 ) && ( i < n ) && ( j >= 0 ) && ( j < n ))
    {
      return M[ i>j ? (i*(i+1))/2+j : (j*(j+1))/2+i ];
    }
    else
    {
      std::ostringstream msg;
      msg << " **** CovMatrix( int, int ) ****\n";
      msg << " Subscript out of range for Matrix size = " << n << std::endl;
      msg << " Requested subscript was [ " << i << " , " 
	<< j << " ]" << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
    return M[0];   //  *** UNUSED - Required by C++ standard ***
  }

  // Dump Elements to vector<double>
  void CovMatrix::dump(std::vector<double>& V) const
  {
    if (n)
    {
      V.resize((n*(n+1))/2);
      memcpy(&V[0], M, ((n*(n+1))/2)*sizeof(double));
    }
    else
    {
      std::ostringstream msg;
      msg << "**** CovMatrix::dump(vector<double>&) ****\n";
      msg << "Cannot dump empty CovMatrix" << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
  }

  // Resize o CovMatrix - After resizing element contents are undefined
  void CovMatrix::resize(int nn)
  {
    if (nn < 0)
    {
      std::ostringstream msg;
      msg << "**** CovMatrix::resize( int ) ****\n";
      msg << "Illegal new size: " << nn << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }

    if ( n == nn ) return;	// No action needed if new size == old size

    n = nn;
    delete[] M;
    M = 0;			// Do NOT delete this line
    if (!n) return;		// CovMatrix has become empty
    M = new double[(n*(n+1))/2];
  }

  // Print a CovMatrix
  void CovMatrix::put(std::ostream &o) const
  {
    int i, j;
    int k = 1; // Index steps through elements of CovMatrix
    int oldp = o.precision(P);
    std::ios::fmtflags oldf = o.setf(std::ios::scientific|std::ios::showpos);

    if (!n)
    {
      o << " *** EMPTY CovMatrix ***";
    }
    else
    {
      // Print first line with leading brace
      o << "{" << std::setw(W) << M[0];
  
      // Print remaining lines
      for ( i = 1; i < n; i++ )
      {
        o << "\n ";	// End of preceding line plus leading space
        for ( j = 0; j <= i; j++)
        {
  	  o << std::setw(W) << M[k++];
        }
      }
    }

    // Terminate last line with a closing brace
    o << " }" << std::endl;

    // Restore previous stream parameters
    o.flags(oldf);
    o.precision(oldp);
  }

  // The following static member function is provided for
  // for BiVector and CovMatrix to replace the missing
  // output function for vector<double>
  void CovMatrix::printv(std::ostream& o, const std::vector<double>& v)
  {
    CovMatrixPutVector( o, v );
    o << std::endl;
  }

  // Multiply CovMatrix and vector<double>
  std::vector<double> operator* (const CovMatrix& C, const std::vector<double>& V)
  {
    if (C.n != V.size())
    {
      std::ostringstream msg;
      msg << "**** CovMatrix * vector<double> ****\n";
      msg << "Order of CovMatrix and vector<double> are not equal\n";
      msg << "CovMatrix order = "  << C.n  << std::endl;
      msg << "vector<double> order  = " << V.size() << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
    std::vector<double> R(C.n);
    int i, j;
    int k = 0;
    double t;

    // Accumulate the vector products using the symmetry and triangular order of C
    for (i = 0; i < C.n; i++)
    {
      t = 0.;
      for (j = 0; j < i; j++)
      {
	t += C.M[k]*V[j];
	R[j] += C.M[k]*V[i];
	k++;
      }
      R[i] = t + C.M[k]*V[i];
      k++;
    }
    return R;
  }

  // Multiply vector<double> and CovMatrix
  std::vector<double> operator* ( const std::vector<double>& V, const CovMatrix& C)
  {
    if (C.n != V.size())
    {
      std::ostringstream msg;
      msg << "**** vector<double> * CovMatrix ****\n";
      msg << "Order of CovMatrix and vector<double> are not equal\n";
      msg << "CovMatrix order = "  << C.n  << std::endl;
      msg << "vector<double> order  = " << V.size() << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
    std::vector<double> R(C.n);
    int i, j;
    int k = 0;
    double t;

    // Accumulate the vector products using the symmetry and triangular order of C
    for (i = 0; i < C.n; i++)
    {
      t = 0.;
      for (j = 0; j < i; j ++)
      {
	t += C.M[k]*V[j];
	R[j] += C.M[k]*V[i];
	k++;
      }
      R[i] = t + C.M[k]*V[i];
      k++;
    }
    return R;
  }

  // Multiply CovMatrix and BiVector
  BiVector operator* (const CovMatrix& C, const BiVector& B)
  {
    if (C.n != B.n)
    {
      std::ostringstream msg;
      msg << "**** CovMatrix * BiVector ****\n";
      msg << "Order of CovMatrix and BiVector are not equal\n";
      msg << "CovMatrix order = "  << C.n  << std::endl;
      msg << "BiVector order  = " << B.n << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
    BiVector R(C.n);
    int i, j;
    int k = 0;
    double t0, t1;

    // Accumulate the vector products using the symmetry and triangular order of C
    for (i = 0; i < C.n; i++)
    {
      t0 = 0.;
      t1 = 0.;
      for (j = 0; j < i; j ++)
      {
	t0 += C.M[k]*B.v0[j];
	R.v0[j] += C.M[k]*B.v0[i];
	t1 += C.M[k]*B.v1[j];
	R.v1[j] += C.M[k]*B.v1[i];
	k++;
      }
      R.v0[i] = t0 + C.M[k]*B.v0[i];
      R.v1[i] = t1 + C.M[k]*B.v1[i];
      k++;
    }
    return R;
  }

  // Multiply BiVector and CovMatrix
  BiVector operator* ( const BiVector& B, const CovMatrix& C)
  {
    if (C.n != B.n)
    {
      std::ostringstream msg;
      msg << "**** BiVector * CovMatrix ****\n";
      msg << "Order of CovMatrix and BiVector are not equal\n";
      msg << "CovMatrix order = "  << C.n  << std::endl;
      msg << "BiVector order  = " << B.n << std::endl;
      CovMatrixThrow(msg.str());
      exit(2);
    }
    BiVector R(C.n);
    int i, j;
    int k = 0;
    double t0, t1;

    // Accumulate the vector products using the symmetry and triangular order of C
    for (i = 0; i < C.n; i++)
    {
      t0 = 0.;
      t1 = 0.;
      for (j = 0; j < i; j ++)
      {
	t0 += C.M[k]*B.v0[j];
	R.v0[j] += C.M[k]*B.v0[i];
	t1 += C.M[k]*B.v1[j];
	R.v1[j] += C.M[k]*B.v1[i];
	k++;
      }
      R.v0[i] = t0 + C.M[k]*B.v0[i];
      R.v1[i] = t1 + C.M[k]*B.v1[i];
      k++;
    }
    return R;
  }

}  // namespace CovMatrices