//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_basic_types.h>
#include <OpenTissue/collision/gjk/gjk.h>
#include <OpenTissue/core/geometry/geometry_obb.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

#include <cmath>

using namespace OpenTissue;

BOOST_AUTO_TEST_SUITE(opentissue_collision_gjk);

BOOST_AUTO_TEST_CASE(face_aligned_separated_boxes)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::real_type                            real_type;

  vector3_type p_a;
  vector3_type p_b;
  real_type tol = 0.01;
  OpenTissue::gjk::obsolete::detail::GJK<vector3_type > gjk;          ///< GJK collision detection Algorithm.
  OpenTissue::geometry::OBB<math_types> A;
  OpenTissue::geometry::OBB<math_types> B;

  A.init(1.0,1.0,1.0);
  B.init(1.0,1.0,1.0);

  vector3_type vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector3_type vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector3_type vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector3_type vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector3_type vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector3_type vector_mz = vector3_type( 0.0, 0.0,-1.0);

  coordsys_type  Awcs = coordsys_type( vector_mx, quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector_px, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  real_type distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_px, quaternion_type());
  Bwcs = coordsys_type( vector_mx, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_py, quaternion_type());
  Bwcs = coordsys_type( vector_my, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_my, quaternion_type());
  Bwcs = coordsys_type( vector_py, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_pz, quaternion_type());
  Bwcs = coordsys_type( vector_mz, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  vector_py = vector3_type( 0.0, 1.0, 0.0);
  vector_my = vector3_type( 0.0,-1.0, 0.0);
  vector_pz = vector3_type( 0.0, 0.0, 1.0);
  vector_mz = vector3_type( 0.0, 0.0,-1.0);
  Awcs = coordsys_type( vector_mz, quaternion_type());
  Bwcs = coordsys_type( vector_pz, quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );
}

BOOST_AUTO_TEST_CASE(non_aligned_cases)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::real_type                            real_type;

  vector3_type p_a;
  vector3_type p_b;
  real_type tol = 0.01;
  OpenTissue::gjk::obsolete::detail::GJK<vector3_type > gjk;          ///< GJK collision detection Algorithm.
  OpenTissue::geometry::OBB<math_types> A;
  OpenTissue::geometry::OBB<math_types> B;

  A.init(1.0,1.0,1.0);
  B.init(1.0,1.0,1.0);

  real_type sqrt_half = std::sqrt(0.5);
  real_type test_distance = 1.0 - (sqrt_half - 0.5);

  quaternion_type q;
  q.Rz( math_types::value_traits::pi()/4.0 );

  vector3_type vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector3_type vector_mx = vector3_type(-1.0, 0.0, 0.0);
  coordsys_type  Awcs = coordsys_type( vector_mx, quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector_px, q);
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  real_type distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, test_distance, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector_mx, quaternion_type());
  Bwcs = coordsys_type( vector3_type( 1.0,0.1,0.1), q);
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, test_distance, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector3_type(-1.0,0.0,0.5), quaternion_type());
  Bwcs = coordsys_type( vector_px, q);
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, test_distance, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  vector_mx = vector3_type(-1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector3_type(-1.0,0.0,-0.5), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 1.0,0.0,0.5), quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK_CLOSE(distance, 1.0, tol);
  BOOST_CHECK( !gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( !gjk.get_common_point(A,B,vector_px,p_a,p_b) );
}

BOOST_AUTO_TEST_CASE(penetrating_cases)
{
  typedef OpenTissue::math::BasicMathTypes<double, size_t> math_types;
  typedef math_types::vector3_type                         vector3_type;
  typedef math_types::quaternion_type                      quaternion_type;
  typedef math_types::coordsys_type                        coordsys_type;
  typedef math_types::real_type                            real_type;

  vector3_type p_a;
  vector3_type p_b;

  OpenTissue::gjk::obsolete::detail::GJK<vector3_type > gjk;          ///< GJK collision detection Algorithm.
  OpenTissue::geometry::OBB<math_types> A;
  OpenTissue::geometry::OBB<math_types> B;

  A.init(1.0,1.0,1.0);
  B.init(1.0,1.0,1.0);

  vector3_type vector_px = vector3_type( 1.0, 0.0, 0.0);
  coordsys_type  Awcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  coordsys_type  Bwcs = coordsys_type( vector3_type( 0.25,0.25,0.25), quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);
  gjk.get_closest_points(A,B,p_a,p_b);
  real_type distance = length( p_a - p_b);
  BOOST_CHECK(fabs(distance) < 10e-7);
  BOOST_CHECK( gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( gjk.get_common_point(A,B,vector_px,p_a,p_b) );

  B.init(0.9,0.9,0.9);

  vector_px = vector3_type( 1.0, 0.0, 0.0);
  Awcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  Bwcs = coordsys_type( vector3_type( 0.0,0.0,0.0), quaternion_type());
  A.place(Awcs);
  B.place(Bwcs);

  gjk.get_closest_points(A,B,p_a,p_b);
  distance = length( p_a - p_b);
  BOOST_CHECK(fabs(distance) < 10e-7);
  BOOST_CHECK( gjk.is_intersecting(A,B,vector_px) );
  BOOST_CHECK( gjk.get_common_point(A,B,vector_px,p_a,p_b) );
}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>

#include <boost/bind/bind.hpp>
#include <boost/math/special_functions/pow.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/moment.hpp>

#include "Filtering.hpp"

#include "RadarCoordinatesTemplate.hpp"

using namespace boost::accumulators;

using namespace boost::numeric::ublas;
using namespace boost::tuples;

matrix<double> stateTransitionMatrix(const std::size_t N, double dt) {
	matrix<double> B = identity_matrix<double>(N);
	for (std::size_t i = 0; i < N; i++) {
		for (std::size_t j = i + 1; j < N; j++) {
			unsigned ji = (unsigned) (j - i);
			double fji = boost::math::factorial<double>(ji);
			B(i, j) = pow(dt, ji) / fji;
		}
	}
	return B;
}

int main() {
	using namespace boost::numeric::ublas;

	std::cout << boost::math::pow<3>(10) << std::endl;

	/*
	matrix<double> TntTn(3,3);
	TntTn(0, 0) = 11.0; TntTn(0, 1) = -5.5; TntTn(0, 2) = 3.85;
	TntTn(1, 0) = -5.5; TntTn(1, 1) = 3.85; TntTn(1, 2) = -3.025;
	TntTn(2, 0) = 3.85; TntTn(2, 1) = -3.025; TntTn(2, 2) = 2.5333;
	vector<double> TntYn(3);
	TntYn(0) = 45.99012356;
	TntYn(1) = -9.88611426;
	TntYn(2) = 0.36213461;

	std::cout << TntTn << std::endl;
	std::cout << TntYn << std::endl;

	permutation_matrix<std::size_t> pm(TntTn.size1());
	lu_factorize(TntTn, pm);
	lu_substitute(TntTn, pm, TntYn);
	std::cout << TntYn << std::endl;

	std::cout << stateTransitionMatrix(8, 0.1) << std::endl;
	*/
	RadarCoordinates rc;

	RealVector E(1), N(1), U(1);
	E(0) = 10;
	N(0) = 20;
	U(0) = 50;
	std::cout << rc.ENU2AER(E, N, U) << std::endl;
	return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */
#pragma once

#include <Eigen/Dense>
#include <dpMM/global.hpp>
#include <dpMM/normal.hpp>
#include <dpMM/iw.hpp>
#include <dpMM/sphere.hpp>

#define LOG_2PI 1.8378770664093453

template<typename T>
class NormalSphere : public Distribution<T>
{
public:
  uint32_t D_; // dimension of the ambient space

  NormalSphere(const Matrix<T,Dynamic,1>& mu,
      const Matrix<T,Dynamic,Dynamic>& Sigma, boost::mt19937* pRndGen);
  NormalSphere(const Matrix<T,Dynamic,1>& mu,
      const Normal<T>& normal, boost::mt19937* pRndGen);
  NormalSphere(const NormalSphere& other);
  ~NormalSphere();

  /* for any point on sphere - maps into T_muS and rotates north before logPdf */
  T logPdf(const Matrix<T,Dynamic,1>& q_i) const;
  /* assumes x_i is already in T_northS */
  T logPdfNorth(const Matrix<T,Dynamic,1>& x_i) const;
  T logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter,
      const Matrix<T,Dynamic,1>& mean, T count) const;
//  T logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter, T count) const;

  Matrix<T,Dynamic,1> sample();

  const Matrix<T,Dynamic,Dynamic>& Sigma() const {return normal_.Sigma();};
  void setSigma(const Matrix<T,Dynamic,Dynamic>& Sigma)
  {return normal_.setSigma(Sigma);};
  T logDetSigma() const {return normal_.logDetSigma();};
  T logNormalizer() const {return -0.5*(normal_.logDetSigma()+D_*LOG_2PI);};

  /* mean on sphere */
  void setMean( const Matrix<T,Dynamic,1>& mu);
  const Matrix<T,Dynamic,1>& getMean() const {return mu_;};

  /* mean in tangent plane */
  void setMuInTpS( const Matrix<T,Dynamic,1>& mu)
  {normal_.mu_ = mu;};

  void setNormal(const Normal<T>& normal) {normal_ = normal;};
  const Normal<T>& normal() const {return normal_;};

private:
  Normal<T> normal_; // zero-mean Gaussian in Tangent plane (dim: D-1)
  Sphere<T> S_;
  Matrix<T,Dynamic,1> mu_; // mean pointing to location in sphere
  Matrix<T,Dynamic,Dynamic> northR_;
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, uint32_t K);

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K);

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    const Matrix<T,Dynamic,Dynamic>& Delta, T nu,
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K);

// ---------------------------------------------------------------------------
template<typename T>
NormalSphere<T>::NormalSphere(const Matrix<T,Dynamic,1>& mu,
    const Matrix<T,Dynamic,Dynamic>& Sigma, boost::mt19937* pRndGen)
  : Distribution<T>(pRndGen),  D_(mu.size()),
    normal_(Sigma,pRndGen), S_(D_)
{
  setMean(mu);
};

template<typename T>
NormalSphere<T>::NormalSphere(const Matrix<T,Dynamic,1>& mu,
      const Normal<T>& normal, boost::mt19937* pRndGen)
  : Distribution<T>(pRndGen),  D_(mu.size()),
    normal_(normal), S_(D_)
{
  setMean(mu);
};

template<typename T>
NormalSphere<T>::NormalSphere(const NormalSphere& other)
  : Distribution<T>(other.pRndGen_), D_(other.mu_.size()),
    normal_(other.normal_), S_(other.D_)
{
  setMean(other.mu_);
};

template<typename T>
NormalSphere<T>::~NormalSphere()
{};

template<typename T>
void NormalSphere<T>::setMean( const Matrix<T,Dynamic,1>& mu)
{
  assert(mu.rows() == D_);
  mu_ = mu;
  northR_ = S_.north_R_TpS2(mu_);
}

template<typename T>
T NormalSphere<T>::logPdf(const Matrix<T,Dynamic,1>& q_i) const
{
//  cout<<q_i.transpose()<<endl;
  Matrix<T,Dynamic,1> x_i = S_.Log_p_single(mu_,q_i);
//  cout<<x_i.transpose()<<endl;
//  cout<<northR_<<endl;

#ifndef NDEBUG
  ASSERT(fabs(x_i.transpose()*mu_)<1e-6, x_i.transpose()*mu_);

  Matrix<T,Dynamic,1> xNorth = (northR_*x_i);
  ASSERT(fabs( xNorth(D_-1)) < 1e-6,
      xNorth.transpose() << endl
      << " northR_ "<<endl<<northR_<<endl
      << " recomputed"<<endl<<S_.north_R_TpS2(mu_)<<endl);
  return normal_.logPdf(xNorth.topRows(D_-1));
#else
  return normal_.logPdf((northR_*x_i).topRows(D_-1));
#endif
//  return normal_.logPdf( S_.Log_p_north(mu_,q_i) );
};

template<typename T>
T NormalSphere<T>::logPdfNorth(const Matrix<T,Dynamic,1>& x_i) const
{
  assert(x_i.rows() == D_-1);
#ifndef NDEBUG
  Matrix<T,Dynamic,1> x(D_);
  x.topRows(D_-1) = x_i;
  x(D_-1) = 1.0;
//  cout<<(x.transpose()*S_.north())<<endl;
  assert(fabs((x.transpose()*S_.north()).norm() -1.) < 1.e-5);
#endif
  return normal_.logPdf(x_i);
};

template<typename T>
T NormalSphere<T>::logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter,
    const Matrix<T,Dynamic,1>& mean, T count) const
{
  return normal_.logPdf(scatter,mean,count);
}

//template<typename T>
//T NormalSphere<T>::logPdfNorth(const Matrix<T,Dynamic,Dynamic>& scatter,
//    T count) const
//{
//  return normal_.logPdf(scatter,count);
//}

template<typename T>
Matrix<T,Dynamic,1> NormalSphere<T>::sample()
{
  Matrix<T,Dynamic,1> xNorth(D_-1);
  xNorth = normal_.sample();
  // if outside radius of PI wrap around
  // TODO
  while(xNorth.norm() > PI)
  {
    cout<<"wrapping around! ---------------------------------------------"<<endl;
    xNorth -= (T(2*PI))*(xNorth/xNorth.norm());
  }
//  cout<<"xNorth = "<<xNorth.transpose()<<endl;
//  cout<<"mu = "<<mu_.transpose()<<endl;
  Matrix<T,Dynamic,1> x = S_.rotate_north2p(mu_,xNorth);
//  cout<<"x = "<<x.transpose()<<endl;
  return S_.Exp_p(mu_,S_.rotate_north2p(mu_,xNorth));
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    const Matrix<T,Dynamic,Dynamic>& Delta, T nu,
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K,
    T minAngle = static_cast<T>(6.))
{
  uint32_t N = x.cols();
  uint32_t D = x.rows();
  Sphere<T> S_(D);
  boost::mt19937 rndGen(9119);

  IW<T> iw(Delta,nu,&rndGen);
  Matrix<T,Dynamic,Dynamic> mus(D,K);
  for(uint32_t k=0; k<K; ++k)
  {
    Matrix<T,Dynamic,Dynamic> Sigma = iw.sample();
//    cout<<Sigma<<endl;
//    cout<<"nu "<<nu<<endl;
//    cout<<Delta<<endl;
    Matrix<T,Dynamic,1> mu = S_.sampleUnif(&rndGen);
    if(k>0)
    {
      bool done = false;
      while(!done)
      {
        mu = S_.sampleUnif(&rndGen);
        done = true;
        for(uint32_t j=0; j<k; ++j)
          done = done & (mu.transpose()*mus.col(j) < cos(minAngle*M_PI/180.0));
      }
    }
    cout<<"sampling data for k="<<k<<" around mu="<<mu.transpose()<<" Sigma:"<<endl;
    cout<<Sigma*(180.0/M_PI)*(180.0/M_PI)<<endl;
    NormalSphere<T> gauss_k(mu,Sigma,&rndGen);
    mus.col(k) = gauss_k.getMean();
    for (uint32_t i=k*(N/K); i<min(N,(k+1)*(N/K)+N%K); ++i)
    {
//      cout<<"--"<<endl;
//      cout<<mus.col(k).transpose()<<endl;
      do{
        x.col(i) = gauss_k.sample();
      }while(fabs(x.col(i).norm()-1.0) > 1e-3);
      if(fabs(x.col(i).norm()-1.0) > 1e-2)
        cout<<x.col(i).norm()<<endl;
      z(i) = k;
//        x.col(i) /= x.col(i).norm();
//      cout<<x.col(i).transpose()<<endl;
    }
  }
  return mus;
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, VectorXu& z, uint32_t K)
{
  uint32_t N = x.cols();
  uint32_t D = x.rows();
  Sphere<T> S_(D);
  boost::mt19937 rndGen(9119);

  Matrix<T,Dynamic,Dynamic> Sigma = Matrix<T,Dynamic,Dynamic>::Identity(D-1,D-1);
  Sigma *= 0.05;
  Matrix<T,Dynamic,Dynamic> mus(D,K);
  for(uint32_t k=0; k<K; ++k)
  {
    NormalSphere<T> gauss_k(S_.sampleUnif(&rndGen),Sigma,&rndGen);
    mus.col(k) = gauss_k.getMean();
    for (uint32_t i=k*(N/K); i<min(N,(k+1)*(N/K)+N%K); ++i)
    {
      do{
        x.col(i) = gauss_k.sample();
      }while(fabs(x.col(i).norm()-1.0) > 1e-3);
      if(fabs(x.col(i).norm()-1.0) > 1e-2)
        cout<<x.col(i).norm()<<endl;
      z(i) = k;
//        x.col(i) /= x.col(i).norm();
//        cout<<x.col(i).transpose()<<endl;
    }
  }
  return mus;
};

template<class T>
inline Matrix<T,Dynamic,Dynamic> sampleClustersOnSphere(
    Matrix<T,Dynamic,Dynamic>& x, uint32_t K)
{
  VectorXu z(x.cols());
  return sampleClustersOnSphere<T>(x,z,K);
};


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2018 Klaus Spanderen

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program 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 license for more details.
*/

/*! \file analyticcevengine.cpp */

#include <ql/exercise.hpp>
#include <ql/math/functional.hpp>
#include <ql/pricingengines/vanilla/analyticcevengine.hpp>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/distributions/non_central_chi_squared.hpp>

namespace QuantLib {

    CEVCalculator::CEVCalculator(Real f0, Real alpha, Real beta)
    : f0_(f0),
      alpha_(alpha),
      beta_(beta),
      delta_((1.0-2.0*beta)/(1.0-beta)),
      x0_(X(f0)) { }

    Real CEVCalculator::X(Real f) const {
        return std::pow(f, 2.0*(1.0-beta_))/square<Real>()(alpha_*(1.0-beta_));
    }

    Real CEVCalculator::value(
        Option::Type optionType, Real strike, Time t) const {

        typedef boost::math::non_central_chi_squared_distribution<Real>
            nc_chi2;

        const Real kTilde = X(strike);

        if (optionType == Option::Call) {
            if (delta_ < 2.0) {
                return f0_ * (1.0 - boost::math::cdf(
                         nc_chi2(4.0-delta_, x0_/t), kTilde/t))
                     - strike * boost::math::cdf(
                         nc_chi2(2.0-delta_, kTilde/t), x0_/t);
            }
            else {
                const Real g =
                    boost::math::gamma_p(0.5*delta_-1.0,x0_/(2.0*t));

                return f0_ * (g - boost::math::cdf(
                         nc_chi2(delta_-2.0, kTilde/t), x0_/t))
                     - strike * boost::math::cdf(
                         nc_chi2(delta_, x0_/t), kTilde/t);
            }
        }
        else if (optionType == Option::Put) {
            if (delta_ < 2.0) {
                return - f0_ * boost::math::cdf(
                           nc_chi2(4.0-delta_, x0_/t), kTilde/t)
                       + strike * (1.0 - boost::math::cdf(
                           nc_chi2(2.0-delta_, kTilde/t), x0_/t));
            }
            else {
                return - f0_ * boost::math::cdf(
                           nc_chi2(delta_-2.0, kTilde/t), x0_/t)
                       + strike * (1.0 - boost::math::cdf(
                           nc_chi2(delta_, x0_/t), kTilde/t));
            }
        }
        else
            QL_FAIL("unknown option type");

    }

    AnalyticCEVEngine::AnalyticCEVEngine(
        Real f0, Real alpha, Real beta,
        const Handle<YieldTermStructure>& discountCurve)
    : calculator_(ext::make_shared<CEVCalculator>(f0, alpha,beta)),
      discountCurve_(discountCurve) {
        registerWith(discountCurve_);
    }

    void AnalyticCEVEngine::calculate() const {

        QL_REQUIRE(arguments_.exercise->type() == Exercise::European,
                   "not an European option");

        ext::shared_ptr<StrikedTypePayoff> payoff =
            ext::dynamic_pointer_cast<StrikedTypePayoff>(arguments_.payoff);
        QL_REQUIRE(payoff, "non-striked payoff given");

        const Date exerciseDate = arguments_.exercise->lastDate();

        results_.value = calculator_->value(
                payoff->optionType(),
                payoff->strike(),
                discountCurve_->timeFromReference(exerciseDate))
            * discountCurve_->discount(exerciseDate);
    }

}


/* ----------------------------------------------------------------------------
 * Copyright 2020, Jesus Tordesillas Torres, Aerospace Controls Laboratory
 * Massachusetts Institute of Technology
 * All Rights Reserved
 * Authors: Jesus Tordesillas, et al.
 * See LICENSE file for the license information
 * -------------------------------------------------------------------------- */

#pragma once

#include "mader_types.hpp"
#include <Eigen/Dense>
#include <CGAL/Polyhedron_3.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Convex_hull_traits_3.h>
#include <decomp_geometry/polyhedron.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Convex_hull_traits_3<K> Traits;
typedef Traits::Polyhedron_3 CGAL_Polyhedron_3;
typedef K::Segment_3 Segment_3;
typedef K::Plane_3 Plane_3;
// define point creator
typedef K::Point_3 Point_3;
typedef K::Vector_3 Vector_3;
typedef CGAL::Creator_uniform_3<double, Point_3> PointCreator;

typedef std::vector<CGAL_Polyhedron_3> ConvexHullsOfCurve;
typedef std::vector<ConvexHullsOfCurve> ConvexHullsOfCurves;

namespace cu  // cgal utils
{
struct Plane_equation
{
  template <class Facet>
  typename Facet::Plane_3 operator()(Facet& f)
  {
    typename Facet::Halfedge_handle h = f.halfedge();
    typedef typename Facet::Plane_3 Plane;
    return Plane(h->vertex()->point(), h->next()->vertex()->point(), h->next()->next()->vertex()->point());
  }
};

mt::ConvexHullsOfCurves_Std vectorGCALPol2vectorStdEigen(ConvexHullsOfCurves& convexHulls);

vec_E<Polyhedron<3>> vectorGCALPol2vectorJPSPol(ConvexHullsOfCurves& convex_hulls_of_curves);

CGAL_Polyhedron_3 convexHullOfPoints(const std::vector<Point_3>& points);

mt::Edges vectorGCALPol2edges(const ConvexHullsOfCurves& convexHulls);
}  // namespace cu


/**
 * \file SecondOrderSVFFilter.cpp
 */

#include <ATK/EQ/SecondOrderSVFFilter.h>

#include <cassert>

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename SVFCoefficients>
  class SecondOrderSVFFilter<SVFCoefficients>::SVFState
  {
  public:
    typename SVFCoefficients::DataType iceq1 = 0;
    typename SVFCoefficients::DataType iceq2 = 0;
  };

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::SecondOrderSVFFilter(gsl::index nb_channels)
  :SVFCoefficients(nb_channels), state(std::make_unique<SVFState[]>(nb_channels))
  {
  }

  template<typename SVFCoefficients>
  SecondOrderSVFFilter<SVFCoefficients>::~SecondOrderSVFFilter()
  {
  }

  template<typename SVFCoefficients>
  void SecondOrderSVFFilter<SVFCoefficients>::full_setup()
  {
    state = std::make_unique<SVFState[]>(nb_input_ports);
  }

  template<typename DataType>
  void SecondOrderSVFFilter<DataType>::process_impl(gsl::index size) const
  {
    assert(nb_input_ports == nb_output_ports);

    for(gsl::index j = 0; j < nb_input_ports; ++j)
    {
      const DataType* ATK_RESTRICT input = converted_inputs[j];
      DataType* ATK_RESTRICT output = outputs[j];

      for(gsl::index i = 0; i < size; ++i)
      {
        DataType v3 = input[i] - state[j].iceq2;
        DataType v1 = a1 * state[j].iceq1 + a2 * v3;
        DataType v2 = state[j].iceq2 + a2 * state[j].iceq1 + a3 * v3;
        state[j].iceq1 = CoeffDataType(2) * v1 - state[j].iceq1;
        state[j].iceq2 = CoeffDataType(2) * v2 - state[j].iceq2;

        output[i] = m0 * input[i] + m1 * v1 + m2 * v2;
      }
    }
  }

  template<typename DataType>
  SecondOrderSVFBaseCoefficients<DataType>::SecondOrderSVFBaseCoefficients(gsl::index nb_channels)
  :TypedBaseFilter<DataType>(nb_channels, nb_channels)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_cut_frequency(CoeffDataType cut_frequency)
  {
    if(cut_frequency <= 0)
    {
      throw std::out_of_range("Frequencies must be positive");
    }
    this->cut_frequency = cut_frequency;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFBaseCoefficients<DataType>::CoeffDataType SecondOrderSVFBaseCoefficients<DataType>::get_cut_frequency() const
  {
    return cut_frequency;
  }

  template<typename DataType_>
  void SecondOrderSVFBaseCoefficients<DataType_>::set_Q(CoeffDataType Q)
  {
    if(Q <= 0)
    {
      throw std::out_of_range("Q must be positive");
    }
    this->Q = Q;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFBaseCoefficients<DataType>::CoeffDataType SecondOrderSVFBaseCoefficients<DataType>::get_Q() const
  {
    return Q;
  }

  template<typename DataType_>
  SecondOrderSVFLowPassCoefficients<DataType_>::SecondOrderSVFLowPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFLowPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1/Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 0;
    m2 = 1;
  }

  template<typename DataType_>
  SecondOrderSVFBandPassCoefficients<DataType_>::SecondOrderSVFBandPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFBandPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 0;
    m1 = 1;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFHighPassCoefficients<DataType_>::SecondOrderSVFHighPassCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFHighPassCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = -1;
  }

  template<typename DataType_>
  SecondOrderSVFNotchCoefficients<DataType_>::SecondOrderSVFNotchCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFNotchCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFPeakCoefficients<DataType_>::SecondOrderSVFPeakCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType>
  void SecondOrderSVFPeakCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = -k;
    m2 = 2;
  }

  template<typename DataType_>
  SecondOrderSVFBellCoefficients<DataType_>::SecondOrderSVFBellCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {

  }

  template<typename DataType_>
  void SecondOrderSVFBellCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    if(gain <= 0)
    {
      throw std::out_of_range("Gain must be positive");
    }
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFBellCoefficients<DataType>::CoeffDataType SecondOrderSVFBellCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFBellCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q * gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain * gain - 1);
    m2 = 0;
  }

  template<typename DataType_>
  SecondOrderSVFLowShelfCoefficients<DataType_>::SecondOrderSVFLowShelfCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFLowShelfCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFLowShelfCoefficients<DataType>::CoeffDataType SecondOrderSVFLowShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFLowShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / Q;
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = 1;
    m1 = k * (gain - 1);
    m2 = gain * gain - 1;
  }

  template<typename DataType_>
  SecondOrderSVFHighShelfCoefficients<DataType_>::SecondOrderSVFHighShelfCoefficients(gsl::index nb_channels)
  :Parent(nb_channels)
  {
  }

  template<typename DataType_>
  void SecondOrderSVFHighShelfCoefficients<DataType_>::set_gain(CoeffDataType gain)
  {
    this->gain = gain;
    setup();
  }

  template<typename DataType>
  typename SecondOrderSVFHighShelfCoefficients<DataType>::CoeffDataType SecondOrderSVFHighShelfCoefficients<DataType>::get_gain() const
  {
    return gain;
  }

  template<typename DataType>
  void SecondOrderSVFHighShelfCoefficients<DataType>::setup()
  {
    auto g = std::tan(boost::math::constants::pi<CoeffDataType>() * cut_frequency / input_sampling_rate);
    auto k = 1 / (Q * gain);
    a1 = 1 / (1 + g * (g + k));
    a2 = g * a1;
    a3 = g * a2;
    m0 = gain * gain;
    m1 = k * (1 - gain) * gain;
    m2 = 1 - gain * gain;
  }

#if ATK_ENABLE_INSTANTIATION
  template class SecondOrderSVFBaseCoefficients<float>;
  template class SecondOrderSVFBaseCoefficients<std::complex<float> >;
  template class SecondOrderSVFBaseCoefficients<std::complex<double> >;

  template class SecondOrderSVFLowPassCoefficients<float>;
  template class SecondOrderSVFLowPassCoefficients<std::complex<float> >;
  template class SecondOrderSVFLowPassCoefficients<std::complex<double> >;
  template class SecondOrderSVFBandPassCoefficients<float>;
  template class SecondOrderSVFBandPassCoefficients<std::complex<float> >;
  template class SecondOrderSVFBandPassCoefficients<std::complex<double> >;
  template class SecondOrderSVFHighPassCoefficients<float>;
  template class SecondOrderSVFHighPassCoefficients<std::complex<float> >;
  template class SecondOrderSVFHighPassCoefficients<std::complex<double> >;
  template class SecondOrderSVFNotchCoefficients<float>;
  template class SecondOrderSVFNotchCoefficients<std::complex<float> >;
  template class SecondOrderSVFNotchCoefficients<std::complex<double> >;
  template class SecondOrderSVFPeakCoefficients<float>;
  template class SecondOrderSVFPeakCoefficients<std::complex<float> >;
  template class SecondOrderSVFPeakCoefficients<std::complex<double> >;
  template class SecondOrderSVFBellCoefficients<float>;
  template class SecondOrderSVFBellCoefficients<std::complex<float> >;
  template class SecondOrderSVFBellCoefficients<std::complex<double> >;
  template class SecondOrderSVFLowShelfCoefficients<float>;
  template class SecondOrderSVFLowShelfCoefficients<std::complex<float> >;
  template class SecondOrderSVFLowShelfCoefficients<std::complex<double> >;
  template class SecondOrderSVFHighShelfCoefficients<float>;
  template class SecondOrderSVFHighShelfCoefficients<std::complex<float> >;
  template class SecondOrderSVFHighShelfCoefficients<std::complex<double> >;

  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<std::complex<double> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<float> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<std::complex<float> > >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<std::complex<double> > >;
#endif
  template class SecondOrderSVFBaseCoefficients<double>;

  template class SecondOrderSVFLowPassCoefficients<double>;
  template class SecondOrderSVFBandPassCoefficients<double>;
  template class SecondOrderSVFHighPassCoefficients<double>;
  template class SecondOrderSVFNotchCoefficients<double>;
  template class SecondOrderSVFPeakCoefficients<double>;
  template class SecondOrderSVFBellCoefficients<double>;
  template class SecondOrderSVFLowShelfCoefficients<double>;
  template class SecondOrderSVFHighShelfCoefficients<double>;

  template class SecondOrderSVFFilter<SecondOrderSVFLowPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBandPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighPassCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFNotchCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFPeakCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFBellCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFLowShelfCoefficients<double> >;
  template class SecondOrderSVFFilter<SecondOrderSVFHighShelfCoefficients<double> >;
}


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "MathExtensions.h"

#include <boost/math/special_functions/factorials.hpp>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>

#include <stdexcept>

namespace mathutils {

const double PI(M_PI);
const double SQRTPI(M_SQRTPI);
const double EULER(M_E);

int sign(double x) {return GSL_SIGN(x);}
int fcmp(double x, double y, double eps) {return gsl_fcmp(x,y,eps);}

double sqr(double x) {return gsl_pow_2(x);}

double sqrAbs(dcomp x) {return sqr(real(x))+sqr(imag(x));} // saves the sqrt

double fact(unsigned n)
{
  if (n>GSL_SF_FACT_NMAX) throw std::out_of_range("Factorial of"+std::to_string(n));
  return gsl_sf_fact(n);
}

double choose(unsigned n, unsigned m)
{
  return gsl_sf_choose(n,m);
}

dcomp coherentElement(unsigned long n, dcomp alpha)
{
  using namespace boost::math;
  return n ? n<max_factorial<double>::value ? pow(alpha,n)/sqrt(factorial<double>(n))
                                            : pow(2*n*PI,-.25)*pow(alpha/sqrt(n/EULER),n)
           : 1.;
}

} // mathutils


/**
 * File: random_var.cpp
 * Date: Mon Nov  9 10:31:35 CET 2020
 * Author: Open Risk  (www.openriskmanagement.com)
 *
 */

#include <cmath>
#include <iostream>
#include <cassert>

#include <Poco/JSON/JSON.h>
#include <Poco/JSON/Parser.h>
#include <armadillo>

#include "random_var.h"

using namespace Poco;

RandomVar &RandomVar::operator=(const RandomVar &R) {
    assert(R.size() == this->size()); // check that size matches
    for (size_t i = 0; i < R.size(); i++) {
        this->setX(i, R.getX(i));
        this->setP(i, R.getP(i));
        this->setC(i, R.getC(i));
    }
    return (*this);
};

/**
 * ... text ...
 */
void RandomVar::Sort() {
    arma::sort(m_S);
}

/**
 * ... text ...
 */
void RandomVar::Cumulative() {
    m_C[0] = m_P[0];
    for (size_t i = 1; i < m_P.size(); i++)
        m_C[i] = m_C[i - 1] + m_P[i];
}

/**
 * ... text ...
 */
void RandomVar::Probability() {
    m_P[0] = m_C[0];
    for (size_t i = 1; i < m_P.size(); i++)
        m_P[i] = m_C[i] - m_C[i - 1];
}

/**
 * ... text ...
 */
double RandomVar::Average() const {
    double expectation = 0.0;
    if (m_type == 0) {
        for (size_t i = 0; i < m_P.size(); i++) {
            expectation += m_P[i] * m_X[i];
        }
    } else if (m_type == 1) {
        for (size_t i = 0; i < m_S.size(); ++i) {
            expectation += m_S[i];
        }
        expectation /= m_S.size();
    }
    return expectation;
}

/**
 * ... text ...
 */
double RandomVar::Mean() const {
    return Average();
}

/**
 * ... text ...
 */
double RandomVar::Median() const {
    return Quantile(0.5);
}

/**
 * ... text ...
 */
double RandomVar::Variance() const {
    double var = 0;
    if (m_type == 0) {
        for (size_t i = 0; i < m_P.size(); i++)
            var += m_P[i] * m_X[i] * m_X[i];
        var -= Average() * Average();
    } else if (m_type == 1) {
        for (size_t i = 0; i < m_S.size(); i++)
            var += m_S[i] * m_S[i];
        var /= m_S.size();
        var -= Average() * Average();
    }
    return var;
}

/**
 * ... text ...
 */
double RandomVar::Vol() const {
    return sqrt(Variance());
}

/**
 * ... text ...
 */
double RandomVar::StandardDeviation() const {
    return Vol();
}

/**
 * ... text ...
 */
double RandomVar::Skeweness() const {
    double skew = 0;
    double mean = Average();
    for (size_t i = 0; i < m_P.size(); i++)
        skew += m_P[i] * pow(m_X[i] - mean, 3);
    skew = skew / pow(Variance(), 3 / 2);
    return skew;
}

/**
 * ... text ...
 */
double RandomVar::Kurtosis() const {
    double kurt = 0;
    double mean = Average();
    for (size_t i = 0; i < m_P.size(); i++)
        kurt += m_P[i] * pow(m_X[i] - mean, 4);
    kurt = kurt / pow(Variance(), 2);
    return kurt;
}

/**
 * ... text ...
 */
int RandomVar::Quantile_Index(double alpha) const {
    int index = 0;
    for (int i = 0; i < m_P.size(); i++) {
        if (m_C[i] > 1 - alpha) {
            index = i;
            break;
        }
    }
    return index;
}

/**
 * ... text ...
 */
double RandomVar::Quantile(double alpha) const {
    int index = this->Quantile_Index(alpha);
    return m_X[index];
}

/**
 * ... text ...
 */
double RandomVar::VaR(double alpha) const {
    return Quantile(1.0 - alpha);
}

/**
 * ... text ...
 */
double RandomVar::ExpectedShortFall(double alpha) const {
    int iVaR = this->Quantile_Index(alpha);
    double es = 0;
    for (int k = iVaR; k < m_P.size(); k++) {
        es += m_P[k] * m_X[k];
    }
    es /= alpha;
    return es;
}

/**
 * ... text ...
 */
double RandomVar::ExceedanceProbability(int index) const {
    double ep = 0;
    for (size_t k = index; k < m_P.size(); k++)
        ep += m_P[k];
    return ep;
}

/**
 * ... text ...
 */
double RandomVar::MeanExcess(int index) const {
    double alpha = ExceedanceProbability(index);
    double es = 0;
    for (size_t k = index; k < m_P.size(); k++)
        es += m_P[k] * m_X[k];
    es /= alpha;
    return es;
}

std::ostream &operator<<(std::ostream &os, const RandomVar &R) {
    std::ostringstream out;
    for (size_t k = 0; k < R.size(); k++)
        out << R.getX(k) << "\t" << R.getP(k) << "\t" << R.getC(k) << std::endl;
    return os << out.str();
};

void RandomVar::Print() {
    if (this->m_type == 1) {
        for (size_t s = 0; s < this->m_S.size(); s++) {
            cout << s << "\t" << this->m_S[s] << std::endl;
        }
    } else if (this->m_type == 0) {
        for (size_t s = 0; s < this->m_X.size(); s++) {
            cout << s << "\t" << this->m_X[s] << "\t" << this->m_P[s] << "\t" << this->m_C[s] << std::endl;
        }
    }
}

void RandomVar::ReadFromJSON(const char *fileName) {

    Poco::JSON::Parser loParser;
    std::ifstream t(fileName);
    std::stringstream buffer;
    buffer << t.rdbuf();
    std::string json = buffer.str();
    // Parse the JSON and get the Results
    Poco::Dynamic::Var loParsedJson = loParser.parse(json);
    Poco::Dynamic::Var loParsedJsonResult = loParser.result();

    // Random variable data are an array of objects
    //[
    // {"value": 1, "probability" : 0.2, "cumulative" : 0.2},
    // {"value": 2, "probability" : 0.2, "cumulative" : 0.4},
    // {"value": 3, "probability" : 0.2, "cumulative" : 0.6},
    // {"value": 4, "probability" : 0.2, "cumulative" : 0.8},
    // {"value": 5, "probability" : 0.2, "cumulative" : 1.0}
    //]

    Poco::JSON::Array::Ptr arr = loParsedJsonResult.extract<Poco::JSON::Array::Ptr>();
    size_t size = arr->size();
    cout << "Reading " << size << " records." << endl;

    m_P.resize(size);
    m_C.resize(size);
    m_X.resize(size);

    // Individual data rows
    Poco::JSON::Object::Ptr object;
    for (size_t i = 0; i < size; i++) {
        object = arr->getObject(i);
        this->setX(i, object->getValue<double>("value"));
        this->setP(i, object->getValue<double>("probability"));
        this->setC(i, object->getValue<double>("cumulative"));
    }

}


#include <tiny_math_types.h>
#include <tiny_type_traits.h>
#include <tiny_quaternion.h>
#include <tiny_quaternion_functions.h>
#include <tiny_matrix_functions.h>
#include <tiny_euler_angles.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

typedef tiny::MathTypes<double>  MT;
typedef MT::quaternion_type      Q;
typedef MT::matrix3x3_type       M;
typedef MT::real_type            T;
typedef MT::value_traits         VT;

void do_zyz_test( T const & phi_in, T const & psi_in, T const & theta_in)
{
  Q Q_in;
  Q Q_out;
  Q identity = Q::identity();
  Q Qz1;
  Q Qy;
  Q Qz2;

  T const too_small = 10e-7;

  T phi_out   = 0.0;
  T psi_out   = 0.0;
  T theta_out = 0.0;

  Qz1 = Q::Rz(theta_in);
  Qy = Q::Ry(psi_in);
  Qz2 = Q::Rz(phi_in);

  Q_in = tiny::prod( Qz2 , tiny::prod( Qy , Qz1) );

  tiny::ZYZ_euler_angles(Q_in,phi_out,psi_out,theta_out);

  if(psi_in > 0.0)
  {
    // we only want to do this if we are not in a gimbal lock
    Qz1 = Q::Rz(theta_out);
    Qy = Q::Ry(psi_out);
    Qz2 = Q::Rz(phi_out);
    Q_out = tiny::prod( Qz2 , tiny::prod( Qy , Qz1) );
    identity = tiny::prod( tiny::conj(Q_out), Q_in );

    T const s = fabs( fabs(identity.real())  - 1.0 );
    T const v0 = fabs(identity.imag()(0));
    T const v1 = fabs(identity.imag()(1));
    T const v2 = fabs(identity.imag()(2));

    BOOST_CHECK( s < too_small);
    BOOST_CHECK( v0 < too_small);
    BOOST_CHECK( v1 < too_small);
    BOOST_CHECK( v2 < too_small);

    T const dphi = fabs(phi_in - phi_out);
    T const dpsi = fabs(psi_in - psi_out);
    T const dtheta = fabs(theta_in - theta_out);
    BOOST_CHECK( dphi < too_small);
    BOOST_CHECK( dpsi < too_small);
    BOOST_CHECK( dtheta < too_small);
  }
  else
  {
    // In gimbal lock phi and theta behaves strangely
    BOOST_CHECK_CLOSE( 0.0, theta_out, 0.01);
    double const dpsi = fabs(psi_out);
    BOOST_CHECK( dpsi < too_small);

    T new_phi = phi_in + theta_in;
    T const pi     = 3.1415926535897932384626433832795;
    T const two_pi = 2.0*pi;
    while(new_phi>pi) new_phi -= two_pi;
    while(new_phi<-pi) new_phi += two_pi;

    T const dphi = fabs(new_phi - phi_out);
    BOOST_CHECK( dphi < too_small);
  }
}

void SAFE_CHECK_CLOSE( double const & l, double const & r, double const tol  )
{
  using std::fabs;

  double const ALMOST_ZERO = 10e-10;

  if( fabs(l) < ALMOST_ZERO || fabs(r) < ALMOST_ZERO )
    BOOST_CHECK( fabs(l-r) < ALMOST_ZERO );
  else
    BOOST_CHECK_CLOSE(l, r, tol);
}

void do_xyz_test( T const & alpha_in, T const & beta_in, T const & gamma_in)
{
  // Q Qx   = Q::Rx(alpha_in);
  // Q Qy   = Q::Ry(beta_in);
  // Q Qz   = Q::Rz(gamma_in);
  // Q Q_in = tiny::prod( Qz, tiny::prod( Qy , Qx) );
  // M R_in = tiny::make(Q_in);

  M const Rx_in = M::Rx(alpha_in);
  M const Ry_in = M::Ry(beta_in);
  M const Rz_in = M::Rz(gamma_in);
  M const R_in  = Rz_in * Ry_in * Rx_in;

  double alpha_out = 0.0;
  double beta_out  = 0.0;
  double gamma_out = 0.0;

  tiny::XYZ_euler_angles(R_in, alpha_out, beta_out, gamma_out);

  M const Rx_out = M::Rx(alpha_out);
  M const Ry_out = M::Ry(beta_out);
  M const Rz_out = M::Rz(gamma_out);
  M const R_out  = Rz_out * Ry_out * Rx_out;

  {
    SAFE_CHECK_CLOSE(R_in(0,0), R_out(0,0), 0.1);
    SAFE_CHECK_CLOSE(R_in(0,1), R_out(0,1), 0.1);
    SAFE_CHECK_CLOSE(R_in(0,2), R_out(0,2), 0.1);
    SAFE_CHECK_CLOSE(R_in(1,0), R_out(1,0), 0.1);
    SAFE_CHECK_CLOSE(R_in(1,1), R_out(1,1), 0.1);
    SAFE_CHECK_CLOSE(R_in(1,2), R_out(1,2), 0.1);
    SAFE_CHECK_CLOSE(R_in(2,0), R_out(2,0), 0.1);
    SAFE_CHECK_CLOSE(R_in(2,1), R_out(2,1), 0.1);
    SAFE_CHECK_CLOSE(R_in(2,2), R_out(2,2), 0.1);
  }

}

BOOST_AUTO_TEST_SUITE(tiny_euler_angles);

BOOST_AUTO_TEST_CASE(ZYZ)
{
  size_t N = 15;

  double const pi     = 3.1415926535897932384626433832795;
  double const two_pi = 2.0*pi;
  double const delta = (two_pi)/(N-1);

  double phi = -pi+delta;
  for(;phi<pi;)
  {
    double psi = 0.0;
    for(;psi<pi;)
    {
      double theta = -pi+delta;
      for(;theta<pi;)
      {
        do_zyz_test( phi, psi, theta );
        theta += delta;
      }
      psi += delta;
    }
    phi += delta;
  }
}

BOOST_AUTO_TEST_CASE(XYZ)
{
  double const pi     = VT::pi();
  unsigned int N      = 40;
  double const dpi    = 2.0*pi/(N-1);

  for (unsigned int i = 0; i < N;++i)
  {
    for (unsigned int j = 0; j < N;++j)
    {
      for (unsigned int k = 0; k < N;++k)
      {
        double const alpha = -pi + i*dpi;
        double const beta  = (-pi + j*dpi)/2.0;
        double const gamma = -pi + k*dpi;

        do_xyz_test( alpha, beta, gamma );
      }
    }
  }
}

BOOST_AUTO_TEST_SUITE_END();


////////////////////////////////////////////////////////////////////////////////
/// Copyright 2018-present Xinyan DAI<xinyan.dai@outlook.com>
///
/// permission is hereby granted, free of charge, to any person obtaining a copy
/// of this software and associated documentation files (the "Software"), to
/// deal in the Software without restriction, including without limitation the
/// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
/// sell copies of the Software, and to permit persons to whom the Software is
/// furnished to do so, subject to the following conditions:
///
/// The above copyright notice and this permission notice shall be included in
/// all copies or substantial portions ofthe Software.
///
/// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
/// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
/// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
/// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
/// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
/// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
/// IN THE SOFTWARE.

/// @version 0.1
/// @author  Xinyan DAI
/// @contact xinyan.dai@outlook.com
//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <eigen3/Eigen/Dense>

#include <map>
#include <vector>
#include <random>
#include <iostream>
#include <functional>
#include <boost/progress.hpp>

#include "map_index.hpp"

namespace ss {

    template<typename DataType>
    class ITQIndex: public MapIndex<DataType, uint64_t > {

        using KeyType = uint64_t;
    public:

        explicit ITQIndex(const parameter & para):  MapIndex<DataType, uint64_t >(para) {}

        ~ITQIndex() {}

        void Train(const Matrix<DataType> & data) override;

    protected:
        KeyType Quantize(const DataType *data) override  {
            KeyType hash_value = 0;
            std::vector<DataType > v(_eigen_vectors.size());
            for (unsigned i = 0; i != v.size(); ++i) {
                v[i] = ss::DiffProduct(data, this->_means.data(), _eigen_vectors[i].data(), _eigen_vectors[i].size());
            }
            for (unsigned i = 0; i != v.size(); ++i) {
                DataType  product  = ss::InnerProduct(v.data(), _rotate_matrix[i].data(), _rotate_matrix[i].size());
                hash_value <<= 1;
                hash_value |= product > 0? 1 : 0;
            }
            return hash_value;
        }

    private:

        std::vector<std::vector<DataType > >  _eigen_vectors;
        std::vector<std::vector<DataType> >   _rotate_matrix;

    };
} // namespace ss

// ------------------------- implementation -------------------------

template<typename DataType>
void ss::ITQIndex<DataType>::Train(const Matrix<DataType> & data) {

    this->InitializeMeans(data); /// TODO(Xinyan): should avoid re-computing means

    std::mt19937 rng(unsigned(std::time(0)));
    std::normal_distribution<DataType > nd;
    std::uniform_int_distribution<unsigned> usBits(0, data.getSize() - 1);

    {
        /// 1. wrap data with eigen
        EigenMatrix< DataType > matrix_data = data.GetEigenMatrix();
        /// 2. zero-centered
        EigenMatrix< DataType > centered = matrix_data.rowwise() - matrix_data.colwise().mean();
        /// 3. use eigen-vectors to project data
        EigenMatrix< DataType > cov = (centered.transpose() * centered) / DataType (matrix_data.rows() - 1);
        Eigen::SelfAdjointEigenSolver<EigenMatrix< DataType >> eig(cov);
        EigenMatrix< DataType > eigen_vectors = eig.eigenvectors().rightCols(this->_para.num_bit);
        EigenMatrix< DataType > V = matrix_data * eigen_vectors;
        // 4. initialize R
        EigenMatrix< DataType > R(this->_para.num_bit, this->_para.num_bit);
        Eigen::JacobiSVD<EigenMatrix< DataType >> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV);
        R = svd.matrixU();

        boost::progress_display progress(this->_para.iteration);
        for (int iter = 0; iter < this->_para.iteration; ++iter && ++progress) {

            EigenMatrix< DataType > VR = V * R;
            EigenMatrix< DataType > B(VR.rows(), VR.cols()); // n * c
            assert(VR.rows() == this->_para.train_size);
            assert(VR.cols() == this->_para.num_bit);

            for (unsigned i = 0; i != VR.rows(); ++i) {
                for (unsigned j = 0; j != VR.cols(); ++j) {
                    B(i, j) = VR(i, j) > 0 ? 1 : -1;
                }
            }
            Eigen::JacobiSVD<EigenMatrix< DataType >> svd_tmp(B.transpose() * V, Eigen::ComputeThinU | Eigen::ComputeThinV);
            R = svd_tmp.matrixV() * svd_tmp.matrixU().transpose();
        }

        _rotate_matrix.resize(this->_para.num_bit);
        for (unsigned i = 0; i != _rotate_matrix.size(); ++i) {

            _rotate_matrix[i].resize(this->_para.num_bit);
            for (unsigned j = 0; j != _rotate_matrix[i].size(); ++j) {
                _rotate_matrix[i][j] = R(j, i);
            }
        }
        _eigen_vectors.resize(this->_para.num_bit);
        for (unsigned i = 0; i != _eigen_vectors.size(); ++i) {

            _eigen_vectors[i].resize(data.getDim());
            for (unsigned dimension = 0; dimension != data.getDim(); ++dimension) {
                _eigen_vectors[i][dimension] = eigen_vectors(dimension, i);
            }
        }
    }
}


#include <boost/simd/algorithm.hpp>
#include <boost/simd/pack.hpp>

#include <boost/simd/constant/one.hpp>
#include <boost/simd/function/exp.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/memory/allocator.hpp>

#include <chrono>
#include <cstdlib>
#include <iostream>
#include <limits>
#include <vector>

namespace bs = boost::simd;
using pack_t = bs::pack<float>;

//! [neural-rec]
struct activation_function_rec
{
  template <typename T>
  BOOST_FORCEINLINE T operator()(T const& a)
  {
    return bs::rec(T(1) + bs::exp(a));
  }
};
//! [neural-rec]

//! [neural-struct]
struct activation_function
{
  template <typename T>
  BOOST_FORCEINLINE T operator()(T const& a)
  {
    return T(1) / (T(1) + bs::exp(a));
  }
};
//! [neural-struct]

int main(int argc, char** argv)
{
  using T  = float;
  int size = std::atoi(argv[1]);
  std::vector<T, bs::allocator<T>> activations(size);
  std::vector<T, bs::allocator<T>> results(size);

  std::generate(activations.begin(), activations.end(),
                []() { return (T)std::rand() / std::numeric_limits<int>::max(); });

  auto t0 = std::chrono::high_resolution_clock::now();
  //! [neural-scalar]
  std::transform(activations.data(), activations.data() + activations.size(), results.data(),
                 [](T const& a) { return T(1) / (T(1) + std::exp(a)); });
  //! [neural-scalar]
  auto t1 = std::chrono::high_resolution_clock::now();
  std::cout << "Scalar std::exp time: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count() << std::endl;

  t0 = std::chrono::high_resolution_clock::now();
  //! [neural-scalar-bs]
  std::transform(activations.data(), activations.data() + activations.size(), results.data(),
                 [](T const& a) { return T(1) / (T(1) + bs::exp(a)); });
  //! [neural-scalar-bs]
  t1 = std::chrono::high_resolution_clock::now();
  std::cout << "Scalar bs::exp time: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count() << std::endl;

  t0 = std::chrono::high_resolution_clock::now();
  //! [neural-transform]
  bs::transform(activations.data(), activations.data() + activations.size(), results.data(),
                activation_function{});
  //! [neural-transform]
  t1 = std::chrono::high_resolution_clock::now();
  std::cout << "SIMD time: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count() << std::endl;

  t0 = std::chrono::high_resolution_clock::now();

  //! [neural-transform-rec]
  bs::transform(activations.data(), activations.data() + activations.size(), results.data(),
                activation_function{});
  //! [neural-transform-rec]
  t1 = std::chrono::high_resolution_clock::now();
  std::cout << "SIMD rec time: "
            << std::chrono::duration_cast<std::chrono::milliseconds>(t1 - t0).count() << std::endl;
}


//==================================================================================================
/**
  EVE - Expressive Vector Engine
  Copyright : EVE Contributors & Maintainers
  SPDX-License-Identifier: MIT
**/
//==================================================================================================
#include "test.hpp"
#include <eve/module/bessel.hpp>
#include <boost/math/special_functions/bessel.hpp>
#include <boost/math/special_functions/bessel_prime.hpp>
#include <eve/detail/diff_div.hpp>

EVE_TEST_TYPES( "Check return types of sph_bessel_y1"
            , eve::test::simd::ieee_reals
            )
<typename T>(eve::as<T>)
{
  using v_t = eve::element_type_t<T>;
  TTS_EXPR_IS(eve::sph_bessel_y1(T(0)), T);
  TTS_EXPR_IS(eve::sph_bessel_y1(v_t(0)), v_t);
};

 EVE_TEST( "Check behavior of sph_bessel_y1 on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(0.0, 5.5),
                              eve::test::randoms(5.5, 9.5),
                              eve::test::randoms(9.5, 60.0))
         )
   <typename T>(T const& a0, T const& a1, T const& a2)
{
  using v_t = eve::element_type_t<T>;
  auto eve__sph_bessel_y1 =  [](auto x) { return eve::sph_bessel_y1(x); };
  auto std__sph_bessel_y1 =  [](auto x)->v_t { return boost::math::sph_neumann(1u, double(x)); };

  if constexpr( eve::platform::supports_invalids )
  {
    TTS_ULP_EQUAL(eve__sph_bessel_y1(eve::inf(eve::as<v_t>())), eve::zero(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_y1(eve::nan(eve::as<v_t>())), eve::nan(eve::as<v_t>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_y1(eve::inf(eve::as< T>())),  eve::zero(eve::as< T>()), 0);
    TTS_ULP_EQUAL(eve__sph_bessel_y1(eve::nan(eve::as< T>())), eve::nan(eve::as< T>()), 0);
  }
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(500)), std__sph_bessel_y1(v_t(500)), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(10)), std__sph_bessel_y1(v_t(10))  , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(5)),  std__sph_bessel_y1(v_t(5))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(2)),  std__sph_bessel_y1(v_t(2))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(1.5)),std__sph_bessel_y1(v_t(1.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(0.5)),std__sph_bessel_y1(v_t(0.5)) , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(1)),  std__sph_bessel_y1(v_t(1))   , 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(v_t(0)),  eve::minf(eve::as<v_t>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(500)),  T(std__sph_bessel_y1(v_t(500)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(10)) ,  T(std__sph_bessel_y1( v_t(10)) ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(5))  ,  T(std__sph_bessel_y1( v_t(5))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(2))  ,  T(std__sph_bessel_y1( v_t(2))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(1.5)),  T(std__sph_bessel_y1( v_t(1.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(0.5)),  T(std__sph_bessel_y1( v_t(0.5))), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(1))  ,  T(std__sph_bessel_y1( v_t(1))  ), 6.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1( T(0))  , eve::minf(eve::as< T>()), 0.0);

  TTS_ULP_EQUAL(eve__sph_bessel_y1(a0), map(std__sph_bessel_y1, a0), 130.0); //BMI2 ??
  TTS_ULP_EQUAL(eve__sph_bessel_y1(a1), map(std__sph_bessel_y1, a1), 20.0);
  TTS_ULP_EQUAL(eve__sph_bessel_y1(a2), map(std__sph_bessel_y1, a2), 20.0);

};

EVE_TEST( "Check behavior of diff(sph_bessel_y1) on wide"
        , eve::test::simd::ieee_reals
        , eve::test::generate(eve::test::randoms(1.0, 10.0))
        )
  <typename T>(T a0 )
{
  auto eve__diff_bessel_y1 =  [](auto x) { return eve::diff(eve::sph_bessel_y1)(x); };
  auto df = [](auto x){return eve::detail::centered_diffdiv(eve::sph_bessel_y1, x); };

  TTS_RELATIVE_EQUAL(eve__diff_bessel_y1(a0),   df(a0), 2.0e-2);
};


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#ifndef NDHIST_STATS_STD_HPP_INCLUDED
#define NDHIST_STATS_STD_HPP_INCLUDED 1

#include <cmath>

#include <boost/python.hpp>

#include <ndhist/ndhist.hpp>
#include <ndhist/stats/var.hpp>

namespace ndhist {
namespace stats {

namespace detail {

template <typename AxisValueType, typename WeightValueType>
double
calc_axis_std_impl(
    ndhist const & h
  , intptr_t const axis
)
{
    return std::sqrt(calc_axis_var_impl<AxisValueType, WeightValueType>(h, axis));
}

}// namespace detail

namespace py {

/**
 * @brief Calculates the standard deviation (std) along the given axis of the
 *     given ndhist object. As in statistics, the standard deviation is defined
 *     as :math:`\sqrt{V[x]}`, where :math:`V[x]` is the variance.
 *     This function generates a projection along the given axis and then
 *     calculates the standard deviation.
 *     If None is given as axis, the standard deviation for all individual axes
 *     of the ndhist object will be calculated and returned as a tuple.
 *     But if the dimensionality of the ndhist object is 1, a scalar value is
 *     returned.
 *
 * @note This function is only defined for ndhist objects with POD axis values
 *     AND POD weight values.
 */
boost::python::object
std(
    ndhist const & h
  , boost::python::object const & axis = boost::python::object()
);

}// namespace py

}// namespace stats
}// namespace ndhist

#endif // !NDHIST_STATS_STD_HPP_INCLUDED


//
// Created by kerin on 2019-12-01.
//
#include "mpi_utils.hpp"
#include "typedefs.hpp"
#include "variational_parameters.hpp"

#include "tools/eigen3.3/Dense"
#include "tools/eigen3.3/Eigenvalues"

#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/students_t.hpp>
#include <boost/math/distributions/complement.hpp>
#include <boost/math/distributions/fisher_f.hpp>

#include <cmath>

namespace boost_m  = boost::math;

void prep_lm(const Eigen::MatrixXd &H,
             const Eigen::MatrixXd &y,
             EigenRefDataMatrix HtH,
             EigenRefDataMatrix HtH_inv,
             EigenRefDataMatrix Hty,
             double &rss,
             EigenRefDataMatrix HtVH) {
	/*** All of the heavy lifting for linear hypothesis tests.
	 * Easier to have in one place if we go down the MPI route.
	 */

	HtH     = H.transpose() * H;
	HtH     = mpiUtils::mpiReduce_inplace(HtH);
	Hty     = H.transpose() * y;
	Hty     = mpiUtils::mpiReduce_inplace(Hty);
	HtH_inv = HtH.inverse();

	EigenDataVector resid = y - H * HtH_inv * Hty;
	HtVH = H.transpose() * resid.cwiseProduct(resid).asDiagonal() * H;
	HtVH = mpiUtils::mpiReduce_inplace(HtVH);

	rss = resid.squaredNorm();
	rss = mpiUtils::mpiReduce_inplace(&rss);
}

void prep_lm(const Eigen::MatrixXd &H,
             const Eigen::MatrixXd &y,
             EigenRefDataMatrix HtH,
             EigenRefDataMatrix HtH_inv,
             EigenRefDataMatrix Hty,
             double &rss) {
	/*** All of the heavy lifting for linear hypothesis tests.
	 * Easier to have in one place if we go down the MPI route.
	 */

	HtH     = H.transpose() * H;
	HtH     = mpiUtils::mpiReduce_inplace(HtH);
	Hty     = H.transpose() * y;
	Hty     = mpiUtils::mpiReduce_inplace(Hty);
	HtH_inv = HtH.inverse();

	EigenDataVector resid = y - H * HtH_inv * Hty;
	rss = resid.squaredNorm();
	rss = mpiUtils::mpiReduce_inplace(&rss);
}

void student_t_test(long nn,
                    const Eigen::MatrixXd &HtH_inv,
                    const Eigen::MatrixXd &Hty,
                    double rss,
                    int jj,
                    double &stat,
                    double &pval) {
	/* 2-sided Student t-test on regression output
	   H0: beta[jj] != 0
	 */
	long pp = HtH_inv.rows();
	assert(jj <= pp);
	nn = mpiUtils::mpiReduce_inplace(&nn);

	auto beta = HtH_inv * Hty;
	stat = beta(jj, 0);
	stat /= std::sqrt(rss * HtH_inv(jj, jj) / (double) (nn - pp));
//	if (std::isnan(stat)){
//		std::cout << "est = " << beta(jj, 0) << std::endl;
//		std::cout << "sd(est) = " << std::sqrt(rss * HtH_inv(jj, jj) / (double) (nn - pp)) << std::endl;
//		std::cout << "rss = " << rss << std::endl;
//	}

	boost_m::students_t t_dist(nn - pp);
	pval  = 2 * boost_m::cdf(boost_m::complement(t_dist, fabs(stat)));
}

void hetero_chi_sq(const Eigen::MatrixXd &HtH_inv,
                   const Eigen::MatrixXd &Hty,
                   const Eigen::MatrixXd &HtVH,
                   int jj,
                   double &stat,
                   double &pval) {
	/* Standard errors adjusted for Heteroscedasticity
	   https://en.wikipedia.org/wiki/Heteroscedasticity-consistent_standard_errors
	   HtVH = (H.transpose() * resid_sq.asDiagonal() * H)
	 */
	long pp = HtH_inv.rows();
	assert(jj <= pp);

	auto beta = HtH_inv * Hty;
	auto var_beta = HtH_inv * HtVH * HtH_inv;
	stat = beta(jj, 0) * beta(jj, 0);
	stat /= var_beta(jj, jj);
	stat = std::abs(stat);
//	if (std::isnan(stat)){
//		std::cout << "est_sq = " << beta(jj, 0) * beta(jj, 0) << std::endl;
//		std::cout << "var(est) = " << var_beta(jj, jj) << std::endl;
//	}

	boost_m::chi_squared chi_dist(1);
	pval = boost_m::cdf(boost_m::complement(chi_dist, stat));
}

void homo_chi_sq(long nn,
                 const Eigen::MatrixXd &HtH_inv,
                 const Eigen::MatrixXd &Hty,
                 const double rss,
                 const int jj,
                 double &stat,
                 double &pval) {
	/* Essentially the square of the t-test from regression
	 */
	long pp = HtH_inv.rows();
	assert(jj <= pp);
	nn = mpiUtils::mpiReduce_inplace(&nn);

	auto beta = HtH_inv * Hty;
	stat = beta(jj, 0) * beta(jj, 0);
	stat /= rss * HtH_inv(jj, jj) / (double) (nn - pp);
	stat = std::abs(stat);
//	if (std::isnan(stat)){
//		std::cout << "est_sq = " << beta(jj, 0) * beta(jj, 0) << std::endl;
//		std::cout << "var(est) = " << rss * HtH_inv(jj, jj) / (double) (nn - pp) << std::endl;
//	}

	boost_m::chi_squared chi_dist(1);
	pval = boost_m::cdf(boost_m::complement(chi_dist, stat));
}

double homo_chi_sq(const long nn,
                   const Eigen::MatrixXd &HtH_inv,
                   const Eigen::MatrixXd &Hty,
                   const double rss,
                   const int jj) {
	double tstat, pval;
	homo_chi_sq(nn, HtH_inv, Hty, rss, jj, tstat, pval);
	return pval;
}

double hetero_chi_sq(const Eigen::MatrixXd &HtH_inv,
                     const Eigen::MatrixXd &Hty,
                     const Eigen::MatrixXd &HtVH,
                     int jj) {
	double tstat, pval;
	hetero_chi_sq(HtH_inv, Hty, HtVH, jj, tstat, pval);
	return pval;
}

double student_t_test(long nn,
                      const Eigen::MatrixXd &HtH_inv,
                      const Eigen::MatrixXd &Hty,
                      double rss,
                      int jj) {
	double tstat, pval;
	student_t_test(nn, HtH_inv, Hty, rss, jj, tstat, pval);
	return pval;
}

template <typename GenoMat>
void compute_LOCO_pvals(const EigenDataVector &resid_pheno,
                        const GenoMat &Xtest,
                        Eigen::MatrixXd &neglogPvals,
                        Eigen::MatrixXd &testStats,
                        const EigenDataVector &eta) {
	bool isGxE     = eta.rows() > 0;
	long n_var     = Xtest.cols();
	long n_samples = Xtest.rows();
	long n_effects = (isGxE ? 2 : 1);
	double Nlocal  = n_samples;
	double Nglobal = mpiUtils::mpiReduce_inplace(&Nlocal);

	neglogPvals.resize(n_var, (isGxE ? 4 : 1));
	testStats.resize(n_var, (isGxE ? 4 : 1));

	// Compute p-vals per variant (p=3 as residuals mean centered)
	Eigen::MatrixXd H(n_samples, 2 + 2 * (isGxE ? 1 : 0));
	H.col(0) = Eigen::VectorXd::Constant(n_samples, 1.0);
	if (isGxE) H.col(3) = eta.cast<double>();
	boost_m::students_t t_dist(n_samples - H.cols() - 1);
	boost_m::fisher_f f_dist(n_effects, n_samples - H.cols() - 1);
	for(std::uint32_t jj = 0; jj < n_var; jj++ ) {
		H.col(1) = Xtest.col(jj);

		double rss_alt, rss_null;
		Eigen::MatrixXd HtH(H.cols(), H.cols()), Hty(H.cols(), 1);
		Eigen::MatrixXd HtH_inv(H.cols(), H.cols()), HtVH(H.cols(), H.cols());
		if(!isGxE) {
			double beta_tstat, beta_pval;
			prep_lm(H, resid_pheno, HtH, HtH_inv, Hty, rss_alt);
			student_t_test(n_samples, HtH_inv, Hty, rss_alt, 1, beta_tstat, beta_pval);

			neglogPvals(jj,0) = -1 * log10(beta_pval);
			testStats(jj,0)   = beta_tstat;
		} else {
			H.col(2) = H.col(1).cwiseProduct(eta.cast<double>());
			try {
				// Single-var tests
				double beta_tstat, gam_tstat, rgam_stat, beta_pval, gam_pval, rgam_pval;
				prep_lm(H, resid_pheno, HtH, HtH_inv, Hty, rss_alt, HtVH);
				hetero_chi_sq(HtH_inv, Hty, HtVH, 2, rgam_stat, rgam_pval);
				student_t_test(n_samples, HtH_inv, Hty, rss_alt, 2, gam_tstat, gam_pval);
				student_t_test(n_samples, HtH_inv, Hty, rss_alt, 1, beta_tstat, beta_pval);

				// F-test over main+int effects of snp_j
				double joint_fstat, joint_pval;
				rss_null = resid_pheno.squaredNorm();
				rss_null = mpiUtils::mpiReduce_inplace(&rss_null);
				joint_fstat = (rss_null - rss_alt) / 2.0;
				joint_fstat /= rss_alt / (Nglobal - 3.0);
				joint_pval = 1.0 - boost_m::cdf(f_dist, joint_fstat);

				neglogPvals(jj, 0) = -1 * std::log10(beta_pval);
				neglogPvals(jj, 1) = -1 * std::log10(gam_pval);
				neglogPvals(jj, 2) = -1 * std::log10(rgam_pval);
				neglogPvals(jj, 3) = -1 * std::log10(joint_pval);
				testStats(jj, 0) = beta_tstat;
				testStats(jj, 1) = gam_tstat;
				testStats(jj, 2) = rgam_stat;
				testStats(jj, 3) = joint_fstat;
			} catch (...) {
				neglogPvals(jj, 0) = std::numeric_limits<double>::quiet_NaN();
				neglogPvals(jj, 1) = std::numeric_limits<double>::quiet_NaN();
				neglogPvals(jj, 2) = std::numeric_limits<double>::quiet_NaN();
				neglogPvals(jj, 3) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 0) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 1) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 2) = std::numeric_limits<double>::quiet_NaN();
				testStats(jj, 3) = std::numeric_limits<double>::quiet_NaN();
			}
		}
	}
}

// Explicit instantiation
// https://stackoverflow.com/questions/2152002/how-do-i-force-a-particular-instance-of-a-c-template-to-instantiate
template void compute_LOCO_pvals(const EigenDataVector&, const EigenDataMatrix&,
                                 Eigen::MatrixXd&, Eigen::MatrixXd&,const EigenDataVector&);
template void compute_LOCO_pvals(const EigenDataVector&, const GenotypeMatrix&,
                                 Eigen::MatrixXd&, Eigen::MatrixXd&,const EigenDataVector&);


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/math_random.h>
#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/optimization/optimization_compute_index_reordering.h>
#include <OpenTissue/core/math/optimization/optimization_make_mbd_bounds.h>
#include <OpenTissue/core/math/optimization/non_smooth_newton/optimization_compute_jacobian.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_non_smooth_newton_compute_full_jacobian);

BOOST_AUTO_TEST_CASE(test_case)
{
  typedef ublas::vector<size_t>   idx_vector_type;
  typedef ublas::vector<double>   vector_type;
  typedef ublas::compressed_matrix<double>   matrix_type;
  typedef OpenTissue::math::ValueTraits<double> value_traits;
  typedef double real_type;
  typedef size_t size_type;

  matrix_type A;
  vector_type mu,lo,hi;
  A.resize(10,10,false);
  mu.resize(10,false);
  lo.resize(10,false);
  hi.resize(10,false);

  OpenTissue::math::Random<double> value(0.0,1.0);
  for(size_t i=0;i<A.size1();++i)
  {
    mu(i) = value();
    lo(i) = value_traits::zero();
    hi(i) = value_traits::infinity();
    for(size_t j=0;j<A.size2();++j)
      A(i,j) = value();
  }

  idx_vector_type bitmask;

  bitmask.resize(10,false);

  static size_t const in_lower  = 1;
  static size_t const in_upper  = 2;
  static size_t const in_active = 4;

  bitmask(0) = in_upper;   // pi = inf
  bitmask(1) = in_lower;   // pi = 0
  bitmask(2) = in_active;  // pi = 0
  bitmask(3) = in_lower;   // pi = inf
  bitmask(4) = in_active;  // pi = 3
  bitmask(5) = in_upper;   // pi = 3
  bitmask(6) = in_active;  // pi = inf
  bitmask(7) = in_upper;   // pi = 6
  bitmask(8) = in_lower;   // pi = 6
  bitmask(9) = in_active;  // pi = inf

  idx_vector_type old2new;
  idx_vector_type new2old;

  OpenTissue::math::optimization::compute_index_reordering( bitmask, old2new, new2old );

  idx_vector_type pi;
  pi.resize(10,false);
  size_type nodep = OpenTissue::math::detail::highest<size_t>();
  pi(0) = nodep;
  pi(1) = 0;
  pi(2) = 0;
  pi(3) = nodep;
  pi(4) = 3;
  pi(5) = 3;
  pi(6) = nodep;
  pi(7) = 6;
  pi(8) = 6;
  pi(9) = nodep;

  matrix_type J;
  OpenTissue::math::optimization::detail::compute_jacobian(
      A
    , OpenTissue::math::optimization::make_lower_mbd_bounds( pi, mu, lo )
    , OpenTissue::math::optimization::make_upper_mbd_bounds( pi, mu, hi )
    , bitmask
    , J
    );

  double tol = 0.01;

  // upper      pi = inf
  BOOST_CHECK_CLOSE( double( J(0,0) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(0,9) ), double( 0.0 ), tol );

  // lower      pi = 0
  BOOST_CHECK_CLOSE( double( J(1,0) ), double( mu(1) ), tol );
  BOOST_CHECK_CLOSE( double( J(1,1) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(1,9) ), double( 0.0 ), tol );

  // active     pi = 0
  BOOST_CHECK_CLOSE( double( J(2,0) ), double( A(2,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,1) ), double( A(2,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,2) ), double( A(2,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,3) ), double( A(2,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,4) ), double( A(2,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,5) ), double( A(2,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,6) ), double( A(2,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,7) ), double( A(2,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,8) ), double( A(2,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(2,9) ), double( A(2,9) ), tol );

  // lower      pi = inf
  BOOST_CHECK_CLOSE( double( J(3,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,3) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(3,9) ), double( 0.0 ), tol );

  // active     pi = 3
  BOOST_CHECK_CLOSE( double( J(4,0) ), double( A(4,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,1) ), double( A(4,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,2) ), double( A(4,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,3) ), double( A(4,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,4) ), double( A(4,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,5) ), double( A(4,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,6) ), double( A(4,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,7) ), double( A(4,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,8) ), double( A(4,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(4,9) ), double( A(4,9) ), tol );

  // upper      pi = 3
  BOOST_CHECK_CLOSE( double( J(5,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,3) ), double( -mu(5) ), tol );
  BOOST_CHECK_CLOSE( double( J(5,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,5) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,6) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(5,9) ), double( 0.0 ), tol );

  // active     pi = inf
  BOOST_CHECK_CLOSE( double( J(6,0) ), double( A(6,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,1) ), double( A(6,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,2) ), double( A(6,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,3) ), double( A(6,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,4) ), double( A(6,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,5) ), double( A(6,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,6) ), double( A(6,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,7) ), double( A(6,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,8) ), double( A(6,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(6,9) ), double( A(6,9) ), tol );

  // upper      pi = 6
  BOOST_CHECK_CLOSE( double( J(7,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,6) ), double( -mu(7) ), tol );
  BOOST_CHECK_CLOSE( double( J(7,7) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,8) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(7,9) ), double( 0.0 ), tol );

  // lower      pi = 6
  BOOST_CHECK_CLOSE( double( J(8,0) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,1) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,2) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,3) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,4) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,5) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,6) ), double( mu(8) ), tol );
  BOOST_CHECK_CLOSE( double( J(8,7) ), double( 0.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,8) ), double( 1.0 ), tol );
  BOOST_CHECK_CLOSE( double( J(8,9) ), double( 0.0 ), tol );

  // active     pi = inf
  BOOST_CHECK_CLOSE( double( J(9,0) ), double( A(9,0) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,1) ), double( A(9,1) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,2) ), double( A(9,2) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,3) ), double( A(9,3) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,4) ), double( A(9,4) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,5) ), double( A(9,5) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,6) ), double( A(9,6) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,7) ), double( A(9,7) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,8) ), double( A(9,8) ), tol );
  BOOST_CHECK_CLOSE( double( J(9,9) ), double( A(9,9) ), tol );

}

BOOST_AUTO_TEST_SUITE_END();


#include <iostream>
#include <tclap/CmdLine.h>

// SISL Main Include
#include <sisl/sisl.hpp>

// odd cartesian function spaces
#include <sisl/lattice/cartesian_odd.hpp>
#include <sisl/basis/tp3cubic.hpp>

// odd BCC function spaces
#include <sisl/lattice/bcc_odd.hpp>
#include <sisl/basis/quintic.hpp>
#include <sisl/basis/linear_rdod.hpp>

// utility functions
#include <poisson/pointset.hpp>
#include <sisl/utility/isosurface.hpp>
#include <sisl/utility/dualbcc.hpp>
#include <sisl/utility/dualfcc.hpp>
#include <sisl/utility/dualcc.hpp>
#include <sisl/utility/scattered.hpp>
#include <sisl/utility/ply_writer.hpp>

#include <Eigen/Dense>

#include <tuple>
#include <cmath>

#define VESION_STRING "0.1"

using namespace sisl;
using namespace std;
using namespace TCLAP;

template <class T>
class MarschnerLobb {
public:
	double a, fm;

	MarschnerLobb(double Fm, double alpha) : fm(Fm), a(alpha){ }
	double rho(double r){
		return cos(2*M_PI*fm*cos(r*M_PI/2.));
	}
	double f(const double &xx, const double &yy, const double &zz) {
		double x = (2.*xx-1.), y = (2.*yy-1.), z = (2.*zz-1.);
		double r = rho(sqrt(x*x + y*y));
		double ret = 1. - sin(M_PI*z/2.) + a * (1. + r);
		return ret/(2. + 2.*a);

	}

	double f(const vector3<T> &p) {return this->f(p.i, p.j, p.k);}
	vector3<double> grad_f(const vector3<T> &p) { return this->grad_f(p.i, p.j, p.k); }
	vector3<double> grad_f(const double &x, const double &y, const double &z) {
		double xx = 2.*x - 1.;
		double yy = 2.*y - 1.;
		double zz = 2.*z - 1.;
		return vector3<double> (
			M_PI*M_PI*a*fm*(xx)*sin(2.*M_PI*fm*cos(0.5*M_PI*sqrt(xx*xx + yy*yy))) *sin(0.5*M_PI*sqrt(xx*xx + yy*yy))/(sqrt(xx*xx + yy*yy)*(a + 1.)),
			M_PI*M_PI*a*fm*(yy)*sin(2.*M_PI*fm*cos(0.5*M_PI*sqrt(xx*xx + yy*yy))) *sin(0.5*M_PI*sqrt(xx*xx + yy*yy))/(sqrt(xx*xx + yy*yy)*(a + 1.)),
			-0.5*M_PI*cos(0.5*M_PI*zz)/(a + 1.)
		);
	}
};

template <class T>
class HamFunction {
public:
	HamFunction(){}
	double f(const double &x, const double &y, const double &z) {
		return -(sin(0.3141592654e1 * x) * sin(0.3141592654e1 * y) * sin(0.3141592654e1 * z) * (sqrt(0.25e0 + pow(0.9e1 * x - 0.45e1, 0.2e1) + pow(0.9e1 * y - 0.45e1, 0.2e1) + pow(0.9e1 * z - 0.45e1, 0.2e1)) - 0.2e1 * cos(0.8e1 * 0.3141592654e1 * (0.9e1 * z - 0.45e1) * pow(0.25e0 + pow(0.9e1 * x - 0.45e1, 0.2e1) + pow(0.9e1 * y - 0.45e1, 0.2e1) + pow(0.9e1 * z - 0.45e1, 0.2e1), -0.1e1 / 0.2e1)) - 0.2e1));
	}

	double f(const vector3<T> &p) {
		return this->f(p.i, p.j, p.k);
	}

	vector3<double> grad_f(const vector3<T> &p) { return this->grad_f(p.i, p.j, p.k); }
	vector3<double> grad_f(const double &x, const double &y, const double &z) {
		return vector3<double> (
			2.*(x-0.5),
			2.*(y-0.5),
			2.*(z-0.5)
		);
	}
};

template<class T>
class SphereFunction{
public:
	SphereFunction(){}
	double f(const double &x, const double &y, const double &z) {
		double xx = (x-0.5), yy = (y-0.5), zz = (z-0.5);
		return xx*xx + yy*yy + zz*zz - 0.25*0.25;
	}

	double f(const vector3<T> &p) {return this->f(p.i, p.j, p.k);}

	vector3<double> grad_f(const vector3<T> &p) { return this->grad_f(p.i, p.j, p.k); }
	vector3<double> grad_f(const double &x, const double &y, const double &z) {
		return vector3<double> (
			2*(x-0.5),
			2*(y-0.5),
			2*(z-0.5)
		);
	}
};

int main(int argc, char *argv[])
{
	try {
		CmdLine cmd("Dual marching cubes for CC/BCC/FCC lattices", ' ', VESION_STRING);
		sisl::utility::marchingCubes<double> mc;
		sisl::utility::dualbcc_isosurface<double> dbcc;
		sisl::utility::dualfcc_isosurface<double> dfcc;
		sisl::utility::dualcc_isosurface<double> dcc;

		ValueArg<std::string> outputArg("o", "output", "Output mesh name", true,"output", "filename");
		ValueArg<std::string> testFunction("t", "test_function", "test function", false, "lobner", "test function");
		ValueArg<double> isoValue("i", "iso_value", "Isovalue for contour", true, 0, "iso-value");
		ValueArg<double> gridGranularity("s", "grid_granularity", "Grid grid granularity",true, 0.25, "dh");

		cmd.add(testFunction);
		cmd.add(outputArg);
		cmd.add(isoValue);
		cmd.add(gridGranularity);

		cmd.parse(argc, argv);
		std::string output = outputArg.getValue();
		std::string function = testFunction.getValue();

		double levelset = isoValue.getValue();

		HamFunction<double> hf;
		SphereFunction<double> sf;
		MarschnerLobb<double> mf(6, 0.25);

		if(function == std::string("lobb")){
			dbcc.contour<MarschnerLobb<double>, double, double>(
				&mf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dfcc.contour<MarschnerLobb<double>, double, double>(
				&mf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dcc.contour<MarschnerLobb<double>, double, double>(
				&mf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
		}else if(function == std::string("ham")){
			dbcc.contour<HamFunction<double>, double, double>(
				&hf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dfcc.contour<HamFunction<double>, double, double>(
				&hf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dcc.contour<HamFunction<double>, double, double>(
				&hf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
		}else if(function == std::string("sphere")){
			dbcc.contour<SphereFunction<double> , double, double>(
				&sf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dfcc.contour<SphereFunction<double> , double, double>(
				&sf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
			dcc.contour<SphereFunction<double> , double, double>(
				&sf, levelset, 1./(2.*100),
				vector3<double>(0,0,0),
				vector3<double>(1,1,1)
			);
		}

		dbcc.writeSurface(output + std::string(".bcc.ply"));
		dfcc.writeSurface(output + std::string(".fcc.ply"));
		dcc.writeSurface(output + std::string(".cc.ply"));

	}catch (ArgException &e) {
		cerr << "error: " << e.error() << " for arg " << e.argId() << endl;
	}catch (char const* e) {
		cerr << e << endl;
	}
}


// Boost.Geometry
// Unit Test

// Copyright (c) 2016 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include "test_formula.hpp"
#include "inverse_cases.hpp"

#include <boost/geometry/formulas/vincenty_inverse.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/andoyer_inverse.hpp>

template <typename Result>
void check_inverse(Result const& result, expected_result const& expected, expected_result const& reference, double reference_error)
{
    check_one(result.distance, expected.distance, reference.distance, reference_error);
    check_one(result.azimuth, expected.azimuth, reference.azimuth, reference_error, true);
    check_one(result.reverse_azimuth, expected.reverse_azimuth, reference.reverse_azimuth, reference_error, true);
    check_one(result.reduced_length, expected.reduced_length, reference.reduced_length, reference_error);
    check_one(result.geodesic_scale, expected.geodesic_scale, reference.geodesic_scale, reference_error);
}

void test_all(expected_results const& results)
{
    double const d2r = bg::math::d2r<double>();
    double const r2d = bg::math::r2d<double>();

    double lon1r = results.p1.lon * d2r;
    double lat1r = results.p1.lat * d2r;
    double lon2r = results.p2.lon * d2r;
    double lat2r = results.p2.lat * d2r;

    // WGS84
    bg::srs::spheroid<double> spheroid(6378137.0, 6356752.3142451793);

    bg::formula::result_inverse<double> result_v, result_t, result_a;

    typedef bg::formula::vincenty_inverse<double, true, true, true, true, true> vi_t;
    result_v = vi_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_v.azimuth *= r2d;
    result_v.reverse_azimuth *= r2d;
    check_inverse(result_v, results.vincenty, results.reference, 0.0000001);

    typedef bg::formula::thomas_inverse<double, true, true, true, true, true> th_t;
    result_t = th_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_t.azimuth *= r2d;
    result_t.reverse_azimuth *= r2d;
    check_inverse(result_t, results.thomas, results.reference, 0.00001);

    typedef bg::formula::andoyer_inverse<double, true, true, true, true, true> an_t;
    result_a = an_t::apply(lon1r, lat1r, lon2r, lat2r, spheroid);
    result_a.azimuth *= r2d;
    result_a.reverse_azimuth *= r2d;
    check_inverse(result_a, results.andoyer, results.reference, 0.001);
}

int test_main(int, char*[])
{
    for (size_t i = 0; i < expected_size; ++i)
    {
        test_all(expected[i]);
    }

    return 0;
}


//
// Copyright 2019 Miral Shah <miralshah2211@gmail.com>
// Copyright 2019 Mateusz Loskot <mateusz at loskot dot net>
// Copyright 2021 Pranam Lashkari <plashkari628@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#include <vector>
#include <iostream>
#include <boost/gil/image_processing/kernel.hpp>
#include <boost/gil/image_processing/convolve.hpp>
#include <boost/gil/extension/io/png.hpp>

#include <boost/gil/extension/io/jpeg.hpp>

using namespace boost::gil;
using namespace std;

// Convolves the image with a 2d kernel.

// Note that the kernel can be fixed or resizable:
// kernel_2d_fixed<float, N> k(elements, centre_y, centre_x) produces a fixed kernel
// kernel_2d<float> k(elements, size, centre_y, centre_x) produces a resizable kernel
// The size of the kernel matrix is deduced as the square root of the number of the elements (9 elements yield a 3x3 matrix)

// See also:
// convolution.cpp - Convolution with 2d kernels

int main()
{
    gray8_image_t img;
    read_image("src_view.png", img, png_tag{});
    gray8_image_t img_out(img.dimensions()), img_out1(img.dimensions());

    std::vector<float> v(9, 1.0f / 9.0f);
    detail::kernel_2d<float> kernel(v.begin(), v.size(), 1, 1);
    detail::convolve_2d(view(img), kernel, view(img_out1));

    write_view("out-convolve2d.png", view(img_out1), png_tag{});

    std::vector<float> v1(3, 1.0f / 3.0f);
    kernel_1d<float> kernel1(v1.begin(), v1.size(), 1);

    detail::convolve_1d<gray32f_pixel_t>(const_view(img), kernel1, view(img_out), boundary_option::extend_zero);
    write_view("out-convolve_option_extend_zero.png", view(img_out), png_tag{});

    if (equal_pixels(view(img_out1), view(img_out)))
      cout << "convolve_option_extend_zero" << endl;

    return 0;
}