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

 *                                                                       *

 * 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 <ode/config.h>

#include <ode/misc.h>

#include <ode/matrix.h>

#include <ode/ode.h>

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

// random numbers

EXPORT_C unsigned long dRand()

{

  GetGlobalData()->seed = (1664525L*GetGlobalData()->seed + 1013904223L) & 0xffffffff;

  return GetGlobalData()->seed;

}

EXPORT_C unsigned long  dRandGetSeed()

{

  return GetGlobalData()->seed;

}

EXPORT_C void dRandSetSeed (unsigned long s)

{

  GetGlobalData()->seed = s;

}

EXPORT_C int dTestRand()

{

  unsigned long oldseed = GetGlobalData()->seed;

  int ret = 1;

  GetGlobalData()->seed = 0;

  if (dRand() != 0x3c6ef35f || dRand() != 0x47502932 ||

      dRand() != 0xd1ccf6e9 || dRand() != 0xaaf95334 ||

      dRand() != 0x6252e503) ret = 0;

  GetGlobalData()->seed = oldseed;

  return ret;

}

// adam's all-int straightforward(?) dRandInt (0..n-1)

EXPORT_C int dRandInt (int n)

{

  // seems good; xor-fold and modulus

  const unsigned long un = n;

  unsigned long r = dRand();

  // 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);

          }

        }

      }

    }

  }

  return (int) (r % un);    

}

EXPORT_C dReal dRandReal()

{

  return REAL(((dReal) dRand()) / ((dReal) 0xffffffff));

}

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

// matrix utility stuff

EXPORT_C void dPrintMatrix (const dReal *A, int n, int m, char *fmt, FILE *f)

{

  int i,j;

  int skip = dPAD(m);

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

    for (j=0; j<m; j++) fprintf (f,fmt,A[i*skip+j]);

    fprintf (f,"\n");

  }

}

EXPORT_C void dMakeRandomVector (dReal *A, int n, dReal range)

{

  int i;

  for (i=0; i<n; i++) A[i] = dMUL((dMUL(dRandReal(),REAL(2.0))-REAL(1.0)),range);

}

EXPORT_C void dMakeRandomMatrix (dReal *A, int n, int m, dReal range)

{

  int i,j;

  int skip = dPAD(m);

  dSetZero (A,n*skip);

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

    for (j=0; j<m; j++) A[i*skip+j] = dMUL((dMUL(dRandReal(),REAL(2.0))-REAL(1.0)),range);

  }

}

EXPORT_C void dClearUpperTriangle (dReal *A, int n)

{

  int i,j;

  int skip = dPAD(n);

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

    for (j=i+1; j<n; j++) A[i*skip+j] = 0;

  }

}

EXPORT_C dReal dMaxDifference (const dReal *A, const dReal *B, int n, int m)

{

  int i,j;

  int skip = dPAD(m);

  dReal diff,max;

  max = 0;

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

    for (j=0; j<m; j++) {

      diff = dFabs(A[i*skip+j] - B[i*skip+j]);

      if (diff > max) max = diff;

    }

  }

  return max;

}

EXPORT_C dReal dMaxDifferenceLowerTriangle (const dReal *A, const dReal *B, int n)

{

  int i,j;

  int skip = dPAD(n);

  dReal diff,max;

  max = 0;

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

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

      diff = dFabs(A[i*skip+j] - B[i*skip+j]);

      if (diff > max) max = diff;

    }

  }

  return max;

}