#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>

#include <CGAL/Delaunay_triangulation_on_sphere_traits_2.h>
#include <CGAL/Delaunay_triangulation_on_sphere_2.h>
#include <CGAL/Projection_on_sphere_traits_3.h>

#include <CGAL/algorithm.h>
#include <CGAL/convex_hull_3.h>
#include <CGAL/Delaunay_triangulation_3.h>
#include <CGAL/point_generators_2.h>
#include <CGAL/point_generators_3.h>
#include <CGAL/Surface_mesh.h>
#include <CGAL/squared_distance_3.h>
#include <CGAL/Timer.h>

#include <boost/iterator/transform_iterator.hpp>

#include <cmath>
#include <fstream>
#include <vector>

typedef CGAL::Exact_predicates_inexact_constructions_kernel    K;
typedef CGAL::Surface_mesh<K>                                  Surface_mesh;

typedef K::Segment_3                                           Segment_3;
typedef CGAL::Delaunay_triangulation_3<K>                      Delaunay;

typedef CGAL::Delaunay_triangulation_on_sphere_traits_2<K>     Gt;
typedef CGAL::Projection_on_sphere_traits_3<K>                 Gt2;
typedef CGAL::Delaunay_triangulation_on_sphere_2<Gt>           DTOS;
typedef CGAL::Delaunay_triangulation_on_sphere_2<Gt2>          DTOS2;
typedef K::Point_3                                             Point;

typedef CGAL::Delaunay_triangulation_3<K, CGAL::Fast_location> Delaunay_fast;
typedef CGAL::Creator_uniform_3<double, Point>                 Creator;

int main(int, char**)
{
  CGAL::Timer time;

  const std::size_t nu_of_pts = 1e7;
  const double radius = 5184.152;

  CGAL::Random random;
  std::cout << "Seed is " << random.get_seed() << std::endl;

  CGAL::Random_points_on_sphere_3<Point, Creator> on_sphere(radius, random);

  std::vector<Point> points;
  points.reserve(nu_of_pts);

  for(std::size_t count=0; count<nu_of_pts; ++count)
    points.push_back(*on_sphere++);
  std::cout << points.size() << " points" << std::endl;

  // Delaunay_traits
  DTOS dtos;
  dtos.set_radius(radius);

  std::cout << " ***STARTING***" << std::endl;
  time.start();
  dtos.insert(points.begin(), points.end());
  time.stop();
  assert(dtos.number_of_vertices() == nu_of_pts);
  std::cout << "Triangulation sphere: "
            << dtos.number_of_vertices() << " vertices in " << time.time() << " sec" << std::endl;

  //Triangulation with points on the sphere (projection_traits)
  Gt2 traits(K::Point_3(0, 0, 0), radius);
  DTOS2 dtos2(traits);
  Gt2::Construct_point_on_sphere_2 cst = traits.construct_point_on_sphere_2_object();

  time.reset();
  time.start();
  dtos2.insert(boost::make_transform_iterator(points.begin(), cst),
               boost::make_transform_iterator(points.end(), cst));
  time.stop();
  std::cout << "Triangulation w/ sphere projection traits: "
            << dtos2.number_of_vertices() << " vertices in " << time.time() << " sec" << std::endl;

//  Surface_mesh sm;

//  time.reset();
//  time.start();
//  CGAL::convex_hull_3(points.begin(), points.end(), sm);
//  time.stop();
//  std::cout << "Convex hull 3D: " << time.time() << " " << std::endl;

  time.reset();
  time.start();
  Delaunay T;
  T.insert(Point(0, 0, 0));
  T.insert(points.begin(), points.end());
  time.stop();
  std::cout << "Delaunay 3D with origin: "
            << T.number_of_vertices() << " vertices in " << time.time() << " sec" << std::endl;

  time.reset();
  time.start();
  Delaunay_fast T_fast_on2;
  T_fast_on2.insert(Point(0, 0, 0));
  T_fast_on2.insert(points.begin(), points.end());
  time.stop();
  std::cout << "Delaunay 3D with origin, fast location: " << time.time() << std::endl;

  return EXIT_SUCCESS;
}


#include "raycast.h"
#include <iostream>
#include <cmath>
#include <Eigen/Eigen>

int signum(int x) {
    return x == 0 ? 0 : x < 0 ? -1 : 1;
}

double mod(double value, double modulus) {
    return fmod(fmod(value, modulus) + modulus, modulus);
}

double intbound(double s, double ds) {
    // Find the smallest positive t such that s+t*ds is an integer.
    if (ds < 0) {
        return intbound(-s, -ds);
    } else {
        s = mod(s, 1);
        // problem is now s+t*ds = 1
        return (1 - s) / ds;
    }
}

bool RayIntersectsAABB(const Eigen::Vector3d &start, const Eigen::Vector3d &end, const Eigen::Vector3d &lb,
                       const Eigen::Vector3d &rt) {
    Eigen::Vector3d dir = (end - start).normalized();
    Eigen::Vector3d dirfrac(1.0f / dir.x(), 1.0f / dir.y(), 1.0f / dir.z());

    // r.dirs_ is unit dirs_ vector of ray
    // lb is the corner of AABB with minimal coordinates - left bottom, rt is maximal corner
    // start is origin of ray
    double t1 = (lb.x() - start.x()) * dirfrac.x();
    double t2 = (rt.x() - start.x()) * dirfrac.x();
    double t3 = (lb.y() - start.y()) * dirfrac.y();
    double t4 = (rt.y() - start.y()) * dirfrac.y();
    double t5 = (lb.z() - start.z()) * dirfrac.z();
    double t6 = (rt.z() - start.z()) * dirfrac.z();

    double tmin = fmax(fmax(fmin(t1, t2), fmin(t3, t4)), fmin(t5, t6));
    double tmax = fmin(fmin(fmax(t1, t2), fmax(t3, t4)), fmax(t5, t6));

    // if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us
    if (tmax < 0) {
        return false;
    }

    // if tmin > tmax, ray doesn't intersect AABB
    if (tmin > tmax) {
        return false;
    }

    return true;
}

void Raycast(const Eigen::Vector3d &start, const Eigen::Vector3d &end,
             const Eigen::Vector3d &min, const Eigen::Vector3d &max,
             std::vector<Eigen::Vector3d> *output) {
//    std::cout << start << ' ' << end << std::endl;
    // From "A Fast Voxel Traversal Algorithm for Ray Tracing"
    // by John Amanatides and Andrew Woo, 1987
    // <http://www.cse.yorku.ca/~amana/research/grid.pdf>
    // <http://citeseer.ist.psu.edu/viewdoc/summary?doi=10.1.1.42.3443>
    // Extensions to the described algorithm:
    //   • Imposed a distance_ limit.
    //   • The face passed through to reach the current cube is provided to
    //     the callback.

    // The foundation of this algorithm is a parameterized representation of
    // the provided ray,
    //                    origin + t * dirs_,
    // except that t is not actually stored; rather, at any given point_ in the
    // traversal, we keep track of the *greater* t values which we would have
    // if we took a step sufficient to cross a cube boundary along that axis
    // (i.e. change the integer part of the coordinate) in the variables
    // tMaxX, tMaxY, and tMaxZ.

    // Cube containing origin point_.
    int x = (int) std::floor(start.x());
    int y = (int) std::floor(start.y());
    int z = (int) std::floor(start.z());
    int endX = (int) std::floor(end.x());
    int endY = (int) std::floor(end.y());
    int endZ = (int) std::floor(end.z());
    Eigen::Vector3d direction = (end - start);
    double maxDist = direction.squaredNorm();

    // Break out dirs_ vector.
    double dx = endX - x;
    double dy = endY - y;
    double dz = endZ - z;

    // Direction to increment x,y,z when stepping.
    int stepX = (int) signum((int) dx);
    int stepY = (int) signum((int) dy);
    int stepZ = (int) signum((int) dz);

    // See description above. The initial values depend on the fractional
    // part of the origin.
    double tMaxX = intbound(start.x(), dx);
    double tMaxY = intbound(start.y(), dy);
    double tMaxZ = intbound(start.z(), dz);

    // The change in t when taking a step (always positive).
    double tDeltaX = ((double) stepX) / dx;
    double tDeltaY = ((double) stepY) / dy;
    double tDeltaZ = ((double) stepZ) / dz;

    output->clear();

    // Avoids an infinite loop.
    if (stepX == 0 && stepY == 0 && stepZ == 0)
        return;

    double dist = 0;
    while (true) {

        if (x >= min.x() && x < max.x() &&
            y >= min.y() && y < max.y() &&
            z >= min.z() && z < max.z()) {
            output->push_back(Eigen::Vector3d(x, y, z));

            dist = (Eigen::Vector3d(x, y, z) - start).squaredNorm();

            if (dist > maxDist) return;

            if (output->size() > 1500) {
                std::cerr << "Error, too many racyast voxels." << std::endl;
                throw std::out_of_range("Too many RaycasMultithread voxels");
            }
        }

        if (x == endX && y == endY && z == endZ) break;

        // tMaxX stores the t-value at which we cross a cube boundary along the
        // X axis, and similarly for Y and Z. Therefore, choosing the least tMax
        // chooses the closest cube boundary. Only the first case of the four
        // has been commented in detail.
        if (tMaxX < tMaxY) {
            if (tMaxX < tMaxZ) {
                // Update which cube we are now in.
                x += stepX;
                // Adjust tMaxX to the next X-oriented boundary crossing.
                tMaxX += tDeltaX;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        } else {
            if (tMaxY < tMaxZ) {
                y += stepY;
                tMaxY += tDeltaY;
            } else {
                z += stepZ;
                tMaxZ += tDeltaZ;
            }
        }
    }
}


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

#include <nt2/toolbox/trigonometric/functions/sincpi.hpp>
#include <nt2/include/functions/scalar/sinpi.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/is_inf.hpp>
#include <nt2/include/constants/eps.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/invpi.hpp>
#include <boost/simd/sdk/config.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sincpi_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL(1)
    {
      return nt2::sincpi(result_type(a0));
    }
  };
} }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is floating_
/////////////////////////////////////////////////////////////////////////////
namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::sincpi_, tag::cpu_
                            , (A0)
                            , (scalar_< floating_<A0> >)
                            )
  {

    typedef A0 result_type;

    NT2_FUNCTOR_CALL(1)
    {
      #ifndef BOOST_SIMD_NO_INFINITIES
      if(nt2::is_inf(a0)) return nt2::Zero<A0>();
      #endif
      return (nt2::abs(a0) < nt2::Eps<A0>()) ? nt2::One<A0>() : nt2::Invpi<A0>()*nt2::sinpi(a0)/a0;
    }
  };
} }

#endif


#ifndef SPATHPP
#define SPATHPP

// -------------------------------------------------------
//
//   Spatially-regularized Levenberg Marquardt algorithm
//   Coded by J. de la Cruz Rodriguez (ISP-SU, 2020)
//
//   Reference: de la Cruz Rodriguez (2019):
//   https://ui.adsabs.harvard.edu/abs/2019A%26A...631A.153D/abstract
//
//   -------------------------------------------------------

#include <omp.h>
#include <vector>
#include <iostream>
#include <string>
#include <cstdio>
#include <cstring>
#include <chrono>

#include "line.hpp"
#include "Milne.hpp"
#include "lm.hpp"
#include "spatially_regularized_tools.hpp"

#include <Eigen/Sparse>
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

namespace spa{

  // ************************************************************** //

  template<typename T, typename iType = long>
  class lms{
  protected:
    iType npar, ny, nx;

    Eigen::SparseMatrix<T, Eigen::RowMajor, iType> A;

    Eigen::Matrix<T,Eigen::Dynamic, 1> B;

    Eigen::SparseMatrix<T,Eigen::RowMajor, iType> L;
    Eigen::SparseMatrix<T,Eigen::RowMajor, iType> LL;

  public:
    lms(int const inpar, int const iny, int const inx):
      npar(inpar), ny(iny), nx(inx),  A(), B(){};

    // ------------------------------------------------------------ //

    static inline T checkLambda(T val, T const &mi, T const& ma)
    {return std::max<T>(std::min<T>(ma, val), mi);}

    // ------------------------------------------------------------ //

    inline static T get_one_JJ(int const ndat, const T* const __restrict__ Jy, const T* const __restrict__ Jx)
    {
      return static_cast<T>(ksumMult<T,double>(ndat, Jy, Jx));
    }

    // ------------------------------------------------------------ //

    void construct_system(int const npar, container<T> const& cont, T* const __restrict__ m,
			  T* const __restrict__ r,  Eigen::Matrix<T,Eigen::Dynamic,1> const& Reg_RHS)
    {

      iType const npix = cont.ny*cont.nx;
      iType const ndat = cont.nDat;
      iType const nthreads = cont.getNthreads();
      iType const nx = cont.nx;
      iType const ny = cont.ny;
      iType const Jstride = npar*ndat;

      // --- Build Sparse system --- //

      B.resize(npix*npar); B.setZero();
      A.resize(0,0); A.data().squeeze(); A.resize(npix*npar, npix*npar);
      A.reserve(Eigen::VectorXi::Constant(npix*npar,npar));

      T* __restrict__ iJ = NULL;
      iType ipix=0, tid=0, pp=0, ii=0, jj=0;
      T iSum = 0;

      // --- parallel block --- //

#pragma omp parallel default(shared) firstprivate(ipix, tid, iSum, pp, ii, jj, iJ) num_threads(nthreads)
      {

	tid = omp_get_thread_num();
	iJ = new T [npar*ndat](); // Allocate thread buffer for derivatives

#pragma omp for
	for(ipix=0; ipix<npix; ++ipix){

	  // --- synthesize_one pixel with derivatives --- //

	  cont.synthesize_der_one(npar, &m[ipix*npar], &r[ipix*ndat], iJ, tid, ipix);

	  // --- Fill in subspace in sparse Hessian matrix --- //

	  for(jj=0; jj<npar; ++jj){

	    // --- RHS of the equation --- //

	    B[ipix*npar+jj] = ksumMult<T,double>(ndat, &iJ[jj*ndat], &r[ipix*ndat]) - Reg_RHS[ipix*npar+jj];

	    for(ii=0; ii<=jj;++ii){

	      // --- Matrix subspaces --- //

	      iSum = get_one_JJ(ndat, &iJ[jj*ndat], &iJ[ii*ndat]);
	      A.insert(ipix*npar + jj, ipix*npar + ii) = iSum;

	      if(ii != jj) // The matrix is symmetric but avoid inserting the diagonal term twice
	       	A.insert(ipix*npar + ii, ipix*npar + jj) = iSum;

	    } // ii
	  } // jj

	} // ipix

	delete [] iJ;
	iJ = NULL;

      }// parallel

    }

    // ------------------------------------------------------------ //

    Chi2<T> getCorrection(container<T> const& cont, T* const __restrict__ m,
			  T* const __restrict__ syn, T* const __restrict__ r, T iLam, int const method)const
    {

      iType const npix = cont.ny*cont.nx;
      iType const ndat = cont.nDat;

      Eigen::SparseMatrix<T,Eigen::RowMajor,iType> Atot = A+LL;

      // --- damp diagonal and get correction --- //

      iType const nDiag = iType(npar)*iType(npix);
      for(iType kk =0; kk<nDiag; ++kk)
	Atot.coeffRef(kk,kk) *= (1+iLam);

      Eigen::Matrix<T,Eigen::Dynamic,1> dx;

      // --- Solve for corrections --- //

      if(method == 0){
	Eigen::ConjugateGradient<Eigen::SparseMatrix<T,Eigen::RowMajor,iType>, Eigen::Lower| Eigen::Upper> solver(Atot);
	dx = solver.solve(B);
      }else if(method == 1){
	Eigen::BiCGSTAB<Eigen::SparseMatrix<T,Eigen::RowMajor,iType>> solver(Atot);
	dx = solver.solve(B);
      }else if(method == 2){
	Eigen::SparseLU<Eigen::SparseMatrix<T,Eigen::RowMajor,iType>> solver(Atot);
	dx = solver.solve(B);
      }

      // --- Check corrections --- //

      for(iType pp=0; pp<npar; ++pp)
	for(iType ipix = 0; ipix<npix; ++ipix){
	  m[ipix*npar+pp] += dx[ipix*npar+pp];
	  cont.Pinfo[pp].CheckNormalized(m[ipix*npar+pp]);

	}

      // --- compute chi2 --- //

      std::vector<T> rnew(ndat*npix,0);
      cont.fx(npar, m, syn, &rnew[0]);
      Eigen::Matrix<T, Eigen::Dynamic, 1> Gam = cont.getGamma(npar, m);

      Chi2<T> chi2(ksum2<T,double>(npix*ndat, &rnew[0]), ksum2<T,double>(Gam.size(), &Gam[0]));

      return chi2;
    }

    // ------------------------------------------------------------ //

    Chi2<T> getStep(container<T> const& cont, T* const __restrict__ m,
		    T* const __restrict__ syn, T* const __restrict__ r, T& iLam, bool bracket,
		    T const minLam, T const maxLam, T const Lam_step,  Eigen::Matrix<T,Eigen::Dynamic,1> const& Reg_RHS,
		    Chi2<T> const& bestChi2, int const method)
    {

      // --- if no bracketing just compute one correction --- //

      if(!bracket){
	return getCorrection(cont, m, syn, r, iLam, method);
      }else{

	// --- Bracketing optimal lambda value --- //

	int const npix = cont.nx*cont.ny;
	std::vector<Chi2<T>> iChi2;
	std::vector<T> Lambdas;

	int idx = 0;

	Eigen::Map<Eigen::Matrix<T,Eigen::Dynamic,1>> input_model(m, npix*npar);
	Eigen::Matrix<T,Eigen::Dynamic,1> Model = input_model;

	Chi2<T> chi2 = getCorrection(cont, &Model[0], syn, r, iLam, method);
	if(chi2.value() > bestChi2.value()) return chi2;

	Eigen::Matrix<T,Eigen::Dynamic,1> bestModel = Model;

	iChi2.emplace_back(chi2);
	Lambdas.emplace_back(iLam);

	// --- First try to bracket by decreasing lambda --- //
	int iter = 0;
	while((iter < 1) || ((iter < 4) && (iChi2[iter].value()<iChi2[iter-1].value()) && (Lambdas[iter] > minLam))){
	  iLam = checkLambda(iLam / Lam_step, minLam, maxLam);
	  Model = input_model;

	  Lambdas.emplace_back(iLam);
	  iChi2.emplace_back(getCorrection(cont, &Model[0], syn, r, iLam, method));
	  if(iChi2[iter+1].value() < iChi2[idx].value()){
	    idx = iter+1;
	    bestModel = Model;
	  }

	  ++iter;

	}// while

	// --- if the best Chi2 is not in the first element we consider it bracketed --- //

	if(idx == 0){
	  // --- Go in the opposite direction, increasing lambda --- //
	  iter = 0;
	  while((iter == 0) ||( (iter++ <= 5) && (Lambdas[0] < maxLam))){
	    Model = input_model;
	    iLam = checkLambda(iLam * Lam_step*Lam_step, minLam, maxLam);

	    Lambdas.insert(Lambdas.begin(), iLam);
	    iChi2.insert(iChi2.begin(), getCorrection(cont, &Model[0], syn, r, iLam, method) );

	    if(iChi2[0].value() < iChi2[1].value()){
	      bestModel = Model;
	      idx = 0;
	    }else{
	      idx += 1;
	      break;
	    }

	  }// while
	}

	input_model = bestModel;
	iLam = Lambdas[idx];
	return iChi2[idx];
      }

    }

    // ------------------------------------------------------------ //

    T fitData(container<T> const& cont, int const npar, T* __restrict__ bestModel,
	      T* __restrict__ bestSyn, int const max_iter = 20, T iLam = 10,
	      T const Chi2_thres = 1.0, T const fx_thres = 2.e-3, int const delay_braket = 2,
	      bool verbose = true, int const method = 0)
    {
      int const nthreads = int(cont.Me.size());
      Eigen::initParallel();
      Eigen::setNbThreads(nthreads);

      static constexpr T const facLam = 3.1622776601683795;
      static constexpr T const maxLam = 1000.;
      static constexpr T const minLam =  3.1622776601683795e-3;
      static constexpr int const max_n_reject = 6;

      // --- Init temporary variables --- //

      Chi2<T> bestChi2(1.e34,1.e34);
      Chi2<T> chi2 = bestChi2;

      iType const npix = cont.ny*cont.nx;
      iType const ndat = cont.nDat;
      iType const nJ = long(npix)*long(npar)*long(ndat);

      T* const __restrict__ r   = new T [npix*ndat]();
      T* const __restrict__ m   = new T [npix*npar]();

      // --- check pars --- //

      cont.checkPars(npar, bestModel);
      cont.NormalizePars(npar, bestModel);
      std::memcpy(m, bestModel, npix*npar*sizeof(T));

      // --- Init residue "r" --- //

      std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
      cont.fx(npar, m, bestSyn, r);

      // --- precompute L (only needed once) --- //

      if(verbose)
	fprintf(stdout, "lms::fitData: pre-computing regularization derivatives matrix ... ");

      L  = cont.get_L(npar, m);
      LL = L.transpose()*L;
      Eigen::Matrix<T,Eigen::Dynamic,1> Reg_RHS;

      // --- Init total Chi2 --- //
      {
	Eigen::Matrix<T, Eigen::Dynamic, 1> Gam = cont.getGamma(npar, m);
	bestChi2 = Chi2<T>(ksum2<T,double>(npix*ndat, r), ksum2<T,double>(Gam.size(), &Gam[0]));
	Reg_RHS = L.transpose()*Gam; // Init RHS regularization term. Vector that only needs to be computed once per successfull iteration.
      }

      // --- Initialize sparse linear system for the first iteration --- //

      construct_system(npar, cont, m, r, Reg_RHS);

      if(verbose)
	fprintf(stdout,"done\n");

      // --- Init iteration --- //

      if(verbose)
	fprintf(stderr, "\nlms::fitData: [Init] Chi2=%s\n", bestChi2.formatted().c_str());
      int iter = 0, n_rejected = 0;
      bool quit = false, tooSmall = false;
      T oLam = 0, dfx = 0;

      iLam = checkLambda(iLam, minLam, maxLam);

      // --- Iterate the solution --- //

      while(iter < max_iter){

	bool do_bracket =  ((delay_braket > iter)? false : true);

      	oLam = iLam;
	std::memcpy(m, bestModel, npar*npix*sizeof(T));

	// --- Get model correction --- //

	chi2 = getStep(cont, m, bestSyn, r, iLam, do_bracket, minLam, maxLam, facLam, Reg_RHS, bestChi2, method);

	// --- have we improved? --- //

	if(chi2.value() < bestChi2.value()){

	  oLam = iLam;
	  dfx = (bestChi2.value() - chi2.value()) / bestChi2.value();
	  bestChi2 = chi2;

	  std::memcpy(bestModel,   m, npar*npix*sizeof(T));

	  if(!do_bracket)
	    if(iLam*1.00001 > minLam)
	      iLam = checkLambda(iLam/facLam, minLam, maxLam);
	    else
	      iLam *= facLam*facLam;
	  else
	    iLam = facLam*iLam;

	  if(dfx < fx_thres){
	    if(tooSmall) quit = true;
	    else tooSmall = true;
	  }
	  n_rejected = 0;

	}else{

	  iLam = checkLambda(iLam*SQ<T>(facLam), minLam, maxLam);
	  n_rejected += 1;
	  if(verbose)
	    fprintf(stderr,"lms::fitData: ----> Chi2=%s > %s -> Increasing lambda %f -> %f\n",  chi2.formatted().c_str(), bestChi2.formatted().c_str(), oLam, iLam);

	  if(n_rejected<max_n_reject) continue;

	} // else

	std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
	double dt = std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count();
	begin = end;

	// --- Check what has happened with Chi2 --- //

	if(n_rejected >= max_n_reject){
	  if(verbose)
	    fprintf(stderr, "lms::fitData: maximum number of rejected iterations reached, finishing inversion");
	  break;
	}

	if(verbose)
	  fprintf(stderr, "lms::fitData: [%3d] Chi2=%s, lambda=%e, dtime=%6.1fs\n", iter, chi2.formatted().c_str(), oLam, dt/1000.);

	if(bestChi2.value() < Chi2_thres){
	  if(verbose)
	    fprintf(stderr, "lms::fitData: Chi2 (%f) < Chi2_threshold (%f), finishing inversion", bestChi2.value(), Chi2_thres);
	  break;
	}

	if(quit){
	  if(verbose)
	    fprintf(stderr, "lms::fitData: Chi2 improvement too small for 2-iterations, finishing inversion\n");
	  break;
	}

	iter++;
	if(iter >= max_iter){
	  break;
	}

	// --- init next iteration --- //

	std::memcpy(m, bestModel, npix*npar*sizeof(T));

	{
	  Eigen::Matrix<T, Eigen::Dynamic, 1> Gam = cont.getGamma(npar, m);
	  Reg_RHS = L.transpose()*Gam;
	}

	// --- Construct sparse matrix with the new model estimate --- //

	construct_system(npar, cont, m, r,  Reg_RHS);
      }

      // --- Synthesize with best model --- //

      cont.fx(npar, bestModel, bestSyn, r);

      // --- scale model parameters ---- //

      cont.ScalePars(npar, bestModel);

      // --- Clean up --- //

      delete [] r;
      delete [] m;

      return bestChi2.value();
    }

    // ------------------------------------------------------------ //

  };

}

#endif


/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Pawel Dlotko, Vincent Rouvreau
 *
 *    Copyright (C) 2016 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */

#include <gudhi/Rips_complex.h>
#include <gudhi/Simplex_tree.h>
#include <gudhi/Persistent_cohomology.h>
#include <gudhi/reader_utils.h>
#include <gudhi/writing_persistence_to_file.h>

#include <boost/program_options.hpp>

#include <string>
#include <vector>
#include <limits>  // infinity
#include <algorithm>  // for sort

// Types definition
using Simplex_tree = Gudhi::Simplex_tree<Gudhi::Simplex_tree_options_fast_persistence>;
using Filtration_value = Simplex_tree::Filtration_value;
using Rips_complex = Gudhi::rips_complex::Rips_complex<Filtration_value>;
using Field_Zp = Gudhi::persistent_cohomology::Field_Zp;
using Persistent_cohomology = Gudhi::persistent_cohomology::Persistent_cohomology<Simplex_tree, Field_Zp>;
using Correlation_matrix = std::vector<std::vector<Filtration_value>>;
using intervals_common = Gudhi::Persistence_interval_common<double, int>;

void program_options(int argc, char* argv[], std::string& csv_matrix_file, std::string& filediag,
                     Filtration_value& correlation_min, int& dim_max, int& p, Filtration_value& min_persistence);

int main(int argc, char* argv[]) {
  std::string csv_matrix_file;
  std::string filediag;
  Filtration_value correlation_min;
  int dim_max;
  int p;
  Filtration_value min_persistence;

  program_options(argc, argv, csv_matrix_file, filediag, correlation_min, dim_max, p, min_persistence);

  Correlation_matrix correlations =
      Gudhi::read_lower_triangular_matrix_from_csv_file<Filtration_value>(csv_matrix_file);

  Filtration_value threshold = 0;

  // Given a correlation matrix M, we compute component-wise M'[i,j] = 1-M[i,j] to get a distance matrix:
  for (size_t i = 0; i != correlations.size(); ++i) {
    for (size_t j = 0; j != correlations[i].size(); ++j) {
      correlations[i][j] = 1 - correlations[i][j];
      // Here we make sure that the values of corelations lie between -1 and 1.
      // If not, we throw an exception.
      if ((correlations[i][j] < -1) || (correlations[i][j] > 1)) {
        std::cerr << "The input matrix is not a correlation matrix. The program will now terminate. \n";
        throw "The input matrix is not a correlation matrix. The program will now terminate. \n";
      }
      if (correlations[i][j] > threshold) threshold = correlations[i][j];
    }
  }

  Rips_complex rips_complex_from_file(correlations, threshold);

  // Construct the Rips complex in a Simplex Tree
  Simplex_tree simplex_tree;

  rips_complex_from_file.create_complex(simplex_tree, dim_max);
  std::clog << "The complex contains " << simplex_tree.num_simplices() << " simplices \n";
  std::clog << "   and has dimension " << simplex_tree.dimension() << " \n";

  // Compute the persistence diagram of the complex
  Persistent_cohomology pcoh(simplex_tree);
  // initializes the coefficient field for homology
  pcoh.init_coefficients(p);
  // compute persistence
  pcoh.compute_persistent_cohomology(min_persistence);

  // invert the persistence diagram. The reason for this procedure is the following:
  // The input to the program is a corelation matrix M. When processing it, it is
  // turned into 1-M and the obtained persistence intervals are in '1-M' units.
  // Below we reverse every (birth,death) pair into (1-birth, 1-death) pair
  // so that the input and the output to the program is expressed in the same
  // units.
  auto pairs = pcoh.get_persistent_pairs();
  std::vector<intervals_common> processed_persistence_intervals;
  processed_persistence_intervals.reserve(pairs.size());
  for (auto pair : pairs) {
    double birth = 1 - simplex_tree.filtration(get<0>(pair));
    double death = 1 - simplex_tree.filtration(get<1>(pair));
    unsigned dimension = (unsigned)simplex_tree.dimension(get<0>(pair));
    int field = get<2>(pair);
    processed_persistence_intervals.push_back(intervals_common(birth, death, dimension, field));
  }

  // sort the processed intervals:
  std::sort(processed_persistence_intervals.begin(), processed_persistence_intervals.end());

  // and write them to a file
  if (filediag.empty()) {
    write_persistence_intervals_to_stream(processed_persistence_intervals);
  } else {
    std::ofstream out(filediag);
    write_persistence_intervals_to_stream(processed_persistence_intervals, out);
  }
  return 0;
}

void program_options(int argc, char* argv[], std::string& csv_matrix_file, std::string& filediag,
                     Filtration_value& correlation_min, int& dim_max, int& p, Filtration_value& min_persistence) {
  namespace po = boost::program_options;
  po::options_description hidden("Hidden options");
  hidden.add_options()(
      "input-file", po::value<std::string>(&csv_matrix_file),
      "Name of file containing a corelation matrix. Can be square or lower triangular matrix. Separator is ';'.");
  po::options_description visible("Allowed options", 100);
  visible.add_options()("help,h", "produce help message")(
      "output-file,o", po::value<std::string>(&filediag)->default_value(std::string()),
      "Name of file in which the persistence diagram is written. Default print in std::clog")(
      "min-edge-corelation,c", po::value<Filtration_value>(&correlation_min)->default_value(0),
      "Minimal corelation of an edge for the Rips complex construction.")(
      "cpx-dimension,d", po::value<int>(&dim_max)->default_value(1),
      "Maximal dimension of the Rips complex we want to compute.")(
      "field-charac,p", po::value<int>(&p)->default_value(11),
      "Characteristic p of the coefficient field Z/pZ for computing homology.")(
      "min-persistence,m", po::value<Filtration_value>(&min_persistence),
      "Minimal lifetime of homology feature to be recorded. Default is 0. Enter a negative value to see zero length "
      "intervals");

  po::positional_options_description pos;
  pos.add("input-file", 1);

  po::options_description all;
  all.add(visible).add(hidden);

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(all).positional(pos).run(), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("input-file")) {
    std::clog << std::endl;
    std::clog << "Compute the persistent homology with coefficient field Z/pZ \n";
    std::clog << "of a Rips complex defined on a corelation matrix.\n \n";
    std::clog << "The output diagram contains one bar per line, written with the convention: \n";
    std::clog << "   p   dim b d \n";
    std::clog << "where dim is the dimension of the homological feature,\n";
    std::clog << "b and d are respectively the birth and death of the feature and \n";
    std::clog << "p is the characteristic of the field Z/pZ used for homology coefficients." << std::endl << std::endl;

    std::clog << "Usage: " << argv[0] << " [options] input-file" << std::endl << std::endl;
    std::clog << visible << std::endl;
    exit(-1);
  }
}


#include <iostream>
#include <igl/slice.h>
#include <Eigen/Dense>

extern "C"
{
	typedef void callback(int k);

	void fd(int numv, int nume, int numfix, double** vertices, int** edges, double** loads, double* q, int* fixed, int* free);
}

void fd(int numv, int nume, int numfix, double** vertices, int** edges, double** loads, double* q, int* fixed, int* free)
{
	int i;
	int numfree = numv - numfix;

	Eigen::MatrixXd X(numv, 3);
	Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(nume, nume);
	Eigen::MatrixXd C = Eigen::MatrixXd::Zero(nume, numv);

	Eigen::MatrixXd P(numv, 3);

	Eigen::MatrixXd Xi(numfree, 3);
	Eigen::MatrixXd Xf(numfix, 3);
	Eigen::MatrixXd Pi(numfree, 3);
	Eigen::MatrixXd Pf(numfix, 3);

	Eigen::MatrixXd Ci(nume, numfree);
	Eigen::MatrixXd Cit(numfree, nume);
	Eigen::MatrixXd Cf(nume, numfix);

	Eigen::VectorXi fixed_vertices(numfix);
	Eigen::VectorXi free_vertices(numfree);

	Eigen::Vector3i cols(0, 1, 2);
	Eigen::VectorXi rows = Eigen::VectorXi::LinSpaced(nume, 0, nume - 1);

	Eigen::MatrixXd A(numfree, numfree);
	Eigen::MatrixXd b(numfree, 3);

	for (i = 0; i < numfree; i++) {
		free_vertices(i) = free[i];
	}

	for (i = 0; i < numfix; i++) {
		fixed_vertices(i) = fixed[i];
	}

	for (i = 0; i < nume; i++) {
		C(i, edges[i][0]) = -1;
		C(i, edges[i][1]) = +1;
		Q(i, i) = q[i];
	}

	for (i = 0; i < numv; i++) {
		X(i, 0) = vertices[i][0];
		X(i, 1) = vertices[i][1];
		X(i, 2) = vertices[i][2];
		P(i, 0) = loads[i][0];
		P(i, 1) = loads[i][1];
		P(i, 2) = loads[i][2];
	}

	igl::slice(P, free_vertices, cols, Pi);
	igl::slice(X, fixed_vertices, cols, Xf);
	igl::slice(P, fixed_vertices, cols, Pf);
	igl::slice(C, rows, free_vertices, Ci);
	igl::slice(C, rows, fixed_vertices, Cf);

	Cit = Ci.transpose();

	A.noalias() = Cit * Q * Ci;
	b.noalias() = Pi - Cit * Q * Cf * Xf;

	Xi = A.colPivHouseholderQr().solve(b);

	// std::cout << Ci << '\n';
	// std::cout << A << '\n';
	// std::cout << b << '\n';
	// std::cout << Xi << '\n';

	for (i = 0; i < numfree; i++) {
		vertices[free[i]][0] = Xi(i, 0);
		vertices[free[i]][1] = Xi(i, 1);
		vertices[free[i]][2] = Xi(i, 2);
	}

}


#include "../include/m_bp_neural_network.h"
#include <vector>
#include <iostream>
#include <armadillo>
#include <math.h>

using namespace std;
using namespace arma;

m_bp_neural_network::m_bp_neural_network(int input_num, initializer_list<int> net_num, initializer_list<string> net_func)
{
  this->step = step;
  int n_layers = net_num.size();
  this->layers_func = new string[n_layers];
  this->layers = new mat[n_layers];
  this->bases = new mat[n_layers];
  this->hessian_layers = new mat[n_layers];

  this->layers_f_input = new mat[n_layers + 1];
  this->layers_net_input = new mat[n_layers + 1];
  this->n_input = input_num;
  this->n_layers = n_layers;
  mat r(1, input_num);

  this->layers_net_input[0] = r;
  this->layers_f_input[0] = r;

  const int *nets_c = net_num.begin();
  const string *nets_func = net_func.begin();
  this->n_output = *(nets_c + n_layers - 1);

  mat w(input_num, *(nets_c));
  mat b(1, *(nets_c));
  r = mat(1, *(nets_c));
  this->layers[0] = w;
  this->layers_func[0] = *(nets_func);
  this->bases[0] = b;
  this->layers_net_input[1] = r;
  this->layers_f_input[1] = r;
  for (int i = 0; i < n_layers - 2; i++)
  {
    w = mat(*(nets_c + i), *(nets_c + i + 1));
    b = mat(1, *(nets_c + i + 1));
    r = mat(1, *(nets_c + i + 1));
    this->layers[i + 1] = w;
    this->layers_func[i + 1] = *(nets_func + i + 1);
    this->bases[i + 1] = b;
    this->layers_f_input[i + 2] = r;
    this->layers_net_input[i + 2] = r;
  }
  w = mat(*(nets_c + n_layers - 2), this->n_output);
  b = mat(1, this->n_output);
  r = mat(1, this->n_output);
  this->layers[n_layers - 1] = w;
  this->layers_func[n_layers - 1] = *(nets_func + n_layers - 2);
  this->bases[n_layers - 1] = b;
  this->layers_f_input[n_layers] = r;
  this->layers_net_input[n_layers] = r;

  this->delta_layers = new mat[n_layers];
  this->last_delta_layers = new mat[n_layers];
  this->delta_bases = new mat[n_layers];
  for (int i = 0; i < n_layers; i++)
  {
    this->delta_layers[i] = mat(this->layers[i].n_rows, this->layers[i].n_cols);
    this->last_delta_layers[i] = mat(this->layers[i].n_rows, this->layers[i].n_cols);
    this->delta_bases[i] = mat(this->bases[i].n_rows, this->bases[i].n_cols);
  }
}

double sigmoid_tanh(double x)
{
  double v = tanhf(x);
  return v;
}

double dsigmoid_tanh(double x)
{
  double d = sigmoid_tanh(x);
  return 1 - d * d;
}

double sigmoid(double x)
{
  double v = 1 / (1 + exp(-x));
  return v;
}

double linear(double x)
{
  return x;
}

double dlinear(double x)
{
  return 1;
}

double dsigmoid(double x)
{
  double d = sigmoid(x);
  return d * (1 - d);
}

void mapper(mat &mat, double (*func)(double))
{
  for (int i = 0; i < mat.n_rows; i++)
  {
    for (int j = 0; j < mat.n_cols; j++)
    {
      mat(i, j) = func(mat(i, j));
    }
  }
}

void sig_func(string name, mat &input)
{
  double (*func)(double) = NULL;
  if (name == "logsig")
  {
    func = sigmoid;
  }
  else if (name == "tansig")
  {
    func = sigmoid_tanh;
  }
  else if (name == "linear")
  {
    func = linear;
  }
  mapper(input, func);
}

void sig_dfunc(string name, mat &input)
{
  double (*func)(double) = NULL;

  if (name == "logsig")
  {
    func = dsigmoid;
  }
  else if (name == "tansig")
  {
    func = dsigmoid_tanh;
  }
  else if (name == "linear")
  {
    func = dlinear;
  }
  mapper(input, func);
}

double init_rand(double x)
{
  return (x - 0.5) * 2;
}

void m_bp_neural_network::init()
{
  for (int i = 0; i < this->n_layers; i++)
  {
    this->layers[i].randu();
    mapper(this->layers[i], init_rand);
    this->bases[i].randu();
    mapper(this->bases[i], init_rand);
  }
}

void m_bp_neural_network::print()
{
  cout << "\n/////////////////INPUT/////////////////" << endl;
  layers_f_input[0].print();
  cout << "=======================================" << endl;
  for (int i = 0; i < this->n_layers; i++)
  {
    layers[i].print();
    cout << "--------------------------" << endl;
    bases[i].print();
    cout << "--------------------------OUT" << endl;
    layers_net_input[i + 1].print();
    cout << "--------------------------Func OUT" << endl;
    layers_f_input[i + 1].print();
    cout << "=======================================" << endl;
  }
}

mat m_bp_neural_network::sim(mat &input)
{
  mat re = input;
  for (int i = 0; i < this->n_layers; i++)
  {
    mat m1 = re * this->layers[i];
    mat m2 = m1 + this->bases[i];
    sig_func(this->layers_func[i], m2);
    re = m2;
  }
  return re;
}

void m_bp_neural_network::sim(mat &input, int index)
{
  this->layers_f_input[0].row(index) = input;
  this->layers_net_input[0].row(index) = input;
  for (int i = 0; i < this->n_layers; i++)
  {
    mat m1 = this->layers_f_input[i].row(index) * this->layers[i];
    mat m2 = m1 + this->bases[i];
    this->layers_net_input[i + 1].row(index) = m2;
    sig_func(this->layers_func[i], m2);
    this->layers_f_input[i + 1].row(index) = m2;
  }
}

double pow2(double x)
{
  return pow(x, 2);
}
//train_func : traingd,trainlm
void m_bp_neural_network::train(string train_func, vector<mat> &input, vector<mat> &result, int max_epoch, double alpha)
{
  this->stop_train = false;
  int kn = this->n_layers;
  this->step = alpha;
  int sample_num = input.size();
  for (int i = 0; i < kn; i++)
  {
    this->hessian_layers[i].set_size(sample_num, this->layers[i].n_cols * (this->layers[i].n_rows + 1));
  }
  for (int i = 0; i < kn + 1; i++)
  {
    this->layers_f_input[i].set_size(sample_num, this->layers_f_input[i].n_cols);
    this->layers_net_input[i].set_size(sample_num, this->layers_f_input[i].n_cols);
  }
  this->mse.set_size(sample_num, 1);
  this->errors.set_size(sample_num, this->n_output);
  this->forward(input, result);
  this->mse_v = as_scalar(sum(sum(this->mse))) / sample_num;
  for (int i = 0; i < max_epoch; i++)
  {
    cout << i + 1 << " => MSE:" << this->mse_v << endl;
    this->back_propagation(sample_num);
    this->update(train_func, input, result);
    if (this->stop_train)
    {
      cout << i + 1 << " End Training!" << endl;
      break;
    }
  }
}

void m_bp_neural_network::forward(vector<mat> &input, vector<mat> &result)
{
  int kn = this->n_layers;
  int sample_num = input.size();

  for (int j = 0; j < sample_num; j++)
  {
    this->sim(input[j], j);
    mat error = result[j] - this->layers_f_input[kn].row(j);
    mat mse = error;
    mapper(mse, pow2);
    this->mse(j, 0) = as_scalar(sum(sum(mse))) / 2;
    this->errors.row(j) = error;
  }
}

void m_bp_neural_network::back_propagation(int sample_num)
{
  int kn = this->n_layers;
  //this->errors.print("eeeeeeeeeeee");
  for (int i = 0; i < sample_num; i++)
  {
    mat error = this->errors.row(i);
    mat net0 = this->layers_net_input[kn].row(i);
    sig_dfunc(this->layers_func[kn - 1], net0);
    mat S = net0 % error * (-1);
    for (int k = kn - 1; k >= 0; k--)
    {
      mat f_i_cur = this->layers_f_input[k].row(i);
      mat St = S.t();
      mat dW = St * f_i_cur;
      mat dB = St;
      mat dWB = mat(dW.n_rows, dW.n_cols + 1);
      dWB.cols(0, dW.n_cols - 1) = dW;

      dWB.col(dWB.n_cols - 1) = dB;
      dWB.set_size(dWB.n_rows * dWB.n_cols, 1);
      dWB = dWB.t();

      this->hessian_layers[k].row(i) = dWB;
      mat net_cur = this->layers_net_input[(k)].row(i);
      mat W_cur = this->layers[k].t();
      if (k > 0)
      {
        sig_dfunc(this->layers_func[k], net_cur);
        mat Ws = S * W_cur;
        S = Ws % (net_cur);
      }
    }
  }
}

double lnf(double x)
{
  return logf(x) / logf(M_E);
}

void m_bp_neural_network::update(string type, vector<mat> &input, vector<mat> &result)
{

  int kn = this->n_layers;

  int max_step_times = 100;
  int sample_num = input.size();
  if (type == "trainlm")
  {
    double mu_step = 2;
    int cols = 0;
    for (int i = 0; i < kn; i++)
    {
      cols += this->hessian_layers[i].n_cols;
    }
    mat hessian = mat(sample_num, cols);
    int last_cols = 0;
    for (int i = 0; i < kn; i++)
    {
      hessian.cols(last_cols, last_cols + this->hessian_layers[i].n_cols - 1) = this->hessian_layers[i];
      last_cols += this->hessian_layers[i].n_cols;
    }
    mat hessian_trans = hessian.t();
    mat HtH = hessian_trans * hessian;
    mat kI;
    kI.copy_size(HtH);
    kI.eye();
    mat delta = hessian_trans * this->mse;

    double old_step = this->step;
    int k = 0;
    for (k = 0; k < max_step_times; k++)
    {
      mat Hi = inv(HtH + this->step * kI);
      mat delta_k = Hi * delta;
      delta_k = delta_k.t();
      //delta_k.print("MMMMMMM");
      last_cols = 0;
      for (int i = 0; i < kn; i++)
      {

        mat dWB = delta_k.cols(last_cols, last_cols + this->hessian_layers[i].n_cols - 1);
        //dW.print("dW");
        dWB.set_size(this->layers[i].n_cols, this->layers[i].n_rows + 1);
        dWB = dWB.t();
        mat dB = dWB.row(dWB.n_rows - 1);
        mat dW = mat(dWB.n_rows - 1, dWB.n_cols);
        dW = dWB.rows(0, dW.n_rows - 1);
        //dB.print("dB");
        this->delta_layers[i] = dW;
        this->delta_bases[i] = dB;
        //this->layers[i].print();
        //this->bases[i].print();
        this->layers[i] -= dW;
        this->bases[i] -= dB;
        last_cols += this->hessian_layers[i].n_cols;
      }
      forward(input, result);
      double mse_ = as_scalar(sum(sum(this->mse))) / sample_num;

      if (mse_ < this->mse_v)
      {
        this->step /= mu_step;
        cout << this->step << "|" << this->mse_v << " => " << mse_ << endl;
        this->mse_v = mse_;
        break;
      }
      for (int i = kn - 1; i >= 0; i--)
      {
        this->layers[i] += this->delta_layers[i];
        this->bases[i] += this->delta_bases[i];
      }
      this->step *= mu_step;
    }
    if (k >= max_step_times && this->step > old_step)
    {
      this->step = old_step;
      this->stop_train = true;
    }
  }
  else if (type == "traingd")
  {
    for (int i = 0; i < kn; i++)
    {
      this->delta_layers[i].fill(0);
      this->delta_bases[i].fill(0);
      for (int j = 0; j < sample_num; j++)
      {
        mat dWB = this->hessian_layers[i].row(j);
        dWB.set_size(this->layers[i].n_cols, this->layers[i].n_rows + 1);

        dWB = dWB.t();
        mat dB = dWB.row(dWB.n_rows - 1);
        mat dW = mat(dWB.n_rows - 1, dWB.n_cols);
        dW = dWB.rows(0, dW.n_rows - 1);
        this->delta_layers[i] += dW;
        this->delta_bases[i] += dB;
      }
    }
    double old_step = this->step;
    int k = 0;
    double alpha = this->step / sample_num;
    for (int i = 0; i < kn; i++)
    {
      this->layers[i] -= alpha * this->delta_layers[i];
      this->bases[i] -= alpha * this->delta_bases[i];
    }
    forward(input, result);
    double mse_ = as_scalar(sum(sum(this->mse))) / sample_num;
    cout << this->step << "|" << this->mse_v << " => " << mse_ << endl;
    this->mse_v = mse_;
  }
}


#pragma once

// system includes ---------------------------------------------------------
#include <boost/math/special_functions/laguerre.hpp>
#include <functional>

// own includes ------------------------------------------------------------
#include "spectral_function_base.hpp"
#include "spectral_weight_function.hpp"

namespace boltzmann {

namespace local_ {
struct laguerre_id_t
{
 private:
  constexpr const static double FUZZY = 1e6;

 public:
  typedef laguerre_id_t id_t;

  /// Default constructor
  laguerre_id_t()
      : fw(-1)
      , k(-1)
      , idw(-1)
  { }

  laguerre_id_t(double fw_, int k_)
      : fw(fw_)
      , k(k_)
      , idw(FUZZY * fw_)
  {}

  /// weight exponent
  double fw;
  /// Laguerre polynomial index
  int k;

  bool operator<(const laguerre_id_t& other) const
  {
    return std::tie(k, idw) < std::tie(other.k, other.idw);
  }

  bool operator==(const laguerre_id_t& other) const
  {
    return std::tie(k, idw) == std::tie(other.k, other.idw);
  }

  friend std::ostream& operator<<(std::ostream& stream, const laguerre_id_t& x)
  {
    stream << x.to_string();
    return stream;
  }

  std::string to_string() const
  {
    return "(fw_" + boost::lexical_cast<std::string>(fw) + ", k_" +
           boost::lexical_cast<std::string>(k) + ") ";
  }

  std::tuple<int, long int> key() const { return std::make_tuple(k, idw); }

  /// weight id
  long int idw;
};
}  // end namespace local_
}  // end namespace boltzmann

namespace std {
// hash functions for id's
template <>
class hash<boltzmann::local_::laguerre_id_t>
{
 public:
  size_t operator()(const boltzmann::local_::laguerre_id_t& id) const
  {
    std::size_t current = std::hash<double>()(id.fw);
    boost::hash_combine(current, std::hash<int>()(id.k));
    return current;
  }
};
}  // end namespace std

namespace boltzmann {
// --------------------------------------------------------------------------------
class LaguerreRR : public weighted<LaguerreRR, true>,
                   public local_::index_policy<local_::laguerre_id_t>
{
 public:
  typedef double numeric_t;

 public:
  LaguerreRR(double fw_, int k_)
      : id_(fw_, k_)
  {
  }

  explicit LaguerreRR(const id_t& id)
      : id_(id)
  {
  }

  LaguerreRR()
      : id_(-1, -1)
  {
  }

  /// evaluate polynomial part
  double evaluate(double r) const;

  /// evaluate weight
  double weight(double r) const;

  /// return weight
  double w() const { return id_.fw; }

  const id_t& get_id() const { return id_; }

 private:
  double evenk(int k, double r) const;
  double oddk(int k, double r) const;

  id_t id_;
} __attribute((deprecated));

// ----------------------------------------------------------------------
inline double
LaguerreRR::evaluate(double r) const
{
  if (id_.k % 2 == 0)
    return this->evenk(id_.k / 2, r);
  else
    return this->oddk((id_.k - 1) / 2, r);
}

// ----------------------------------------------------------------------
inline double
LaguerreRR::weight(double r) const
{
  return std::exp(-r * r * id_.fw);
}

// ----------------------------------------------------------------------
inline double
LaguerreRR::evenk(int kk, double r) const
{
  return boost::math::laguerre(kk, 0, r * r);
}

// ----------------------------------------------------------------------
inline double
LaguerreRR::oddk(int kk, double r) const
{
  return std::sqrt(1. / (kk + 1.)) * r * boost::math::laguerre(kk, 1, r * r);
}
}  // end namespace boltzmann


#include <cmath>
#include <tuple>
#include <unordered_map>

#include <boost/math/constants/constants.hpp>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Math/Vector.h>

namespace Euclid
{

template<typename Mesh>
void SpinImage<Mesh>::build(const Mesh& mesh,
                            const std::vector<Vector_3>* vnormals,
                            FT resolution)
{
    this->mesh = &mesh;

    if (vnormals != nullptr) {
        this->vnormals.reset(vnormals, false);
    }
    else {
        auto face_normals = Euclid::face_normals(mesh);
        auto vert_normals = Euclid::vertex_normals(mesh, face_normals);
        this->vnormals.reset(new std::vector<Vector_3>(vert_normals), true);
    }

    if (resolution != 0.0) {
        this->resolution = resolution;
    }
    else {
        this->resolution = 0.0;
        for (auto e : edges(mesh)) {
            this->resolution += edge_length(e, mesh);
        }
        this->resolution /= static_cast<FT>(num_edges(mesh));
    }
}

template<typename Mesh>
template<typename Derived>
void SpinImage<Mesh>::compute(Eigen::ArrayBase<Derived>& spin_img,
                              float bin_scale,
                              int image_width,
                              float support_angle)
{
    auto vpmap = get(boost::vertex_point, *this->mesh);
    auto vimap = get(boost::vertex_index, *this->mesh);
    auto cos_range =
        std::cos(support_angle * boost::math::float_constants::degree);
    auto bin_size = this->resolution * static_cast<FT>(bin_scale);
    auto support_distance = bin_size * image_width;
    auto beta_max = support_distance * 0.5;
    spin_img.derived().setZero(image_width * image_width,
                               num_vertices(*this->mesh));

    for (auto vi : vertices(*this->mesh)) {
        auto ii = get(vimap, vi);
        auto pi = get(vpmap, vi);
        auto ni = (*this->vnormals)[ii];

        // Find all vertices that lie in the support and compute the spin image
        for (auto vj : vertices(*this->mesh)) {
            auto ij = get(vimap, vj);
            auto pj = get(vpmap, vj);

            if (ni * (*this->vnormals)[ij] < cos_range) {
                continue;
            }

            auto beta = ni * (pj - pi);
            auto alpha = std::sqrt((pj - pi).squared_length() - beta * beta);

            auto col = static_cast<int>(std::floor(alpha / bin_size));
            if (col > image_width - 2) {
                continue;
            }
            auto row =
                static_cast<int>(std::floor((beta_max - beta) / bin_size));
            if (row > image_width - 2 || row < 0) {
                continue;
            }

            // Bilinear interpolation
            auto a = alpha / bin_size - col;
            auto b = beta_max / bin_size - beta / bin_size - row;
            EASSERT(a <= 1.0 && a >= 0.0);
            EASSERT(b <= 1.0 && b >= 0.0);
            spin_img(row * image_width + col, ii) += (1.0f - a) * (1.0f - b);
            spin_img(row * image_width + col + 1, ii) += a * (1.0f - b);
            spin_img((row + 1) * image_width + col, ii) += (1.0f - a) * b;
            spin_img((row + 1) * image_width + col + 1, ii) += a * b;
        }
    }
}

} // namespace Euclid


#include <boost/multiprecision/cpp_int.hpp>
#include <iostream>

using namespace std;
using boost::multiprecision::cpp_int;

// Function that calculates the pow of two numbers, with a given modulus.
cpp_int mPow(cpp_int base, long long exponent, long long modulus) {
	cpp_int result = 1;
	while(exponent > 0) {
		if(exponent % 2 == 1) {
			result *= base % modulus;
		}
		exponent >>= 1;
		base *= base % modulus;
	}
	return result;
}

// We can try to just use the large number library to handle this trivially.
int main(int argc, char *argv[]) {
	cout << (28433 * mPow(2, 7830457, 10'000'000'000) + 1) % 10'000'000'000 << endl;
	return 0;
}


//  Copyright (c) 2006 Xiaogang Zhang
//  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)
//
//  History:
//  XZ wrote the original of this file as part of the Google
//  Summer of Code 2006.  JM modified it to fit into the
//  Boost.Math conceptual framework better, and to correctly
//  handle the y < 0 case.
//

#ifndef BOOST_MATH_ELLINT_RC_HPP
#define BOOST_MATH_ELLINT_RC_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/math/policies/error_handling.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

// Carlson's degenerate elliptic integral
// R_C(x, y) = R_F(x, y, y) = 0.5 * \int_{0}^{\infty} (t+x)^{-1/2} (t+y)^{-1} dt
// Carlson, Numerische Mathematik, vol 33, 1 (1979)

namespace boost { namespace math { namespace detail{

template <typename T, typename Policy>
T ellint_rc_imp(T x, T y, const Policy& pol)
{
    T value, S, u, lambda, tolerance, prefix;
    unsigned long k;

    BOOST_MATH_STD_USING
    using namespace boost::math::tools;

    static const char* function = "boost::math::ellint_rc<%1%>(%1%,%1%)";

    if(x < 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument x must be non-negative but got %1%", x, pol);
    }
    if(y == 0)
    {
       return policies::raise_domain_error<T>(function,
            "Argument y must not be zero but got %1%", y, pol);
    }

    // error scales as the 6th power of tolerance
    tolerance = pow(4 * tools::epsilon<T>(), T(1) / 6);

    // for y < 0, the integral is singular, return Cauchy principal value
    if (y < 0)
    {
        prefix = sqrt(x / (x - y));
        x = x - y;
        y = -y;
    }
    else
       prefix = 1;

    // duplication:
    k = 1;
    do
    {
        u = (x + y + y) / 3;
        S = y / u - 1;               // 1 - x / u = 2 * S

        if (2 * abs(S) < tolerance)
           break;

        T sx = sqrt(x);
        T sy = sqrt(y);
        lambda = 2 * sx * sy + y;
        x = (x + lambda) / 4;
        y = (y + lambda) / 4;
        ++k;
    }while(k < policies::get_max_series_iterations<Policy>());
    // Check to see if we gave up too soon:
    policies::check_series_iterations(function, k, pol);

    // Taylor series expansion to the 5th order
    value = (1 + S * S * (T(3) / 10 + S * (T(1) / 7 + S * (T(3) / 8 + S * T(9) / 22)))) / sqrt(u);

    return value * prefix;
}

} // namespace detail

template <class T1, class T2, class Policy>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y, const Policy& pol)
{
   typedef typename tools::promote_args<T1, T2>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::ellint_rc_imp(
         static_cast<value_type>(x),
         static_cast<value_type>(y), pol), "boost::math::ellint_rc<%1%>(%1%,%1%)");
}

template <class T1, class T2>
inline typename tools::promote_args<T1, T2>::type
   ellint_rc(T1 x, T2 y)
{
   return ellint_rc(x, y, policies::policy<>());
}

}} // namespaces

#endif // BOOST_MATH_ELLINT_RC_HPP


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2008 Gael Guennebaud <gael.guennebaud@inria.fr>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#include "quaternion_demo.h"
#include "icosphere.h"

#include <Eigen/Geometry>
#include <Eigen/QR>
#include <Eigen/LU>

#include <iostream>
#include <QEvent>
#include <QMouseEvent>
#include <QInputDialog>
#include <QGridLayout>
#include <QButtonGroup>
#include <QRadioButton>
#include <QDockWidget>
#include <QPushButton>
#include <QGroupBox>

using namespace Eigen;

class FancySpheres
{
  public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    FancySpheres()
    {
      const int levels = 4;
      const float scale = 0.33;
      float radius = 100;
      std::vector<int> parents;

      // leval 0
      mCenters.push_back(Vector3f::Zero());
      parents.push_back(-1);
      mRadii.push_back(radius);

      // generate level 1 using icosphere vertices
      radius *= 0.45;
      {
        float dist = mRadii[0]*0.9;
        for (int i=0; i<12; ++i)
        {
          mCenters.push_back(mIcoSphere.vertices()[i] * dist);
          mRadii.push_back(radius);
          parents.push_back(0);
        }
      }

      static const float angles [10] = {
        0, 0,
        M_PI, 0.*M_PI,
        M_PI, 0.5*M_PI,
        M_PI, 1.*M_PI,
        M_PI, 1.5*M_PI
      };

      // generate other levels
      int start = 1;
      for (int l=1; l<levels; l++)
      {
        radius *= scale;
        int end = mCenters.size();
        for (int i=start; i<end; ++i)
        {
          Vector3f c = mCenters[i];
          Vector3f ax0 = (c - mCenters[parents[i]]).normalized();
          Vector3f ax1 = ax0.unitOrthogonal();
          Quaternionf q;
          q.setFromTwoVectors(Vector3f::UnitZ(), ax0);
          Affine3f t = Translation3f(c) * q * Scaling(mRadii[i]+radius);
          for (int j=0; j<5; ++j)
          {
            Vector3f newC = c + ( (AngleAxisf(angles[j*2+1], ax0)
                                * AngleAxisf(angles[j*2+0] * (l==1 ? 0.35 : 0.5), ax1)) * ax0)
                                * (mRadii[i] + radius*0.8);
            mCenters.push_back(newC);
            mRadii.push_back(radius);
            parents.push_back(i);
          }
        }
        start = end;
      }
    }

    void draw()
    {
      int end = mCenters.size();
      glEnable(GL_NORMALIZE);
      for (int i=0; i<end; ++i)
      {
        Affine3f t = Translation3f(mCenters[i]) * Scaling(mRadii[i]);
        gpu.pushMatrix(GL_MODELVIEW);
        gpu.multMatrix(t.matrix(),GL_MODELVIEW);
        mIcoSphere.draw(2);
        gpu.popMatrix(GL_MODELVIEW);
      }
      glDisable(GL_NORMALIZE);
    }
  protected:
    std::vector<Vector3f> mCenters;
    std::vector<float> mRadii;
    IcoSphere mIcoSphere;
};

// generic linear interpolation method
template<typename T> T lerp(float t, const T& a, const T& b)
{
  return a*(1-t) + b*t;
}

// quaternion slerp
template<> Quaternionf lerp(float t, const Quaternionf& a, const Quaternionf& b)
{ return a.slerp(t,b); }

// linear interpolation of a frame using the type OrientationType
// to perform the interpolation of the orientations
template<typename OrientationType>
inline static Frame lerpFrame(float alpha, const Frame& a, const Frame& b)
{
  return Frame(lerp(alpha,a.position,b.position),
               Quaternionf(lerp(alpha,OrientationType(a.orientation),OrientationType(b.orientation))));
}

template<typename _Scalar> class EulerAngles
{
public:
  enum { Dim = 3 };
  typedef _Scalar Scalar;
  typedef Matrix<Scalar,3,3> Matrix3;
  typedef Matrix<Scalar,3,1> Vector3;
  typedef Quaternion<Scalar> QuaternionType;

protected:

  Vector3 m_angles;

public:

  EulerAngles() {}
  inline EulerAngles(Scalar a0, Scalar a1, Scalar a2) : m_angles(a0, a1, a2) {}
  inline EulerAngles(const QuaternionType& q) { *this = q; }

  const Vector3& coeffs() const { return m_angles; }
  Vector3& coeffs() { return m_angles; }

  EulerAngles& operator=(const QuaternionType& q)
  {
    Matrix3 m = q.toRotationMatrix();
    return *this = m;
  }

  EulerAngles& operator=(const Matrix3& m)
  {
    // mat =  cy*cz          -cy*sz           sy
    //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
    //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
    m_angles.coeffRef(1) = std::asin(m.coeff(0,2));
    m_angles.coeffRef(0) = std::atan2(-m.coeff(1,2),m.coeff(2,2));
    m_angles.coeffRef(2) = std::atan2(-m.coeff(0,1),m.coeff(0,0));
    return *this;
  }

  Matrix3 toRotationMatrix(void) const
  {
    Vector3 c = m_angles.array().cos();
    Vector3 s = m_angles.array().sin();
    Matrix3 res;
    res <<  c.y()*c.z(),                    -c.y()*s.z(),                   s.y(),
            c.z()*s.x()*s.y()+c.x()*s.z(),  c.x()*c.z()-s.x()*s.y()*s.z(),  -c.y()*s.x(),
            -c.x()*c.z()*s.y()+s.x()*s.z(), c.z()*s.x()+c.x()*s.y()*s.z(),  c.x()*c.y();
    return res;
  }

  operator QuaternionType() { return QuaternionType(toRotationMatrix()); }
};

// Euler angles slerp
template<> EulerAngles<float> lerp(float t, const EulerAngles<float>& a, const EulerAngles<float>& b)
{
  EulerAngles<float> res;
  res.coeffs() = lerp(t, a.coeffs(), b.coeffs());
  return res;
}

RenderingWidget::RenderingWidget()
{
  mAnimate = false;
  mCurrentTrackingMode = TM_NO_TRACK;
  mNavMode = NavTurnAround;
  mLerpMode = LerpQuaternion;
  mRotationMode = RotationStable;
  mTrackball.setCamera(&mCamera);

  // required to capture key press events
  setFocusPolicy(Qt::ClickFocus);
}

void RenderingWidget::grabFrame(void)
{
    // ask user for a time
    bool ok = false;
    double t = 0;
    if (!m_timeline.empty())
      t = (--m_timeline.end())->first + 1.;
    t = QInputDialog::getDouble(this, "Eigen's RenderingWidget", "time value: ",
      t, 0, 1e3, 1, &ok);
    if (ok)
    {
      Frame aux;
      aux.orientation = mCamera.viewMatrix().linear();
      aux.position = mCamera.viewMatrix().translation();
      m_timeline[t] = aux;
    }
}

void RenderingWidget::drawScene()
{
  static FancySpheres sFancySpheres;
  float length = 50;
  gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitX(), Color(1,0,0,1));
  gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitY(), Color(0,1,0,1));
  gpu.drawVector(Vector3f::Zero(), length*Vector3f::UnitZ(), Color(0,0,1,1));

  // draw the fractal object
  float sqrt3 = std::sqrt(3.);
  glLightfv(GL_LIGHT0, GL_AMBIENT, Vector4f(0.5,0.5,0.5,1).data());
  glLightfv(GL_LIGHT0, GL_DIFFUSE, Vector4f(0.5,1,0.5,1).data());
  glLightfv(GL_LIGHT0, GL_SPECULAR, Vector4f(1,1,1,1).data());
  glLightfv(GL_LIGHT0, GL_POSITION, Vector4f(-sqrt3,-sqrt3,sqrt3,0).data());

  glLightfv(GL_LIGHT1, GL_AMBIENT, Vector4f(0,0,0,1).data());
  glLightfv(GL_LIGHT1, GL_DIFFUSE, Vector4f(1,0.5,0.5,1).data());
  glLightfv(GL_LIGHT1, GL_SPECULAR, Vector4f(1,1,1,1).data());
  glLightfv(GL_LIGHT1, GL_POSITION, Vector4f(-sqrt3,sqrt3,-sqrt3,0).data());

  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT, Vector4f(0.7, 0.7, 0.7, 1).data());
  glMaterialfv(GL_FRONT_AND_BACK, GL_DIFFUSE, Vector4f(0.8, 0.75, 0.6, 1).data());
  glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, Vector4f(1, 1, 1, 1).data());
  glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 64);

  glEnable(GL_LIGHTING);
  glEnable(GL_LIGHT0);
  glEnable(GL_LIGHT1);

  sFancySpheres.draw();
  glVertexPointer(3, GL_FLOAT, 0, mVertices[0].data());
  glNormalPointer(GL_FLOAT, 0, mNormals[0].data());
  glEnableClientState(GL_VERTEX_ARRAY);
  glEnableClientState(GL_NORMAL_ARRAY);
  glDrawArrays(GL_TRIANGLES, 0, mVertices.size());
  glDisableClientState(GL_VERTEX_ARRAY);
  glDisableClientState(GL_NORMAL_ARRAY);

  glDisable(GL_LIGHTING);
}

void RenderingWidget::animate()
{
  m_alpha += double(m_timer.interval()) * 1e-3;

  TimeLine::const_iterator hi = m_timeline.upper_bound(m_alpha);
  TimeLine::const_iterator lo = hi;
  --lo;

  Frame currentFrame;

  if(hi==m_timeline.end())
  {
    // end
    currentFrame = lo->second;
    stopAnimation();
  }
  else if(hi==m_timeline.begin())
  {
    // start
    currentFrame = hi->second;
  }
  else
  {
    float s = (m_alpha - lo->first)/(hi->first - lo->first);
    if (mLerpMode==LerpEulerAngles)
      currentFrame = ::lerpFrame<EulerAngles<float> >(s, lo->second, hi->second);
    else if (mLerpMode==LerpQuaternion)
      currentFrame = ::lerpFrame<Eigen::Quaternionf>(s, lo->second, hi->second);
    else
    {
      std::cerr << "Invalid rotation interpolation mode (abort)\n";
      exit(2);
    }
    currentFrame.orientation.coeffs().normalize();
  }

  currentFrame.orientation = currentFrame.orientation.inverse();
  currentFrame.position = - (currentFrame.orientation * currentFrame.position);
  mCamera.setFrame(currentFrame);

  updateGL();
}

void RenderingWidget::keyPressEvent(QKeyEvent * e)
{
    switch(e->key())
    {
      case Qt::Key_Up:
        mCamera.zoom(2);
        break;
      case Qt::Key_Down:
        mCamera.zoom(-2);
        break;
      // add a frame
      case Qt::Key_G:
        grabFrame();
        break;
      // clear the time line
      case Qt::Key_C:
        m_timeline.clear();
        break;
      // move the camera to initial pos
      case Qt::Key_R:
        resetCamera();
        break;
      // start/stop the animation
      case Qt::Key_A:
        if (mAnimate)
        {
          stopAnimation();
        }
        else
        {
          m_alpha = 0;
          connect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
          m_timer.start(1000/30);
          mAnimate = true;
        }
        break;
      default:
        break;
    }

    updateGL();
}

void RenderingWidget::stopAnimation()
{
  disconnect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
  m_timer.stop();
  mAnimate = false;
  m_alpha = 0;
}

void RenderingWidget::mousePressEvent(QMouseEvent* e)
{
  mMouseCoords = Vector2i(e->pos().x(), e->pos().y());
  bool fly = (mNavMode==NavFly) || (e->modifiers()&Qt::ControlModifier);
  switch(e->button())
  {
    case Qt::LeftButton:
      if(fly)
      {
        mCurrentTrackingMode = TM_LOCAL_ROTATE;
        mTrackball.start(Trackball::Local);
      }
      else
      {
        mCurrentTrackingMode = TM_ROTATE_AROUND;
        mTrackball.start(Trackball::Around);
      }
      mTrackball.track(mMouseCoords);
      break;
    case Qt::MidButton:
      if(fly)
        mCurrentTrackingMode = TM_FLY_Z;
      else
        mCurrentTrackingMode = TM_ZOOM;
      break;
    case Qt::RightButton:
        mCurrentTrackingMode = TM_FLY_PAN;
      break;
    default:
      break;
  }
}
void RenderingWidget::mouseReleaseEvent(QMouseEvent*)
{
    mCurrentTrackingMode = TM_NO_TRACK;
    updateGL();
}

void RenderingWidget::mouseMoveEvent(QMouseEvent* e)
{
    // tracking
    if(mCurrentTrackingMode != TM_NO_TRACK)
    {
        float dx =   float(e->x() - mMouseCoords.x()) / float(mCamera.vpWidth());
        float dy = - float(e->y() - mMouseCoords.y()) / float(mCamera.vpHeight());

        // speedup the transformations
        if(e->modifiers() & Qt::ShiftModifier)
        {
          dx *= 10.;
          dy *= 10.;
        }

        switch(mCurrentTrackingMode)
        {
          case TM_ROTATE_AROUND:
          case TM_LOCAL_ROTATE:
            if (mRotationMode==RotationStable)
            {
              // use the stable trackball implementation mapping
              // the 2D coordinates to 3D points on a sphere.
              mTrackball.track(Vector2i(e->pos().x(), e->pos().y()));
            }
            else
            {
              // standard approach mapping the x and y displacements as rotations
              // around the camera's X and Y axes.
              Quaternionf q = AngleAxisf( dx*M_PI, Vector3f::UnitY())
                            * AngleAxisf(-dy*M_PI, Vector3f::UnitX());
              if (mCurrentTrackingMode==TM_LOCAL_ROTATE)
                mCamera.localRotate(q);
              else
                mCamera.rotateAroundTarget(q);
            }
            break;
          case TM_ZOOM :
            mCamera.zoom(dy*100);
            break;
          case TM_FLY_Z :
            mCamera.localTranslate(Vector3f(0, 0, -dy*200));
            break;
          case TM_FLY_PAN :
            mCamera.localTranslate(Vector3f(dx*200, dy*200, 0));
            break;
          default:
            break;
        }

        updateGL();
    }

    mMouseCoords = Vector2i(e->pos().x(), e->pos().y());
}

void RenderingWidget::paintGL()
{
  glEnable(GL_DEPTH_TEST);
  glDisable(GL_CULL_FACE);
  glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
  glDisable(GL_COLOR_MATERIAL);
  glDisable(GL_BLEND);
  glDisable(GL_ALPHA_TEST);
  glDisable(GL_TEXTURE_1D);
  glDisable(GL_TEXTURE_2D);
  glDisable(GL_TEXTURE_3D);

  // Clear buffers
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

  mCamera.activateGL();

  drawScene();
}

void RenderingWidget::initializeGL()
{
  glClearColor(1., 1., 1., 0.);
  glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, 1);
  glDepthMask(GL_TRUE);
  glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE);

  mCamera.setPosition(Vector3f(-200, -200, -200));
  mCamera.setTarget(Vector3f(0, 0, 0));
  mInitFrame.orientation = mCamera.orientation().inverse();
  mInitFrame.position = mCamera.viewMatrix().translation();
}

void RenderingWidget::resizeGL(int width, int height)
{
    mCamera.setViewport(width,height);
}

void RenderingWidget::setNavMode(int m)
{
  mNavMode = NavMode(m);
}

void RenderingWidget::setLerpMode(int m)
{
  mLerpMode = LerpMode(m);
}

void RenderingWidget::setRotationMode(int m)
{
  mRotationMode = RotationMode(m);
}

void RenderingWidget::resetCamera()
{
  if (mAnimate)
    stopAnimation();
  m_timeline.clear();
  Frame aux0 = mCamera.frame();
  aux0.orientation = aux0.orientation.inverse();
  aux0.position = mCamera.viewMatrix().translation();
  m_timeline[0] = aux0;

  Vector3f currentTarget = mCamera.target();
  mCamera.setTarget(Vector3f::Zero());

  // compute the rotation duration to move the camera to the target
  Frame aux1 = mCamera.frame();
  aux1.orientation = aux1.orientation.inverse();
  aux1.position = mCamera.viewMatrix().translation();
  float duration = aux0.orientation.angularDistance(aux1.orientation) * 0.9;
  if (duration<0.1) duration = 0.1;

  // put the camera at that time step:
  aux1 = aux0.lerp(duration/2,mInitFrame);
  // and make it look at the target again
  aux1.orientation = aux1.orientation.inverse();
  aux1.position = - (aux1.orientation * aux1.position);
  mCamera.setFrame(aux1);
  mCamera.setTarget(Vector3f::Zero());

  // add this camera keyframe
  aux1.orientation = aux1.orientation.inverse();
  aux1.position = mCamera.viewMatrix().translation();
  m_timeline[duration] = aux1;

  m_timeline[2] = mInitFrame;
  m_alpha = 0;
  animate();
  connect(&m_timer, SIGNAL(timeout()), this, SLOT(animate()));
  m_timer.start(1000/30);
  mAnimate = true;
}

QWidget* RenderingWidget::createNavigationControlWidget()
{
  QWidget* panel = new QWidget();
  QVBoxLayout* layout = new QVBoxLayout();

  {
    QPushButton* but = new QPushButton("reset");
    but->setToolTip("move the camera to initial position (with animation)");
    layout->addWidget(but);
    connect(but, SIGNAL(clicked()), this, SLOT(resetCamera()));
  }
  {
    // navigation mode
    QGroupBox* box = new QGroupBox("navigation mode");
    QVBoxLayout* boxLayout = new QVBoxLayout;
    QButtonGroup* group = new QButtonGroup(panel);
    QRadioButton* but;
    but = new QRadioButton("turn around");
    but->setToolTip("look around an object");
    group->addButton(but, NavTurnAround);
    boxLayout->addWidget(but);
    but = new QRadioButton("fly");
    but->setToolTip("free navigation like a spaceship\n(this mode can also be enabled pressing the \"shift\" key)");
    group->addButton(but, NavFly);
    boxLayout->addWidget(but);
    group->button(mNavMode)->setChecked(true);
    connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setNavMode(int)));
    box->setLayout(boxLayout);
    layout->addWidget(box);
  }
  {
    // track ball, rotation mode
    QGroupBox* box = new QGroupBox("rotation mode");
    QVBoxLayout* boxLayout = new QVBoxLayout;
    QButtonGroup* group = new QButtonGroup(panel);
    QRadioButton* but;
    but = new QRadioButton("stable trackball");
    group->addButton(but, RotationStable);
    boxLayout->addWidget(but);
    but->setToolTip("use the stable trackball implementation mapping\nthe 2D coordinates to 3D points on a sphere");
    but = new QRadioButton("standard rotation");
    group->addButton(but, RotationStandard);
    boxLayout->addWidget(but);
    but->setToolTip("standard approach mapping the x and y displacements\nas rotations around the camera's X and Y axes");
    group->button(mRotationMode)->setChecked(true);
    connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setRotationMode(int)));
    box->setLayout(boxLayout);
    layout->addWidget(box);
  }
  {
    // interpolation mode
    QGroupBox* box = new QGroupBox("spherical interpolation");
    QVBoxLayout* boxLayout = new QVBoxLayout;
    QButtonGroup* group = new QButtonGroup(panel);
    QRadioButton* but;
    but = new QRadioButton("quaternion slerp");
    group->addButton(but, LerpQuaternion);
    boxLayout->addWidget(but);
    but->setToolTip("use quaternion spherical interpolation\nto interpolate orientations");
    but = new QRadioButton("euler angles");
    group->addButton(but, LerpEulerAngles);
    boxLayout->addWidget(but);
    but->setToolTip("use Euler angles to interpolate orientations");
    group->button(mNavMode)->setChecked(true);
    connect(group, SIGNAL(buttonClicked(int)), this, SLOT(setLerpMode(int)));
    box->setLayout(boxLayout);
    layout->addWidget(box);
  }
  layout->addItem(new QSpacerItem(0,0,QSizePolicy::Minimum,QSizePolicy::Expanding));
  panel->setLayout(layout);
  return panel;
}

QuaternionDemo::QuaternionDemo()
{
  mRenderingWidget = new RenderingWidget();
  setCentralWidget(mRenderingWidget);

  QDockWidget* panel = new QDockWidget("navigation", this);
  panel->setAllowedAreas((QFlags<Qt::DockWidgetArea>)(Qt::RightDockWidgetArea | Qt::LeftDockWidgetArea));
  addDockWidget(Qt::RightDockWidgetArea, panel);
  panel->setWidget(mRenderingWidget->createNavigationControlWidget());
}

int main(int argc, char *argv[])
{
  std::cout << "Navigation:\n";
  std::cout << "  left button:           rotate around the target\n";
  std::cout << "  middle button:         zoom\n";
  std::cout << "  left button + ctrl     quake rotate (rotate around camera position)\n";
  std::cout << "  middle button + ctrl   walk (progress along camera's z direction)\n";
  std::cout << "  left button:           pan (translate in the XY camera's plane)\n\n";
  std::cout << "R : move the camera to initial position\n";
  std::cout << "A : start/stop animation\n";
  std::cout << "C : clear the animation\n";
  std::cout << "G : add a key frame\n";

  QApplication app(argc, argv);
  QuaternionDemo demo;
  demo.resize(600,500);
  demo.show();
  return app.exec();
}

#include "quaternion_demo.moc"


#pragma once

#include "calotypes/KernelFunctions.hpp"
#include "calotypes/ProbabilityDensity.hpp"

#include <boost/math/distributions/normal.hpp>
#include <memory>
// #include <nanoflann.hpp> // TODO Use

namespace calotypes
{

// TODO Allow adding/removing of data?
/*! \brief Parzen-Rosenblatt type kernel density estimator. */
template <class Data>
class KernelDensityEstimator
: public ProbabilityDensityFunction<Data>
{
public:

	typedef std::shared_ptr<KernelDensityEstimator> Ptr;
	typedef std::vector<Data> Dataset;

	/*! \brief Construct a KDE with specified data, distance function, kernel function,
	 * and bandwidth. */
	KernelDensityEstimator( const Dataset& d, typename KernelFunction<Data>::Ptr k, double h )
	: data( d ), kernel( k )
	{
		bandwidthNormalizer = 1.0 / ( data.size() * h );
		bandwidthReciprocal = 1.0 / h;
	}

	/*! \brief Return an unnormalized PDF estimate. */
	virtual double operator()( const Data& query ) const
	{
		double acc = 0;
		for( unsigned int i = 0; i < data.size(); i++ )
		{
			double x = kernel->Difference( query, data[i] );
			acc += kernel->Evaluate( x * bandwidthReciprocal );
		}
		return acc * bandwidthNormalizer;
	}

	/*! \brief This implementation is not normalized. */
	virtual bool IsNormalized() const { return false; }

	double inline EvaluateKernel( const Data& a, const Data& b ) const { return (*kernel)( a, b ); }

private:

	Dataset data;
	typename KernelFunction<Data>::Ptr kernel;
	double bandwidthNormalizer;
	double bandwidthReciprocal;

};

}


#ifndef VPYTHON_UTIL_VECTOR_HPP
#define VPYTHON_UTIL_VECTOR_HPP

// Copyright (c) 2000, 2001, 2002, 2003 by David Scherer and others.
// Copyright (c) 2004 by Jonathan Brandmeyer and others.
// See the file license.txt for complete license terms.
// See the file authors.txt for a complete list of contributors.

#include "wrap_gl.hpp"
#include <boost/python/numeric.hpp>
#include <iosfwd>
#include <cmath>
#include <cassert>
#include <sstream>

namespace cvisual {

class vector
{
public:
	double x;
	double y;
	double z;

public:
	explicit vector( double a = 0.0, double b = 0.0, double c = 0.0) throw()
		: x(a), y(b), z(c) {}

	inline explicit vector( const double* v)
		: x(v[0]), y(v[1]), z(v[2]) {}

	// Overloaded binary +, -, *, and /
	inline vector
	operator+( const vector& v) const throw()
	{ return vector( x+v.x, y+v.y, z+v.z); }

	inline vector
	operator-( const vector& v) const throw()
	{ return vector( x-v.x, y-v.y, z-v.z); }

	inline vector
	operator*( const double s) const throw()
	{ return vector( s*x, s*y, s*z); }

	// Element-wise multiplication used in frame.cpp; not exposed to users
	inline vector
	operator*( const vector& v) const throw()
	{ return vector( x*v.x, y*v.y, z*v.z); }

	inline vector
	operator/( const double s) const throw()
	{ return vector( x/s, y/s, z/s); }

    // This operator describes a strict weak ordering as defined by the STL.
	bool
	stl_cmp( const vector& v) const;

	inline bool
	operator==( const vector& v) const throw()
	{ return (v.x == this->x && v.y == this->y && v.z == this->z); }

	inline bool
	operator!=( const vector& v) const throw()
	{ return !(v == *this); }

	// Overloaded uniary !, probably bad coding practice.
	inline bool
	operator!( void) const throw()
	{ return !x && !y && !z; }

	bool nonzero() const throw() { return x || y || z; }

    // Overloaded assignment: +=, -=, *=, /=
	inline const vector&
	operator+=( const vector& v) throw()
	{ x=x+v.x; y=y+v.y; z=z+v.z; return *this; }

	inline const vector&
	operator-=( const vector& v) throw()
	{ x=x-v.x; y=y-v.y; z=z-v.z; return *this; }

	inline const vector&
	operator*=( const double s) throw()
	{ x=x*s; y=y*s; z=z*s; return *this; }

	inline const vector&
	operator/=( const double s) throw()
	{ x=x/s; y=y/s; z=z/s; return *this; }

 	inline vector
	operator-() const throw()
	{ return vector( -x, -y, -z); }

	// return the magnitude of this vector
	inline double
	mag( void) const throw()
	{ return std::sqrt( x*x + y*y + z*z); }

	// This is a magnitude algorithm that is intended to be stable at values
	// greater than 1e154 (or so).  It is much slower since it uses sin, cos,
	// and atan to get the result.
	double
	stable_mag(void) const;

	// return the square of the this vector's magnitude
	inline double
	mag2( void) const throw()
	{ return (x*x + y*y + z*z); }

	// return the unit vector of this vector
	vector
	norm( void) const throw();

	inline void
	set_mag( double m) throw()
	{ *this = norm()*m; }

	inline void
	set_mag2( double m2) throw()
	{ *this = norm()*std::sqrt(m2); }
	// Pythonic function to provide a "representation" of this object.
	// object.__repr__() should return a string that, were it executed as python
	// code, should regenerate the object.
	std::string
	repr() const;

	// return the dot product of this vector and another
	inline double
	dot( const vector& v) const throw()
	{ return ( v.x * this->x + v.y * this->y + v.z * this->z); }

	// Return the cross product of this vector and another.
	vector
	cross( const vector& v) const throw();

	// Return the scalar triple product
	double
	dot_b_cross_c( const vector& b, const vector& c) const throw();

	// Return the vector triple product
	vector
	cross_b_cross_c( const vector& b, const vector& c) const throw();

	// Scalar projection of this to v
	double
	comp( const vector& v) const throw();

	// Vector projection of this to v
	vector
	proj( const vector& v) const throw();

	// Returns the angular difference between two vectors, in radians, between 0 and pi.
	double
	diff_angle( const vector& v) const throw();

	// Scale this vector to another, by elementwise multiplication
	inline vector
	scale( const vector& v) const throw()
	{ return vector( this->x*v.x, this->y*v.y, this->z*v.z); }

    // Inversely scale this vector to another, by elementwise division
    inline vector
    scale_inv( const vector& v) const throw()
    { return vector( x/v.x, y/v.y, z/v.z); }

	vector
	rotate( double angle, vector axis = vector(0,0,1)) throw();

	// Last ditch direct read/write access to the private variables
	inline double
	get_x( void) const throw() { return x; }

	inline void
	set_x( double s) throw() { this->x = s; }

	inline double
	get_y( void) const throw() { return y; }

	inline void
	set_y( double s) throw() { this->y = s; }

	inline double
	get_z( void) const throw() { return z; }

	inline void
	set_z( double s) throw() { this->z = s; }

	// zero the state of the vector. Potentially useful for reusing a temporary.
	inline void
	clear( void) { x=0.0; y=0.0; z=0.0; }

    inline int
	py_len() { return 3; }

	double py_getitem( int i) const;

	void py_setitem(int i, double value);

	inline double&
	operator[]( size_t ref)
	{
		assert( ref < 3);
		switch (ref) {
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
			default:
				assert( true == false);
		}
	}

	inline const double&
	operator[]( size_t ref) const
	{
		assert( ref < 3);
		switch (ref) {
			case 0:
				return x;
			case 1:
				return y;
			case 2:
				return z;
		}
	}

	inline vector
	fabs() const
	{ return vector( std::fabs(x), std::fabs(y), std::fabs(z)); }

	inline void
	gl_render() const
	{ glVertex3dv( &x); }

	inline void
	gl_normal() const
	{ glNormal3dv( &x); }

	inline double
	sum() const
	{ return x + y + z; }
};

// Free functions for mag, mag2, dot, unit, cross, and tripleproducts.
// All of these functions merely call their class-member variants to save code.
inline double
mag( const vector& v)
{ return v.mag(); }

inline double
mag2( const vector& v)
{ return v.mag2(); }

inline vector
norm( const vector& v)
{ return v.norm(); }

inline double
dot( const vector& v1, const vector& v2)
{ return v1.dot( v2); }

inline vector
cross( const vector& v1, const vector& v2)
{ return v1.cross( v2); }

inline double
a_dot_b_cross_c( const vector& a, const vector& b, const vector& c)
{  return a.dot_b_cross_c( b, c);  }

inline vector
a_cross_b_cross_c( const vector& a, const vector& b, const vector& c)
{ return a.cross_b_cross_c( b, c); }

// Scalar projection of v1 -> v2
inline double
comp( const vector& v1, const vector& v2)
{ return v1.comp( v2); }

// Vector projection of v1 to v2
inline vector
proj( const vector& v1, const vector& v2)
{ return v1.proj( v2); }

// Returns the angular difference between two vectors, in radians, from 0 - pi.
inline double
diff_angle( const vector& v1, const vector& v2)
{ return v1.diff_angle( v2); }

inline vector
rotate( vector v, double angle, const vector axis = vector( 0,0,1))
{ return v.rotate( angle, axis); }

// Definitions of the global functions for operator *, with a vector on the RHS,
// and scalar on the LHS.

inline vector
operator*( const double& s, const vector& v)
{
  return vector( s*v.x, s*v.y, s*v.z);
}
} // !namespace cvisual

// We should not need to place this in namespace std, but GCC's Koenig L/U fails
//   if we don't.
namespace std {
// Insertion operator.  Example output: <xxxx, yyyy, zzzz>
// Based on "The C++ Standard Library", N. M. Josuttis, section 13.12.1
template<typename char_T, typename traits>
basic_ostream<char_T, traits>&
operator<<( basic_ostream<char_T, traits>& stream, const cvisual::vector& v)
{
	basic_ostringstream<char_T, traits> s;
	s.copyfmt( stream);
	s.width( 0);

	s << "<" << v.x << ", " << v.y << ", " << v.z << ">";
	stream << s.str();

	return stream;
}

} // !namespace std

namespace cvisual {

typedef vector shared_vector;

} // !namespace cvisual

#endif // !VPYTHON_UTIL_VECTOR_HPP


#pragma once
#include <Eigen/Core>

//! The gradient of the shape function (on the reference element)
//!
//! We have three shape functions
//!
//! @param i integer between 0 and 2 (inclusive). Decides which shape function to return.
//! @param x x coordinate in the reference element.
//! @param y y coordinate in the reference element.
inline Eigen::Vector2d gradientLambda(const int i, double x, double y) {
  //// ANCSE_START_TEMPLATE
    return Eigen::Vector2d(-1 + (i > 0) + (i==1),
                           -1 + (i > 0) + (i==2));
    //// ANCSE_END_TEMPLATE
    return Eigen::Vector2d(0,0); //remove when implemented
}


//  (C) Copyright John Maddock 2006.
//  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)

#ifndef BOOST_MATH_TOOLS_POLYNOMIAL_HPP
#define BOOST_MATH_TOOLS_POLYNOMIAL_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/assert.hpp>
#include <boost/math/tools/rational.hpp>
#include <boost/math/tools/real_cast.hpp>
#include <boost/math/special_functions/binomial.hpp>

#include <vector>
#include <ostream>
#include <algorithm>

namespace boost{ namespace math{ namespace tools{

template <class T>
T chebyshev_coefficient(unsigned n, unsigned m)
{
   BOOST_MATH_STD_USING
   if(m > n)
      return 0;
   if((n & 1) != (m & 1))
      return 0;
   if(n == 0)
      return 1;
   T result = T(n) / 2;
   unsigned r = n - m;
   r /= 2;

   BOOST_ASSERT(n - 2 * r == m);

   if(r & 1)
      result = -result;
   result /= n - r;
   result *= boost::math::binomial_coefficient<T>(n - r, r);
   result *= ldexp(1.0f, m);
   return result;
}

template <class Seq>
Seq polynomial_to_chebyshev(const Seq& s)
{
   // Converts a Polynomial into Chebyshev form:
   typedef typename Seq::value_type value_type;
   typedef typename Seq::difference_type difference_type;
   Seq result(s);
   difference_type order = s.size() - 1;
   difference_type even_order = order & 1 ? order - 1 : order;
   difference_type odd_order = order & 1 ? order : order - 1;

   for(difference_type i = even_order; i >= 0; i -= 2)
   {
      value_type val = s[i];
      for(difference_type k = even_order; k > i; k -= 2)
      {
         val -= result[k] * chebyshev_coefficient<value_type>(static_cast<unsigned>(k), static_cast<unsigned>(i));
      }
      val /= chebyshev_coefficient<value_type>(static_cast<unsigned>(i), static_cast<unsigned>(i));
      result[i] = val;
   }
   result[0] *= 2;

   for(difference_type i = odd_order; i >= 0; i -= 2)
   {
      value_type val = s[i];
      for(difference_type k = odd_order; k > i; k -= 2)
      {
         val -= result[k] * chebyshev_coefficient<value_type>(static_cast<unsigned>(k), static_cast<unsigned>(i));
      }
      val /= chebyshev_coefficient<value_type>(static_cast<unsigned>(i), static_cast<unsigned>(i));
      result[i] = val;
   }
   return result;
}

template <class Seq, class T>
T evaluate_chebyshev(const Seq& a, const T& x)
{
   // Clenshaw's formula:
   typedef typename Seq::difference_type difference_type;
   T yk2 = 0;
   T yk1 = 0;
   T yk = 0;
   for(difference_type i = a.size() - 1; i >= 1; --i)
   {
      yk2 = yk1;
      yk1 = yk;
      yk = 2 * x * yk1 - yk2 + a[i];
   }
   return a[0] / 2 + yk * x - yk1;
}

template <class T>
class polynomial
{
public:
   // typedefs:
   typedef typename std::vector<T>::value_type value_type;
   typedef typename std::vector<T>::size_type size_type;

   // construct:
   polynomial(){}
   template <class U>
   polynomial(const U* data, unsigned order)
      : m_data(data, data + order + 1)
   {
   }
   template <class U>
   polynomial(const U& point)
   {
      m_data.push_back(point);
   }

   // copy:
   polynomial(const polynomial& p)
      : m_data(p.m_data) { }

   template <class U>
   polynomial(const polynomial<U>& p)
   {
      for(unsigned i = 0; i < p.size(); ++i)
      {
         m_data.push_back(boost::math::tools::real_cast<T>(p[i]));
      }
   }

   // access:
   size_type size()const { return m_data.size(); }
   size_type degree()const { return m_data.size() - 1; }
   value_type& operator[](size_type i)
   {
      return m_data[i];
   }
   const value_type& operator[](size_type i)const
   {
      return m_data[i];
   }
   T evaluate(T z)const
   {
      return boost::math::tools::evaluate_polynomial(&m_data[0], z, m_data.size());;
   }
   std::vector<T> chebyshev()const
   {
      return polynomial_to_chebyshev(m_data);
   }

   // operators:
   template <class U>
   polynomial& operator +=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(value);
      else
      {
         m_data[0] += value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator -=(const U& value)
   {
      if(m_data.size() == 0)
         m_data.push_back(-value);
      else
      {
         m_data[0] -= value;
      }
      return *this;
   }
   template <class U>
   polynomial& operator *=(const U& value)
   {
      for(size_type i = 0; i < m_data.size(); ++i)
         m_data[i] *= value;
      return *this;
   }
   template <class U>
   polynomial& operator +=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] += value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator -=(const polynomial<U>& value)
   {
      size_type s1 = (std::min)(m_data.size(), value.size());
      for(size_type i = 0; i < s1; ++i)
         m_data[i] -= value[i];
      for(size_type i = s1; i < value.size(); ++i)
         m_data.push_back(-value[i]);
      return *this;
   }
   template <class U>
   polynomial& operator *=(const polynomial<U>& value)
   {
      // TODO: FIXME: use O(N log(N)) algorithm!!!
      BOOST_ASSERT(value.size());
      polynomial base(*this);
      *this *= value[0];
      for(size_type i = 1; i < value.size(); ++i)
      {
         polynomial t(base);
         t *= value[i];
         size_type s = size() - i;
         for(size_type j = 0; j < s; ++j)
         {
            m_data[i+j] += t[j];
         }
         for(size_type j = s; j < t.size(); ++j)
            m_data.push_back(t[j]);
      }
      return *this;
   }

private:
   std::vector<T> m_data;
};

template <class T>
inline polynomial<T> operator + (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result += b;
   return result;
}

template <class T>
inline polynomial<T> operator - (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class T>
inline polynomial<T> operator * (const polynomial<T>& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result *= b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator + (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result += b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator - (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class T, class U>
inline polynomial<T> operator * (const polynomial<T>& a, const U& b)
{
   polynomial<T> result(a);
   result *= b;
   return result;
}

template <class U, class T>
inline polynomial<T> operator + (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(b);
   result += a;
   return result;
}

template <class U, class T>
inline polynomial<T> operator - (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(a);
   result -= b;
   return result;
}

template <class U, class T>
inline polynomial<T> operator * (const U& a, const polynomial<T>& b)
{
   polynomial<T> result(b);
   result *= a;
   return result;
}

template <class charT, class traits, class T>
inline std::basic_ostream<charT, traits>& operator << (std::basic_ostream<charT, traits>& os, const polynomial<T>& poly)
{
   os << "{ ";
   for(unsigned i = 0; i < poly.size(); ++i)
   {
      if(i) os << ", ";
      os << poly[i];
   }
   os << " }";
   return os;
}

} // namespace tools
} // namespace math
} // namespace boost

#endif // BOOST_MATH_TOOLS_POLYNOMIAL_HPP


#include <Eigen/Core>
#include <Eigen/Sparse>
#include <iostream>
#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <random>
#include <unordered_set>
#include <vector>

template <typename Real> struct SGDMF {
  using SparseMatrix = Eigen::SparseMatrix<Real, Eigen::RowMajor>;
  using DenseMatrix =
      Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
  using DenseVector = Eigen::Matrix<Real, Eigen::Dynamic, 1>;

  using Index = typename DenseMatrix::StorageIndex;
  using Sample = std::tuple<Index, Index, int64_t>;

  inline SGDMF(const SparseMatrix &X, int dim, int random_seed, Real lr,
               Real lambda, Real std, size_t n_negative)
      : X_(X), dim(dim), rng(random_seed), lr(lr), lambda(lambda),
        n_negative(n_negative) {
    X_.makeCompressed();
    P.resize(X_.rows(), dim);
    Q.resize(X_.cols(), dim);
    P_cache.resize(dim);
    Q_cache.resize(dim);
    P_b = DenseVector::Zero(X_.rows());
    Q_b = DenseVector::Zero(X_.cols());
    auto fill_normal = [this, std](DenseMatrix &U) {
      int rows = U.rows();
      int cols = U.cols();
      std::normal_distribution<Real> dist(0, std);
      for (int i = 0; i < rows; i++) {
        for (int j = 0; j < cols; j++) {
          U(i, j) = dist(this->rng);
        }
      }
    };
    fill_normal(P);
    fill_normal(Q);

    size_t dsize = X_.nonZeros() + X_.rows() * n_negative;
    dataset.resize(dsize);
  }

  inline void start_epoch() {
    size_t cursor = 0;
    std::uniform_int_distribution<> dist(0, X_.cols() - 1);
    for (int u = 0; u < X_.rows(); u++) {
      for (typename SparseMatrix::InnerIterator iter(X_, u); iter; ++iter) {
        int j = iter.col();
        Sample q(u, j, 1);
        dataset[cursor++] = std::move(q);
      }
      for (size_t m_ = 0; m_ < n_negative; m_++) {
        dataset[cursor++] = {u, dist(rng), 0};
      }
    }
    if (static_cast<size_t>(X_.nonZeros() + X_.rows() * n_negative) != cursor) {
      throw std::runtime_error("somethong nasty");
    }
    std::shuffle(dataset.begin(), dataset.end(), rng);
  }

  inline Real run_epoch() {
    start_epoch();
    Real mean_loss = 0;
    for (auto &s : dataset) {
      mean_loss += sgd(s);
    }
    return mean_loss / dataset.size();
  }

  inline Real sgd(const Sample &s) {
    const Index &u = std::get<0>(s);
    const Index &i = std::get<1>(s);
    const int64_t &y = std::get<2>(s);
    P_cache.noalias() = P.row(u).transpose();
    Q_cache.noalias() = Q.row(i).transpose();
    Real score = (P_cache.transpose() * Q_cache) + bias + P_b(u) + Q_b(i);
    Real sigma_score;
    Real loss;
    if (score > 0) {
      sigma_score = 1 / (1 + std::exp(-score));
      loss = -std::log(sigma_score) + (1 - y) * score;
    } else {
      Real exp_score = std::exp(score);
      sigma_score = exp_score / (1 + exp_score);
      loss = -y * score + std::log(1 + exp_score);
    }

    Real grad = (y - sigma_score);

    P.row(u).noalias() += lr * (grad * Q_cache - lambda * P_cache).transpose();
    Q.row(i).noalias() += lr * (grad * P_cache - lambda * Q_cache).transpose();
    P_b(u) += lr * (grad - lambda * P_b(u));
    Q_b(i) += lr * (grad - lambda * Q_b(i));
    bias += lr * (grad - lambda * bias);
    return loss;
  }

  SparseMatrix X_;

  Real bias;
  DenseMatrix P, Q;
  DenseVector P_b, Q_b;
  DenseVector P_cache, Q_cache;
  int dim;

  std::vector<Sample> dataset;

private:
  std::mt19937 rng;

  Real lr, lambda;
  size_t n_negative;
};

namespace py = pybind11;
using std::vector;

PYBIND11_MODULE(_sgd_mf, m) {
  using Real = double;
  using MF = SGDMF<Real>;
  py::class_<MF>(m, "_MF")
      .def(py::init<const typename MF::SparseMatrix &, int, int, Real, Real,
                    Real, size_t>())
      .def("step", &MF::run_epoch)
      .def_readonly("dataset", &MF::dataset)
      .def_readwrite("P", &MF::P)
      .def_readwrite("Q", &MF::Q)
      .def_readwrite("P_b", &MF::P_b)
      .def_readwrite("Q_b", &MF::Q_b)
      .def_readwrite("bias", &MF::bias);
}


/*

MIT License

Copyright (c) 2020, R. Gregor Weiß, Benjamin Ries

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 of the 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
*/

#include <boost/math/constants/constants.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/beta.hpp>

#include "geometry.h"

namespace Geometry {

    double regularized_intersection_volume(const double dist,
                                           const double cut,
                                           const unsigned int n) {
        double nd(static_cast<double>(n));
        double nhalfplusonehalf(0.5 * (nd + 1.0));
        double sin2phi(-1.0 * 0.25 * dist * dist);
        sin2phi /= (cut * cut);
        sin2phi += 1.0;
        double incomplete_beta(boost::math::ibeta<double, double, double>(nhalfplusonehalf, 0.5, sin2phi));

        return incomplete_beta;
    }

}