// $Id: Similarity.cc,v 1.1 2003/07/02 16:21:08 sachs Exp $
//
// David Sachs - Fermilab - June 2003

#include "CovMatrices/Similarity.h"

void CovMatrices::similarityPAPt(
	const CovMatrix& a,
	const std::vector < std::vector<double> > p,
	CovMatrix& result)
{
  // Pointers to use internal triangular representation
  // of CovMatrix classes
  const int n = a.size();
  result.resize(n);
  const double * const ap = &a(0,0);
  double *rp = &result(0,0);

  for (int i = 0; i < n; ++i)
  {
    const double *pi = &p[i][0];
    for (int j = 0; j <= i; ++j)
    {
      const double *pj = &p[j][0];

      const double *ip = ap;
      double r = 0.;

      for (int k = 0; k < n; ++k)
      {
	for (int l = 0; l < k; ++l)
	{
	  // Add contributions from off diagonal elements of a
	  r += (*ip++)*(pi[k]*pj[l]+pj[k]*pi[l]);
	}
	// Add contribution from diagonal elements of a
	r += (*ip++)*pi[k]*pj[k];
      }
      *rp++ = r;
    }
  }
}


void CovMatrices::similarityPtAP(
	const CovMatrix& a,
	const std::vector < std::vector<double> > p,
	CovMatrix& result)
{
  // Pointers to use internal triangular representation
  // of CovMatrix classes
  const int n = a.size();
  result.resize(n);
  const double* const ap = &a(0,0);
  double *rp = &result(0,0);

  for (int i = 0; i < n; ++i)
  {
    for (int j = 0; j <= i; ++j)
    {
      const double *ip = ap;
      double r = 0.;

      for (int k = 0; k < n; ++k)
      {
	for (int l = 0; l < k; ++l)
	{
	  // Add contributions from off diagonal elements of a
	  r += (*ip++)*(p[k][i]*p[l][j]+p[k][j]*p[l][i]);
	}
	// Add contribution from diagonal elements of a
	r += (*ip++)*p[k][i]*p[k][j];
      }
      *rp++ = r;
    }
  }
}