#include <math.h>
#include <stdlib.h>

#include "toolsmith.h"
#include "Vector.h"
#include "Matrix.h"
#include "Matrixlt.h"
#include "McTools.h"
#include "McMath.h"


Matrixlt chol(const Matrix &A)
// Cholesky decomposition A = L % t(L)
{
   Matrixlt L(A.nrow());

   chol(A, L);

   return L;
}

Matrix cholinv(const Matrix &A)
// Inverse of positive-definite symmetric matrix using Cholesky decomposition
{
   Matrix Ai(A.nrow(), A.ncol());

   cholinv(A, Ai);

   return Ai;
}

double dot(const Vector &a, const Vector &b)
// Dot product of two vectors
{
   int i;
   double sum = 0.0;

   if(a.length() != b.length())
      error("dot(Vector, Vector)", "non-conformable vectors", ERRNCM);

   for(i = 0; i < a.length(); i++) {
      sum += a[i] * b[i];
   }

   return sum;
}

Vector exp(const Vector &a)
// Exponentiate vector elements
{
   return apply(a, exp);
}

Matrix exp(const Matrix &A)
// Exponentiate matrix elements
{
   return apply(A, exp);
}

Matrix inv(const Matrix &A)
// Inverse of positive-definite matrix
{
   Matrix Ai(A.nrow(), A.ncol());

   inv(A, Ai);

   return Ai;
}

Matrixlt inv(const Matrixlt &A)
// Inverse of lower triangular matrix
{
   Matrixlt Ai(A.nrow());

   inv(A, Ai);

   return Ai;
}

Vector log(const Vector &a)
// Base e logarithm of vector elements
{
   return apply(a, log);
}

Matrix log(const Matrix &A)
// Base e logarithm of matrix elements
{
   return apply(A, log);
}

double max(const Vector &a)
// Maximum vector element
{
   int i, n = a.length();
   double val;

   if(n == 0)
      error("max(Vector)", "null Vector", ERREXIT);

   val = a[0];
   for(i = 1; i < n; i++) {
      val = MAX(val, a[i]);
   }

   return val;
}

double min(const Vector &a)
// Minimum vector element
{
   int i, n = a.length();
   double val;

   if(n == 0)
      error("min(Vector)", "null Vector", ERREXIT);

   val = a[0];
   for(i = 1; i < n; i++) {
      val = MIN(val, a[i]);
   }

   return val;
}

double prod(const Vector &a)
// Product of all vector elements
{
   int i;
   double prod = 1.0;

   for(i = 0; i < a.length(); i++) {
      prod *= a[i];
   }

   return prod;
}

Vector range(const Vector &a)
// Minimum and maximum vector elements
{
   int i, n = a.length();
   double amin, amax;
   Vector b(2);

   if(n == 0)
      error("range(Vector)", "null Vector", ERREXIT);

   amin = amax = a[0];
   for(i = 1; i < n; i++) {
      amin = MIN(amin, a[i]);
      amax = MAX(amax, a[i]);
   }
   b[0] = amin;
   b[1] = amax;

   return b;
}

double sum(const Vector &a)
// Sum of all vector elements
{
   int i;
   double sum = 0.0;

   for(i = 0; i < a.length(); i++) {
      sum += a[i];
   }

   return sum;
}

Matrix t(const Matrix &A)
// Transpose of matrix
{
   int i, j, m = A.ncol(), n = A.nrow();
   Matrix B(m, n);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         B[i][j] = A[j][i];
      }
   }

   return B;
}

Matrix t(const Matrixlt &A)
// Transpose of matrix
{
   int i, j, n = A.nrow();
   Matrix B(n, n);

   for(i = 0; i < n; i++) {
      for(j = i; j < n; j++) {
         B[i][j] = A[j][i];
      }
   }

   return B;
}


/******************************************************************************
*                          Low-Level Matrix Routines                          *
******************************************************************************/

void backsub(const Matrixlt &L, const Vector &x, Vector &b)
// Solution to t(L) % b = x using backward substitution
{
   int i, j, n = L.nrow();
   double sum;

   if(x.length() != n || b.length() != n)
      error("backsub(Matrixlt, Vector, Vector)",
            "subscript out of bounds", ERRSOB);

   for(i = n - 1; i >= 0; i--) {
      sum = x[i];
      for(j = i + 1; j < n; j++) {
         sum -= L[j][i] * b[j];
      }
      b[i] = sum / L[i][i];
   }
}

void chol(const Matrix &A, Matrixlt &L)
// Cholesky decomposition A = L % t(L)
{
   int i, j, k, n = A.nrow();
   double sum;

   if(A.ncol() != n || L.nrow() != n)
      error("chol(Matrix, Matrixlt)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j <= i; j++) {
         sum = A[i][j];
         for(k = 0; k < j; k++) {
            sum -= L[i][k] * L[j][k];
         }
         if(i != j) {
            L[i][j] = sum / L[j][j];
         } else {
            if(sum <= 0.0)
               error("chol(Matrix, Matrixlt)",
                     "Matrix is not positive definite", ERRNPD);
            L[i][i] = sqrt(sum);
         }
      }
   }
}

void chol(const Matrix &A, Matrixlt &L, Matrixlt &Li)
// Cholesky decomposition A = L % t(L) and inv(L)
{
   int i, j, k, n = A.nrow();
   double sum;

   if(A.ncol() != n || L.nrow() != n || Li.nrow() != n)
      error("chol(Matrix, Matrixlt, Matrixlt)",
            "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j <= i; j++) {
         sum = A[i][j];
         for(k = 0; k < j; k++) {
            sum -= L[i][k] * L[j][k];
         }
         if(i != j) {
            L[i][j] = Li[i][j] = sum / L[j][j];
         } else {
            if(sum <= 0.0)
               error("chol(Matrix, Matrixlt, Matrixlt)",
                     "Matrix is not positive definite", ERRNPD);
            L[i][i] = sqrt(sum);
         }
      }
   }

   for(i = 0; i < n; i++) {
      Li[i][i] = 1.0 / L[i][i];
      for(j = i + 1; j < n; j++) {
         sum = 0.0;
         for(k = i; k < j; k++) {
            sum -= Li[j][k] * Li[k][i];
         }
         Li[j][i] = sum / L[j][j];
      }
   }
}

void chold(const Matrix &A, Matrixlt &L, Vector &d)
// Cholesky decomposition A = L % diag(d) % t(L) such that diag(L) = 1
{
   int i, j, k, n = A.nrow();
   double sum;
   static double tol = 1.0e-15;

   if(A.ncol() != n || L.nrow() != n || d.length() != n)
      error("chold(Matrix, Matrixlt, Vector)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j <= i; j++) {
         sum = A[i][j];
         for(k = 0; k < j; k++) {
            sum -= d[k] * L[i][k] * L[j][k];
         }
         if(i != j) {
            L[i][j] = sum / d[j];
         } else {
            if(ABS(sum) < tol)
               error("chold(Matrix, Matrixlt, Vector)",
                     "Matrix is not positive definite", ERRNPD);
            L[i][j] = 1.0;
            d[i] = sum;
         }
      }
   }
}

void cholgmw(const Matrix &A, Matrixlt &L)
// Modified Cholesky Decomposition (Gill, Murray, and Wright 1981)
// Note: pivotting not implemented
{
   int i, j, k, n = A.nrow();
   double beta2, nu, psi, val, zeta;
   static double delta = 1.0e-7, eps = 1.0e-15;

   if(A.ncol() != n || L.nrow() != n)
      error("cholgmw(Matrix, Matrixlt)", "subscript out of bounds", ERRSOB);

   nu = MAX(1.0, sqrt(n * n - 1.0));
   psi = zeta = 0.0;
   for(i = 0; i < n; i++) {
      val = ABS(A[i][i]);
      if(val > psi) psi = val;
      for(j = 0; j < i; j++) {
         val = ABS(A[i][j]);
         if(val > zeta) zeta = val;
      }
      L[i][i] = A[i][i];
   }
   beta2 = MAX(MAX(psi, zeta / nu), eps);

   for(j = 0; j < n; j++) {
      zeta = 0.0;
      for(i = j + 1; i < n; i++) {
         val = A[i][j];
         for(k = 0; k < j; k++) {
            val -= L[j][k] * L[i][k] / L[k][k];
         }
         L[i][j] = val;
         val = ABS(L[i][j]);
         if(val > zeta) zeta = val;
      }

      L[j][j] = MAX(MAX(ABS(L[j][j]), zeta * zeta / beta2), delta);
      for(k = 0; k < j; k++) {
         L[j][k] /= L[k][k];
      }

      for(i = j + 1; i < n; i++) {
         L[i][i] -= L[i][j] * L[i][j] / L[j][j];
      }
   }

   for(j = 0; j < n; j++) {
      L[j][j] = sqrt(L[j][j]);
      for(i = j + 1; i < n; i++) {
         L[i][j] *= L[j][j];
      }
   }
}

void cholgmw(const Matrix &A, Matrixlt &L, Matrixlt &Li)
// Modified Cholesky Decomposition (Gill, Murray, and Wright 1981)
// Note: pivotting not implemented
{
   int i, j, k, n = A.nrow();
   double beta2, nu, psi, val, zeta;
   static double delta = 1.0e-7, eps = 1.0e-15;

   if(A.ncol() != n || L.nrow() != n)
      error("cholgmw(Matrix, Matrixlt, Matrixlt)",
            "subscript out of bounds", ERRSOB);

   nu = MAX(1.0, sqrt(n * n - 1.0));
   psi = zeta = 0.0;
   for(i = 0; i < n; i++) {
      val = ABS(A[i][i]);
      if(val > psi) psi = val;
      for(j = 0; j < i; j++) {
         val = ABS(A[i][j]);
         if(val > zeta) zeta = val;
      }
      L[i][i] = A[i][i];
   }
   beta2 = MAX(MAX(psi, zeta / nu), eps);

   for(j = 0; j < n; j++) {
      zeta = 0.0;
      for(i = j + 1; i < n; i++) {
         val = A[i][j];
         for(k = 0; k < j; k++) {
            val -= L[j][k] * L[i][k] / L[k][k];
         }
         L[i][j] = val;
         val = ABS(L[i][j]);
         if(val > zeta) zeta = val;
      }

      L[j][j] = MAX(MAX(ABS(L[j][j]), zeta * zeta / beta2), delta);
      for(k = 0; k < j; k++) {
         L[j][k] /= L[k][k];
      }

      for(i = j + 1; i < n; i++) {
         L[i][i] -= L[i][j] * L[i][j] / L[j][j];
      }
   }

   for(j = 0; j < n; j++) {
      L[j][j] = sqrt(L[j][j]);
      for(i = j + 1; i < n; i++) {
         L[i][j] *= L[j][j];
         L[i][j] = L[i][j];
      }
   }

   for(i = 0; i < n; i++) {
      Li[i][i] = 1.0 / L[i][i];
      for(j = i + 1; j < n; j++) {
         val = 0.0;
         for(k = i; k < j; k++) {
            val -= Li[j][k] * Li[k][i];
         }
         Li[j][i] = val / L[j][j];
      }
   }
}

void cholinv(const Matrix &A, Matrix &Ai)
// Inverse of positive-definite symmetric matrix using Cholesky decomposition
{
   int i, j, k, n = A.nrow();
   double sum;

   if(A.ncol() != n || Ai.nrow() != n || Ai.ncol() != n)
      error("cholinv(Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j <= i; j++) {
         sum = A[i][j];
         for(k = 0; k < j; k++) {
            sum -= Ai[i][k] * Ai[j][k];
         }
         if(i != j) {
            Ai[i][j] = sum / Ai[j][j];
         } else {
            if(sum <= 0.0)
               error("cholinv(Matrix, Matrix)",
                     "Matrix is not positive definite", ERRNPD);
            Ai[i][i] = sqrt(sum);
         }
      }
   }

   for(i = 0; i < n; i++) {
      Ai[i][i] = 1.0 / Ai[i][i];
      for(j = i + 1; j < n; j++) {
         sum = 0.0;
         for(k = i; k < j; k++) {
            sum -= Ai[j][k] * Ai[k][i];
         }
         Ai[j][i] = sum / Ai[j][j];
      }
   }

   for(i = 0; i < n; i++) {
      for(j = i; j < n; j++) {
         sum = 0.0;
         for(k = j; k < n; k++) {
            sum += Ai[k][i] * Ai[k][j];
         }
         Ai[i][j] = Ai[j][i] = sum;
      }
   }
}

void cholls(const Matrixlt &L, const Vector &x, Vector &b)
// Solution to (L % t(L)) % b = x
{
   int i, j, n = L.nrow();
   double sum;

   if(x.length() != n || b.length() != n)
      error("cholls(Matrixlt, Vector, Vector)",
            "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      sum = x[i];
      for(j = 0; j < i; j++) {
         sum -= L[i][j] * b[j];
      }
      b[i] = sum / L[i][i];
   }
   for(i = n - 1; i >= 0; i--) {
      sum = b[i];
      for(j = i + 1; j < n; j++) {
         sum -= L[j][i] * b[j];
      }
      b[i] = sum / L[i][i];
   }
}

void choltls(const Matrixlt &L, const Vector &x, Vector &b)
// Solution to (t(L) % L) % b = x
{
   int i, j, n = L.nrow();
   double sum;

   if(x.length() != n || b.length() != n)
      error("choltls(Matrixlt, Vector, Vector)",
            "subscript out of bounds", ERRSOB);

   for(i = n - 1; i >= 0; i--) {
      sum = x[i];
      for(j = i + 1; j < n; j++) {
         sum -= L[j][i] * b[j];
      }
      b[i] = sum / L[i][i];
   }
   for(i = 0; i < n; i++) {
      sum = b[i];
      for(j = 0; j < i; j++) {
         sum -= L[i][j] * b[j];
      }
      b[i] = sum / L[i][i];
   }
}

void cholsum(const Vector &d, double tau, const Matrixlt &Li, Matrixlt &Lsum)
// Lsum = chol(diag(d) + tau * t(Li) % Li)
{
   int i, j, k, n = d.length();
   double sum, val;

   if(Li.nrow() != n || Lsum.nrow() != n)
      error("colsum(Vector, double, Matrixlt, Matrixlt)",
            "subscript out of bounds", ERRSOB);

   for(j = 0; j < n; j++) {
      for(i = j; i < n; i++) {
         sum = val = 0.0;
         for(k = 0; k < MIN(n - i, j); k++) {
            sum += Li[i + k][i] * Li[i + k][j];
            val -= Lsum[i][k] * Lsum[j][k];
         }
         for( ; k < n - i; k++) {
            sum += Li[i + k][i] * Li[i + k][j];
         }
         for( ; k < j; k++) {
            val -= Lsum[i][k] * Lsum[j][k];
         }
         val += tau * sum;
         if(i != j) {
            Lsum[i][j] = val / Lsum[j][j];
         } else {
            val += d[i];
            if(val <= 0.0)
               error("cholsum(Vector, double, Matrixlt, Matrixlt)",
                      "Matrix is not positive definite", ERRNPD);
            Lsum[i][i] = sqrt(val);
         }
      }
   }
}

void forsub(const Matrixlt &L, const Vector &x, Vector &b)
// Solution to L % b = x using forward substitution
{
   int i, j, n = L.nrow();
   double sum;

   if(x.length() != n || b.length() != n)
      error("forsub(Matrixlt, Vector, Vector)",
            "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      sum = x[i];
      for(j = 0; j < i; j++) {
         sum -= L[i][j] * b[j];
      }
      b[i] = sum / L[i][i];
   }
}

void inv(const Matrix &A, Matrix &Ai)
// Inverse of positive-definite matrix
{
   int i, j, k, l, n = A.nrow(), n2 = 2 * n;
   double m, tol = 1.0e-15;
   Matrix B(n, n2);
   Vector v(n2);

   if(A.ncol() != n || Ai.nrow() != n || Ai.ncol() != n)
      error("inv(Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j < n; j++) {
         B[i][j] = A[i][j];
         if(i == j) B[i][j + n] = 1.0;
      }
   }
   for(i = 0; i < n - 1; i++) {
      for(l = i; ABS(B[l][i]) < tol && l < n; l++);
      if(l == n)
         error("inv(Matrix, Matrix)", "Matrix is singular", ERRSING);
      else if(l != i) {
         for(j = 0; j < n2; j++) {
            v[j] = B[l][j];
            B[l][j] = B[i][j];
            B[i][j] = v[j];
         }
      }
      for(j = i + 1; j < n; j++) {
         m = B[j][i] / B[i][i];
         for(k = 0; k < n2; k++) {
            B[j][k] -= m * B[i][k];
         }
      }
   }

   if(ABS(B[n - 1][n - 1]) < tol)
      error("inv(Matrix, Matrix)", "Matrix is singular", ERRSING);

   for(i = n - 1; i >= 0; i--) {
      m = B[i][i];
      for(j = 0; j < n2; j++) {
         B[i][j] /= m;
      }
      for(j = i - 1; j >= 0; j--) {
         m = B[j][i];
         for(k = 0; k < n2; k++) {
            B[j][k] -= m * B[i][k];
         }
      }
   }

   for(i = 0; i < n; i++) {
      for(j = 0; j < n; j++) {
         Ai[i][j] = B[i][n + j];
      }
   }
}

void inv(const Matrixlt &A, Matrixlt &Ai)
// Inverse of lower triangular matrix
{
   int i, j, k, n = A.nrow();
   double sum;

   if(Ai.nrow() != n)
      error("inv(Matrixlt, Matrixlt)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j <= i; j++) {
         Ai[i][j] = A[i][j];
      }
   }

   for(i = 0; i < n; i++) {
      Ai[i][i] = 1.0 / A[i][i];
      for(j = i + 1; j < n; j++) {
         sum = 0.0;
         for(k = i; k < j; k++) {
            sum -= Ai[j][k] * Ai[k][i];
         }
         Ai[j][i] = sum / A[j][j];
      }
   }
}

void matern(const Matrix &D, const Vector &theta, Matrix &R) {
   int i, j, n = D.nrow();
   double a, b, d;

   if(D.ncol() != n || R.nrow() != n || R.ncol() != n || theta.length() != 2)
      error("matern(Matrix, int, double, Matrix)", "index out of range",
            ERREXIT);

   a = (1.0 - theta[0]) * log(2.0) - lgamma(theta[0]);
   b = 2.0 * sqrt(theta[0]) / theta[1];
   for(i = 0; i < n; i++){
      for(j = 0; j < i; j++) {
         d = b * D[i][j];
         try {
            R[i][j] = R[j][i]
                    = exp(a + theta[0] * log(d)) * besselK(d, theta[0]);
         }
         catch(int err) {
            if(err == ERRNAN) R[i][j] = R[j][i] = 0.0;
            else exit(err);
         }
      }
      R[i][i] = 1.0;
   }
}

void mult(double a, const Vector &b, Vector &c)
//  c = a * b
{
   int i, n = b.length();

   if(c.length() != n)
      error("mult(double, Vector, Vector)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      c[i] = a * b[i];
   }
}

void mult(double a, const Matrix &B, Matrix &C)
// C = a * B
{
   int i, j, m = B.nrow(), n = B.ncol();

   if(C.nrow() != m || C.ncol() != n)
      error("mult(double, Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         C[i][j] = a * B[i][j];
      }
   }
}

void mult(double a, const Matrixlt &B, Matrixlt &C)
// C = a * B
{
   int i, j, n = B.nrow();

   if(C.nrow() != n)
      error("mult(double, Matrixlt, Matrixlt)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j <= i; j++) {
         C[i][j] = a * B[i][j];
      }
   }
}

void prod(const Vector &a, const Vector &b, double &ab)
// ab = dot(a, b)
{
   int i, n = a.length();
   double sum = 0.0;

   if(b.length() != n)
      error("prod(Vector, Vector, double*)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      sum += a[i] * b[i];
   }
   ab = sum;
}

void prod(const Vector &a, const Matrix &B, Vector &aB)
// aB = a % B
{
   int i, j, m = B.nrow(), n = B.ncol();
   double sum;

   if(a.length() != m || aB.length() != n)
      error("prod(Vector, Matrix, Vector)", "subscript out of bounds", ERRSOB);

   for(j = 0; j < n; j++) {
      sum = 0.0;
      for(i = 0; i < m; i++) {
         sum += a[i] * B[i][j];
      }
      aB[j] = sum;
   }
}

void prod(const Vector &a, const Matrixlt &B, Vector &aB)
// aB = a % B
{
   int i, j, n = B.nrow();
   double sum;

   if(a.length() != n || aB.length() != n)
      error("prod(Vector, Matrixlt, Vector)", "subscript out of bounds", ERRSOB);

   for(j = 0; j < n; j++) {
      sum = 0.0;
      for(i = j; i < n; i++) {
         sum += a[i] * B[i][j];
      }
      aB[j] = sum;
   }
}

void prod(const Matrix &A, const Vector &b, Vector &Ab)
// Ab = A % b
{
   int i, j, m = A.nrow(), n = A.ncol();
   double sum;

   if(b.length() != n || Ab.length() != m)
      error("prod(Matrix, Vector, Vector)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      sum = 0.0;
      for(j = 0; j < n; j++) {
         sum += A[i][j] * b[j];
      }
      Ab[i] = sum;
   }
}

void prod(const Matrixlt &A, const Vector &b, Vector &Ab)
// Ab = A % b
{
   int i, j, n = A.nrow();
   double sum;

   if(b.length() != n || Ab.length() != n)
      error("prod(Matrixlt, Vector, Vector)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      sum = 0.0;
      for(j = 0; j <= i; j++) {
         sum += A[i][j] * b[j];
      }
      Ab[i] = sum;
   }
}

void prod(const Matrix &A, const Matrix &B, Matrix &AB)
// AB = A % B
{
   int i, j, k, m = AB.nrow(), n = AB.ncol(), s = A.ncol();
   double sum;

   if(A.nrow() != m || B.nrow() != s || B.ncol() != n)
      error("prod(Matrix, Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         sum = 0.0;
         for(k = 0; k < s; k++) {
            sum += A[i][k] * B[k][j];
         }
         AB[i][j] = sum;
      }
   }
}

void prod(const Matrix &A, const Matrixlt &B, Matrix &AB)
// AB = A % B
{
   int i, j, k, m = AB.nrow(), n = AB.ncol();
   double sum;

   if(A.nrow() != m || A.ncol() != B.nrow() || B.ncol() != n)
      error("prod(Matrix, Matrixlt, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         sum = 0.0;
         for(k = j; k < n; k++) {
            sum += A[i][k] * B[k][j];
         }
         AB[i][j] = sum;
      }
   }
}

void prod(const Matrixlt &A, const Matrix &B, Matrix &AB)
// AB = A % B
{
   int i, j, k, m = AB.nrow(), n = AB.ncol();
   double sum;

   if(A.nrow() != m || A.ncol() != B.nrow() || B.ncol() != n)
      error("prod(Matrixlt, Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         sum = 0.0;
         for(k = 0; k <= i; k++) {
            sum += A[i][k] * B[k][j];
         }
         AB[i][j] = sum;
      }
   }
}

void prod(const Matrixlt &A, const Matrixlt &B, Matrix &AB)
// AB = A % B
{
   int i, j, k, m = AB.nrow(), n = AB.ncol();
   double sum;

   if(A.nrow() != m || A.ncol() != B.nrow() || B.ncol() != n)
      error("prod(Matrixlt, Matrixlt, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j <= i; j++) {
         sum = 0.0;
         for(k = j; k <= i; k++) {
            sum += A[i][k] * B[k][j];
         }
         AB[i][j] = sum;
      }
      for( ; j < n; j++) {
         AB[i][j] = 0.0;
      }
   }
}

void prodd(const Vector &A, const Matrix &B, Matrix &AB)
// AB = diag(A) % B
{
   int i, j, m = AB.nrow(), n = AB.ncol();

   if(A.length() != m || B.nrow() != m || B.ncol() != n)
      error("prodd(Vector, Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         AB[i][j] = A[i] * B[i][j];
      }
   }
}

void prodd(const Matrix &A, const Vector &B, Matrix &AB)
// AB = A % diag(B)
{
   int i, j, m = AB.nrow(), n = AB.ncol();

   if(A.nrow() != m || A.ncol() != n || B.length() != n)
      error("prodd(Matrix, Vector, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         AB[i][j] = A[i][j] * B[j];
      }
   }
}

void prodt(const Matrix &A, Matrix &AtA)
// AtA = t(A) % A
{
   int i, j, k, m = A.nrow(), n = A.ncol();
   double sum;

   if(AtA.nrow() != n || AtA.ncol() != n)
      error("prodt(Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = 0; j <= i; j++) {
         sum = 0.0;
         for(k = 0; k < m; k++) {
            sum += A[k][i] * A[k][j];
         }
         AtA[i][j] = AtA[j][i] = sum;
      }
   }
}

void prodt(const Matrixlt &A, Matrix &AtA)
// AtA = t(A) % A
{
   int i, j, k, n = A.nrow();
   double sum;

   if(AtA.nrow() != n || AtA.ncol() != n)
      error("prodt(Matrixlt, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < n; i++) {
      for(j = i; j < n; j++) {
        sum = 0.0;
        for(k = j; k < n; k++) {
           sum += A[k][i] * A[k][j];
        }
        AtA[i][j] = AtA[j][i] = sum;
      }
   }
}

void prodt(const Matrix &A, const Matrix &B, Matrix &AtB)
// AtB = t(A) % B
{
   int i, j, k, m = AtB.nrow(), n = AtB.ncol(), s = A.nrow();
   double sum;

   if(B.nrow() != s || A.ncol() != m || B.ncol() != n)
      error("prodt(Matrix, Matrix, Matrix)", "subscript out of bounds", ERRSOB);

   for(i = 0; i < m; i++) {
      for(j = 0; j < n; j++) {
         sum = 0.0;
         for(k = 0; k < s; k++) {
            sum += A[k][i] * B[k][j];
         }
         AtB[i][j] = sum;
      }
   }
}

void prodt(const Matrix &A, const Vector &b, Vector &Atb)
// Atb = t(A) % b
{
   int i, j, n = Atb.length(), s = A.nrow();
   double sum;

   if(A.ncol() != n || b.length() != s)
      error("prodt(Matrix, Vector, Vector)", "subscript out of bounds", ERRSOB);

   for(j = 0; j < n; j++) {
      sum = 0.0;
      for(i = 0; i < s; i++) {
         sum += A[i][j] * b[i];
      }
      Atb[j] = sum;
   }
}