#include  <stdio.h>
#include <string.h>
#include <unistd.h>
#include <math.h>

#include "vsubs.h"  
#include "strsubs.h" 
#include "linsubs.h" 
static int
linsolvx (int nDim, double* pfMatr, double* pfVect, double* pfSolution);
static int calcdet = NO;

int
setcalcdet (int val)
{
  calcdet = val;
}
void
bal (double *a, double *b, int n)
/** 
 normalize mean 0 s.d 1 
 */
{
  double t;
  t = asum (b, n) / (double) n;
  vsp (a, b, -t, n);

  t = asum2 (a, n) / (double) n;
  vst (a, a, 1.0 / sqrt (t), n);
}

void
mulmat (double *a, double *b, double *c, int a1, int a2, int a3)
/* b is a1 x a2 , c a2 x a3 so a is a1 x a3  */
{
  double *t;
  int i, j, k;
  ZALLOC(t, a1*a3, double);

  for (i = 0; i < a1; i++)
    for (j = 0; j < a3; j++)
      for (k = 0; k < a2; k++)
        t[i * a3 + j] += b[i * a2 + k] * c[k * a3 + j];

  copyarr (t, a, a1 * a3);

  free (t);

}
void
imulmat (int *a, int *b, int *c, int a1, int a2, int a3)
/* b is a1 x a2 , c a2 x a3 so a is a1 x a3  */
{
  int *t;
  int i, j, k;
  ZALLOC(t, a1*a3, int);

  for (i = 0; i < a1; i++)
    for (j = 0; j < a3; j++)
      for (k = 0; k < a2; k++)
        t[i * a3 + j] += b[i * a2 + k] * c[k * a3 + j];

  copyiarr (t, a, a1 * a3);

  free (t);

}

double
pdinv (double *cinv, double *coeff, int n)
// cinv and coeff can be same
// cinv can be NULL 
// return log det (coeff) 
{
  double *tt;
  double *p;
  double t, sum, y;
  int i, j, k;

  /**
   pmat(coeff, n) ;
   */
  ZALLOC(tt, n * n, double);
  ZALLOC(p, n, double);

  copyarr (coeff, tt, n * n);

  choldc (tt, n, p);

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

        }
    }

  for (i = 0; i < n; i++)
    for (j = i; j < n; j++)
      {
        sum = 0.0;
        if (cinv == NULL)
          break;
        for (k = j; k < n; k++)
          {
            sum += tt[k * n + j] * tt[k * n + i];
          }
        cinv[i * n + j] = cinv[j * n + i] = sum;
      }

  vlog (p, p, n);
  y = 2.0 * asum (p, n);

  free (tt);
  free (p);

  return y;

}

int
solvit (double *prod, double *rhs, int n, double *ans)
{
  //The coefficient matrix should be positive definite

  /*AT : changed this code to take in matrix in a linear array form*/
  double *ttt;
  double *b;
  double *p;
  int i;
  int ret;

  ZALLOC(ttt, n * n, double);
  ZALLOC(p, n, double);
  ZALLOC(b, n, double);

  copyarr (prod, ttt, n * n);
  copyarr (rhs, b, n);

  ret = choldc (ttt, n, p);
  if (ret < 0)
    return -1;  // not pos def
  cholsl (ttt, n, p, b, ans);

  free (ttt);
  free (b);
  free (p);

//  printf("zzsol: %d %g %g %g\n", n, prod[0], rhs[0], ans[0]) ;

  return 1;
}

void
cholsl (double *a, int n, double p[], double b[], double x[])

/** 
 Numerical Recipes.  Must change 
 */

{
  /*AT: Changing the code*/

  int i, k;
  double sum;

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

  for (i = (n - 1); i >= 0; i--)
    {
      sum = x[i];
      for (k = i + 1; k < n; k++)
        sum -= a[k * n + i] * x[k];
      x[i] = sum / p[i];
    }

}

int
choldc (double *a, int n, double p[])
{
  int i, j, k;
  double sum;

  for (i = 0; i < n; i++)
    {
      for (j = i; j < n; j++)
        {
          sum = a[i * n + j];
          for (k = i - 1; k >= 0; k--)
            sum -= a[i * n + k] * a[j * n + k];
          if (i == j)
            {
              /**                                       
               printf("zzchol %d %20.10f %9.3f\n",i, sum, a[i][i]) ; 
               */
              if (sum <= 0.0)
                {
                  return -1; // not pos def
                }
              p[i] = sqrt (sum);
            }
          else
            {
              a[j * n + i] = sum / p[i];

            }
        }
    }

  return 1;

}

void
pmat (double *mat, int n)

/** 
 print square matrix 
 */

{
  int i, j;
  double *diag;

  ZALLOC(diag,n, double);
  getdiag (diag, mat, n);

  printf ("pmat:\n");

  for (i = 0; i < n; i++)
    {
      printf ("diag %5d %9.3f\n", i, diag[i]);
      for (j = 0; j < n; j++)
        {
          if ((j % 10) == 9)
            printf ("\n");
          if ((n % 10) != 0)
            printf ("%9.3f ", mat[i * n + j]);
        }
      printf ("\n");
    }
  printf ("\n");
  printf ("\n");

  free (diag);
}

void
cholesky (double *cf, double *a, int n)
{
  int i, j, k;
  double *tt;
  double *p;

  ZALLOC(tt, n*n, double);
  ZALLOC(p, n, double);
  copyarr (a, tt, n * n);

  choldc (tt, n, p);

  vzero (cf, n * n);

  for (i = 0; i < n; i++)
    {
      tt[i * n + i] = p[i];

      for (j = 0; j <= i; j++)
        {
          k = i * n + j;
          cf[k] = tt[i * n + j];
        }
    }

  free (tt);
  free (p);
}
//==============================================================================
//
// Linear System Solution by Gauss method
//
// Developer: Henry Guennadi Levkin
//
//==============================================================================

int
linsolv (int n, double *pfMatr, double *pfVect, double *sol)
// 1 on failure 
{

  int ret;
  double *a, *rhs;

  ZALLOC(a, n*n, double);
  ZALLOC(rhs, n, double);

  copyarr (pfMatr, a, n * n);
  copyarr (pfVect, rhs, n);

  ret = linsolvx (n, a, rhs, sol);

  free (a);
  free (rhs);

  return ret;

}

int
linsolvx (int nDim, double *pfMatr, double *pfVect, double *pfSolution)

{
  double fMaxElem;
  double fAcc;

  int i, j, k, m;

  for (k = 0; k < (nDim - 1); k++) // base row of matrix
    {
      // search of line with max element
      fMaxElem = fabs (pfMatr[k * nDim + k]);
      m = k;
      for (i = k + 1; i < nDim; i++)
        {
          if (fMaxElem < fabs (pfMatr[i * nDim + k]))
            {
              fMaxElem = pfMatr[i * nDim + k];
              m = i;
            }
        }

      // permutation of base line (index k) and max element line(index m)
      if (m != k)
        {
          for (i = k; i < nDim; i++)
            {
              fAcc = pfMatr[k * nDim + i];
              pfMatr[k * nDim + i] = pfMatr[m * nDim + i];
              pfMatr[m * nDim + i] = fAcc;
            }
          fAcc = pfVect[k];
          pfVect[k] = pfVect[m];
          pfVect[m] = fAcc;
        }

      if (pfMatr[k * nDim + k] == 0.)
        return 1; // needs improvement !!!

      // triangulation of matrix with coefficients
      for (j = (k + 1); j < nDim; j++) // current row of matrix
        {
          fAcc = -pfMatr[j * nDim + k] / pfMatr[k * nDim + k];
          for (i = k; i < nDim; i++)
            {
              pfMatr[j * nDim + i] = pfMatr[j * nDim + i]
                  + fAcc * pfMatr[k * nDim + i];
            }
          pfVect[j] = pfVect[j] + fAcc * pfVect[k]; // free member recalculation
        }
    }

  for (k = (nDim - 1); k >= 0; k--)
    {
      pfSolution[k] = pfVect[k];
      for (i = (k + 1); i < nDim; i++)
        {
          pfSolution[k] -= (pfMatr[k * nDim + i] * pfSolution[i]);
        }
      pfSolution[k] = pfSolution[k] / pfMatr[k * nDim + k];
    }

  return 0;
}
