/* Generates gaussian random deviates from uniform random deviates.
**
** Pseudo code of the algorithm is given at http://home.online.no/~pjacklam/notes/invnorm
**
** Copyright © 2010-2023 Dynare Team
**
** This file is part of Dynare.
**
** Dynare is free software: you can redistribute it and/or modify
** it under the terms of the GNU General Public License as published by
** the Free Software Foundation, either version 3 of the License, or
** (at your option) any later version.
**
** Dynare 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
** GNU General Public License for more details.
**
** You should have received a copy of the GNU General Public License
** along with Dynare.  If not, see <https://www.gnu.org/licenses/>.
**
** AUTHOR(S): stephane DOT adjemian AT univ DASH lemans DOT fr
*/

#include <algorithm>
#include <array>
#include <cmath>
#include <limits>
#include <numbers>
#include <vector>

#include <omp.h>

#include <dynblas.h>

using namespace std;

constexpr double lb = .02425;
constexpr double ub = .97575;

template<typename T>
T
icdf(const T uniform)
/*
**  This function invert the gaussian cumulative distribution function.
**
*/
{
  const array<T, 6> A {-3.969683028665376e+01, 2.209460984245205e+02,  -2.759285104469687e+02,
                       1.383577518672690e+02,  -3.066479806614716e+01, 2.506628277459239e+00};
  const array<T, 5> B {-5.447609879822406e+01, 1.615858368580409e+02, -1.556989798598866e+02,
                       6.680131188771972e+01, -1.328068155288572e+01};
  const array<T, 6> C {-7.784894002430293e-03, -3.223964580411365e-01, -2.400758277161838e+00,
                       -2.549732539343734e+00, 4.374664141464968e+00,  2.938163982698783e+00};
  const array<T, 4> D {7.784695709041462e-03, 3.224671290700398e-01, 2.445134137142996e+00,
                       3.754408661907416e+00};
  T gaussian = static_cast<T>(0.0);
  if (0 < uniform && uniform < lb)
    {
      T tmp;
      tmp = sqrt(-2 * log(uniform));
      gaussian = (((((C[0] * tmp + C[1]) * tmp + C[2]) * tmp + C[3]) * tmp + C[4]) * tmp + C[5])
                 / ((((D[0] * tmp + D[1]) * tmp + D[2]) * tmp + D[3]) * tmp + 1);
    }
  else
    {
      if (lb <= uniform && uniform <= ub)
        {
          T tmp, TMP;
          tmp = uniform - .5;
          TMP = tmp * tmp;
          gaussian = (((((A[0] * TMP + A[1]) * TMP + A[2]) * TMP + A[3]) * TMP + A[4]) * TMP + A[5])
                     * tmp
                     / (((((B[0] * TMP + B[1]) * TMP + B[2]) * TMP + B[3]) * TMP + B[4]) * TMP + 1);
        }
      else
        {
          if (ub < uniform && uniform < 1)
            {
              T tmp;
              tmp = sqrt(-2 * log(1 - uniform));
              gaussian
                  = -(((((C[0] * tmp + C[1]) * tmp + C[2]) * tmp + C[3]) * tmp + C[4]) * tmp + C[5])
                    / ((((D[0] * tmp + D[1]) * tmp + D[2]) * tmp + D[3]) * tmp + 1);
            }
        }
    }
  if (0 < uniform && uniform < 1)
    {
      T tmp, tmp_;
      tmp = .5 * erfc(-gaussian / sqrt(2.0)) - uniform;
      tmp_ = tmp * sqrt(2 * numbers::pi) * exp(.5 * gaussian * gaussian);
      gaussian = gaussian - tmp_ / (1 + .5 * gaussian * tmp_);
    }
  if (uniform == 0)
    gaussian = -numeric_limits<T>::infinity();

  if (uniform == 1)
    gaussian = numeric_limits<T>::infinity();

  return gaussian;
}

template<typename T>
void
icdfm(int n, T* U)
{
#pragma omp parallel for
  for (int i = 0; i < n; i++)
    U[i] = icdf(U[i]);
  return;
}

template<typename T>
void
icdfmSigma(int d, int n, T* U, const double* LowerCholSigma)
{
  double one = 1.0;
  double zero = 0.0;
  blas_int dd(d);
  blas_int nn(n);
  icdfm(n * d, U);
  vector<double> tmp(n * d);
  dgemm("N", "N", &dd, &nn, &dd, &one, LowerCholSigma, &dd, U, &dd, &zero, tmp.data(), &dd);
  copy_n(tmp.begin(), d * n, U);
}

template<typename T>
void
usphere(int d, int n, T* U)
{
  icdfm(n * d, U);
#pragma omp parallel for
  for (int j = 0; j < n; j++) // sequence index.
    {
      int k = j * d;
      double norm = 0.0;
      for (int i = 0; i < d; i++) // dimension index.
        norm = norm + U[k + i] * U[k + i];

      norm = sqrt(norm);
      for (int i = 0; i < d; i++) // dimension index.
        U[k + i] = U[k + i] / norm;
    }
}

template<typename T>
void
usphereRadius(int d, int n, double radius, T* U)
{
  icdfm(n * d, U);
#pragma omp parallel for
  for (int j = 0; j < n; j++) // sequence index.
    {
      int k = j * d;
      double norm = 0.0;
      for (int i = 0; i < d; i++) // dimension index.
        norm = norm + U[k + i] * U[k + i];

      norm = sqrt(norm);
      for (int i = 0; i < d; i++) // dimension index.
        U[k + i] = radius * U[k + i] / norm;
    }
}