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

 *                                                                       *

 * 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 "object.h"

#include "joint.h"

#include <ode/config.h>

#include <ode/odemath.h>

#include <ode/rotation.h>

#include <ode/timer.h>

#include <ode/error.h>

#include <ode/matrix.h>

#include "lcp.h"

#include "util.h"

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

// misc defines

// memory allocation system

#ifdef dUSE_MALLOC_FOR_ALLOCA

unsigned int dMemoryFlag;

#define REPORT_OUT_OF_MEMORY fprintf(stderr, "Insufficient memory to complete rigid body simulation.  Results will not be accurate.\n")

#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

// undef to use the fast decomposition

#define DIRECT_CHOLESKY

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

// special matrix multipliers

// this assumes the 4th and 8th rows of B and C are zero.

static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,

			   int p, int r, int Askip)

{

  int i,j;

  dReal sum,*bb,*cc;

  bb = B;

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

    cc = C;

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

      sum = dMUL(bb[0],cc[0]);

      sum += dMUL(bb[1],cc[1]);

      sum += dMUL(bb[2],cc[2]);

      sum += dMUL(bb[4],cc[4]);

      sum += dMUL(bb[5],cc[5]);

      sum += dMUL(bb[6],cc[6]);

      *(A++) = sum; 

      cc += 8;

    }

    A += Askip - r;

    bb += 8;

  }

}

// this assumes the 4th and 8th rows of B and C are zero.

static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,

			      int p, int r, int Askip)

{

  int i,j;

  dReal sum,*bb,*cc;

  bb = B;

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

    cc = C;

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

      sum = dMUL(bb[0],cc[0]);

      sum += dMUL(bb[1],cc[1]);

      sum += dMUL(bb[2],cc[2]);

      sum += dMUL(bb[4],cc[4]);

      sum += dMUL(bb[5],cc[5]);

      sum += dMUL(bb[6],cc[6]);

      *(A++) += sum; 

      cc += 8;

    }

    A += Askip - r;

    bb += 8;

  }

}

// this assumes the 4th and 8th rows of B are zero.

static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)

{

  int i;

  dReal sum;

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

    sum =  dMUL(B[0],C[0]);

    sum += dMUL(B[1],C[1]);

    sum += dMUL(B[2],C[2]);

    sum += dMUL(B[4],C[4]);

    sum += dMUL(B[5],C[5]);

    sum += dMUL(B[6],C[6]);

    *(A++) = sum;

    B += 8;

  }

}

// this assumes the 4th and 8th rows of B are zero.

static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)

{

  int i;

  dReal sum;

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

    sum =  dMUL(B[0],C[0]);

    sum += dMUL(B[1],C[1]);

    sum += dMUL(B[2],C[2]);

    sum += dMUL(B[4],C[4]);

    sum += dMUL(B[5],C[5]);

    sum += dMUL(B[6],C[6]);

    *(A++) += sum;

    B += 8;

  }

}

// this assumes the 4th and 8th rows of B are zero.

static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)

{

  int k;

  dReal sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[k*8],C[k]);

  A[0] += sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[1+k*8],C[k]);

  A[1] += sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[2+k*8],C[k]);

  A[2] += sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[4+k*8],C[k]);

  A[4] += sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[5+k*8],C[k]);

  A[5] += sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[6+k*8],C[k]);

  A[6] += sum;

}

// this assumes the 4th and 8th rows of B are zero.

static void Multiply1_8q1 (dReal *A, dReal *B, dReal *C, int q)

{

  int k;

  dReal sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[k*8],C[k]);

  A[0] = sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[1+k*8],C[k]);

  A[1] = sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[2+k*8],C[k]);

  A[2] = sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[4+k*8],C[k]);

  A[4] = sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[5+k*8],C[k]);

  A[5] = sum;

  sum = 0;

  for (k=0; k<q; k++) sum += dMUL(B[6+k*8],C[k]);

  A[6] = sum;

}

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

// the slow, but sure way

// note that this does not do any joint feedback!

// given lists of bodies and joints that form an island, perform a first

// order timestep.

//

// `body' is the body array, `nb' is the size of the array.

// `_joint' is the body array, `nj' is the size of the array.

void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int nb,

			     dxJoint * const *_joint, int nj, dReal stepsize)

{

  int i,j,k;

  int n6 = 6*nb;

  // number all bodies in the body list - set their tag values

  for (i=0; i<nb; i++) body[i]->tag = i;

  // make a local copy of the joint array, because we might want to modify it.

  // (the "dxJoint *const*" declaration says we're allowed to modify the joints

  // but not the joint array, because the caller might need it unchanged).

  ALLOCA(dxJoint*,joint,nj*sizeof(dxJoint*));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (joint == NULL) {

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  memcpy (joint,_joint,nj * sizeof(dxJoint*));

  // for all bodies, compute the inertia tensor and its inverse in the global

  // frame, and compute the rotational force and add it to the torque

  // accumulator.

  // @@@ check computation of rotational force.

  ALLOCA(dReal,I,3*nb*4*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (I == NULL) {

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  ALLOCA(dReal,invI,3*nb*4*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (invI == NULL) {

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  //dSetZero (I,3*nb*4);

  //dSetZero (invI,3*nb*4);

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

    dReal tmp[12];

    // compute inertia tensor in global frame

    dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->posr.R);

    dMULTIPLY0_333 (I+i*12,body[i]->posr.R,tmp);

    // compute inverse inertia tensor in global frame

    dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->posr.R);

    dMULTIPLY0_333 (invI+i*12,body[i]->posr.R,tmp);

    // compute rotational force

    dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);

    dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);

  }

  // add the gravity force to all bodies

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

    if ((body[i]->flags & dxBodyNoGravity)==0) {

      body[i]->facc[0] += dMUL(body[i]->mass.mass,world->gravity[0]);

      body[i]->facc[1] += dMUL(body[i]->mass.mass,world->gravity[1]);

      body[i]->facc[2] += dMUL(body[i]->mass.mass,world->gravity[2]);

    }

  }

  // get m = total constraint dimension, nub = number of unbounded variables.

  // create constraint offset array and number-of-rows array for all joints.

  // the constraints are re-ordered as follows: the purely unbounded

  // constraints, the mixed unbounded + LCP constraints, and last the purely

  // LCP constraints.

  //

  // joints with m=0 are inactive and are removed from the joints array

  // entirely, so that the code that follows does not consider them.

  int m = 0;

  ALLOCA(dxJoint::Info1,info,nj*sizeof(dxJoint::Info1));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (info == NULL) {

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  ALLOCA(int,ofs,nj*sizeof(int));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (ofs == NULL) {

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  for (i=0, j=0; j<nj; j++) {	// i=dest, j=src

    joint[j]->vtable->getInfo1 (joint[j],info+i);

    if (info[i].m > 0) {

      joint[i] = joint[j];

      i++;

    }

  }

  nj = i;

  // the purely unbounded constraints

  for (i=0; i<nj; i++) if (info[i].nub == info[i].m) {

    ofs[i] = m;

    m += info[i].m;

  }

  // the mixed unbounded + LCP constraints

  for (i=0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) {

    ofs[i] = m;

    m += info[i].m;

  }

  // the purely LCP constraints

  for (i=0; i<nj; i++) if (info[i].nub == 0) {

    ofs[i] = m;

    m += info[i].m;

  }

  // create (6*nb,6*nb) inverse mass matrix `invM', and fill it with mass

  // parameters

  int nskip = dPAD (n6);

  ALLOCA(dReal, invM, n6*nskip*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (invM == NULL) {

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  dSetZero (invM,n6*nskip);

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

    dReal *MM = invM+(i*6)*nskip+(i*6);

    MM[0] = body[i]->invMass;

    MM[nskip+1] = body[i]->invMass;

    MM[2*nskip+2] = body[i]->invMass;

    MM += 3*nskip+3;

    for (j=0; j<3; j++) for (k=0; k<3; k++) {

      MM[j*nskip+k] = invI[i*12+j*4+k];

    }

  }

  // assemble some body vectors: fe = external forces, v = velocities

  ALLOCA(dReal,fe,n6*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (fe == NULL) {

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  ALLOCA(dReal,v,n6*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (v == NULL) {

      UNALLOCA(fe);

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  //dSetZero (fe,n6);

  //dSetZero (v,n6);

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

    for (j=0; j<3; j++) fe[i*6+j] = body[i]->facc[j];

    for (j=0; j<3; j++) fe[i*6+3+j] = body[i]->tacc[j];

    for (j=0; j<3; j++) v[i*6+j] = body[i]->lvel[j];

    for (j=0; j<3; j++) v[i*6+3+j] = body[i]->avel[j];

  }

  // this will be set to the velocity update

  ALLOCA(dReal,vnew,n6*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (vnew == NULL) {

      UNALLOCA(v);

      UNALLOCA(fe);

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

  dSetZero (vnew,n6);

  // if there are constraints, compute cforce

  if (m > 0) {

    // create a constraint equation right hand side vector `c', a constraint

    // force mixing vector `cfm', and LCP low and high bound vectors, and an

    // 'findex' vector.

    ALLOCA(dReal,c,m*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (c == NULL) {

      UNALLOCA(vnew);

      UNALLOCA(v);

      UNALLOCA(fe);

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

    ALLOCA(dReal,cfm,m*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (cfm == NULL) {

      UNALLOCA(c);

      UNALLOCA(vnew);

      UNALLOCA(v);

      UNALLOCA(fe);

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

    ALLOCA(dReal,lo,m*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (lo == NULL) {

      UNALLOCA(cfm);

      UNALLOCA(c);

      UNALLOCA(vnew);

      UNALLOCA(v);

      UNALLOCA(fe);

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

    ALLOCA(dReal,hi,m*sizeof(dReal));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (hi == NULL) {

      UNALLOCA(lo);

      UNALLOCA(cfm);

      UNALLOCA(c);

      UNALLOCA(vnew);

      UNALLOCA(v);

      UNALLOCA(fe);

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

    ALLOCA(int,findex,m*sizeof(int));

#ifdef dUSE_MALLOC_FOR_ALLOCA

    if (findex == NULL) {

      UNALLOCA(hi);

      UNALLOCA(lo);

      UNALLOCA(cfm);

      UNALLOCA(c);

      UNALLOCA(vnew);

      UNALLOCA(v);

      UNALLOCA(fe);

      UNALLOCA(invM);

      UNALLOCA(ofs);

      UNALLOCA(info);

      UNALLOCA(invI);

      UNALLOCA(I);

      UNALLOCA(joint);

      dMemoryFlag = d_MEMORY_OUT_OF_MEMORY;

      return;

    }

#endif

    dSetZero (c,m);

    dSetValue (cfm,m,world->global_cfm);

    dSetValue (lo,m,-dInfinity);

    dSetValue (hi,m, dInfinity);

    for (i=0; i<m; i++) findex[i] = -1;

    // create (m,6*nb) jacobian mass matrix `J', and fill it with constraint