/*************************************************************************

 *                                                                       *

 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *

 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *

 *                                                                       *

 * This library is free software; you can redistribute it and/or         *

 * modify it under the terms of EITHER:                                  *

 *   (1) The GNU Lesser General Public License as published by the Free  *

 *       Software Foundation; either version 2.1 of the License, or (at  *

 *       your option) any later version. The text of the GNU Lesser      *

 *       General Public License is included with this library in the     *

 *       file LICENSE.TXT.                                               *

 *   (2) The BSD-style license that is included with this library in     *

 *       the file LICENSE-BSD.TXT.                                       *

 *                                                                       *

 * This library is distributed in the hope that it will be useful,       *

 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *

 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *

 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *

 *                                                                       *

 *************************************************************************/

#include <ode/common.h>

#include <ode/matrix.h>

// misc defines

//#define ALLOCA dALLOCA16

EXPORT_C void dSetZero (dReal *a, int n)

{

  while (n > 0) {

    *(a++) = 0;

    n--;

  }

}

EXPORT_C void dSetValue (dReal *a, int n, dReal value)

{

  while (n > 0) {

    *(a++) = value;

    n--;

  }

}

EXPORT_C void dMultiply0 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)

{

  int i,j,k,qskip,rskip,rpad;

  qskip = dPAD(q);

  rskip = dPAD(r);

  rpad = rskip - r;

  dReal sum;

  const dReal *b,*c,*bb;

  bb = B;

  for (i=p; i; i--) {

    for (j=0 ; j<r; j++) {

      c = C + j;

      b = bb;

      sum = 0;

      for (k=q; k; k--, c+=rskip) sum += dMUL((*(b++)),(*c));

      *(A++) = sum; 

    }

    A += rpad;

    bb += qskip;

  }

}

EXPORT_C void dMultiply1 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)

{

  int i,j,k,pskip,rskip;

  dReal sum;

  pskip = dPAD(p);

  rskip = dPAD(r);

  for (i=0; i<p; i++) {

    for (j=0; j<r; j++) {

      sum = 0;

      for (k=0; k<q; k++) sum += dMUL(B[i+k*pskip],C[j+k*rskip]);

      A[i*rskip+j] = sum;

    }

  }

}

EXPORT_C void dMultiply2 (dReal *A, const dReal *B, const dReal *C, int p, int q, int r)

{

  int i,j,k,z,rpad,qskip;

  dReal sum;

  const dReal *bb,*cc;

  rpad = dPAD(r) - r;

  qskip = dPAD(q);

  bb = B;

  for (i=p; i; i--) {

    cc = C;

    for (j=r; j; j--) {

      z = 0;

      sum = 0;

      for (k=q; k; k--,z++) sum += dMUL(bb[z],cc[z]);

      *(A++) = sum; 

      cc += qskip;

    }

    A += rpad;

    bb += qskip;

  }

}

EXPORT_C int dFactorCholesky (dReal *A, int n)

{

  int i,j,k,nskip;

  dReal sum,*a,*b,*aa,*bb,*cc,*recip;

  nskip = dPAD (n);

  recip = (dReal*) malloc (n * sizeof(dReal));

  if(recip == NULL) {

  	return 0;

  }

  aa = A;

  for (i=0; i<n; i++) {

    bb = A;

    cc = A + i*nskip;

    for (j=0; j<i; j++) {

      sum = *cc;

      a = aa;

      b = bb;

      for (k=j; k; k--) sum -= dMUL((*(a++)),(*(b++)));

      *cc = dMUL(sum,recip[j]);

      bb += nskip;

      cc++;

    }

    sum = *cc;

    a = aa;

    for (k=i; k; k--, a++) sum -= dMUL((*a),(*a));

    if (sum <= REAL(0.0)) {

        free(recip);

        return 0;

    }

    *cc = dSqrt(sum);

    recip[i] = dRecip (*cc);

    aa += nskip;

  }

  free(recip);

  return 1;

}

EXPORT_C void dSolveCholesky (const dReal *L, dReal *b, int n)

{

  int i,k,nskip;

  dReal sum,*y;

  nskip = dPAD (n);

  y = (dReal*) malloc (n*sizeof(dReal));

  if (y == NULL) {

  	return;

  }

  for (i=0; i<n; i++) {

    sum = 0;

    for (k=0; k < i; k++) sum += dMUL(L[i*nskip+k],y[k]);

    y[i] = dDIV((b[i]-sum),L[i*nskip+i]);

  }

  for (i=n-1; i >= 0; i--) {

    sum = 0;

    for (k=i+1; k < n; k++) sum += dMUL(L[k*nskip+i],b[k]);

    b[i] = dDIV((y[i]-sum),L[i*nskip+i]);

  }

  free(y);

}

EXPORT_C int dInvertPDMatrix (const dReal *A, dReal *Ainv, int n)

{

  int i,j,nskip;

  dReal *L,*x;

  nskip = dPAD (n);

  L = (dReal*) malloc (nskip*n*sizeof(dReal));

  if (L == NULL) {

  	return 0;

  }

  memcpy (L,A,nskip*n*sizeof(dReal));

  x = (dReal*) malloc (n*sizeof(dReal));

  if (x == NULL) {

  	free(L);

  	return 0;

  }

  if (dFactorCholesky (L,n)==0) {

    free(L);

    free(x);

    return 0;

  }

  dSetZero (Ainv,n*nskip);	// make sure all padding elements set to 0

  for (i=0; i<n; i++) {

    for (j=0; j<n; j++) x[j] = REAL(0.0);

    x[i] = REAL(1.0);

    dSolveCholesky (L,x,n);

    for (j=0; j<n; j++) Ainv[j*nskip+i] = x[j];

  }

  free(L);

  free(x);

  return 1;  

}

EXPORT_C int dIsPositiveDefinite (const dReal *A, int n)

{

  dReal *Acopy;

  int nskip = dPAD (n);

  Acopy = (dReal*) malloc (nskip*n * sizeof(dReal));

  if ( Acopy == NULL) {

  	return 0;

  }

  memcpy (Acopy,A,nskip*n * sizeof(dReal));

  int ret = dFactorCholesky (Acopy,n);

  free(Acopy);

  return ret;

}

/***** this has been replaced by a faster version

void dSolveL1T (const dReal *L, dReal *b, int n, int nskip)

{

  int i,j;

  dReal sum;

  for (i=n-2; i>=0; i--) {

    sum = 0;

    for (j=i+1; j<n; j++) sum += L[j*nskip+i]*b[j];

    b[i] -= sum;

  }

}

*/

EXPORT_C void dVectorScale (dReal *a, const dReal *d, int n)

{

  for (int i=0; i<n; i++) a[i] = dMUL(a[i],d[i]);

}

EXPORT_C void dSolveLDLT (const dReal *L, const dReal *d, dReal *b, int n, int nskip)

{

  dSolveL1 (L,b,n,nskip);

  dVectorScale (b,d,n);

  dSolveL1T (L,b,n,nskip);

}

EXPORT_C void dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip)

{

  int j,p;

  dReal *W1,*W2,W11,W21,alpha1,alpha2,alphanew,gamma1,gamma2,k1,k2,Wp,ell,dee;

  if (n < 2) return;

  W1 = (dReal*) malloc (n*sizeof(dReal));

  if (W1 == NULL) {

  	return;

  }

  W2 = (dReal*) malloc (n*sizeof(dReal));

  if (W2 == NULL) {

  	free(W1);

  	return;

  }

  W1[0] = 0;

  W2[0] = 0;

  for (j=1; j<n; j++) W1[j] = W2[j] = dMUL(a[j],dSQRT1_2);

  W11 = dMUL((dMUL(REAL(0.5),a[0])+REAL(1.0)),dSQRT1_2);

  W21 = dMUL((dMUL(REAL(0.5),a[0])-REAL(1.0)),dSQRT1_2);

  alpha1=REAL(1.0);

  alpha2=REAL(1.0);

  dee = d[0];

  alphanew = alpha1 + dMUL(dMUL(W11,W11),dee);

  dee = dDIV(dee,alphanew);

  gamma1 = dMUL(W11,dee);

  dee = dMUL(dee,alpha1);

  alpha1 = alphanew;

  alphanew = alpha2 - dMUL(dMUL(W21,W21),dee);

  dee = dDIV(dee,alphanew);

  gamma2 = dMUL(W21,dee);

  alpha2 = alphanew;

  k1 = REAL(1.0) - dMUL(W21,gamma1);

  k2 = dMUL(W21,dMUL(gamma1,W11)) - W21;

  for (p=1; p<n; p++) {

    Wp = W1[p];

    ell = L[p*nskip];

    W1[p] =    Wp - dMUL(W11,ell);

    W2[p] = dMUL(k1,Wp) +  dMUL(k2,ell);

  }

  for (j=1; j<n; j++) {

    dee = d[j];

    alphanew = alpha1 + dMUL(dMUL(W1[j],W1[j]),dee);

    dee = dDIV(dee,alphanew);

    gamma1 = dMUL(W1[j],dee);

    dee = dMUL(dee,alpha1);

    alpha1 = alphanew;

    alphanew = alpha2 - dMUL(dMUL(W2[j],W2[j]),dee);

    dee = dDIV(dee,alphanew);

    gamma2 = dMUL(W2[j],dee);

    dee = dMUL(dee,alpha2);

    d[j] = dee;

    alpha2 = alphanew;

    k1 = W1[j];

    k2 = W2[j];

    for (p=j+1; p<n; p++) {

      ell = L[p*nskip+j];

      Wp = W1[p] - dMUL(k1,ell);

      ell += dMUL(gamma1,Wp);

      W1[p] = Wp;

      Wp = W2[p] - dMUL(k2,ell);

      ell -= dMUL(gamma2,Wp);

      W2[p] = Wp;

      L[p*nskip+j] = ell;

    }

  }

  free(W1);

  free(W2);

}

// macros for dLDLTRemove() for accessing A - either access the matrix

// directly or access it via row pointers. we are only supposed to reference

// the lower triangle of A (it is symmetric), but indexes i and j come from

// permutation vectors so they are not predictable. so do a test on the

// indexes - this should not slow things down too much, as we don't do this

// in an inner loop.

#define _GETA(i,j) (A[i][j])

//#define _GETA(i,j) (A[(i)*nskip+(j)])

#define GETA(i,j) ((i > j) ? _GETA(i,j) : _GETA(j,i))

EXPORT_C void dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,

		  int /*n1*/, int n2, int r, int nskip)

{

  int i;

  if (r==n2-1) {

    return;		// deleting last row/col is easy

  }

  else if (r==0) {

    dReal *a = (dReal*) malloc (n2 * sizeof(dReal));

    if (a == NULL) {

    	return;

    }

    for (i=0; i<n2; i++) a[i] = -GETA(p[i],p[0]);

    a[0] += REAL(1.0);

    dLDLTAddTL (L,d,a,n2,nskip);

    free(a);

  }

  else {

    dReal *t = (dReal*) malloc (r * sizeof(dReal));

    if (t == NULL) {

    	return;

    }

    dReal *a = (dReal*) malloc ((n2-r) * sizeof(dReal));

    if (a == NULL) {

    	free(t);

    	return;

    }

    for (i=0; i<r; i++) t[i] = dDIV(L[r*nskip+i],d[i]);

    for (i=0; i<(n2-r); i++)

      a[i] = dDot(L+(r+i)*nskip,t,r) - GETA(p[r+i],p[r]);

    a[0] += REAL(1.0);

    dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip);

    free(t);

    free(a);

  }

  // snip out row/column r from L and d

  dRemoveRowCol (L,n2,nskip,r);

  if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal));

}

EXPORT_C void dRemoveRowCol (dReal *A, int n, int nskip, int r)

{

  int i;

  if (r >= n-1) return;

  if (r > 0) {

    for (i=0; i<r; i++)

      memmove (A+i*nskip+r,A+i*nskip+r+1,(n-r-1)*sizeof(dReal));

    for (i=r; i<(n-1); i++)

      memcpy (A+i*nskip,A+i*nskip+nskip,r*sizeof(dReal));

  }

  for (i=r; i<(n-1); i++)

    memcpy (A+i*nskip+r,A+i*nskip+nskip+r+1,(n-r-1)*sizeof(dReal));

}