/*************************************************************************
 *                                                                       *
 * 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 "odeconfig.h"
#include "ode_misc.hpp"
#include "matrix.h"
//#include "error.h"
#include "odeou.h"

//****************************************************************************
// random numbers

static volatile duint32 seed = 0;

unsigned long dRand()
{
    duint32 origSeed, newSeed;
#if !dTHREADING_INTF_DISABLED
    do {
#endif
        origSeed = seed;
        newSeed = ((duint32)1664525 * origSeed + (duint32)1013904223) & (duint32)0xffffffff;
#if dTHREADING_INTF_DISABLED
        seed = newSeed;
#else
    } while (!AtomicCompareExchange((volatile atomicord32 *)&seed, origSeed, newSeed));
#endif
    return newSeed;
}


unsigned long  dRandGetSeed()
{
    return seed;
}


void dRandSetSeed (unsigned long s)
{
    seed = s;
}


int dTestRand()
{
    unsigned long oldseed = seed;
    int ret = 1;
    seed = 0;
    if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||
        dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||
        dRand() != 0x6252e503) ret = 0;
    seed = oldseed;
    return ret;
}


// adam's all-int straightforward(?) dRandInt (0..n-1)
int dRandInt (int n)
{
    int result;
    // Since there is no memory barrier macro in ODE assign via volatile variable 
    // to prevent compiler reusing seed as value of `r'
    volatile unsigned long raw_r = dRand();
    duint32 r = (duint32)raw_r;
    
    duint32 un = n;
    dIASSERT(sizeof(n) == sizeof(un));

    // note: probably more aggressive than it needs to be -- might be
    //       able to get away without one or two of the innermost branches.
    // if (un <= 0x00010000UL) {
    //     r ^= (r >> 16);
    //     if (un <= 0x00000100UL) {
    //         r ^= (r >> 8);
    //         if (un <= 0x00000010UL) {
    //             r ^= (r >> 4);
    //             if (un <= 0x00000004UL) {
    //                 r ^= (r >> 2);
    //                 if (un <= 0x00000002UL) {
    //                     r ^= (r >> 1);
    //                 }
    //             }
    //         }
    //     }
    // }
    // Optimized version of above
    if (un <= (duint32)0x00000010) {
        r ^= (r >> 16);
        r ^= (r >> 8);
        r ^= (r >> 4);
        if (un <= (duint32)0x00000002) {
            r ^= (r >> 2);
            r ^= (r >> 1);
            result = (r/* & (duint32)0x01*/) & (un >> 1);
        } else {
            if (un <= (duint32)0x00000004) {
                r ^= (r >> 2);
                result = ((r & (duint32)0x03) * un) >> 2;
            } else {
                result = ((r & (duint32)0x0F) * un) >> 4;
            }
        }
    } else {
        if (un <= (duint32)0x00000100) {
            r ^= (r >> 16);
            r ^= (r >> 8);
            result = ((r & (duint32)0xFF) * un) >> 8;
        } else {
            if (un <= (duint32)0x00010000) {
                r ^= (r >> 16);
                result = ((r & (duint32)0xFFFF) * un) >> 16;
            } else {
                result = (int)(((duint64)r * un) >> 32);
            }
        }
    }

    return result;
}


dReal dRandReal()
{
    return (dReal)(((double) dRand()) / ((double) 0xffffffff));
}

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

void dPrintMatrix (const dReal *A, int n, int m, const char *fmt, FILE *f)
{
    int skip = dPAD(m);
    const dReal *Arow = A;
    for (int i=0; i<n; Arow+=skip, ++i) {
        for (int j=0; j<m; ++j) fprintf (f,fmt,Arow[j]);
        fprintf (f,"\n");
    }
}


void dMakeRandomVector (dReal *A, int n, dReal range)
{
    int i;
    for (i=0; i<n; i++) A[i] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
}


void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)
{
    int skip = dPAD(m);
    //  dSetZero (A,n*skip);
    dReal *Arow = A;
    for (int i=0; i<n; Arow+=skip, ++i) {
        for (int j=0; j<m; ++j) Arow[j] = (dRandReal()*REAL(2.0)-REAL(1.0))*range;
    }
}


void dClearUpperTriangle (dReal *A, int n)
{
    int skip = dPAD(n);
    dReal *Arow = A;
    for (int i=0; i<n; Arow+=skip, ++i) {
        for (int j=i+1; j<n; ++j) Arow[j] = 0;
    }
}


dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)
{
    int skip = dPAD(m);
    dReal max = REAL(0.0);
    const dReal *Arow = A, *Brow = B;
    for (int i=0; i<n; Arow+=skip, Brow +=skip, ++i) {
        for (int j=0; j<m; ++j) {
            dReal diff = dFabs(Arow[j] - Brow[j]);
            if (diff > max) max = diff;
        }
    }
    return max;
}


dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)
{
    int skip = dPAD(n);
    dReal max = REAL(0.0);
    const dReal *Arow = A, *Brow = B;
    for (int i=0; i<n; Arow+=skip, Brow+=skip, ++i) {
        for (int j=0; j<=i; ++j) {
            dReal diff = dFabs(Arow[j] - Brow[j]);
            if (diff > max) max = diff;
        }
    }
    return max;
}