/*************************************************************************
 *                                                                       *
 * 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 <setjmp.h>
#include <ode/ode.h>

#ifdef _MSC_VER
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

//****************************************************************************
// matrix sizes

#define dALIGN_SIZE(buf_size, alignment) (((buf_size) + (alignment - 1)) & (int)(~(alignment - 1))) // Casting the mask to int ensures sign-extension to larger integer sizes
#define dALIGN_PTR(buf_ptr, alignment) ((void *)(((duintptr)(buf_ptr) + ((alignment) - 1)) & (int)(~(alignment - 1)))) // Casting the mask to int ensures sign-extension to larger integer sizes

#define MSIZE 21
#define MSIZE4 dALIGN_SIZE(MSIZE, 4)	// MSIZE rounded up to 4



//****************************************************************************
// matrix accessors

#define _A(i,j) A[(i)*4+(j)]
#define _I(i,j) I[(i)*4+(j)]
#define _R(i,j) R[(i)*4+(j)]

//****************************************************************************
// tolerances

#ifdef dDOUBLE
const double tol = 1e-10;
#endif

#ifdef dSINGLE
const double tol = 1e-5;
#endif

//****************************************************************************
// misc messages and error handling

#ifdef __GNUC__
#define HEADER printf ("%s()\n", __FUNCTION__);
#else
#define HEADER printf ("%s:%d\n",__FILE__,__LINE__);
#endif

static jmp_buf jump_buffer;


void myMessageFunction (int num, const char *msg, va_list ap)
{
    printf ("(Message %d: ",num);
    vprintf (msg,ap);
    printf (")");
    dSetMessageHandler (0);
    longjmp (jump_buffer,1);
}


#define TRAP_MESSAGE(do,ifnomsg,ifmsg) \
    dSetMessageHandler (&myMessageFunction); \
    if (setjmp (jump_buffer)) { \
        dSetMessageHandler (0); \
        ifmsg ; \
    } \
    else { \
        dSetMessageHandler (&myMessageFunction); \
        do ; \
        ifnomsg ; \
    } \
    dSetMessageHandler (0);

//****************************************************************************
// utility stuff

// compare two numbers, within a threshhold, return 1 if approx equal

int cmp (dReal a, dReal b)
{
    return (fabs(a-b) < tol);
}

//****************************************************************************
// matrix utility stuff

// compare a 3x3 matrix with the identity

int cmpIdentityMat3 (dMatrix3 A)
{
    return
        (cmp(_A(0,0),1.0) && cmp(_A(0,1),0.0) && cmp(_A(0,2),0.0) &&
        cmp(_A(1,0),0.0) && cmp(_A(1,1),1.0) && cmp(_A(1,2),0.0) &&
        cmp(_A(2,0),0.0) && cmp(_A(2,1),0.0) && cmp(_A(2,2),1.0));
}


// transpose a 3x3 matrix in-line

void transpose3x3 (dMatrix3 A)
{
    dReal tmp;
    tmp=A[4]; A[4]=A[1]; A[1]=tmp;
    tmp=A[8]; A[8]=A[2]; A[2]=tmp;
    tmp=A[9]; A[9]=A[6]; A[6]=tmp;
}

//****************************************************************************
// test miscellaneous math functions

void testRandomNumberGenerator()
{
    HEADER;
    if (dTestRand()) printf ("\tpassed\n");
    else printf ("\tFAILED\n");
}


void testInfinity()
{
    HEADER;
    if (1e10 < dInfinity && -1e10 > -dInfinity && -dInfinity < dInfinity)
        printf ("\tpassed\n");
    else printf ("\tFAILED\n");
}


void testPad()
{
    HEADER;
    char s[100];
    s[0]=0;
    for (int i=0; i<=16; i++) sprintf (s+strlen(s),"%d ",dPAD(i));
    printf ("\t%s\n", strcmp(s,"0 1 4 4 4 8 8 8 8 12 12 12 12 16 16 16 16 ") ?
        "FAILED" : "passed");
}


void testCrossProduct()
{
    HEADER;

    dVector3 a1,a2,b,c;
    dMatrix3 B;
    dMakeRandomVector (b,3,1.0);
    dMakeRandomVector (c,3,1.0);

    dCalcVectorCross3(a1,b,c);

    dSetZero (B,12);
    dSetCrossMatrixPlus(B,b,4);
    dMultiply0 (a2,B,c,3,3,1);

    dReal diff = dMaxDifference(a1,a2,3,1);
    printf ("\t%s\n", diff > tol ? "FAILED" : "passed");
}


void testSetZero()
{
    HEADER;
    dReal a[100];
    dMakeRandomVector (a,100,1.0);
    dSetZero (a,100);
    for (int i=0; i<100; i++) if (a[i] != 0.0) {
        printf ("\tFAILED\n");
        return;
    }
    printf ("\tpassed\n");
}


void testNormalize3()
{
    HEADER;
    int i,j,bad=0;
    dVector3 n1,n2;
    for (i=0; i<1000; i++) {
        dMakeRandomVector (n1,3,1.0);
        for (j=0; j<3; j++) n2[j]=n1[j];
        dNormalize3 (n2);
        if (dFabs(dCalcVectorDot3(n2,n2) - 1.0) > tol) bad |= 1;
        if (dFabs(n2[0]/n1[0] - n2[1]/n1[1]) > tol) bad |= 2;
        if (dFabs(n2[0]/n1[0] - n2[2]/n1[2]) > tol) bad |= 4;
        if (dFabs(n2[1]/n1[1] - n2[2]/n1[2]) > tol) bad |= 8;
        if (dFabs(dCalcVectorDot3(n2,n1) - dSqrt(dCalcVectorDot3(n1,n1))) > tol) bad |= 16;
        if (bad) {
            printf ("\tFAILED (code=%x)\n",bad);
            return;
        }
    }
    printf ("\tpassed\n");
}


/*
void testReorthonormalize()
{
HEADER;
dMatrix3 R,I;
dMakeRandomMatrix (R,3,3,1.0);
for (int i=0; i<30; i++) dReorthonormalize (R);
dMultiply2 (I,R,R,3,3,3);
printf ("\t%s\n",cmpIdentityMat3 (I) ? "passed" : "FAILED");
}
*/


void testPlaneSpace()
{
    HEADER;
    dVector3 n,p,q;
    int bad = 0;
    for (int i=0; i<1000; i++) {
        dMakeRandomVector (n,3,1.0);
        dNormalize3 (n);
        dPlaneSpace (n,p,q);
        if (fabs(dCalcVectorDot3(n,p)) > tol) bad = 1;
        if (fabs(dCalcVectorDot3(n,q)) > tol) bad = 1;
        if (fabs(dCalcVectorDot3(p,q)) > tol) bad = 1;
        if (fabs(dCalcVectorDot3(p,p)-1) > tol) bad = 1;
        if (fabs(dCalcVectorDot3(q,q)-1) > tol) bad = 1;
    }
    printf ("\t%s\n", bad ? "FAILED" : "passed");
}

//****************************************************************************
// test matrix functions

void testMatrixMultiply()
{
    // A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4
    dReal A[8],B[12],A2[12],B2[16],C[8];
    int i;

    HEADER;
    dSetZero (A,8);
    for (i=0; i<3; i++) A[i] = i+2;
    for (i=0; i<3; i++) A[i+4] = i+3+2;
    for (i=0; i<12; i++) B[i] = i+8;
    dSetZero (A2,12);
    for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3];
    dSetZero (B2,16);
    for (i=0; i<12; i++) B2[i+i/3] = B[i];

    dMultiply0 (C,A,B,2,3,4);
    if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 ||
        C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278)
        printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n");

    dMultiply1 (C,A2,B,2,3,4);
    if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 ||
        C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241)
        printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n");

    dMultiply2 (C,A,B2,2,3,4);
    if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 ||
        C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326)
        printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n");
}


void testSmallMatrixMultiply()
{
    dMatrix3 A,B,C,A2;
    dVector3 a,a2,x;

    HEADER;
    dMakeRandomMatrix (A,3,3,1.0);
    dMakeRandomMatrix (B,3,3,1.0);
    dMakeRandomMatrix (C,3,3,1.0);
    dMakeRandomMatrix (x,3,1,1.0);

    // dMultiply0_331()
    dMultiply0_331 (a,B,x);
    dMultiply0 (a2,B,x,3,3,1);
    printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
        "passed");

    // dMultiply1_331()
    dMultiply1_331 (a,B,x);
    dMultiply1 (a2,B,x,3,3,1);
    printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
        "passed");

    // dMultiply0_133
    dMultiply0_133 (a,x,B);
    dMultiply0 (a2,x,B,1,3,3);
    printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" :
        "passed");

    // dMultiply0_333()
    dMultiply0_333 (A,B,C);
    dMultiply0 (A2,B,C,3,3,3);
    printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
        "passed");

    // dMultiply1_333()
    dMultiply1_333 (A,B,C);
    dMultiply1 (A2,B,C,3,3,3);
    printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
        "passed");

    // dMultiply2_333()
    dMultiply2_333 (A,B,C);
    dMultiply2 (A2,B,C,3,3,3);
    printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
        "passed");
}


void testCholeskyFactorization()
{
    dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *B = (dReal *)dAlloc(matrixSize), *C = (dReal *)dAlloc(matrixSize), diff;

    HEADER;
    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
    memcpy (A,B,MSIZE4*MSIZE*sizeof(dReal));
    if (dFactorCholesky (B,MSIZE)) printf ("\tpassed (1)\n");
    else printf ("\tFAILED (1)\n");
    dClearUpperTriangle (B,MSIZE);
    dMultiply2 (C,B,B,MSIZE,MSIZE,MSIZE);
    diff = dMaxDifference(A,C,MSIZE,MSIZE);
    printf ("\tmaximum difference = %.6e - %s (2)\n",diff,
        diff > tol ? "FAILED" : "passed");

    dFree(C, matrixSize);
    dFree(B, matrixSize);
    dFree(A, matrixSize);
}


void testCholeskySolve()
{
    dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize);
    dReal *b = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff;

    HEADER;

    // get A,L = PD matrix
    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
    memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));

    // get b,x = right hand side
    dMakeRandomMatrix (b,MSIZE,1,1.0);
    memcpy (x,b,MSIZE*sizeof(dReal));

    // factor L
    if (dFactorCholesky (L,MSIZE)) printf ("\tpassed (1)\n");
    else printf ("\tFAILED (1)\n");
    dClearUpperTriangle (L,MSIZE);

    // solve A*x = b
    dSolveCholesky (L,x,MSIZE);

    // compute A*x and compare it with b
    dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
    diff = dMaxDifference(b,btest,MSIZE,1);
    printf ("\tmaximum difference = %.6e - %s (2)\n",diff,
        diff > tol ? "FAILED" : "passed");

    dFree(btest, vectorSize);
    dFree(x, vectorSize);
    dFree(b, vectorSize);
    dFree(L, matrixSize);
    dFree(A, matrixSize);
}


void testInvertPDMatrix()
{
    int i,j,ok;
    dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *Ainv = (dReal *)dAlloc(matrixSize), *I = (dReal *)dAlloc(matrixSize);

    HEADER;

    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE);
    memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal));
    dSetZero (Ainv,MSIZE4*MSIZE);

    if (dInvertPDMatrix (A,Ainv,MSIZE))
        printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n");
    dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE);

    // compare with identity
    ok = 1;
    for (i=0; i<MSIZE; i++) {
        for (j=0; j<MSIZE; j++) {
            if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0;
        }
    }
    for (i=0; i<MSIZE; i++) {
        if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0;
    }
    if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n");

    dFree(I, matrixSize);
    dFree(Ainv, matrixSize);
    dFree(A, matrixSize);
}


void testIsPositiveDefinite()
{
    dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *B = (dReal *)dAlloc(matrixSize);

    HEADER;

    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (B,A,A,MSIZE,MSIZE,MSIZE);
    printf ("\t%s\n",dIsPositiveDefinite(A,MSIZE) ? "FAILED (1)":"passed (1)");
    printf ("\t%s\n",dIsPositiveDefinite(B,MSIZE) ? "passed (2)":"FAILED (2)");

    dFree(B, matrixSize);
    dFree(A, matrixSize);
}


void testFastLDLTFactorization()
{
    int i,j;
    dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), *DL = (dReal *)dAlloc(matrixSize),
        *ATEST = (dReal *)dAlloc(matrixSize), *d = (dReal *)dAlloc(vectorSize), diff;

    HEADER;

    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
    memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));

    dFactorLDLT (L,d,MSIZE,MSIZE4);

    dClearUpperTriangle (L,MSIZE);
    for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;

    dSetZero (DL,MSIZE4*MSIZE);
    for (i=0; i<MSIZE; i++) {
        for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
    }

    dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);
    diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
    printf ("\tmaximum difference = %.6e - %s\n",diff,
        diff > tol ? "FAILED" : "passed");

    dFree(d, vectorSize);
    dFree(ATEST, matrixSize);
    dFree(DL, matrixSize);
    dFree(L, matrixSize);
    dFree(A, matrixSize);
}


void testCoopLDLTFactorization()
{
    int i,j;

    const dsizeint COOP_MSIZE = MSIZE * 51, COOP_MSIZE4 = dALIGN_SIZE(COOP_MSIZE, 4);

    dsizeint matrixSize = sizeof(dReal) * COOP_MSIZE4 * COOP_MSIZE, vectorSize = sizeof(dReal) * COOP_MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), *DL = (dReal *)dAlloc(matrixSize),
        *ATEST = (dReal *)dAlloc(matrixSize), *d = (dReal *)dAlloc(vectorSize), diff;

    const unsigned threadCountMaximum = 8;
    dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
    dCooperativeID cooperative = dCooperativeCreate(dThreadingImplementationGetFunctions(threading), threading);
    dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(threadCountMaximum, 0, dAllocateFlagBasicData, NULL);
    dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);

    dResourceRequirementsID requirements = dResourceRequirementsCreate(cooperative);
    dEstimateCooperativelyFactorLDLTResourceRequirements(requirements, threadCountMaximum, COOP_MSIZE);
    dResourceContainerID resources = dResourceContainerAcquire(requirements);

    HEADER;

    for (int pass = 0; pass != 4; ++pass)
    {
        dTimerStart ("Factoring LDLT");

        const unsigned allowedThreads = 4;
        const unsigned PASS_MSIZE = COOP_MSIZE - pass, PASS_MSIZE4 = dALIGN_SIZE(PASS_MSIZE, 4);

        dTimerNow ("Preparing data");
        dMakeRandomMatrix (L, PASS_MSIZE, PASS_MSIZE, 1.0);
        dMultiply2 (A, L, L, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE);
        memcpy (L, A, sizeof(dReal) * PASS_MSIZE4 * PASS_MSIZE);

        dTimerNow ("Factoring multi threaded");
        dCooperativelyFactorLDLT (resources, allowedThreads, L, d, PASS_MSIZE, PASS_MSIZE4);

        dTimerNow ("Verifying");
        dClearUpperTriangle (L, PASS_MSIZE);
        for (i = 0; i < PASS_MSIZE; i++) L[i * PASS_MSIZE4 + i] = 1.0;

        dSetZero (DL, PASS_MSIZE4 * PASS_MSIZE);
        for (i = 0; i < PASS_MSIZE; i++) {
            for (j = 0; j < PASS_MSIZE; j++) DL[i * PASS_MSIZE4 + j] = L[i * PASS_MSIZE4 + j] / d[j];
        }

        dMultiply2 (ATEST, L, DL, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE);
        diff = dMaxDifference(A, ATEST, PASS_MSIZE, PASS_MSIZE);
        printf ("\tN=%u: maximum difference = %.6e - %s\n", PASS_MSIZE, diff, diff > 1e2 * tol ? "FAILED" : "passed");

        dTimerEnd();
        dTimerReport(stdout, 0);
    }

    dResourceContainerDestroy(resources);
    dResourceRequirementsDestroy(requirements);

    dThreadingImplementationShutdownProcessing(threading);
    dThreadingFreeThreadPool(pool);
    dCooperativeDestroy(cooperative);
    dThreadingFreeImplementation(threading);

    dFree(d, vectorSize);
    dFree(ATEST, matrixSize);
    dFree(DL, matrixSize);
    dFree(L, matrixSize);
    dFree(A, matrixSize);
}


void testSolveLDLT()
{
    dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), 
        *d = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), 
        *b = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff;

    HEADER;

    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
    memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));

    dMakeRandomMatrix (b,MSIZE,1,1.0);
    memcpy (x,b,MSIZE*sizeof(dReal));

    dFactorLDLT (L,d,MSIZE,MSIZE4);
    dSolveLDLT (L,d,x,MSIZE,MSIZE4);

    dMultiply2 (btest,A,x,MSIZE,MSIZE,1);
    diff = dMaxDifference(b,btest,MSIZE,1);
    printf ("\tmaximum difference = %.6e - %s\n",diff,
        diff > tol ? "FAILED" : "passed");

    dFree(btest, vectorSize);
    dFree(b, vectorSize);
    dFree(x, vectorSize);
    dFree(d, vectorSize);
    dFree(L, matrixSize);
    dFree(A, matrixSize);
}

void testCoopSolveLDLT()
{
    const dsizeint COOP_MSIZE = MSIZE * 51, COOP_MSIZE4 = dALIGN_SIZE(COOP_MSIZE, 4);

    dsizeint matrixSize = sizeof(dReal) * COOP_MSIZE4 * COOP_MSIZE, vectorSize = sizeof(dReal) * COOP_MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), 
        *d = (dReal *)dAlloc(vectorSize), *x = (dReal *)dAlloc(vectorSize), 
        *b = (dReal *)dAlloc(vectorSize), *btest = (dReal *)dAlloc(vectorSize), diff;

    const unsigned threadCountMaximum = 8;
    dThreadingImplementationID threading = dThreadingAllocateMultiThreadedImplementation();
    dCooperativeID cooperative = dCooperativeCreate(dThreadingImplementationGetFunctions(threading), threading);
    dThreadingThreadPoolID pool = dThreadingAllocateThreadPool(threadCountMaximum, 0, dAllocateFlagBasicData, NULL);
    dThreadingThreadPoolServeMultiThreadedImplementation(pool, threading);

    dResourceRequirementsID requirements = dResourceRequirementsCreate(cooperative);
    dEstimateCooperativelySolveLDLTResourceRequirements(requirements, threadCountMaximum, COOP_MSIZE);
    dResourceContainerID resources = dResourceContainerAcquire(requirements);

    HEADER;

    for (int pass = 0; pass != 4; ++pass)
    {
        dTimerStart ("Solving LDLT");

        const unsigned allowedThreads = 4;
        const unsigned PASS_MSIZE = COOP_MSIZE - pass, PASS_MSIZE4 = dALIGN_SIZE(PASS_MSIZE, 4);

        dTimerNow ("Preparing data");
        dMakeRandomMatrix (b, PASS_MSIZE, 1, 1.0);

        dMakeRandomMatrix (L, PASS_MSIZE, PASS_MSIZE, 1.0);
        dMultiply2 (A, L, L, PASS_MSIZE, PASS_MSIZE, PASS_MSIZE);

        memcpy (x, b, PASS_MSIZE * sizeof(dReal));
        memcpy (L, A, sizeof(dReal) * PASS_MSIZE4 * PASS_MSIZE);

        dTimerNow ("Factoring");
        dFactorLDLT (L, d, PASS_MSIZE, PASS_MSIZE4);

        dTimerNow ("Solving multi-threaded");
        dCooperativelySolveLDLT(resources, allowedThreads, L, d, x, PASS_MSIZE, PASS_MSIZE4);

        dTimerNow ("Verifying solution");
        dMultiply2 (btest, A, x, PASS_MSIZE, PASS_MSIZE, 1);
        diff = dMaxDifference(b, btest, PASS_MSIZE, 1);
        printf ("\tN=%u: maximum difference = %.6e - %s\n", PASS_MSIZE, diff, diff > 1e2 * tol ? "FAILED" : "passed");

        dTimerEnd();
        dTimerReport(stdout, 0);
    }

    dResourceContainerDestroy(resources);
    dResourceRequirementsDestroy(requirements);

    dThreadingImplementationShutdownProcessing(threading);
    dThreadingFreeThreadPool(pool);
    dCooperativeDestroy(cooperative);
    dThreadingFreeImplementation(threading);

    dFree(btest, vectorSize);
    dFree(b, vectorSize);
    dFree(x, vectorSize);
    dFree(d, vectorSize);
    dFree(L, matrixSize);
    dFree(A, matrixSize);
}


void testLDLTAddTL()
{
    int i,j;
    dsizeint matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE;
    dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), 
        *DL = (dReal *)dAlloc(matrixSize), *ATEST = (dReal *)dAlloc(matrixSize), 
        *d = (dReal *)dAlloc(vectorSize), *a = (dReal *)dAlloc(vectorSize), diff;

    HEADER;

    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
    memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
    dFactorLDLT (L,d,MSIZE,MSIZE4);

    // delete first row and column of factorization
    for (i=0; i<MSIZE; i++) a[i] = -A[i*MSIZE4];
    a[0] += 1;
    dLDLTAddTL (L,d,a,MSIZE,MSIZE4);
    for (i=1; i<MSIZE; i++) L[i*MSIZE4] = 0;
    d[0] = 1;

    // get modified L*D*L'
    dClearUpperTriangle (L,MSIZE);
    for (i=0; i<MSIZE; i++) L[i*MSIZE4+i] = 1.0;
    dSetZero (DL,MSIZE4*MSIZE);
    for (i=0; i<MSIZE; i++) {
        for (j=0; j<MSIZE; j++) DL[i*MSIZE4+j] = L[i*MSIZE4+j] / d[j];
    }
    dMultiply2 (ATEST,L,DL,MSIZE,MSIZE,MSIZE);

    // compare it to A with its first row/column removed
    for (i=1; i<MSIZE; i++) A[i*MSIZE4] = A[i] = 0;
    A[0] = 1;
    diff = dMaxDifference(A,ATEST,MSIZE,MSIZE);
    printf ("\tmaximum difference = %.6e - %s\n",diff,
        diff > tol ? "FAILED" : "passed");

    dFree(a, vectorSize);
    dFree(d, vectorSize);
    dFree(ATEST, matrixSize);
    dFree(DL, matrixSize);
    dFree(L, matrixSize);
    dFree(A, matrixSize);
}


void testLDLTRemove()
{
    int i,j,r;
    dsizeint intVectorSize = sizeof(int) * MSIZE, matrixSize = sizeof(dReal) * MSIZE4 * MSIZE, vectorSize = sizeof(dReal) * MSIZE;
    int *p = (int *)dAlloc(intVectorSize);
    dReal *A = (dReal *)dAlloc(matrixSize), *L = (dReal *)dAlloc(matrixSize), 
        *L2 = (dReal *)dAlloc(matrixSize), *DL2 = (dReal *)dAlloc(matrixSize), 
        *Atest1 = (dReal *)dAlloc(matrixSize), *Atest2 = (dReal *)dAlloc(matrixSize), 
        *d = (dReal *)dAlloc(vectorSize), *d2 = (dReal *)dAlloc(vectorSize), diff, maxdiff;

    HEADER;

    // make array of A row pointers
    dReal *Arows[MSIZE];
    for (i=0; i<MSIZE; i++) Arows[i] = A+i*MSIZE4;

    // fill permutation vector
    for (i=0; i<MSIZE; i++) p[i]=i;

    dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
    dMultiply2 (L,A,A,MSIZE,MSIZE,MSIZE);
    memcpy (A,L,MSIZE4*MSIZE*sizeof(dReal));
    dFactorLDLT (L,d,MSIZE,MSIZE4);

    maxdiff = 1e10;
    for (r=0; r<MSIZE; r++) {
        // get Atest1 = A with row/column r removed
        memcpy (Atest1,A,MSIZE4*MSIZE*sizeof(dReal));
        dRemoveRowCol (Atest1,MSIZE,MSIZE4,r);

        // test that the row/column removal worked
        int bad = 0;
        for (i=0; i<MSIZE; i++) {
            for (j=0; j<MSIZE; j++) {
                if (i != r && j != r) {
                    int ii = i;
                    int jj = j;
                    if (ii >= r) ii--;
                    if (jj >= r) jj--;
                    if (A[i*MSIZE4+j] != Atest1[ii*MSIZE4+jj]) bad = 1;
                }
            }
        }
        if (bad) printf ("\trow/col removal FAILED for row %d\n",r);

        // zero out last row/column of Atest1
        for (i=0; i<MSIZE; i++) {
            Atest1[(MSIZE-1)*MSIZE4+i] = 0;
            Atest1[i*MSIZE4+MSIZE-1] = 0;
        }    

        // get L2*D2*L2' = adjusted factorization to remove that row
        memcpy (L2,L,MSIZE4*MSIZE*sizeof(dReal));
        memcpy (d2,d,MSIZE*sizeof(dReal));
        dLDLTRemove (/*A*/ Arows,p,L2,d2,MSIZE,MSIZE,r,MSIZE4);

        // get Atest2 = L2*D2*L2'
        dClearUpperTriangle (L2,MSIZE);
        for (i=0; i<(MSIZE-1); i++) L2[i*MSIZE4+i] = 1.0;
        for (i=0; i<MSIZE; i++) L2[(MSIZE-1)*MSIZE4+i] = 0;
        d2[MSIZE-1] = 1;
        dSetZero (DL2,MSIZE4*MSIZE);
        for (i=0; i<(MSIZE-1); i++) {
            for (j=0; j<MSIZE-1; j++) DL2[i*MSIZE4+j] = L2[i*MSIZE4+j] / d2[j];
        }

        dMultiply2 (Atest2,L2,DL2,MSIZE,MSIZE,MSIZE);

        diff = dMaxDifference(Atest1,Atest2,MSIZE,MSIZE);
        if (diff < maxdiff) maxdiff = diff;

        /*
        dPrintMatrix (Atest1,MSIZE,MSIZE);
        printf ("\n");
        dPrintMatrix (Atest2,MSIZE,MSIZE);
        printf ("\n");
        */
    }
    printf ("\tmaximum difference = %.6e - %s\n",maxdiff,
        maxdiff > tol ? "FAILED" : "passed");

    dFree(d2, vectorSize);
    dFree(d, vectorSize);
    dFree(Atest2, matrixSize);
    dFree(Atest1, matrixSize);
    dFree(DL2, matrixSize);
    dFree(L2, matrixSize);
    dFree(L, matrixSize);
    dFree(A, matrixSize);
}

//****************************************************************************
// test mass stuff

#define NUMP 10		// number of particles


void printMassParams (dMass *m)
{
    printf ("mass = %.4f\n",m->mass);
    printf ("com  = (%.4f,%.4f,%.4f)\n",m->c[0],m->c[1],m->c[2]);
    printf ("I    = [ %10.4f %10.4f %10.4f ]\n"
        "       [ %10.4f %10.4f %10.4f ]\n"
        "       [ %10.4f %10.4f %10.4f ]\n",
        m->_I(0,0),m->_I(0,1),m->_I(0,2),
        m->_I(1,0),m->_I(1,1),m->_I(1,2),
        m->_I(2,0),m->_I(2,1),m->_I(2,2));
}


void compareMassParams (dMass *m1, dMass *m2, const char *msg)
{
    int i,j,ok = 1;
    if (!(cmp(m1->mass,m2->mass) && cmp(m1->c[0],m2->c[0]) &&
        cmp(m1->c[1],m2->c[1]) && cmp(m1->c[2],m2->c[2])))
        ok = 0;
    for (i=0; i<3; i++) for (j=0; j<3; j++)
        if (cmp (m1->_I(i,j),m2->_I(i,j))==0) ok = 0;
    if (ok) printf ("\tpassed (%s)\n",msg); else printf ("\tFAILED (%s)\n",msg);
}


// compute the mass parameters of a particle set

void computeMassParams (dMass *m, dReal q[NUMP][3], dReal pm[NUMP])
{
    int i,j;
    dMassSetZero (m);
    for (i=0; i<NUMP; i++) {
        m->mass += pm[i];
        for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j];
        m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]);
        m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]);
        m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]);
        m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]);
        m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]);
        m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]);
    }
    for (j=0; j<3; j++) m->c[j] /= m->mass;
    m->_I(1,0) = m->_I(0,1);
    m->_I(2,0) = m->_I(0,2);
    m->_I(2,1) = m->_I(1,2);
}


void testMassFunctions()
{
    dMass m;
    int i,j;
    dReal q[NUMP][3];		// particle positions
    dReal pm[NUMP];		// particle masses
    dMass m1,m2;
    dMatrix3 R;

    HEADER;

    printf ("\t");
    dMassSetZero (&m);
    TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6),
        printf (" FAILED (1)\n"), printf (" passed (1)\n"));

    printf ("\t");
    dMassSetZero (&m);
    TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4),
        printf ("passed (2)\n") , printf (" FAILED (2)\n"));
    if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) &&
        m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 &&
        m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 &&
        m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4)
        printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n");

    dMassSetZero (&m);
    dMassSetSphere (&m,1.4, 0.86);
    if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
        cmp(m._I(0,0),1.10349124669826) &&
        cmp(m._I(1,1),1.10349124669826) &&
        cmp(m._I(2,2),1.10349124669826) &&
        m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
        m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
        printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n");

    dMassSetZero (&m);
    dMassSetCapsule (&m,1.3,1,0.76,1.53);
    if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
        cmp(m._I(0,0),1.59461986077384) &&
        cmp(m._I(1,1),4.21878433864904) &&
        cmp(m._I(2,2),4.21878433864904) &&
        m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
        m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
        printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n");

    dMassSetZero (&m);
    dMassSetBox (&m,0.27,3,4,5);
    if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
        cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) &&
        m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
        m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
        printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n");

    // test dMassAdjust?

    // make random particles and compute the mass, COM and inertia, then
    // translate and repeat.
    for (i=0; i<NUMP; i++) {
        pm[i] = dRandReal()+0.5;
        for (j=0; j<3; j++) {
            q[i][j] = 2.0*(dRandReal()-0.5);
        }
    }
    computeMassParams (&m1,q,pm);
    memcpy (&m2,&m1,sizeof(dMass));
    dMassTranslate (&m2,1,2,-3);
    for (i=0; i<NUMP; i++) {
        q[i][0] += 1;
        q[i][1] += 2;
        q[i][2] -= 3;
    }
    computeMassParams (&m1,q,pm);
    compareMassParams (&m1,&m2,"7");

    // rotate the masses
    _R(0,0) = -0.87919618797635;
    _R(0,1) = 0.15278881840384;
    _R(0,2) = -0.45129772879842;
    _R(1,0) = -0.47307856232664;
    _R(1,1) = -0.39258064912909;
    _R(1,2) = 0.78871864932708;
    _R(2,0) = -0.05666336483842;
    _R(2,1) = 0.90693771059546;
    _R(2,2) = 0.41743652473765;
    dMassRotate (&m2,R);
    for (i=0; i<NUMP; i++) {
        dReal a[3];
        dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1);
        q[i][0] = a[0];
        q[i][1] = a[1];
        q[i][2] = a[2];
    }
    computeMassParams (&m1,q,pm);
    compareMassParams (&m1,&m2,"8");
}

//****************************************************************************
// test rotation stuff

void makeRandomRotation (dMatrix3 R)
{
    dReal *u1 = R, *u2=R+4, *u3=R+8;
    dMakeRandomVector (u1,3,1.0);
    dNormalize3 (u1);
    dMakeRandomVector (u2,3,1.0);
    dReal d = dCalcVectorDot3(u1,u2);
    u2[0] -= d*u1[0];
    u2[1] -= d*u1[1];
    u2[2] -= d*u1[2];
    dNormalize3(u2);
    dCalcVectorCross3(u3,u1,u2);
}


void testRtoQandQtoR()
{
    HEADER;
    dMatrix3 R,I,R2;
    dQuaternion q;
    int i;

    // test makeRandomRotation()
    makeRandomRotation (R);
    dMultiply2 (I,R,R,3,3,3);
    printf ("\tmakeRandomRotation() - %s (1)\n",
        cmpIdentityMat3(I) ? "passed" : "FAILED");

    // test QtoR() on random normalized quaternions
    int ok = 1;
    for (i=0; i<100; i++) {
        dMakeRandomVector (q,4,1.0);
        dNormalize4 (q);
        dQtoR (q,R);
        dMultiply2 (I,R,R,3,3,3);
        if (cmpIdentityMat3(I)==0) ok = 0;
    }
    printf ("\tQtoR() orthonormality %s (2)\n", ok ? "passed" : "FAILED");

    // test R -> Q -> R works
    dReal maxdiff=0;
    for (i=0; i<100; i++) {
        makeRandomRotation (R);
        dRtoQ (R,q);
        dQtoR (q,R2);
        dReal diff = dMaxDifference (R,R2,3,3);
        if (diff > maxdiff) maxdiff = diff;
    }
    printf ("\tmaximum difference = %e - %s (3)\n",maxdiff,
        (maxdiff > tol) ? "FAILED" : "passed");
}


void testQuaternionMultiply()
{
    HEADER;
    dMatrix3 RA,RB,RC,Rtest;
    dQuaternion qa,qb,qc;
    dReal diff,maxdiff=0;

    for (int i=0; i<100; i++) {
        makeRandomRotation (RB);
        makeRandomRotation (RC);
        dRtoQ (RB,qb);
        dRtoQ (RC,qc);

        dMultiply0 (RA,RB,RC,3,3,3);
        dQMultiply0 (qa,qb,qc);
        dQtoR (qa,Rtest);
        diff = dMaxDifference (Rtest,RA,3,3);
        if (diff > maxdiff) maxdiff = diff;

        dMultiply1 (RA,RB,RC,3,3,3);
        dQMultiply1 (qa,qb,qc);
        dQtoR (qa,Rtest);
        diff = dMaxDifference (Rtest,RA,3,3);
        if (diff > maxdiff) maxdiff = diff;

        dMultiply2 (RA,RB,RC,3,3,3);
        dQMultiply2 (qa,qb,qc);
        dQtoR (qa,Rtest);
        diff = dMaxDifference (Rtest,RA,3,3);
        if (diff > maxdiff) maxdiff = diff;

        dMultiply0 (RA,RC,RB,3,3,3);
        transpose3x3 (RA);
        dQMultiply3 (qa,qb,qc);
        dQtoR (qa,Rtest);
        diff = dMaxDifference (Rtest,RA,3,3);
        if (diff > maxdiff) maxdiff = diff;
    }
    printf ("\tmaximum difference = %e - %s\n",maxdiff,
        (maxdiff > tol) ? "FAILED" : "passed");
}


void testRotationFunctions()
{
    dMatrix3 R1;
    HEADER;

    printf ("\tdRSetIdentity - ");
    dMakeRandomMatrix (R1,3,3,1.0);
    dRSetIdentity (R1);
    if (cmpIdentityMat3(R1)) printf ("passed\n"); else printf ("FAILED\n");

    printf ("\tdRFromAxisAndAngle - ");

    printf ("\n");
    printf ("\tdRFromEulerAngles - ");

    printf ("\n");
    printf ("\tdRFrom2Axes - ");

    printf ("\n");
}

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

#include <assert.h>

template<class T>
class simplevector
{
private:
    int n;
    int max;
    T* data;

public:
    simplevector() { initialize(); }
    ~simplevector() { finalize(); }
    T& operator[](int i) { assert(i>=0 && i<n); return data[i]; }
    const T& operator[](int i) const { assert(i>=0 && i<n); return data[i]; }
    void push_back(const T& elem)
    {
        if (n == max)
        {
            max *= 2;
            T* newdata = new T[max];
            memcpy(newdata, data, sizeof(T)*n);
            delete[] data;
            data = newdata;
        }
        data[n++] = elem;
    }
    int size() const { return n; }
    void clear() { finalize(); initialize(); }

private:
    void finalize() { delete[] data; }
    void initialize() { data = new T[32]; max = 32; n = 0; }
};

// matrix header on the stack

class dMatrixComparison {
    struct dMatInfo;
    simplevector<dMatInfo*> mat;
    int afterfirst,index;

public:
    dMatrixComparison();
    ~dMatrixComparison();

    dReal nextMatrix (dReal *A, int n, int m, int lower_tri, const char *name, ...);
    // add a new n*m matrix A to the sequence. the name of the matrix is given
    // by the printf-style arguments (name,...). if this is the first sequence
    // then this object will simply record the matrices and return 0.
    // if this the second or subsequent sequence then this object will compare
    // the matrices with the first sequence, and report any differences.
    // the matrix error will be returned. if `lower_tri' is 1 then only the
    // lower triangle of the matrix (including the diagonal) will be compared
    // (the matrix must be square).

    void end();
    // end a sequence.

    void reset();
    // restarts the object, so the next sequence will be the first sequence.

    void dump();
    // print out info about all the matrices in the sequence
};

struct dMatrixComparison::dMatInfo {
    int n,m;		// size of matrix
    char name[128];	// name of the matrix
    dReal *data;		// matrix data
    int size;		// size of `data'
};



dMatrixComparison::dMatrixComparison()
{
    afterfirst = 0;
    index = 0;
}


dMatrixComparison::~dMatrixComparison()
{
    reset();
}


dReal dMatrixComparison::nextMatrix (dReal *A, int n, int m, int lower_tri,
                                     const char *name, ...)
{
    if (A==0 || n < 1 || m < 1 || name==0) dDebug (0,"bad args to nextMatrix");
    int num = n*dPAD(m);

    if (afterfirst==0) {
        dMatInfo *mi = (dMatInfo*) dAlloc (sizeof(dMatInfo));
        mi->n = n;
        mi->m = m;
        mi->size = num * sizeof(dReal);
        mi->data = (dReal*) dAlloc (mi->size);
        memcpy (mi->data,A,mi->size);

        va_list ap;
        va_start (ap,name);
        vsprintf (mi->name,name,ap);
        va_end (ap);
        if (strlen(mi->name) >= sizeof (mi->name)) dDebug (0,"name too long");

        mat.push_back(mi);
        return 0;
    }
    else {
        if (lower_tri && n != m)
            dDebug (0,"dMatrixComparison, lower triangular matrix must be square");
        if (index >= mat.size()) dDebug (0,"dMatrixComparison, too many matrices");
        dMatInfo *mp = mat[index];
        index++;

        dMatInfo mi;
        va_list ap;
        va_start (ap,name);
        vsprintf (mi.name,name,ap);
        va_end (ap);
        if (strlen(mi.name) >= sizeof (mi.name)) dDebug (0,"name too long");

        if (strcmp(mp->name,mi.name) != 0)
            dDebug (0,"dMatrixComparison, name mismatch (\"%s\" and \"%s\")",
            mp->name,mi.name);
        if (mp->n != n || mp->m != m)
            dDebug (0,"dMatrixComparison, size mismatch (%dx%d and %dx%d)",
            mp->n,mp->m,n,m);

        dReal maxdiff;
        if (lower_tri) {
            maxdiff = dMaxDifferenceLowerTriangle (A,mp->data,n);
        }
        else {
            maxdiff = dMaxDifference (A,mp->data,n,m);
        }
        if (maxdiff > tol)
            dDebug (0,"dMatrixComparison, matrix error (size=%dx%d, name=\"%s\", "
            "error=%.4e)",n,m,mi.name,maxdiff);
        return maxdiff;
    }
}


void dMatrixComparison::end()
{
    if (mat.size() <= 0) dDebug (0,"no matrices in sequence");
    afterfirst = 1;
    index = 0;
}


void dMatrixComparison::reset()
{
    for (int i=0; i<mat.size(); i++) {
        dFree (mat[i]->data,mat[i]->size);
        dFree (mat[i],sizeof(dMatInfo));
    }
    mat.clear();
    afterfirst = 0;
    index = 0;
}


void dMatrixComparison::dump()
{
    for (int i=0; i<mat.size(); i++)
        printf ("%d: %s (%dx%d)\n",i,mat[i]->name,mat[i]->n,mat[i]->m);
}

//****************************************************************************
// unit test

#include <setjmp.h>

// static jmp_buf jump_buffer;

static void myDebug (int /*num*/, const char* /*msg*/, va_list /*ap*/)
{
    // printf ("(Error %d: ",num);
    // vprintf (msg,ap);
    // printf (")\n");
    longjmp (jump_buffer,1);
}


extern "C" void dTestMatrixComparison()
{
    volatile int i;
    printf ("dTestMatrixComparison()\n");
    dMessageFunction *orig_debug = dGetDebugHandler();

    dMatrixComparison mc;
    dReal A[50*50];

    // make first sequence
    unsigned long seed = dRandGetSeed();
    for (i=1; i<49; i++) {
        dMakeRandomMatrix (A,i,i+1,1.0);
        mc.nextMatrix (A,i,i+1,0,"A%d",i);
    }
    mc.end();

    //mc.dump();

    // test identical sequence
    dSetDebugHandler (&myDebug);
    dRandSetSeed (seed);
    if (setjmp (jump_buffer)) {
        printf ("\tFAILED (1)\n");
    }
    else {
        for (i=1; i<49; i++) {
            dMakeRandomMatrix (A,i,i+1,1.0);
            mc.nextMatrix (A,i,i+1,0,"A%d",i);
        }
        mc.end();
        printf ("\tpassed (1)\n");
    }
    dSetDebugHandler (orig_debug);

    // test broken sequences (with matrix error)
    dRandSetSeed (seed);
    volatile int passcount = 0;
    for (i=1; i<49; i++) {
        if (setjmp (jump_buffer)) {
            passcount++;
        }
        else {
            dSetDebugHandler (&myDebug);
            dMakeRandomMatrix (A,i,i+1,1.0);
            A[(i-1)*dPAD(i+1)+i] += REAL(0.01);
            mc.nextMatrix (A,i,i+1,0,"A%d",i);
            dSetDebugHandler (orig_debug);
        }
    }
    mc.end();
    printf ("\t%s (2)\n",(passcount == 48) ? "passed" : "FAILED");

    // test broken sequences (with name error)
    dRandSetSeed (seed);
    passcount = 0;
    for (i=1; i<49; i++) {
        if (setjmp (jump_buffer)) {
            passcount++;
        }
        else {
            dSetDebugHandler (&myDebug);
            dMakeRandomMatrix (A,i,i+1,1.0);
            mc.nextMatrix (A,i,i+1,0,"B%d",i);
            dSetDebugHandler (orig_debug);
        }
    }
    mc.end();
    printf ("\t%s (3)\n",(passcount == 48) ? "passed" : "FAILED");

    // test identical sequence again
    dSetDebugHandler (&myDebug);
    dRandSetSeed (seed);
    if (setjmp (jump_buffer)) {
        printf ("\tFAILED (4)\n");
    }
    else {
        for (i=1; i<49; i++) {
            dMakeRandomMatrix (A,i,i+1,1.0);
            mc.nextMatrix (A,i,i+1,0,"A%d",i);
        }
        mc.end();
        printf ("\tpassed (4)\n");
    }
    dSetDebugHandler (orig_debug);
}

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

// internal unit tests
extern "C" void dTestDataStructures();
extern "C" void dTestMatrixComparison();
extern "C" int dTestSolveLCP();


int main()
{
    dInitODE();
    testRandomNumberGenerator();
    testInfinity();
    testPad();
    testCrossProduct();
    testSetZero();
    testNormalize3();
    //testReorthonormalize();     ... not any more
    testPlaneSpace();
    testMatrixMultiply();
    testSmallMatrixMultiply();
    testCholeskyFactorization();
    testCholeskySolve();
    testInvertPDMatrix();
    testIsPositiveDefinite();
    testFastLDLTFactorization();
    testCoopLDLTFactorization();
    testSolveLDLT();
    testCoopSolveLDLT();
    testLDLTAddTL();
    testLDLTRemove();
    testMassFunctions();
    testRtoQandQtoR();
    testQuaternionMultiply();
    testRotationFunctions();
    dTestMatrixComparison();
    dTestSolveLCP();
    // dTestDataStructures();
    dCloseODE();
    return 0;
}