/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include <mrpt/math/CMatrixDynamic.h>
#include <mrpt/math/CMatrixFixed.h>
#include <mrpt/math/CVectorDynamic.h>
#include <mrpt/random.h>

#include "common.h"

using namespace mrpt;
using namespace mrpt::random;
using namespace mrpt::math;
using namespace std;

// ------------------------------------------------------
//				Benchmark Random Generators
// ------------------------------------------------------
double random_test_1(int a1, int a2)
{
  CRandomGenerator rg;

  // test 1: draw uint32
  // ----------------------------------------
  const long N = 100000000;
  CTicTac tictac;
  for (long i = 0; i < N; i++)
  {
    rg.drawUniform32bit();
  }
  return tictac.Tac() / N;
}
double random_test_2(int a1, int a2)
{
  CRandomGenerator rg;

  // test 2: drawUniform
  // ----------------------------------------
  const long N = 100000000;
  CTicTac tictac;
  for (long i = 0; i < N; i++)
  {
    rg.drawUniform(0, 1);
  }
  return tictac.Tac() / N;
}
double random_test_3(int a1, int a2)
{
  CRandomGenerator rg;

  // test 3: drawGaussian1D_normalized
  // ----------------------------------------
  const long N = 10000000;
  CTicTac tictac;
  for (long i = 0; i < N; i++)
  {
    rg.drawGaussian1D_normalized();
  }
  return tictac.Tac() / N;
}
double random_test_4(int a1, int a2)
{
  CRandomGenerator rg;

  // test 4: drawGaussian1D
  // ----------------------------------------
  const long N = 10000000;
  CTicTac tictac;
  for (long i = 0; i < N; i++)
  {
    rg.drawGaussian1D(5.0, 3.0);
  }
  return tictac.Tac() / N;
}
double random_test_5(int a1, int a2)
{
  CRandomGenerator rg;

  // test 5: system rand()
  // ----------------------------------------
  const long N = 10000000;
  CTicTac tictac;
  for (long i = 0; i < N; i++)
  {
    rand();
  }
  return tictac.Tac() / N;
}
double random_test_6(int a1, int a2)
{
  CRandomGenerator rg;
  double COVs[] = {6.0, -5.0, 2.0, -5.0, 3.0, 1.0, 2.0, 1.0, 7.0};

  CMatrixDouble33 COV(COVs);

  // test 6:
  // ----------------------------------------
  const long N = 100000;
  CTicTac tictac;
  CVectorDouble res;
  for (long i = 0; i < N; i++)
  {
    rg.drawGaussianMultivariate(res, COV);
  }
  return tictac.Tac() / N;
}
double random_test_7(int a1, int a2)
{
  CRandomGenerator rg;
  double COVs[] = {6.0, -5.0, 2.0, -5.0, 3.0, 1.0, 2.0, 1.0, 7.0};

  CMatrixDouble COV(3, 3, COVs);

  // test 7:
  // ----------------------------------------
  const long N = 100000;
  CTicTac tictac;
  CVectorDouble res;
  for (long i = 0; i < N; i++)
  {
    rg.drawGaussianMultivariate(res, COV);
  }
  return tictac.Tac() / N;
}

template <size_t DIM>
double random_test_8(int a1, int a2)
{
  CRandomGenerator rg;

  CMatrixFixed<double, DIM, DIM> R;
  rg.drawGaussian1DMatrix(R, 0.0, 1.0);

  CMatrixFixed<double, DIM, DIM> COV;
  COV.matProductOf_AAt(R);

  const size_t NSAMPS = 1000;

  // test 8:
  // ----------------------------------------
  const long N = 1000;
  CTicTac tictac;
  std::vector<CVectorDouble> res;
  for (long i = 0; i < N; i++)
  {
    rg.drawGaussianMultivariateMany(res, NSAMPS, COV);
  }
  return tictac.Tac() / (N * NSAMPS);
}

double random_test_9(int a1, int a2)
{
  CRandomGenerator rg;

  CMatrixDynamic<double> R(a1, a1);
  rg.drawGaussian1DMatrix(R, 0.0, 1.0);

  CMatrixDynamic<double> COV;
  COV.matProductOf_AAt(R);
  const size_t NSAMPS = 1000;

  // test 9:
  // ----------------------------------------
  const long N = 1000;
  CTicTac tictac;
  std::vector<CVectorDouble> res;
  for (long i = 0; i < N; i++)
  {
    rg.drawGaussianMultivariateMany(res, NSAMPS, COV);
  }
  return tictac.Tac() / (N * NSAMPS);
}

double random_test_10(int a1, int a2)
{
  CRandomGenerator rg;

  CVectorDouble vec(a1);
  rg.drawUniformVector(vec, 0.0, 1.0);

  // test 10: permute
  // ----------------------------------------
  const long N = 10000;
  CVectorDouble vec2;
  CTicTac tictac;
  for (long i = 0; i < N; i++)
  {
    rg.permuteVector(vec, vec2);
  }
  return tictac.Tac() / N;
}

// ------------------------------------------------------
// register_tests_random
// ------------------------------------------------------
void register_tests_random()
{
  lstTests.emplace_back("random: drawUniform32bit", random_test_1);
  lstTests.emplace_back("random: drawUniform", random_test_2);
  lstTests.emplace_back("random: drawGaussian1D_normalized", random_test_3);
  lstTests.emplace_back("random: drawGaussian1D", random_test_4);
  lstTests.emplace_back("random: system rand()", random_test_5);

  lstTests.emplace_back("random: drawGaussianMultivariate(fixed 3x3)", random_test_6);
  lstTests.emplace_back("random: drawGaussianMultivariate(dyn 3x3)", random_test_7);

  lstTests.emplace_back("random: drawGaussianMultivariateMany(fixed 2x2, 1000)", random_test_8<2>);
  lstTests.emplace_back("random: drawGaussianMultivariateMany(fixed 3x3, 1000)", random_test_8<3>);
  lstTests.emplace_back("random: drawGaussianMultivariateMany(fixed 6x6, 1000)", random_test_8<6>);
  lstTests.emplace_back("random: drawGaussianMultivariateMany(dyn 2x2, 1000)", random_test_9, 2);
  lstTests.emplace_back("random: drawGaussianMultivariateMany(dyn 3x3, 1000)", random_test_9, 3);
  lstTests.emplace_back("random: drawGaussianMultivariateMany(dyn 6x6, 1000)", random_test_9, 6);

  lstTests.emplace_back("random: permuteVector (len=10)", random_test_10, 10);
  lstTests.emplace_back("random: permuteVector (len=100)", random_test_10, 100);
  lstTests.emplace_back("random: permuteVector (len=1000)", random_test_10, 1000);
}
