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

 *                                                                       *

 * 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.                     *

 *                                                                       *

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

/*

THE ALGORITHM

-------------

solve A*x = b+w, with x and w subject to certain LCP conditions.

each x(i),w(i) must lie on one of the three line segments in the following

diagram. each line segment corresponds to one index set :

     w(i)

     /|\      |           :

      |       |           :

      |       |i in N     :

  w>0 |       |state[i]=0 :

      |       |           :

      |       |           :  i in C

  w=0 +       +-----------------------+

      |                   :           |

      |                   :           |

  w<0 |                   :           |i in N

      |                   :           |state[i]=1

      |                   :           |

      |                   :           |

      +-------|-----------|-----------|----------> x(i)

             lo           0           hi

the Dantzig algorithm proceeds as follows:

  for i=1:n

    * if (x(i),w(i)) is not on the line, push x(i) and w(i) positive or

      negative towards the line. as this is done, the other (x(j),w(j))

      for j<i are constrained to be on the line. if any (x,w) reaches the

      end of a line segment then it is switched between index sets.

    * i is added to the appropriate index set depending on what line segment

      it hits.

we restrict lo(i) <= 0 and hi(i) >= 0. this makes the algorithm a bit

simpler, because the starting point for x(i),w(i) is always on the dotted

line x=0 and x will only ever increase in one direction, so it can only hit

two out of the three line segments.

NOTES

-----

this is an implementation of "lcp_dantzig2_ldlt.m" and "lcp_dantzig_lohi.m".

the implementation is split into an LCP problem object (dLCP) and an LCP

driver function. most optimization occurs in the dLCP object.

a naive implementation of the algorithm requires either a lot of data motion

or a lot of permutation-array lookup, because we are constantly re-ordering

rows and columns. to avoid this and make a more optimized algorithm, a

non-trivial data structure is used to represent the matrix A (this is

implemented in the fast version of the dLCP object).

during execution of this algorithm, some indexes in A are clamped (set C),

some are non-clamped (set N), and some are "don't care" (where x=0).

A,x,b,w (and other problem vectors) are permuted such that the clamped

indexes are first, the unclamped indexes are next, and the don't-care

indexes are last. this permutation is recorded in the array `p'.

initially p = 0..n-1, and as the rows and columns of A,x,b,w are swapped,

the corresponding elements of p are swapped.

because the C and N elements are grouped together in the rows of A, we can do

lots of work with a fast dot product function. if A,x,etc were not permuted

and we only had a permutation array, then those dot products would be much

slower as we would have a permutation array lookup in some inner loops.

A is accessed through an array of row pointers, so that element (i,j) of the

permuted matrix is A[i][j]. this makes row swapping fast. for column swapping

we still have to actually move the data.

during execution of this algorithm we maintain an L*D*L' factorization of

the clamped submatrix of A (call it `AC') which is the top left nC*nC

submatrix of A. there are two ways we could arrange the rows/columns in AC.

(1) AC is always permuted such that L*D*L' = AC. this causes a problem

    when a row/column is removed from C, because then all the rows/columns of A

    between the deleted index and the end of C need to be rotated downward.

    this results in a lot of data motion and slows things down.

(2) L*D*L' is actually a factorization of a *permutation* of AC (which is

    itself a permutation of the underlying A). this is what we do - the

    permutation is recorded in the vector C. call this permutation A[C,C].

    when a row/column is removed from C, all we have to do is swap two

    rows/columns and manipulate C.

*/

#include <ode/common.h>

#include "lcp.h"

#include <ode/matrix.h>

#include <ode/misc.h>

#include "mat.h"		// for testing

#include <ode/timer.h>  // for testing

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

// code generation parameters

#define dLCP_FAST		// use fast dLCP object

#define dUSE_MALLOC_FOR_ALLOCA

// option 1 : matrix row pointers (less data copying)

#define ROWPTRS

#define ATYPE dReal **

#define AROW(i) (A[i])

// option 2 : no matrix row pointers (slightly faster inner loops)

//#define NOROWPTRS

//#define ATYPE dReal *

//#define AROW(i) (A+(i)*nskip)

// use protected, non-stack memory allocation system

#ifdef dUSE_MALLOC_FOR_ALLOCA

extern unsigned int dMemoryFlag;

#define ALLOCA(t,v,s) t* v = (t*) malloc(s)

#define UNALLOCA(t)  free(t)

#else

#define ALLOCA(t,v,s) t* v =(t*)dALLOCA16(s)

#define UNALLOCA(t)  /* nothing */

#endif

#define NUB_OPTIMIZATIONS

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

// swap row/column i1 with i2 in the n*n matrix A. the leading dimension of

// A is nskip. this only references and swaps the lower triangle.

// if `do_fast_row_swaps' is nonzero and row pointers are being used, then

// rows will be swapped by exchanging row pointers. otherwise the data will

// be copied.

static void swapRowsAndCols (ATYPE A, int n, int i1, int i2, int /*nskip*/,

			     int do_fast_row_swaps)

{

  int i;

# ifdef ROWPTRS

  for (i=i1+1; i<i2; i++) A[i1][i] = A[i][i1];

  for (i=i1+1; i<i2; i++) A[i][i1] = A[i2][i];

  A[i1][i2] = A[i1][i1];

  A[i1][i1] = A[i2][i1];

  A[i2][i1] = A[i2][i2];

  // swap rows, by swapping row pointers

  if (do_fast_row_swaps) {

    dReal *tmpp;

    tmpp = A[i1];

    A[i1] = A[i2];

    A[i2] = tmpp;

  }

  else {

    ALLOCA (dReal,tmprow,n * sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (tmprow == NULL) {

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;      

      return;

    }

#endif

    memcpy (tmprow,A[i1],n * sizeof(dReal));

    memcpy (A[i1],A[i2],n * sizeof(dReal));

    memcpy (A[i2],tmprow,n * sizeof(dReal));

    UNALLOCA(tmprow);

  }

  // swap columns the hard way

  for (i=i2+1; i<n; i++) {

    dReal tmp = A[i][i1];

    A[i][i1] = A[i][i2];

    A[i][i2] = tmp;

  }

# else

  dReal tmp;

  ALLOCA (dReal,tmprow,n * sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

  if (tmprow == NULL) {

    return;

  }

#endif

  if (i1 > 0) {

    memcpy (tmprow,A+i1*nskip,i1*sizeof(dReal));

    memcpy (A+i1*nskip,A+i2*nskip,i1*sizeof(dReal));

    memcpy (A+i2*nskip,tmprow,i1*sizeof(dReal));

  }

  for (i=i1+1; i<i2; i++) {

    tmp = A[i2*nskip+i];

    A[i2*nskip+i] = A[i*nskip+i1];

    A[i*nskip+i1] = tmp;

  }

  tmp = A[i1*nskip+i1];

  A[i1*nskip+i1] = A[i2*nskip+i2];

  A[i2*nskip+i2] = tmp;

  for (i=i2+1; i<n; i++) {

    tmp = A[i*nskip+i1];

    A[i*nskip+i1] = A[i*nskip+i2];

    A[i*nskip+i2] = tmp;

  }

  UNALLOCA(tmprow);

# endif

}

// swap two indexes in the n*n LCP problem. i1 must be <= i2.

static void swapProblem (ATYPE A, dReal *x, dReal *b, dReal *w, dReal *lo,

			 dReal *hi, int *p, int *state, int *findex,

			 int n, int i1, int i2, int nskip,

			 int do_fast_row_swaps)

{

  dReal tmp;

  int tmpi;

  if (i1==i2) return;

  swapRowsAndCols (A,n,i1,i2,nskip,do_fast_row_swaps);

#ifdef dUSE_MALLOC_FOR_ALLOCA

  if (dMemoryFlag == d_MEMORY_OUT_OF_MEMORY)

    return;

#endif

  tmp = x[i1];

  x[i1] = x[i2];

  x[i2] = tmp;

  tmp = b[i1];

  b[i1] = b[i2];

  b[i2] = tmp;

  tmp = w[i1];

  w[i1] = w[i2];

  w[i2] = tmp;

  tmp = lo[i1];

  lo[i1] = lo[i2];

  lo[i2] = tmp;

  tmp = hi[i1];

  hi[i1] = hi[i2];

  hi[i2] = tmp;

  tmpi = p[i1];

  p[i1] = p[i2];

  p[i2] = tmpi;

  tmpi = state[i1];

  state[i1] = state[i2];

  state[i2] = tmpi;

  if (findex) {

    tmpi = findex[i1];

    findex[i1] = findex[i2];

    findex[i2] = tmpi;

  }

}

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

// dLCP manipulator object. this represents an n*n LCP problem.

//

// two index sets C and N are kept. each set holds a subset of

// the variable indexes 0..n-1. an index can only be in one set.

// initially both sets are empty.

//

// the index set C is special: solutions to A(C,C)\A(C,i) can be generated.

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

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

// fast implementation of dLCP. see the above definition of dLCP for

// interface comments.

//

// `p' records the permutation of A,x,b,w,etc. p is initially 1:n and is

// permuted as the other vectors/matrices are permuted.

//

// A,x,b,w,lo,hi,state,findex,p,c are permuted such that sets C,N have

// contiguous indexes. the don't-care indexes follow N.

//

// an L*D*L' factorization is maintained of A(C,C), and whenever indexes are

// added or removed from the set C the factorization is updated.

// thus L*D*L'=A[C,C], i.e. a permuted top left nC*nC submatrix of A.

// the leading dimension of the matrix L is always `nskip'.

//

// at the start there may be other indexes that are unbounded but are not

// included in `nub'. dLCP will permute the matrix so that absolutely all

// unbounded vectors are at the start. thus there may be some initial

// permutation.

//

// the algorithms here assume certain patterns, particularly with respect to

// index transfer.

#ifdef dLCP_FAST

struct dLCP {

  int n,nskip,nub;

  ATYPE A;				// A rows

  dReal *Adata,*x,*b,*w,*lo,*hi;	// permuted LCP problem data

  dReal *L,*d;				// L*D*L' factorization of set C

  dReal *Dell,*ell,*tmp;

  int *state,*findex,*p,*C;

  int nC,nN;				// size of each index set

  dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,

	dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,

	dReal *_Dell, dReal *_ell, dReal *_tmp,

	int *_state, int *_findex, int *_p, int *_C, dReal **Arows);

  int getNub() { return nub; }

  void transfer_i_to_C (int i);

  void transfer_i_to_N (int /*i*/)

    { nN++; }			// because we can assume C and N span 1:i-1

  void transfer_i_from_N_to_C (int i);

  void transfer_i_from_C_to_N (int i);

  int numC() { return nC; }

  int numN() { return nN; }

  int indexC (int i) { return i; }

  int indexN (int i) { return i+nC; }

  dReal Aii (int i) { return AROW(i)[i]; }

  dReal AiC_times_qC (int i, dReal *q) { return dDot (AROW(i),q,nC); }

  dReal AiN_times_qN (int i, dReal *q) { return dDot (AROW(i)+nC,q+nC,nN); }

  void pN_equals_ANC_times_qC (dReal *p, dReal *q);

  void pN_plusequals_ANi (dReal *p, int i, int sign=1);

  void pC_plusequals_s_times_qC (dReal *p, dReal s, dReal *q)

    { for (int i=0; i<nC; i++) p[i] += dMUL(s,q[i]); }

  void pN_plusequals_s_times_qN (dReal *p, dReal s, dReal *q)

    { for (int i=0; i<nN; i++) p[i+nC] += dMUL(s,q[i+nC]); }

  void solve1 (dReal *a, int i, int dir=1, int only_transfer=0);

  void unpermute();

};

dLCP::dLCP (int _n, int _nub, dReal *_Adata, dReal *_x, dReal *_b, dReal *_w,

	    dReal *_lo, dReal *_hi, dReal *_L, dReal *_d,

	    dReal *_Dell, dReal *_ell, dReal *_tmp,

	    int *_state, int *_findex, int *_p, int *_C, dReal **Arows)

{

  n = _n;

  nub = _nub;

  Adata = _Adata;

  A = 0;

  x = _x;

  b = _b;

  w = _w;

  lo = _lo;

  hi = _hi;

  L = _L;

  d = _d;

  Dell = _Dell;

  ell = _ell;

  tmp = _tmp;

  state = _state;

  findex = _findex;

  p = _p;

  C = _C;

  nskip = dPAD(n);

  dSetZero (x,n);

  int k;

# ifdef ROWPTRS

  // make matrix row pointers

  A = Arows;

  for (k=0; k<n; k++) A[k] = Adata + k*nskip;

# else

  A = Adata;

# endif

  nC = 0;

  nN = 0;

  for (k=0; k<n; k++) p[k]=k;		// initially unpermuted

  /*

  // for testing, we can do some random swaps in the area i > nub

  if (nub < n) {

    for (k=0; k<100; k++) {

      int i1,i2;

      do {

	i1 = dRandInt(n-nub)+nub;

	i2 = dRandInt(n-nub)+nub;

      }

      while (i1 > i2); 

      //printf ("--> %d %d\n",i1,i2);

      swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i1,i2,nskip,0);

    }

  }

  */

  // permute the problem so that *all* the unbounded variables are at the

  // start, i.e. look for unbounded variables not included in `nub'. we can

  // potentially push up `nub' this way and get a bigger initial factorization.

  // note that when we swap rows/cols here we must not just swap row pointers,

  // as the initial factorization relies on the data being all in one chunk.

  // variables that have findex >= 0 are *not* considered to be unbounded even

  // if lo=-inf and hi=inf - this is because these limits may change during the

  // solution process.

  for (k=nub; k<n; k++) {

    if (findex && findex[k] >= 0) continue;

    if (lo[k]==-dInfinity && hi[k]==dInfinity) {

      swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nub,k,nskip,0);

      nub++;

    }

  }

  // if there are unbounded variables at the start, factorize A up to that

  // point and solve for x. this puts all indexes 0..nub-1 into C.

  if (nub > 0) {

    for (k=0; k<nub; k++) memcpy (L+k*nskip,AROW(k),(k+1)*sizeof(dReal));

    dFactorLDLT (L,d,nub,nskip);

    memcpy (x,b,nub*sizeof(dReal));

    dSolveLDLT (L,d,x,nub,nskip);

    dSetZero (w,nub);

    for (k=0; k<nub; k++) C[k] = k;

    nC = nub;

  }

  // permute the indexes > nub such that all findex variables are at the end

  if (findex) {

    int num_at_end = 0;

    for (k=n-1; k >= nub; k--) {

      if (findex[k] >= 0) {

	swapProblem (A,x,b,w,lo,hi,p,state,findex,n,k,n-1-num_at_end,nskip,1);

	num_at_end++;

      }

    }

  }

  // print info about indexes

  /*

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

    if (k<nub) printf ("C");

    else if (lo[k]==-dInfinity && hi[k]==dInfinity) printf ("c");

    else printf (".");

  }

  printf ("\n");

  */

}

void dLCP::transfer_i_to_C (int i)

{

  int j;

  if (nC > 0) {

    // ell,Dell were computed by solve1(). note, ell = D \ L1solve (L,A(i,C))

    for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];

    d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));

  }

  else {

    d[0] = dRecip (AROW(i)[i]);

  }

  swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);

  C[nC] = nC;

  nC++;

}

void dLCP::transfer_i_from_N_to_C (int i)

{

  int j;

  if (nC > 0) {

    dReal *aptr = AROW(i);

#   ifdef NUB_OPTIMIZATIONS

    // if nub>0, initial part of aptr unpermuted

    for (j=0; j<nub; j++) Dell[j] = aptr[j];

    for (j=nub; j<nC; j++) Dell[j] = aptr[C[j]];

#   else

    for (j=0; j<nC; j++) Dell[j] = aptr[C[j]];

#   endif

    dSolveL1 (L,Dell,nC,nskip);

    for (j=0; j<nC; j++) ell[j] = dMUL(Dell[j],d[j]);

    for (j=0; j<nC; j++) L[nC*nskip+j] = ell[j];

    d[nC] = dRecip (AROW(i)[i] - dDot(ell,Dell,nC));

  }

  else {

    d[0] = dRecip (AROW(i)[i]);

  }

  swapProblem (A,x,b,w,lo,hi,p,state,findex,n,nC,i,nskip,1);

  C[nC] = nC;

  nN--;

  nC++;

  // @@@ TO DO LATER

  // if we just finish here then we'll go back and re-solve for

  // delta_x. but actually we can be more efficient and incrementally

  // update delta_x here. but if we do this, we wont have ell and Dell

  // to use in updating the factorization later.

}

void dLCP::transfer_i_from_C_to_N (int i)

{

  // remove a row/column from the factorization, and adjust the

  // indexes (black magic!)

  int j,k;

  for (j=0; j<nC; j++) if (C[j]==i) {

    dLDLTRemove (A,C,L,d,n,nC,j,nskip);

    for (k=0; k<nC; k++) if (C[k]==nC-1) {

      C[k] = C[j];

      if (j < (nC-1)) memmove (C+j,C+j+1,(nC-j-1)*sizeof(int));

      break;

    }

    break;

  }

  swapProblem (A,x,b,w,lo,hi,p,state,findex,n,i,nC-1,nskip,1);

  nC--;

  nN++;