/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001-2003 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 "objects.h"
#include "joints/joint.h"
#include <ode/odeconfig.h>
#include "config.h"
#include <ode/odemath.h>
#include <ode/rotation.h>
#include <ode/timer.h>
#include <ode/error.h>
#include <ode/matrix.h>
#include <ode/misc.h>
#include "lcp.h"
#include "util.h"

#ifdef PSP_MEMCPY
	inline void MEM_COPY_4(void * dst, void * src, int len) {
		int * psrc = (int*)src;
		int * pdst = (int*)dst;
		while (len > 0) {
			*pdst = *psrc;
			pdst++; psrc++; len=len-4;
		}
	}

	typedef struct {
	     int val[4];
	} u128;
	inline void MEM_COPY_16(void * dst, void * src, int len) {
		u128 * psrc = (u128*)src;
		u128 * pdst = (u128*)dst;
		while (len > 0) {
			*pdst = *psrc;
			pdst++; psrc++; len=len-16;
		}
	}
	#undef u128
#else
	#define   MEM_COPY_4(dst,src,len) memcpy(dst,src,len);
	#define   MEM_COPY_16(dst,src,len) memcpy(dst,src,len);
#endif

#define ALLOCA dALLOCA16

typedef const dReal *dRealPtr;
typedef dReal *dRealMutablePtr;
#define dRealAllocaArray(name,n) dReal *name = (dReal*) ALLOCA ((n)*sizeof(dReal));

//***************************************************************************
// configuration

// for the SOR and CG methods:
// uncomment the following line to use warm starting. this definitely
// help for motor-driven joints. unfortunately it appears to hurt
// with high-friction contacts using the SOR method. use with care

//#define WARM_STARTING 1


// for the SOR method:
// uncomment the following line to determine a new constraint-solving
// order for each iteration. however, the qsort per iteration is expensive,
// and the optimal order is somewhat problem dependent.
// @@@ try the leaf->root ordering.

//#define REORDER_CONSTRAINTS 1


// for the SOR method:
// uncomment the following line to randomly reorder constraint rows
// during the solution. depending on the situation, this can help a lot
// or hardly at all, but it doesn't seem to hurt.

#define RANDOMLY_REORDER_CONSTRAINTS 1

//****************************************************************************
// special matrix multipliers
// multiply block of B matrix (q x 6) with 12 dReal per row with C vektor (q)
#ifdef PSPOPTS_QUICKSTEP
// Everything can be unaligned!! Difficult to align, but should improve a little
static void Multiply1_12q1 (dReal *A, dReal *B, dReal *C, int q) {
    int i, k;
  dIASSERT (q>0 && A && B && C);

__asm__ volatile ("vzero.q  R002\n vzero.p R003\n");
	
  for(i=0, k = 0; i<q; i++, k += 12) {
	__asm__ volatile (
	"lv.s     S033, %1\n"
	"ulv.q    R000, %0\n"
	"vscl.q   R000, R000, S033\n"
	"lv.s     S001, 16+%0\n"
	"lv.s     S011, 20+%0\n"
	"vscl.p   R001, R001, S033\n"
	"vadd.q   R002, R002, R000\n"
	"vadd.p   R003, R003, R001\n"
	: : "m"(B[k]),"m"(C[i]):"$f0" );
  }

	__asm__ volatile (
	"usv.q    R002, %0\n"
	"sv.s     S003, 16+ %0\n"
	"sv.s     S013, 20+ %0\n"
	: "+m"(A[0]));
}
#else
static void Multiply1_12q1 (dReal *A, dReal *B, dReal *C, int q) {
    int i, k;
  dIASSERT (q>0 && A && B && C);

  dReal ACC[6];
  dReal s;
  for(i=0, k = 0; i<q; i++, k += 12) {
    s = C[i]; //C[i] and B[n+k] cannot overlap because its value has been read into a temporary.

    //For the rest of the loop, the only memory dependency (array) is from B[]
    ACC[0] += B[  k] * s;
    ACC[1] += B[1+k] * s;
    ACC[2] += B[2+k] * s;
    ACC[3] += B[3+k] * s;
    ACC[4] += B[4+k] * s;
    ACC[5] += B[5+k] * s;
  }

  A[0] = ACC[0];
  A[1] = ACC[1];
  A[2] = ACC[2];
  A[3] = ACC[3];
  A[4] = ACC[4];
  A[5] = ACC[5];
}
#endif

//***************************************************************************
// testing stuff

#ifdef TIMING
#define IFTIMING(x) x
#else
#define IFTIMING(x) /* */
#endif

//***************************************************************************
// various common computations involving the matrix J

// compute iMJ = inv(M)*J'


static void compute_invM_JT (int m, dRealMutablePtr J, dRealMutablePtr iMJ, int *jb, dxBody * const *body, dRealPtr invI) {
	int i,j;
	dRealMutablePtr iMJ_ptr = iMJ;
	dRealMutablePtr J_ptr = J;
	for (i=0; i<m; i++) {
		int b1 = jb[i*2];
		int b2 = jb[i*2+1];
		dReal k = body[b1]->invMass;
		for (j=0; j<3; j++) iMJ_ptr[j] = k*J_ptr[j];
		dMULTIPLY0_331_ (iMJ_ptr + 3, invI + 12*b1, J_ptr + 3);
		if (b2 >= 0) {
			k = body[b2]->invMass;
			for (j=0; j<3; j++) iMJ_ptr[j+6] = k*J_ptr[j+6];
			dMULTIPLY0_331_ (iMJ_ptr + 9, invI + 12*b2, J_ptr + 9);
		}
		J_ptr += 12;
		iMJ_ptr += 12;
	}
}

#ifdef PSPOPTS_QUICKSTEP
static void multiply_J (int m, dRealMutablePtr J, int *jb, dRealMutablePtr in, dRealMutablePtr out) {
	int i,j;
	dRealPtr J_ptr = J;
	for (i=0; i<m; i++) {
		int b1 = jb[i*2];
		int b2 = jb[i*2+1];
		dRealMutablePtr in_ptr = in + b1*6;
		__asm__ volatile (
//		"lv.s S000, 0+%0\n  lv.s S010, 4+%0\n  lv.s S020, 8+%0\n"
//		"lv.s S030, 12+%0\n lv.s S001, 16+%0\n lv.s S011, 20+%0\n"
//		"lv.s S002, 0+%1\n  lv.s S012, 4+%1\n  lv.s S022, 8+%1\n"
//		"lv.s S032, 12+%1\n lv.s S003, 16+%1\n lv.s S013, 20+%1\n"
		"ulv.q	R000,   %0\n"
		"lv.s S001, 16+%0\n lv.s S011, 20+%0\n"
		"ulv.q	R002,   %1\n"
		"lv.s S003, 16+%1\n lv.s S013, 20+%1\n"
		
		"vdot.q S023, R000, R002\n"
		"vdot.p S033, R001, R003\n"
		"vadd.s S100, S033, S023\n"
		:: "m"(J_ptr[0]),"m"(in_ptr[0]));

		J_ptr += 6;
		if (b2 >= 0) {
			in_ptr = in + b2*6;
			__asm__ volatile (
//			"lv.s S000, 0+%0\n  lv.s S010, 4+%0\n  lv.s S020, 8+%0\n"
//			"lv.s S030, 12+%0\n lv.s S001, 16+%0\n lv.s S011, 20+%0\n"
//			"lv.s S002, 0+%1\n  lv.s S012, 4+%1\n  lv.s S022, 8+%1\n"
//			"lv.s S032, 12+%1\n lv.s S003, 16+%1\n lv.s S013, 20+%1\n"
			"ulv.q	R000,   %0\n"
			"lv.s S001, 16+%0\n lv.s S011, 20+%0\n"
			"ulv.q	R002,   %1\n"
			"lv.s S003, 16+%1\n lv.s S013, 20+%1\n"

			"vdot.q S023, R000, R002\n"
			"vdot.p S033, R001, R003\n"
			"vadd.s S101, S033, S023\n"
			"vadd.s S100, S101, S100\n"
			:: "m"(J_ptr[0]),"m"(in_ptr[0]));
		}
		J_ptr += 6;
		__asm__ volatile ("sv.s  S100, %0\n":"+m"(out[i]));
	}	
}
#else
static void multiply_J (int m, dRealMutablePtr J, int *jb, dRealMutablePtr in, dRealMutablePtr out) {
	int i,j;
	dRealPtr J_ptr = J;
	for (i=0; i<m; i++) {
		int b1 = jb[i*2];
		int b2 = jb[i*2+1];
		dReal sum = 0;
		dRealMutablePtr in_ptr = in + b1*6;
		for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
		J_ptr += 6;
		if (b2 >= 0) {
			in_ptr = in + b2*6;
			for (j=0; j<6; j++) sum += J_ptr[j] * in_ptr[j];
		}
		J_ptr += 6;
		out[i] = sum;
	}
}
#endif

//***************************************************************************
// SOR-LCP method

// nb is the number of bodies in the body array.
// J is an m*12 matrix of constraint rows
// jb is an array of first and second body numbers for each constraint row
// invI is the global frame inverse inertia for each body (stacked 3x3 matrices)
//
// this returns lambda and fc (the constraint force).
// note: fc is returned as inv(M)*J'*lambda, the constraint force is actually J'*lambda
//
// b, lo and hi are modified on exit


struct IndexError {
#ifdef REORDER_CONSTRAINTS
	dReal error;		// error to sort on
	int findex;
#endif
	int index;		// row index
};


#ifdef REORDER_CONSTRAINTS

static int compare_index_error (const void *a, const void *b)
{
	const IndexError *i1 = (IndexError*) a;
	const IndexError *i2 = (IndexError*) b;
	if (i1->findex < 0 && i2->findex >= 0) return -1;
	if (i1->findex >= 0 && i2->findex < 0) return 1;
	if (i1->error < i2->error) return -1;
	if (i1->error > i2->error) return 1;
	return 0;
}

#endif


static void SOR_LCP (int m, int nb, dRealMutablePtr J, int *jb, dxBody * const *body,
	dRealPtr invI, dRealMutablePtr lambda, dRealMutablePtr fc, dRealMutablePtr b,
	dRealMutablePtr lo, dRealMutablePtr hi, dRealPtr cfm, int *findex,
	dxQuickStepParameters *qs)
{
	const int num_iterations = qs->num_iterations;
	const dReal sor_w = qs->w;		// SOR over-relaxation parameter

	int i,j;

#ifdef WARM_STARTING
	// for warm starting, this seems to be necessary to prevent
	// jerkiness in motor-driven joints. i have no idea why this works.
	for (i=0; i<m; i++) lambda[i] *= 0.9;
#else
	dSetZero (lambda,m);
#endif

#ifdef REORDER_CONSTRAINTS
	// the lambda computed at the previous iteration.
	// this is used to measure error for when we are reordering the indexes.
	dRealAllocaArray (last_lambda,m);
#endif

	// a copy of the 'hi' vector in case findex[] is being used
	dRealAllocaArray (hicopy,m);
	//memcpy (hicopy,hi,m*sizeof(dReal));
	MEM_COPY_4 (hicopy,hi,m*sizeof(dReal));

	// precompute iMJ = inv(M)*J'
	dRealAllocaArray (iMJ,m*12);
	compute_invM_JT (m,J,iMJ,jb,body,invI);

	// compute fc=(inv(M)*J')*lambda. we will incrementally maintain fc
	// as we change lambda.
#ifdef WARM_STARTING
	multiply_invM_JT (m,nb,iMJ,jb,lambda,fc);
#else
	dSetZero (fc,nb*6);
#endif

	// precompute 1 / diagonals of A
	dRealAllocaArray (Ad,m);
	dRealPtr iMJ_ptr = iMJ;
	dRealMutablePtr J_ptr = J;
#ifdef PSPOPTS_QUICKSTEP
	// Preload sor_w which is constant
	__asm__ volatile ("lv.s    S200, %0\n":: "m"(sor_w));
#endif
	for (i=0; i<m; i++) {
#ifdef PSPOPTS_QUICKSTEP
		if (jb[i*2+1] >= 0) {  // Accumulate 12 floats
			__asm__ volatile (
			"ulv.q    R000, 0+    %0\n"
			"ulv.q    R001, 16+   %0\n"
			"ulv.q    R002, 32+   %0\n"

			"ulv.q    R300, 0+    %1\n"
			"ulv.q    R301, 16+   %1\n"
			"ulv.q    R302, 32+   %1\n"

			"vdot.q   S003, R000, R300\n"
			"vdot.q   S013, R001, R301\n"
			"vdot.q   S023, R002, R302\n"

			"vfad.t   S033, R003\n"
			:: "m"(iMJ_ptr[0]),"m"(J_ptr[0]):"$f0","$f1","$f2","$f12","$f13","$f14");
		}else{  // only accum 6 floats
			__asm__ volatile (
			"ulv.q    R000, 0+   %0\n"
			"ulv.q    R002, 0+   %1\n"
			"lv.s     S001, 16+  %0\n"
			"lv.s     S011, 20+  %0\n"
			"vdot.q   S103, R000, R002\n"
			"lv.s     S003, 16+  %1\n"
			"lv.s     S013, 20+  %1\n"
			"vdot.p   S113, R001, R003\n"
		
			"vfad.p   S033, R103\n"
			:: "m"(iMJ_ptr[0]),"m"(J_ptr[0]):"$f0","$f2");
		}
		// leave sum at S033
		__asm__ volatile (
		"lv.s    S201, %1\n"
		"vadd.s  S033, S033, S201\n"
		"vrcp.s  S033, S033\n"
		"vmul.s  S033, S033, S200\n"
		"sv.s    S033, %0\n"
		:"=m"(Ad[i]) : "m"(cfm[i]));
#else
		dReal sum = 0;
		for (j=0; j<6; j++) sum += iMJ_ptr[j] * J_ptr[j];
		if (jb[i*2+1] >= 0) {
			for (j=6; j<12; j++) sum += iMJ_ptr[j] * J_ptr[j];
		}
		Ad[i] = sor_w / (sum + cfm[i]);
#endif
		iMJ_ptr += 12;
		J_ptr += 12;
	}

	// scale J and b by Ad
	J_ptr = J;
	for (i=0; i<m; i++) {
	#ifdef PSPOPTS_QUICKSTEP
		__asm__ volatile (
		"vzero.q R003\n"
		"lv.s    S200, %2\n"
		"ulv.q   R000, 0+    %0\n"
		"ulv.q   R001, 16+   %0\n"
		"ulv.q   R002, 32+   %0\n"
		"vmscl.q M100, M000, S200\n"
		"usv.q   R100, 0+    %0\n"
		"usv.q   R101, 16+   %0\n"
		"usv.q   R102, 32+   %0\n"
		"lv.s    S201, %1\n"
		"vmul.s  S201, S201, S200\n"
		"sv.s    S201, %1\n"
		"lv.s    S202, %3\n"
		"vmul.s  S200, S200, S202\n"
		"sv.s    S200, %2\n"
		:"+m"(J_ptr[0]),"+m"(b[i]), "+m"(Ad[i]): "m"(cfm[i]):"$f0","$f1","$f2");
		J_ptr = J_ptr+12;
	#else
		for (j=0; j<12; j++) {
			J_ptr[0] *= Ad[i];
			J_ptr++;
		}
		b[i] *= Ad[i];
		// scale Ad by CFM. N.B. this should be done last since it is used above
		Ad[i] *= cfm[i];
	#endif
	}

	// order to solve constraint rows in
	IndexError *order = (IndexError*) ALLOCA (m*sizeof(IndexError));

#ifndef REORDER_CONSTRAINTS
	// make sure constraints with findex < 0 come first.
	j=0;
	int k=1;

	// Fill the array from both ends
	for (i=0; i<m; i++)
		if (findex[i] < 0)
			order[j++].index = i; // Place them at the front
		else
			order[m-k++].index = i; // Place them at the end

	dIASSERT ((j+k-1)==m); // -1 since k was started at 1 and not 0
#endif

	for (int iteration=0; iteration < num_iterations; iteration++) {

#ifdef REORDER_CONSTRAINTS
		// constraints with findex < 0 always come first.
		if (iteration < 2) {
			// for the first two iterations, solve the constraints in
			// the given order
			for (i=0; i<m; i++) {
				order[i].error = i;
				order[i].findex = findex[i];
				order[i].index = i;
			}
		}
		else {
			// sort the constraints so that the ones converging slowest
			// get solved last. use the absolute (not relative) error.
			for (i=0; i<m; i++) {
				dReal v1 = dFabs (lambda[i]);
				dReal v2 = dFabs (last_lambda[i]);
				dReal max = (v1 > v2) ? v1 : v2;
				if (max > 0) {
					//@@@ relative error: order[i].error = dFabs(lambda[i]-last_lambda[i])/max;
					order[i].error = dFabs(lambda[i]-last_lambda[i]);
				}
				else {
					order[i].error = dInfinity;
				}
				order[i].findex = findex[i];
				order[i].index = i;
			}
		}
		qsort (order,m,sizeof(IndexError),&compare_index_error);

		//@@@ potential optimization: swap lambda and last_lambda pointers rather
		//    than copying the data. we must make sure lambda is properly
		//    returned to the caller
		memcpy (last_lambda,lambda,m*sizeof(dReal));
#endif
#ifdef RANDOMLY_REORDER_CONSTRAINTS
		if ((iteration & 7) == 0) {
			for (i=1; i<m; ++i) {
				IndexError tmp = order[i];
				int swapi = dRandInt(i+1);
				order[i] = order[swapi];
				order[swapi] = tmp;
			}
		}
#endif

		for (int i=0; i<m; i++) {
			// @@@ potential optimization: we could pre-sort J and iMJ, thereby
			//     linearizing access to those arrays. hmmm, this does not seem
			//     like a win, but we should think carefully about our memory
			//     access pattern.

			int index = order[i].index;
			J_ptr = J + index*12;
			iMJ_ptr = iMJ + index*12;

			// set the limits for this constraint. note that 'hicopy' is used.
			// this is the place where the QuickStep method differs from the
			// direct LCP solving method, since that method only performs this
			// limit adjustment once per time step, whereas this method performs
			// once per iteration per constraint row.
			// the constraints are ordered so that all lambda[] values needed have
			// already been computed.
			if (findex[index] >= 0) {
				hi[index] = dFabs (hicopy[index] * lambda[findex[index]]);
				lo[index] = -hi[index];
			}

			int b1 = jb[index*2];
			int b2 = jb[index*2+1];
			dReal delta = b[index] - lambda[index]*Ad[index];
			dRealMutablePtr fc_ptr = fc + 6*b1;

#ifdef PSPOPTS_QUICKSTEP
		   __asm__ volatile (
		       "ulv.q    R000, 0+  %1\n"     // fc_ptr 0..3
		       "ulv.q    R002, 0+  %2\n"     // J_ptr 0..3
		       "vdot.q   S021, R000, R002\n"  // half dot pr

		       "lv.s     S001, 16+ %1\n"     // fc_ptr 4
		       "lv.s     S011, 20+ %1\n"     // fc_ptr 5
		       "lv.s     S003, 16+ %2\n"     // J_ptr 4
		       "lv.s     S013, 20+ %2\n"     // J_ptr 5
		       "vdot.p   S031, R001, R003\n"  // half dot pr

		       "lv.s     S033, 0+ %0\n"      // delta
		       "vadd.s   S031, S021, S031\n"  // all dot product
		       "vsub.s   S033, S033, S031\n"  // substract delta!
		       : "+m"(delta): "m"(fc_ptr[0]),"m"(J_ptr[0]):"$f0","$f2");

			if (b2 >= 0) {
				fc_ptr = fc + 6*b2;
			        __asm__ volatile (
			       "ulv.q    R100, 0+  %1\n"     // fc_ptr 0..3
			       "ulv.q    R102, 0+  %2\n"     // J_ptr 6..9
			       "vdot.q   S121, R100, R102\n"  // half dot pr

			       "lv.s     S101, 16+ %1\n"     // fc_ptr 4
			       "lv.s     S111, 20+ %1\n"     // fc_ptr 5
			       "lv.s     S103, 16+ %2\n"     // J_ptr 10
			       "lv.s     S113, 20+ %2\n"     // J_ptr 11
			       "vdot.p   S131, R101, R103\n"  // half dot pr

			       "vadd.s   S131, S121, S131\n"  // all dot product
			       "vsub.s   S033, S033, S131\n"  // substract delta!
			       : "+m"(delta): "m"(fc_ptr[0]),"m"(J_ptr[6]):"$f4","$f6");
			}

			// No need to save delta back, always on S033
//		        __asm__ volatile ("sv.s     S033, %0\n": "+m"(delta) );
#else
			// @@@ potential optimization: SIMD-ize this and the b2 >= 0 case
			delta -=fc_ptr[0] * J_ptr[0] + fc_ptr[1] * J_ptr[1] +
				fc_ptr[2] * J_ptr[2] + fc_ptr[3] * J_ptr[3] +
				fc_ptr[4] * J_ptr[4] + fc_ptr[5] * J_ptr[5];
			// @@@ potential optimization: handle 1-body constraints in a separate
			//     loop to avoid the cost of test & jump?
			if (b2 >= 0) {
				fc_ptr = fc + 6*b2;
				delta -=fc_ptr[0] * J_ptr[6] + fc_ptr[1] * J_ptr[7] +
					fc_ptr[2] * J_ptr[8] + fc_ptr[3] * J_ptr[9] +
					fc_ptr[4] * J_ptr[10] + fc_ptr[5] * J_ptr[11];
			}
#endif

			// compute lambda and clamp it to [lo,hi].
			// @@@ potential optimization: does SSE have clamping instructions
			//     to save test+jump penalties here?
#ifdef PSPOPTS_QUICKSTEP
			// We add delta to lambda [index] and clamp the value to lo[index] and hi[index].
			// Delta should be the distance moved after the clamp (S033)
		        __asm__ volatile (
			"lv.s	S232, %0\n"  		// lambda
			"lv.s	S210, %1\n"		// hi
			"vadd.s S200, S232, S033\n"	// increment lambda with delta (s033)
			"lv.s	S220, %2\n"		// lo
			"vmin.s S230, S200, S210\n"
			"vmax.s S230, S230, S220\n"
			"vsub.s S033, S230, S232\n"	// new delta
			"sv.s   S230, %0\n"
			:"+m"(lambda[index]):"m"(hi[index]),"m"(lo[index]));
#else
			dReal new_lambda = lambda[index] + delta;
			if (new_lambda < lo[index]) {
				delta = lo[index]-lambda[index];
				lambda[index] = lo[index];
			}
			else if (new_lambda > hi[index]) {
				delta = hi[index]-lambda[index];
				lambda[index] = hi[index];
			}
			else {
				lambda[index] = new_lambda;
			}
#endif

			//@@@ a trick that may or may not help
			//dReal ramp = (1-((dReal)(iteration+1)/(dReal)num_iterations));
			//delta *= ramp;

#ifdef PSPOPTS_QUICKSTEP
			// update fc.
			// @@@ potential optimization: SIMD for this and the b2 >= 0 case
			// PSP: All the variables already in registers except delta and iMJ_ptr
			// R000, R001 fc_ptr (b1)   R100, R101 fc_ptr (b2)
			fc_ptr = fc + 6*b1;
		   __asm__ volatile (
		       "ulv.q    R002, 0+  %1\n"     // iMJ_ptr 0..3
//		       "lv.s     S033, 0+ %2\n"      // delta
		       "vscl.q   R002, R002, S033\n"  // scale by delta

		       "lv.s     S003, 16+ %1\n"     // iMJ_ptr 4
		       "lv.s     S013, 20+ %1\n"     // iMJ_ptr 5
		       "vscl.p   R003, R003, S033\n"  // scale by delta

		       "vadd.q   R000, R000, R002\n"  // accumulate 
		       "vadd.p   R001, R001, R003\n"  // accumulate

		       "usv.q    R000, 0+  %0\n"   // save back
		       "sv.s     S001, 16+ %0\n"
		       "sv.s     S011, 20+ %0\n"
		       : "+m"(fc_ptr[0]): "m"((iMJ_ptr)[0]),"m"(delta):"$f2");

			if (b2 >= 0) {
				fc_ptr = fc + 6*b2;
			   __asm__ volatile (
			       "ulv.q    R102, 0+  %1\n"     // iMJ_ptr 6..9
			       "vscl.q   R102, R102, S033\n"  // scale by delta

			       "lv.s     S103, 16+ %1\n"     // iMJ_ptr 10
			       "lv.s     S113, 20+ %1\n"     // iMJ_ptr 11
			       "vscl.p   R103, R103, S033\n"  // scale by delta

			       "vadd.q   R100, R100, R102\n"  // accumulate 
			       "vadd.p   R101, R101, R103\n"  // accumulate

			       "usv.q    R100, 0+  %0\n"   // save back
			       "sv.s     S101, 16+ %0\n"
			       "sv.s     S111, 20+ %0\n"
			       : "+m"(fc_ptr[0]): "m"((iMJ_ptr)[6]):"$f6");
			}
#else
			// update fc.
			// @@@ potential optimization: SIMD for this and the b2 >= 0 case
			fc_ptr = fc + 6*b1;
			fc_ptr[0] += delta * iMJ_ptr[0];
			fc_ptr[1] += delta * iMJ_ptr[1];
			fc_ptr[2] += delta * iMJ_ptr[2];
			fc_ptr[3] += delta * iMJ_ptr[3];
			fc_ptr[4] += delta * iMJ_ptr[4];
			fc_ptr[5] += delta * iMJ_ptr[5];
			// @@@ potential optimization: handle 1-body constraints in a separate
			//     loop to avoid the cost of test & jump?
			if (b2 >= 0) {
				fc_ptr = fc + 6*b2;
				fc_ptr[0] += delta * iMJ_ptr[6];
				fc_ptr[1] += delta * iMJ_ptr[7];
				fc_ptr[2] += delta * iMJ_ptr[8];
				fc_ptr[3] += delta * iMJ_ptr[9];
				fc_ptr[4] += delta * iMJ_ptr[10];
				fc_ptr[5] += delta * iMJ_ptr[11];
			}
#endif
		}
	}
}

void dxQuickStepper (dxWorld *world, dxBody * const *body, int nb,
		     dxJoint * const *_joint, int nj, dReal stepsize)
{
	int i,j;
	IFTIMING(dTimerStart("preprocessing");)

	dReal stepsize1 = dRecip(stepsize);

	// 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).
	//@@@ do we really need to do this? we'll be sorting constraint rows individually, not joints
	dxJoint **joint = (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
	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. I and invI are a vertical stack of 3x4 matrices, one per body.
        dRealAllocaArray (invI,3*4*nb);

#ifdef PSPOPTS_QUICKSTEP  // ERROR!
	 __asm__ volatile ("lv.q    R500, %0\n":: "m"(world->gravity[0]) );
	for (i=0; i<nb; i++) {
		// tmp is stored in M200
		// body invI and posr.R matrices are aligned to 16 bytes boundary, look at objects.h file
	   __asm__ volatile (
	       "lv.q    R000, 0+  %1\n"       // invI
	       "lv.q    R001, 16+ %1\n"
	       "lv.q    R002, 32+ %1\n"
	       "lv.q    R100, 0+  %2\n"       // posr.R in M100!
	       "lv.q    R101, 16+ %2\n"
	       "lv.q    R102, 32+ %2\n"
	       "vmmul.t  M200, M000, E100\n"   // compute tmp and leave it in the register M200
	       "vmmul.t  M000, M100, M200\n"   // compute invI+12*i and then store it again
	       "sv.q    R000, 0+  %0\n"      // invI is 16 byte aligned!!!! ALLOCA16 remember?
	       "sv.q    R001, 16+ %0\n"
	       "sv.q    R002, 32+ %0\n"
	       : "+m"((invI+12*i)[0]): "m"(body[i]->invI[0]),"m"(body[i]->posr.R[0]) );

		if (body[i]->flags & dxBodyGyroscopic) {
			// I matrix is stored in M300, tmp in M200
			// Everything is aligned to 16 bytes! Look at objects.h
		   __asm__ volatile (
		       "lv.q    R000, 0+  %1\n"
		       "lv.q    R001, 16+ %1\n"
		       "lv.q    R002, 32+ %1\n"   // load mass.I in M000

		       "vmmul.t  M200, M000, E100\n"   // compute tmp and leave it in the register M200 again
		       "vmmul.t  M300, M100, M200\n"   // compute I in register M300

		       "lv.q    C400, %2\n"      // load avel in C400
		       "lv.q    C420, %0\n"      // load tacc in C420
		       
		       "vtfm3.t  C410, E300, C400\n"   // We store the vector in C410
		       "vcrsp.t  C430, C400, C410\n"   // Cross product with avel and tmp store in C430
		       "vsub.t   C420, C420, C430\n"   // sub the result to tacc

		       "sv.q    C420, %0\n"       // store back
		       : "+m"((body[i]->tacc)[0]): "m"(body[i]->mass.I[0]),"m"(body[i]->avel[0]));
		}
		if ((body[i]->flags & dxBodyNoGravity)==0) {
			 __asm__ volatile (
		       "lv.s    S033, %1\n"  // mass
		       "vscl.t  R001, R500, S033\n"   // scale the gravity stored in R500
		       "lv.q    R002, %0\n"  // facc
		       "vadd.t  R003, R002, R001\n"
		       "sv.q    R003, %0\n"       // store back
		       : "+m"((body[i]->facc)[0]): "m"(body[i]->mass.mass) );
		}
	}
#else
	for (i=0; i<nb; i++) {
		dMatrix3 tmp;

		// compute inverse inertia tensor in global frame
		dMULTIPLY2_333_ (tmp,body[i]->invI,body[i]->posr.R);
		dMULTIPLY0_333_ (invI+12*i,body[i]->posr.R,tmp);

		if (body[i]->flags & dxBodyGyroscopic) {
		    dMatrix3 I;
		    // compute inertia tensor in global frame
		    dMULTIPLY2_333_ (tmp,body[i]->mass.I,body[i]->posr.R);
		    dMULTIPLY0_333_ (I,body[i]->posr.R,tmp);
		    // compute rotational force
		    dMULTIPLY0_331_ (tmp,I,body[i]->avel);
		    dCROSS (body[i]->tacc,-=,body[i]->avel,tmp);
		}
		if ((body[i]->flags & dxBodyNoGravity)==0) {
			body[i]->facc[0] += body[i]->mass.mass * world->gravity[0];
			body[i]->facc[1] += body[i]->mass.mass * world->gravity[1];
			body[i]->facc[2] += body[i]->mass.mass * world->gravity[2];
		}
	}

#endif


	// get joint information (m = total constraint dimension, nub = number of unbounded variables).
	// joints with m=0 are inactive and are removed from the joints array
	// entirely, so that the code that follows does not consider them.
	//@@@ do we really need to save all the info1's
	dxJoint::Info1 *info = (dxJoint::Info1*) ALLOCA (nj*sizeof(dxJoint::Info1));
	for (i=0, j=0; j<nj; j++) {	// i=dest, j=src
		joint[j]->getInfo1 (info+i);
		dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m);
		if (info[i].m > 0) {
			joint[i] = joint[j];
			i++;
		}
	}
	nj = i;

	// create the row offset array
	int m = 0;
	int *ofs = (int*) ALLOCA (nj*sizeof(int));
	for (i=0; i<nj; i++) {
		ofs[i] = m;
		m += info[i].m;
	}

	// if there are constraints, compute the constraint force
	dRealAllocaArray (J,m*12);
	int *jb = (int*) ALLOCA (m*2*sizeof(int));
	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.
		dRealAllocaArray (c,m);
		dRealAllocaArray (cfm,m);
		dRealAllocaArray (lo,m);
		dRealAllocaArray (hi,m);
		int *findex = (int*) ALLOCA (m*sizeof(int));
		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;

		// get jacobian data from constraints. an m*12 matrix will be created
		// to store the two jacobian blocks from each constraint. it has this
		// format:
		//
		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 \    .
		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2  }-- jacobian for joint 0, body 1 and body 2 (3 rows)
		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 /
		//   l1 l1 l1 a1 a1 a1 l2 l2 l2 a2 a2 a2 }--- jacobian for joint 1, body 1 and body 2 (3 rows)
		//   etc...
		//
		//   (lll) = linear jacobian data
		//   (aaa) = angular jacobian data
		//
		IFTIMING (dTimerNow ("create J");)
		dSetZero (J,m*12);
		dxJoint::Info2 Jinfo;
		Jinfo.rowskip = 12;
		Jinfo.fps = stepsize1;
		Jinfo.erp = world->global_erp;
		int mfb = 0; // number of rows of Jacobian we will have to save for joint feedback
		for (i=0; i<nj; i++) {
			Jinfo.J1l = J + ofs[i]*12;
			Jinfo.J1a = Jinfo.J1l + 3;
			Jinfo.J2l = Jinfo.J1l + 6;
			Jinfo.J2a = Jinfo.J1l + 9;
			Jinfo.c = c + ofs[i];
			Jinfo.cfm = cfm + ofs[i];
			Jinfo.lo = lo + ofs[i];
			Jinfo.hi = hi + ofs[i];
			Jinfo.findex = findex + ofs[i];
			joint[i]->getInfo2 (&Jinfo);
			// adjust returned findex values for global index numbering
			for (j=0; j<info[i].m; j++) {
				if (findex[ofs[i] + j] >= 0) findex[ofs[i] + j] += ofs[i];
			}
			if (joint[i]->feedback)
				mfb += info[i].m;
		}

		// we need a copy of Jacobian for joint feedbacks
		// because it gets destroyed by SOR solver
		// instead of saving all Jacobian, we can save just rows
		// for joints, that requested feedback (which is normaly much less)
                dReal *Jcopy = NULL;
                if (mfb > 0) {
                  Jcopy = (dReal*) ALLOCA (mfb*12*sizeof(dReal));
                  mfb = 0;
                  for (i=0; i<nj; i++)
                    if (joint[i]->feedback) {
                      //memcpy(Jcopy+mfb*12, J+ofs[i]*12, info[i].m*12*sizeof(dReal));
		      MEM_COPY_16 (Jcopy+mfb*12, J+ofs[i]*12, info[i].m*12*sizeof(dReal));
                      mfb += info[i].m;
                    }
		}


		// create an array of body numbers for each joint row
		int *jb_ptr = jb;
		for (i=0; i<nj; i++) {
			int b1 = (joint[i]->node[0].body) ? (joint[i]->node[0].body->tag) : -1;
			int b2 = (joint[i]->node[1].body) ? (joint[i]->node[1].body->tag) : -1;
			for (j=0; j<info[i].m; j++) {
				jb_ptr[0] = b1;
				jb_ptr[1] = b2;
				jb_ptr += 2;
			}
		}
		dIASSERT (jb_ptr == jb+2*m);

		// compute the right hand side `rhs'
		IFTIMING (dTimerNow ("compute rhs");)
		dRealAllocaArray (tmp1,nb*6);
		// put v/h + invM*fe into tmp1

#ifdef PSPOPTS_QUICKSTEP
		for (i=0; i<nb; i++) {
			__asm__ volatile (
			"lv.s    S033, %1\n"   // invMass
			"lv.q    R000, %2\n"   // facc
			"vscl.t  R000, R000, S033\n"   // scale facc

			"lv.s    S023, %5\n"   // stepsize
			"lv.q    R002, %3\n"   // lvel
			"vscl.t  R002, R002, S023\n"   // scale lvel
			"vadd.t  R000, R002, R000\n"  // facc * body_invMass + lvel * stepsize1;

			"lv.q    R001, %6\n"   // tacc
			"lv.q    R100, %4    \n"   // InvI +i*12
			"lv.q    R101, 16+ %4\n"
			"lv.q    R102, 32+ %4\n"

			"vtfm3.t R002, E100, R001\n"   // tmp[i*6] in R002

			"usv.q   R000, %0\n"    // save first 3 values of tmp1[i*6], note we eliminate the  4th value in the write
			"lv.q    R000, %7\n"   // avel
			"vscl.t  R000, R000, S023\n"   // avel*stepsize

			"vadd.t  R002, R002, R000\n"   // increment the 3 last numbers of tmp

			"sv.s    S002, 12+ %0\n"   // save last 3 floats of tmp
			"sv.s    S012, 16+ %0\n"
			"sv.s    S022, 20+ %0\n"
			: "+m"(tmp1[i*6]):
			"m"(body[i]->invMass),"m"(body[i]->facc[0]),"m"(body[i]->lvel[0]),"m"((invI + i*12)[0]) ,
			"m"(stepsize1),"m"(body[i]->tacc[0]),"m"(body[i]->avel[0]) );
		}
#else
		for (i=0; i<nb; i++) {
			dReal body_invMass = body[i]->invMass;
			for (j=0; j<3; j++) tmp1[i*6+j] = body[i]->facc[j] * body_invMass + body[i]->lvel[j] * stepsize1;
			dMULTIPLY0_331_ (tmp1 + i*6 + 3,invI + i*12,body[i]->tacc);
			for (j=0; j<3; j++) tmp1[i*6+3+j] += body[i]->avel[j] * stepsize1;
		}
#endif

		// put J*tmp1 into rhs
		dRealAllocaArray (rhs,m);
		multiply_J (m,J,jb,tmp1,rhs);

#ifdef PSPOPTS_QUICKSTEP
		__asm__ volatile ( "lv.s   S033, %0 \n" :: "m"(stepsize1));
		for (i=0; (i+3)<(m); i=i+4) {
			__asm__ volatile (
			"lv.q    R000, %2\n"   // c
			"vscl.q  R000, R000, S033\n"
			"lv.q    R001, %0\n"  // rhs
			"vsub.q  R001, R000, R001\n"
			"lv.q    R002, %1\n"  // cfm
			"vscl.q  R002, R002, S033\n"

			"sv.q    R001, %0\n"
			"sv.q    R002, %1\n"
			: "+m"(rhs[i]),"+m"(cfm[i]): "m"(c[i]));
		}
		for (; i<m; i++) { rhs[i] = c[i]*stepsize1 - rhs[i]; cfm[i] *= stepsize1; }
#else
		for (i=0; i<m; i++) rhs[i] = c[i]*stepsize1 - rhs[i];   // complete rhs
		for (i=0; i<m; i++) cfm[i] *= stepsize1;  // scale CFM
#endif

		// load lambda from the value saved on the previous iteration
		dRealAllocaArray (lambda,m);
#ifdef WARM_STARTING
		dSetZero (lambda,m);	//@@@ shouldn't be necessary
		for (i=0; i<nj; i++) {
			memcpy (lambda+ofs[i],joint[i]->lambda,info[i].m * sizeof(dReal));
		}
#endif

		// solve the LCP problem and get lambda and invM*constraint_force
		IFTIMING (dTimerNow ("solving LCP problem");)
		dRealAllocaArray (cforce,nb*6);
		SOR_LCP (m,nb,J,jb,body,invI,lambda,cforce,rhs,lo,hi,cfm,findex,&world->qs);

#ifdef WARM_STARTING
		// save lambda for the next iteration
		//@@@ note that this doesn't work for contact joints yet, as they are
		// recreated every iteration
		for (i=0; i<nj; i++) {
			memcpy (joint[i]->lambda,lambda+ofs[i],info[i].m * sizeof(dReal));
		}
#endif

		// note that the SOR method overwrites rhs and J at this point, so
		// they should not be used again.

		// add stepsize * cforce to the body velocity
#ifdef PSPOPTS_QUICKSTEP
		__asm__ volatile ("lv.s S133, %0\n" :: "m"(stepsize));
		for (i=0; i<nb; i++) {
			__asm__ volatile (
			"ulv.q    R000, %2\n"   // cforce
			"vmov.s  S001, S030\n"  //cforce
			"lv.s    S011, 16+ %2\n"   // cforce
			"lv.s    S021, 20+ %2\n"   // cforce

			"vscl.t  R000, R000, S133\n"
			"lv.q    R002, %0\n"  // lvel
			"vscl.t  R001, R001, S133\n"
			"lv.q    R003, %1\n"  // avel

			"vadd.t  R002, R002, R000\n"
			"vadd.t  R003, R003, R001\n"

			"sv.q    R002, %0\n"
			"sv.q    R003, %1\n"
			: "+m"(body[i]->lvel[0]),"+m"(body[i]->avel[0]): "m"(cforce[i*6]):"$f0");
                }
#else
		for (i=0; i<nb; i++) {
			for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * cforce[i*6+j];
			for (j=0; j<3; j++) body[i]->avel[j] += stepsize * cforce[i*6+3+j];
                }
#endif


		if (mfb > 0) {
			// straightforward computation of joint constraint forces:
			// multiply related lambdas with respective J' block for joints
			// where feedback was requested
			mfb = 0;
			for (i=0; i<nj; i++) {
				if (joint[i]->feedback) {
					dJointFeedback *fb = joint[i]->feedback;
					dReal data[6];
					Multiply1_12q1 (data, Jcopy+mfb*12, lambda+ofs[i], info[i].m);
					fb->f1[0] = data[0];
					fb->f1[1] = data[1];
					fb->f1[2] = data[2];
					fb->t1[0] = data[3];
					fb->t1[1] = data[4];
					fb->t1[2] = data[5];
					if (joint[i]->node[1].body)
					{
						Multiply1_12q1 (data, Jcopy+mfb*12+6, lambda+ofs[i], info[i].m);
						fb->f2[0] = data[0];
						fb->f2[1] = data[1];
						fb->f2[2] = data[2];
						fb->t2[0] = data[3];
						fb->t2[1] = data[4];
                                                fb->t2[2] = data[5];
					}
					mfb += info[i].m;
				}
			}
		}
	}

	// compute the velocity update:
	// add stepsize * invM * fe to the body velocity

	IFTIMING (dTimerNow ("compute velocity update");)
#ifdef PSPOPTS_QUICKSTEP
	for (i=0; i<nb; i++) {
		__asm__ volatile (
		"lv.s    S033, %3\n"   // invMass
		"lv.s    S023, %5\n"   // stepsize
		"vmul.s  S013, S033, S023\n"   // mul

		"lv.q    R000, %4\n"   // facc
		"lv.q    R001, %1\n"   // tacc
		"vscl.t  R000, R000, S013\n"   // scale facc

		"lv.q    R002, %0\n"   // lvel
		"vadd.t  R002, R002, R000\n"   // increment lvel
		"vscl.t  R001, R001, S023\n"   // scale tacc

		"lv.q    R100, 0+  %6\n"      // load invI + i*12
		"lv.q    R101, 16+ %6\n"
		"lv.q    R102, 32+ %6\n"

		"sv.q    R001, %1\n"		// save tacc
		"sv.q    R002, %0\n"		// save lvel

		"vtfm3.t  R002, E100, R001\n"   // transform
		"lv.q     R003, %2\n"		// load avel
		"vadd.t   R003, R003, R002\n"   // add

		"sv.q     R003, %2\n"		// save avel
		: "+m"((body[i]->lvel)[0]),"+m"((body[i]->tacc)[0]),"+m"((body[i]->avel)[0]):
		"m"(body[i]->invMass),"m"(body[i]->facc[0]),"m"(stepsize),"m"((invI + i*12)[0]) );

	}
#else
	for (i=0; i<nb; i++) {
		dReal body_invMass = body[i]->invMass;
		for (j=0; j<3; j++) body[i]->lvel[j] += stepsize * body_invMass * body[i]->facc[j];
		for (j=0; j<3; j++) body[i]->tacc[j] *= stepsize;
		dMULTIPLYADD0_331_ (body[i]->avel,invI + i*12,body[i]->tacc);
	}
#endif

	// update the position and orientation from the new linear/angular velocity
	// (over the given timestep)
	IFTIMING (dTimerNow ("update position");)
	for (i=0; i<nb; i++) dxStepBody (body[i],stepsize);

	IFTIMING (dTimerNow ("tidy up");)

	// zero all force accumulators
	
#ifdef PSPOPTS_QUICKSTEP
	__asm__ volatile ("vzero.t R000\n");
	for (i=0; i<nb; i++) {
		__asm__ volatile (
		"sv.q    R000, %0\n"
		"sv.q    R000, %1\n"
		: "+m"((body[i]->tacc)[0]),"+m"((body[i]->facc)[0]) );
	}
#else
	for (i=0; i<nb; i++) {
		dSetZero (body[i]->facc,3);
		dSetZero (body[i]->tacc,3);
	}
#endif

	IFTIMING (dTimerEnd();)
	IFTIMING (if (m > 0) dTimerReport (stdout,1);)
}

