/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2011-2016 Jose Luis Blanco (joseluisblancoc@gmail.com).
 *   All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/

#include <nanoflann.hpp>

#include <ctime>
#include <cstdlib>
#include <iostream>

#include <Eigen/Dense>

using namespace Eigen;
using namespace std;
using namespace nanoflann;

const int SAMPLES_DIM = 15;

template <typename Der>
void generateRandomPointCloud(Eigen::MatrixBase<Der> &mat, const size_t N, const size_t dim, const typename Der::Scalar max_range = 10)
{
	std::cout << "Generating "<< N << " random points...";
	mat.resize(N,dim);
	for (size_t i = 0; i < N; i++)
		for (size_t d = 0; d < dim; d++)
			mat(i,d) = max_range * (rand() % 1000) / typename Der::Scalar(1000);
	std::cout << "done\n";
}

template <typename num_t>
void kdtree_demo(const size_t nSamples, const size_t dim)
{
	Eigen::Matrix<num_t, Dynamic, Dynamic>  mat(nSamples, dim);

	const num_t max_range = 20;

	// Generate points:
	generateRandomPointCloud(mat, nSamples, dim, max_range);

//	cout << mat << endl;

	// Query point:
	std::vector<num_t> query_pt(dim);
	for (size_t d = 0; d < dim; d++)
		query_pt[d] = max_range * (rand() % 1000) / num_t(1000);

	// ------------------------------------------------------------
	// construct a kd-tree index:
	//    Some of the different possibilities (uncomment just one)
	// ------------------------------------------------------------
	// Dimensionality set at run-time (default: L2)
	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t, Dynamic, Dynamic> >  my_kd_tree_t;

	// Dimensionality set at compile-time
//	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic> >  my_kd_tree_t;

	// Dimensionality set at compile-time: Explicit selection of the distance metric: L2
//	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic>,nanoflann::metric_L2>  my_kd_tree_t;

	// Dimensionality set at compile-time: Explicit selection of the distance metric: L2_simple
//	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic>,nanoflann::metric_L2_Simple>  my_kd_tree_t;

	// Dimensionality set at compile-time: Explicit selection of the distance metric: L1
//	typedef KDTreeEigenMatrixAdaptor< Eigen::Matrix<num_t,Dynamic,Dynamic>,nanoflann::metric_L1>  my_kd_tree_t;

	my_kd_tree_t   mat_index(mat, 10 /* max leaf */ );
	mat_index.index->buildIndex();

	// do a knn search
	const size_t num_results = 3;
	vector<size_t>   ret_indexes(num_results);
	vector<num_t> out_dists_sqr(num_results);

	nanoflann::KNNResultSet<num_t> resultSet(num_results);

	resultSet.init(&ret_indexes[0], &out_dists_sqr[0] );
	mat_index.index->findNeighbors(resultSet, &query_pt[0], nanoflann::SearchParams(10));

	std::cout << "knnSearch(nn="<<num_results<<"): \n";
	for (size_t i=0; i<num_results; i++)
		std::cout << "ret_index["<<i<<"]=" << ret_indexes[i] << " out_dist_sqr=" << out_dists_sqr[i] << endl;

}

int main(int argc, char** argv)
{
	// Randomize Seed
	srand(time(NULL));
	kdtree_demo<float>(1e3 /* samples */, SAMPLES_DIM /* dim */);

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "boost::simd::range SIMD"

#include <boost/simd/sdk/simd/pack.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/sdk/simd/range.hpp>
#include <boost/simd/sdk/memory/allocator.hpp>
#include <boost/simd/include/functions/load.hpp>
#include <boost/simd/include/functions/plus.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/type_traits/is_same.hpp>
#include <vector>
#include <algorithm>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/unit/module.hpp>

////////////////////////////////////////////////////////////////////////////////
// Test boost::simd::input_range
////////////////////////////////////////////////////////////////////////////////

template<class It, class Val>
Val accumulate(It begin, It end, Val const& val_)
{
  Val val = val_;
  for(; begin != end; ++begin)
    val = (Val)(val + *begin);
  return val;
}

template<class Range, class Val>
Val accumulate(Range const& range, Val const& val)
{
  return ::accumulate(boost::begin(range), boost::end(range), val);
}

NT2_TEST_CASE_TPL(input_range_interface, BOOST_SIMD_SIMD_TYPES)
{
  std::vector<T,boost::simd::memory::allocator<T> > v(1024);
  std::vector<T,boost::simd::memory::allocator<T> > c(1024);
  for(std::size_t i=0; i<v.size(); ++i) v[i] = T(i);

  typedef boost::simd::pack<T> p_t;

  p_t x,z;
  T r;
  z = boost::simd::Zero<p_t>();
  x = ::accumulate(boost::simd::range(v),z);
  r = ::accumulate(x.begin(),x.end(),T(0.0));
  NT2_TEST_EQUAL(r,std::accumulate(v.begin(),v.end(),T(0.0)));
}


#include <stan/math/prim/mat.hpp>
#include <stan/math/prim/scal.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/util.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <limits>
#include <vector>

class ParetoTestRig : public VectorRealRNGTestRig {
 public:
  ParetoTestRig()
      : VectorRealRNGTestRig(10000, 10, {0.5, 1.0, 1.3, 2.0}, {1, 2, 3},
                             {-2.5, -1.7, -0.1, 0.0}, {-3, -2, -1, 0},
                             {0.1, 1.0, 1.7, 2.1}, {1, 2, 3, 4},
                             {-2.7, -1.5, -0.5, 0.0}, {-3, -2, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& ymin, const T2& alpha, const T3&,
                        T_rng& rng) const {
    return stan::math::pareto_rng(ymin, alpha, rng);
  }

  std::vector<double> generate_quantiles(double ymin, double alpha,
                                         double) const {
    std::vector<double> quantiles;
    double K = stan::math::round(2 * std::pow(N_, 0.4));
    boost::math::pareto_distribution<> dist(ymin, alpha);

    for (int i = 1; i < K; ++i) {
      double frac = i / K;
      quantiles.push_back(quantile(dist, frac));
    }
    quantiles.push_back(std::numeric_limits<double>::max());

    return quantiles;
  }
};

TEST(ProbDistributionsPareto, errorCheck) {
  check_dist_throws_all_types(ParetoTestRig());
}

TEST(ProbDistributionsPareto, distributionTest) {
  check_quantiles_real_real(ParetoTestRig());
}

TEST(ProbDistributionsPareto, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::pareto_rng(2.0, 1.0, rng));

  EXPECT_THROW(stan::math::pareto_rng(2.0, -1.0, rng), std::domain_error);
  EXPECT_THROW(stan::math::pareto_rng(-2.0, 1.0, rng), std::domain_error);
  EXPECT_THROW(
      stan::math::pareto_rng(stan::math::positive_infinity(), 1.0, rng),
      std::domain_error);
  EXPECT_THROW(stan::math::pareto_rng(2, stan::math::positive_infinity(), rng),
               std::domain_error);
}

TEST(ProbDistributionsPareto, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));

  std::vector<double> samples;
  for (int i = 0; i < N; ++i) {
    samples.push_back(stan::math::pareto_rng(2.0, 1.0, rng));
  }

  // Generate quantiles from boost's Pareto distribution
  boost::math::pareto_distribution<> dist(2.0, 1.0);
  std::vector<double> quantiles;
  for (int i = 1; i < K; ++i) {
    double frac = static_cast<double>(i) / K;
    quantiles.push_back(quantile(dist, frac));
  }
  quantiles.push_back(std::numeric_limits<double>::max());

  // Assert that they match
  assert_matches_quantiles(samples, quantiles, 1e-6);
}


#include <blitzml/base/capsule.h>

#include <blitzml/base/math_util.h>

namespace BlitzML {

CapsuleCalculator::CapsuleCalculator(value_t gamma, value_t Delta, value_t d_sq)
                : gamma(gamma), Delta(Delta) {
  if (Delta < 0.) Delta = 0.;
  if (d_sq < 0.) d_sq = 0.;

  r = (Delta != 0.) ? gamma * d_sq / Delta / 2 : 0.;
  d = sqrt(d_sq);
}

void CapsuleCalculator::compute_capsule_params(value_t xi, CapsuleParams &params) {
  params.radius = compute_radius(xi);
  params.xi = xi;
  if (d == 0.) {
    params.left_beta = 0;
    params.right_beta = 0;
  } else {
    value_t left = compute_left_boundary(xi);
    left = (left > 0.) ? 0. : left;
    params.left_beta = (left + params.radius) / d;
    value_t right = compute_right_boundary(xi);
    params.right_beta = (right - params.radius) / d;
  }
}

value_t CapsuleCalculator::compute_radius(value_t xi) {
  return compute_max_tau(xi, 0);
}

value_t CapsuleCalculator::compute_left_boundary(value_t xi) {
  return -compute_max_tau(xi, -1);
}

value_t CapsuleCalculator::compute_right_boundary(value_t xi) {
  return compute_max_tau(xi, 1);
}

value_t CapsuleCalculator::compute_tau(value_t beta, value_t xi, int shift_term_multiplier) {
  value_t xi_term = compute_xi_term(beta, xi);
  value_t d_term = compute_d_term(beta, xi);
  value_t sqrt_term = 1 + d_term - xi_term;
  value_t pre_sqrt = 2 * Delta * sqrt_term / gamma;
  value_t tau = 0.;
  if (pre_sqrt > 0) {
    tau = beta * sqrt(pre_sqrt);
  }
  return tau + beta * shift_term_multiplier * d;
}

value_t CapsuleCalculator::compute_deriv_tau(value_t beta, value_t xi,
                              int shift_term_multiplier) {
  value_t min_deriv = 1e-12;
  if (beta == 0.)
    return min_deriv;
  if (beta == 0.5) {
    if (xi < 1.0) {
      return -min_deriv;
    } else {
      return min_deriv;
    }
  }

  value_t xi_term = compute_xi_term(beta, xi);
  value_t d_term = compute_d_term(beta, xi);
  value_t sqrt_term = 1 + d_term - xi_term;
  if (sqrt_term < 0) {
    return -min_deriv;
  }

  value_t right_term = 1 + beta * ((1 - r) / sq(1 - beta) - 2 * (1 - xi) / sq(1 - 2 * beta)) / 2 / sqrt_term;
  value_t exclude_shift = sqrt(2 * Delta * sqrt_term / gamma) * right_term;

  return exclude_shift + shift_term_multiplier * d;
}

value_t CapsuleCalculator::compute_max_tau(value_t xi, int shift_term_multiplier) {
  value_t beta_below = 0.;
  value_t beta_above = 0.5;
  for (int itr = 0; itr < 25; ++itr) {
    value_t beta_middle = (beta_below + beta_above) / 2;
    value_t deriv = compute_deriv_tau(beta_middle, xi, shift_term_multiplier);
    if (deriv < 0) {
      beta_above = beta_middle;
    } else {
      beta_below = beta_middle;
    }
  }

  value_t value_below = compute_tau(beta_below, xi, shift_term_multiplier);
  if (value_below <= 0.) {
    return 0;
  }
  value_t deriv_below = compute_deriv_tau(beta_below, xi, shift_term_multiplier);
  value_t theta_above = beta_above / (1 - beta_above);
  value_t theta_below = beta_below / (1 - beta_below);
  value_t deriv_log_below_theta = deriv_below * sq(1 - beta_below) / value_below;
  value_t tau_upper_est = value_below * exp(deriv_log_below_theta * (theta_above - theta_below) / 2);
  return tau_upper_est;
}

value_t CapsuleCalculator::compute_xi_term(value_t beta, value_t xi) {
  if (xi == 1.0) {
    return 0;
  }
  return (1 - xi) / (1 - 2 * beta);
}

value_t CapsuleCalculator::compute_d_term(value_t beta, value_t xi) {
  return beta / (1 - beta) * (1 - r);
}

}