/*************************************************************************
 *                                                                       *
 * 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;
}