// boost\math\distributions\poisson.hpp

// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007.

// 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)

// Poisson distribution is a discrete probability distribution.
// It expresses the probability of a number (k) of
// events, occurrences, failures or arrivals occurring in a fixed time,
// assuming these events occur with a known average or mean rate (lambda)
// and are independent of the time since the last event.
// The distribution was discovered by Simeon-Denis Poisson (1781-1840).

// Parameter lambda is the mean number of events in the given time interval.
// The random variate k is the number of events, occurrences or arrivals.
// k argument may be integral, signed, or unsigned, or floating point.
// If necessary, it has already been promoted from an integral type.

// Note that the Poisson distribution
// (like others including the binomial, negative binomial & Bernoulli)
// is strictly defined as a discrete function:
// only integral values of k are envisaged.
// However because the method of calculation uses a continuous gamma function,
// it is convenient to treat it as if a continous function,
// and permit non-integral values of k.
// To enforce the strict mathematical model, users should use floor or ceil functions
// on k outside this function to ensure that k is integral.

// See http://en.wikipedia.org/wiki/Poisson_distribution
// http://documents.wolfram.com/v5/Add-onsLinks/StandardPackages/Statistics/DiscreteDistributions.html

#ifndef BOOST_MATH_SPECIAL_POISSON_HPP
#define BOOST_MATH_SPECIAL_POISSON_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp> // for incomplete gamma. gamma_q
#include <boost/math/special_functions/trunc.hpp> // for incomplete gamma. gamma_q
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/special_functions/factorials.hpp> // factorials.
#include <boost/math/tools/roots.hpp> // for root finding.
#include <boost/math/distributions/detail/inv_discrete_quantile.hpp>

#include <utility>

namespace boost
{
  namespace math
  {
     namespace detail{
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_nearest>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_up>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_down>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_outwards>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::integer_round_inwards>&,
            boost::uintmax_t& max_iter);
      template <class Dist>
      inline typename Dist::value_type
         inverse_discrete_quantile(
            const Dist& dist,
            const typename Dist::value_type& p,
            const typename Dist::value_type& guess,
            const typename Dist::value_type& multiplier,
            const typename Dist::value_type& adder,
            const policies::discrete_quantile<policies::real>&,
            boost::uintmax_t& max_iter);
     }
    namespace poisson_detail
    {
      // Common error checking routines for Poisson distribution functions.
      // These are convoluted, & apparently redundant, to try to ensure that
      // checks are always performed, even if exceptions are not enabled.

      template <class RealType, class Policy>
      inline bool check_mean(const char* function, const RealType& mean, RealType* result, const Policy& pol)
      {
        if(!(boost::math::isfinite)(mean) || (mean < 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Mean argument is %1%, but must be >= 0 !", mean, pol);
          return false;
        }
        return true;
      } // bool check_mean

      template <class RealType, class Policy>
      inline bool check_mean_NZ(const char* function, const RealType& mean, RealType* result, const Policy& pol)
      { // mean == 0 is considered an error.
        if( !(boost::math::isfinite)(mean) || (mean <= 0))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Mean argument is %1%, but must be > 0 !", mean, pol);
          return false;
        }
        return true;
      } // bool check_mean_NZ

      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& mean, RealType* result, const Policy& pol)
      { // Only one check, so this is redundant really but should be optimized away.
        return check_mean_NZ(function, mean, result, pol);
      } // bool check_dist

      template <class RealType, class Policy>
      inline bool check_k(const char* function, const RealType& k, RealType* result, const Policy& pol)
      {
        if((k < 0) || !(boost::math::isfinite)(k))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Number of events k argument is %1%, but must be >= 0 !", k, pol);
          return false;
        }
        return true;
      } // bool check_k

      template <class RealType, class Policy>
      inline bool check_dist_and_k(const char* function, RealType mean, RealType k, RealType* result, const Policy& pol)
      {
        if((check_dist(function, mean, result, pol) == false) ||
          (check_k(function, k, result, pol) == false))
        {
          return false;
        }
        return true;
      } // bool check_dist_and_k

      template <class RealType, class Policy>
      inline bool check_prob(const char* function, const RealType& p, RealType* result, const Policy& pol)
      { // Check 0 <= p <= 1
        if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Probability argument is %1%, but must be >= 0 and <= 1 !", p, pol);
          return false;
        }
        return true;
      } // bool check_prob

      template <class RealType, class Policy>
      inline bool check_dist_and_prob(const char* function, RealType mean,  RealType p, RealType* result, const Policy& pol)
      {
        if((check_dist(function, mean, result, pol) == false) ||
          (check_prob(function, p, result, pol) == false))
        {
          return false;
        }
        return true;
      } // bool check_dist_and_prob

    } // namespace poisson_detail

    template <class RealType = double, class Policy = policies::policy<> >
    class poisson_distribution
    {
    public:
      typedef RealType value_type;
      typedef Policy policy_type;

      poisson_distribution(RealType mean = 1) : m_l(mean) // mean (lambda).
      { // Expected mean number of events that occur during the given interval.
        RealType r;
        poisson_detail::check_dist(
           "boost::math::poisson_distribution<%1%>::poisson_distribution",
          m_l,
          &r, Policy());
      } // poisson_distribution constructor.

      RealType mean() const
      { // Private data getter function.
        return m_l;
      }
    private:
      // Data member, initialized by constructor.
      RealType m_l; // mean number of occurrences.
    }; // template <class RealType, class Policy> class poisson_distribution

    typedef poisson_distribution<double> poisson; // Reserved name of type double.

    // Non-member functions to give properties of the distribution.

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> range(const poisson_distribution<RealType, Policy>& /* dist */)
    { // Range of permissible values for random variable k.
       using boost::math::tools::max_value;
       return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>()); // Max integer?
    }

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> support(const poisson_distribution<RealType, Policy>& /* dist */)
    { // Range of supported values for random variable k.
       // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
       using boost::math::tools::max_value;
       return std::pair<RealType, RealType>(static_cast<RealType>(0),  max_value<RealType>());
    }

    template <class RealType, class Policy>
    inline RealType mean(const poisson_distribution<RealType, Policy>& dist)
    { // Mean of poisson distribution = lambda.
      return dist.mean();
    } // mean

    template <class RealType, class Policy>
    inline RealType mode(const poisson_distribution<RealType, Policy>& dist)
    { // mode.
      BOOST_MATH_STD_USING // ADL of std functions.
      return floor(dist.mean());
    }

    //template <class RealType, class Policy>
    //inline RealType median(const poisson_distribution<RealType, Policy>& dist)
    //{ // median = approximately lambda + 1/3 - 0.2/lambda
    //  RealType l = dist.mean();
    //  return dist.mean() + static_cast<RealType>(0.3333333333333333333333333333333333333333333333)
    //   - static_cast<RealType>(0.2) / l;
    //} // BUT this formula appears to be out-by-one compared to quantile(half)
    // Query posted on Wikipedia.
    // Now implemented via quantile(half) in derived accessors.

    template <class RealType, class Policy>
    inline RealType variance(const poisson_distribution<RealType, Policy>& dist)
    { // variance.
      return dist.mean();
    }

    // RealType standard_deviation(const poisson_distribution<RealType, Policy>& dist)
    // standard_deviation provided by derived accessors.

    template <class RealType, class Policy>
    inline RealType skewness(const poisson_distribution<RealType, Policy>& dist)
    { // skewness = sqrt(l).
      BOOST_MATH_STD_USING // ADL of std functions.
      return 1 / sqrt(dist.mean());
    }

    template <class RealType, class Policy>
    inline RealType kurtosis_excess(const poisson_distribution<RealType, Policy>& dist)
    { // skewness = sqrt(l).
      return 1 / dist.mean(); // kurtosis_excess 1/mean from Wiki & MathWorld eq 31.
      // http://mathworld.wolfram.com/Kurtosis.html explains that the kurtosis excess
      // is more convenient because the kurtosis excess of a normal distribution is zero
      // whereas the true kurtosis is 3.
    } // RealType kurtosis_excess

    template <class RealType, class Policy>
    inline RealType kurtosis(const poisson_distribution<RealType, Policy>& dist)
    { // kurtosis is 4th moment about the mean = u4 / sd ^ 4
      // http://en.wikipedia.org/wiki/Curtosis
      // kurtosis can range from -2 (flat top) to +infinity (sharp peak & heavy tails).
      // http://www.itl.nist.gov/div898/handbook/eda/section3/eda35b.htm
      return 3 + 1 / dist.mean(); // NIST.
      // http://mathworld.wolfram.com/Kurtosis.html explains that the kurtosis excess
      // is more convenient because the kurtosis excess of a normal distribution is zero
      // whereas the true kurtosis is 3.
    } // RealType kurtosis

    template <class RealType, class Policy>
    RealType pdf(const poisson_distribution<RealType, Policy>& dist, const RealType& k)
    { // Probability Density/Mass Function.
      // Probability that there are EXACTLY k occurrences (or arrivals).
      BOOST_FPU_EXCEPTION_GUARD

      BOOST_MATH_STD_USING // for ADL of std functions.

      RealType mean = dist.mean();
      // Error check:
      RealType result = 0;
      if(false == poisson_detail::check_dist_and_k(
        "boost::math::pdf(const poisson_distribution<%1%>&, %1%)",
        mean,
        k,
        &result, Policy()))
      {
        return result;
      }

      // Special case of mean zero, regardless of the number of events k.
      if (mean == 0)
      { // Probability for any k is zero.
        return 0;
      }
      if (k == 0)
      { // mean ^ k = 1, and k! = 1, so can simplify.
        return exp(-mean);
      }
      return boost::math::gamma_p_derivative(k+1, mean, Policy());
    } // pdf

    template <class RealType, class Policy>
    RealType cdf(const poisson_distribution<RealType, Policy>& dist, const RealType& k)
    { // Cumulative Distribution Function Poisson.
      // The random variate k is the number of occurrences(or arrivals)
      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      // Returns the sum of the terms 0 through k of the Poisson Probability Density or Mass (pdf).

      // But note that the Poisson distribution
      // (like others including the binomial, negative binomial & Bernoulli)
      // is strictly defined as a discrete function: only integral values of k are envisaged.
      // However because of the method of calculation using a continuous gamma function,
      // it is convenient to treat it as if it is a continous function
      // and permit non-integral values of k.
      // To enforce the strict mathematical model, users should use floor or ceil functions
      // outside this function to ensure that k is integral.

      // The terms are not summed directly (at least for larger k)
      // instead the incomplete gamma integral is employed,

      BOOST_MATH_STD_USING // for ADL of std function exp.

      RealType mean = dist.mean();
      // Error checks:
      RealType result = 0;
      if(false == poisson_detail::check_dist_and_k(
        "boost::math::cdf(const poisson_distribution<%1%>&, %1%)",
        mean,
        k,
        &result, Policy()))
      {
        return result;
      }
      // Special cases:
      if (mean == 0)
      { // Probability for any k is zero.
        return 0;
      }
      if (k == 0)
      { // return pdf(dist, static_cast<RealType>(0));
        // but mean (and k) have already been checked,
        // so this avoids unnecessary repeated checks.
       return exp(-mean);
      }
      // For small integral k could use a finite sum -
      // it's cheaper than the gamma function.
      // BUT this is now done efficiently by gamma_q function.
      // Calculate poisson cdf using the gamma_q function.
      return gamma_q(k+1, mean, Policy());
    } // binomial cdf

    template <class RealType, class Policy>
    RealType cdf(const complemented2_type<poisson_distribution<RealType, Policy>, RealType>& c)
    { // Complemented Cumulative Distribution Function Poisson
      // The random variate k is the number of events, occurrences or arrivals.
      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      // But note that the Poisson distribution
      // (like others including the binomial, negative binomial & Bernoulli)
      // is strictly defined as a discrete function: only integral values of k are envisaged.
      // However because of the method of calculation using a continuous gamma function,
      // it is convenient to treat it as is it is a continous function
      // and permit non-integral values of k.
      // To enforce the strict mathematical model, users should use floor or ceil functions
      // outside this function to ensure that k is integral.

      // Returns the sum of the terms k+1 through inf of the Poisson Probability Density/Mass (pdf).
      // The terms are not summed directly (at least for larger k)
      // instead the incomplete gamma integral is employed,

      RealType const& k = c.param;
      poisson_distribution<RealType, Policy> const& dist = c.dist;

      RealType mean = dist.mean();

      // Error checks:
      RealType result = 0;
      if(false == poisson_detail::check_dist_and_k(
        "boost::math::cdf(const poisson_distribution<%1%>&, %1%)",
        mean,
        k,
        &result, Policy()))
      {
        return result;
      }
      // Special case of mean, regardless of the number of events k.
      if (mean == 0)
      { // Probability for any k is unity, complement of zero.
        return 1;
      }
      if (k == 0)
      { // Avoid repeated checks on k and mean in gamma_p.
         return -boost::math::expm1(-mean, Policy());
      }
      // Unlike un-complemented cdf (sum from 0 to k),
      // can't use finite sum from k+1 to infinity for small integral k,
      // anyway it is now done efficiently by gamma_p.
      return gamma_p(k + 1, mean, Policy()); // Calculate Poisson cdf using the gamma_p function.
      // CCDF = gamma_p(k+1, lambda)
    } // poisson ccdf

    template <class RealType, class Policy>
    inline RealType quantile(const poisson_distribution<RealType, Policy>& dist, const RealType& p)
    { // Quantile (or Percent Point) Poisson function.
      // Return the number of expected events k for a given probability p.
      static const char* function = "boost::math::quantile(const poisson_distribution<%1%>&, %1%)";
      RealType result = 0; // of Argument checks:
      if(false == poisson_detail::check_prob(
        function,
        p,
        &result, Policy()))
      {
        return result;
      }
      // Special case:
      if (dist.mean() == 0)
      { // if mean = 0 then p = 0, so k can be anything?
         if (false == poisson_detail::check_mean_NZ(
         function,
         dist.mean(),
         &result, Policy()))
        {
          return result;
        }
      }
      if(p == 0)
      {
         return 0; // Exact result regardless of discrete-quantile Policy
      }
      if(p == 1)
      {
         return policies::raise_overflow_error<RealType>(function, 0, Policy());
      }
      typedef typename Policy::discrete_quantile_type discrete_type;
      boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
      RealType guess, factor = 8;
      RealType z = dist.mean();
      if(z < 1)
         guess = z;
      else
         guess = boost::math::detail::inverse_poisson_cornish_fisher(z, p, RealType(1-p), Policy());
      if(z > 5)
      {
         if(z > 1000)
            factor = 1.01f;
         else if(z > 50)
            factor = 1.1f;
         else if(guess > 10)
            factor = 1.25f;
         else
            factor = 2;
         if(guess < 1.1)
            factor = 8;
      }

      return detail::inverse_discrete_quantile(
         dist,
         p,
         1-p,
         guess,
         factor,
         RealType(1),
         discrete_type(),
         max_iter);
   } // quantile

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<poisson_distribution<RealType, Policy>, RealType>& c)
    { // Quantile (or Percent Point) of Poisson function.
      // Return the number of expected events k for a given
      // complement of the probability q.
      //
      // Error checks:
      static const char* function = "boost::math::quantile(complement(const poisson_distribution<%1%>&, %1%))";
      RealType q = c.param;
      const poisson_distribution<RealType, Policy>& dist = c.dist;
      RealType result = 0;  // of argument checks.
      if(false == poisson_detail::check_prob(
        function,
        q,
        &result, Policy()))
      {
        return result;
      }
      // Special case:
      if (dist.mean() == 0)
      { // if mean = 0 then p = 0, so k can be anything?
         if (false == poisson_detail::check_mean_NZ(
         function,
         dist.mean(),
         &result, Policy()))
        {
          return result;
        }
      }
      if(q == 0)
      {
         return policies::raise_overflow_error<RealType>(function, 0, Policy());
      }
      if(q == 1)
      {
         return 0;  // Exact result regardless of discrete-quantile Policy
      }
      typedef typename Policy::discrete_quantile_type discrete_type;
      boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
      RealType guess, factor = 8;
      RealType z = dist.mean();
      if(z < 1)
         guess = z;
      else
         guess = boost::math::detail::inverse_poisson_cornish_fisher(z, RealType(1-q), q, Policy());
      if(z > 5)
      {
         if(z > 1000)
            factor = 1.01f;
         else if(z > 50)
            factor = 1.1f;
         else if(guess > 10)
            factor = 1.25f;
         else
            factor = 2;
         if(guess < 1.1)
            factor = 8;
      }

      return detail::inverse_discrete_quantile(
         dist,
         1-q,
         q,
         guess,
         factor,
         RealType(1),
         discrete_type(),
         max_iter);
   } // quantile complement.

  } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>
#include <boost/math/distributions/detail/inv_discrete_quantile.hpp>

#endif // BOOST_MATH_SPECIAL_POISSON_HPP


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

  Computation of a convex hull in 3D for a point cloud.

  Copyright (c) 2013
  Dzmitry Hlindzich <hlindzich@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

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

#ifndef CONVEX_HULL_3D_HPP_1CB416FB_1F43_4159_A19B_202B43247622_
#define CONVEX_HULL_3D_HPP_1CB416FB_1F43_4159_A19B_202B43247622_

#include <set>
#include <map>
#include <complex>
#include <algorithm>
#include <boost/assert.hpp>

#include "bo/core/vector.hpp"
#include "bo/core/mesh.hpp"
#include "bo/core/triangle.hpp"

namespace bo {
namespace surfaces {

// Computes the convex hull of the given points using the incremental algorithm (Michael Kallay,
// "The Complexity of Incremental Convex Hull Algorithms in Rd" Inf. Process. Lett. 19(4): 197 (1984)).
// The input points must be not coplanar.
template <typename RealType>
class IncrementalConvexHull3D
{
public:
    typedef bo::Vector<RealType, 3> Point3D;
    typedef std::vector<Point3D> Points3D;
    typedef bo::Triangle<Point3D> Face3D;
    typedef std::vector<Face3D> Faces3D;
    typedef bo::Mesh<RealType> Mesh;

    IncrementalConvexHull3D(const Points3D &points)
    {
        // Remove all duplicate points.
        std::set<Point3D> uniq(points.begin(), points.end());
        points_ = Points3D(uniq.begin(), uniq.end());

        // Compute the convex hull.
        initialize_convex_hull();
        expand_convex_hull();
    }

    // Returns the convex hull as a mesh.
    Mesh get_mesh()
    {
        Mesh mesh(faces_.size());

        // Create a reference map (Point3D -> indices of the mesh vertices).
        std::map<Point3D, std::size_t> mymap;

        // Fill in the mesh.
        for (typename FaceSet3D::const_iterator it = faces_.begin(); it != faces_.end(); ++it)
        {
            Face3D f = *it;

            // Add the vertices.
            for (std::size_t i = 0; i < 3; ++i)
            {
                Point3D p = f[i];

                // If the current vertex was not used before, add it.
                if (mymap.find(p) == mymap.end())
                {
                    std::size_t index = mesh.add_vertex(p);
                    mymap[p] = index;
                }
            }

            // Add the face.
            mesh.add_face(typename Mesh::Face(mymap[f.A()], mymap[f.B()], mymap[f.C()]));
        }

        return mesh;
    }

    // Returns the convex hull as the collection of faces.
    Faces3D get_faces()
    {
        return Faces3D(faces_.begin(), faces_.end());
    }

    // Computes the volume of the convex hull using the discrete case
    // of the Gauss-Ostrogradsky's Divergence theorem.
    RealType get_volume()
    {
        RealType v = 0;

        Point3D mass = centroid();

        for (typename FaceSet3D::const_iterator it = faces_.begin();
             it != faces_.end(); ++it)
        {
            Face3D f = *it;

            // Normal vector must be normalized.
            Point3D n = normal(f);
            n /= n.euclidean_norm();

            // Face barycenter.
            Point3D c = (f.A() + f.B() + f.C()) / 3;

            // Direct the face normal outside the volume.
            Point3D in_direction = mass - c;
            if (n * in_direction > 0)
                n = -n;

            RealType a = area(f);

            BOOST_ASSERT(a >= 0);

            v += (n * c) * a;
        }

        BOOST_ASSERT(v >= 0);

        return v / 3;
    }

private:

    // Faces comparator used for the set container.
    struct FaceCompare
    {
        bool operator()(const Face3D &f1, const Face3D &f2) const
        {
            typedef std::set<Point3D> PointSet3D;

            // Sort the vertices using the sorted set.
            PointSet3D fs1;
            fs1.insert(f1.A());
            fs1.insert(f1.B());
            fs1.insert(f1.C());

            PointSet3D fs2;
            fs2.insert(f2.A());
            fs2.insert(f2.B());
            fs2.insert(f2.C());

            BOOST_ASSERT(fs1.size() == fs2.size());

            typename PointSet3D::const_iterator it1 = fs1.begin();
            typename PointSet3D::const_iterator it2 = fs2.begin();

            while (it1 != fs1.end() && it2 != fs2.end())
            {
                if (*it1 < *it2)
                    return true;
                if (*it2 < *it1)
                    return false;

                ++it1;
                ++it2;
            }

            return false;
        }
    };

    typedef std::set<Face3D, FaceCompare> FaceSet3D;

    // Finds the initial tetrahedron.
    void initialize_convex_hull()
    {
        const RealType kEpsilon(0.001);

        if (points_.size() >= 4)
        {
            typedef typename Points3D::iterator Iterator;

            for (Iterator it1 = points_.begin(); it1 != points_.end() - 3; ++it1)
                for (Iterator it2 = it1 + 1; it2 != points_.end() - 2; ++it2)
                    for (Iterator it3 = it2 + 1; it3 != points_.end() - 1; ++it3)
                    {
                        Point3D v1 = *it2 - *it1;
                        Point3D v2 = *it3 - *it1;

                        // Only if non-collinear.
                        if (std::abs(std::abs(v1 * v2) -
                                     v1.euclidean_norm() * v2.euclidean_norm()) > kEpsilon)
                        {
                            Point3D c = v1.cross_product(v2);

                            for (Iterator it4 = it3 + 1; it4 != points_.end(); ++it4)
                            {
                                Point3D v3 = *it4 - *it1;

                                // Only if non-planar.
                                if (std::abs(c * v3) > kEpsilon)
                                {
                                    // Create faces of the initial tetrahedron.
                                    insert_tetrahedron(*it1, *it2, *it3, *it4);

                                    // Remove the points from the list.
                                    points_.erase(it4);
                                    points_.erase(it3);
                                    points_.erase(it2);
                                    points_.erase(it1);

                                    return;
                                }
                            }
                        }
                    }
        }
    }

    // Inserts faces of the tetrahedron.
    void insert_tetrahedron(const Point3D &p1, const Point3D &p2, const Point3D &p3, const Point3D &p4)
    {
        process_face(Face3D(p1, p2, p3));
        process_face(Face3D(p1, p2, p4));
        process_face(Face3D(p2, p3, p4));
        process_face(Face3D(p3, p1, p4));
    }

    // Inserts the face if it is not in the set yet, otherwise deletes it from the set.
    void process_face(const Face3D &f)
    {
        typename FaceSet3D::iterator it = faces_.find(f);

        if (it == faces_.end())
        {
            faces_.insert(f);
        }
        else
        {
            faces_.erase(it);
        }
    }

    // Incremental expansion of the convex hull.
    void expand_convex_hull()
    {
        const RealType kEpsilon(0.0001);

        if (faces_.size() > 3)
        {
            for (typename Points3D::const_iterator itp = points_.begin(); itp != points_.end(); ++itp)
            {
                Point3D p = *itp;

                Point3D c = centroid();

                Faces3D visible_faces;

                // Find the "visible" faces.
                for (typename FaceSet3D::const_iterator it = faces_.begin(); it != faces_.end(); ++it)
                {
                    Face3D f = *it;
                    Point3D v_c = c - f.A();
                    v_c = v_c / v_c.euclidean_norm();
                    Point3D v_p = p - f.A();
                    v_p = v_p / v_p.euclidean_norm();

                    Point3D n = normal(f);
                    n = n / n.euclidean_norm();

                    RealType dot_c = n * v_c;
                    RealType dot_p = n * v_p;

                    // If the face is "visible" from point p.
                    if (dot_c * dot_p < -kEpsilon)
                    {
                        visible_faces.push_back(f);
                    }
                }

                // Expand the hull and remove the "visible" faces.
                for (typename Faces3D::const_iterator it = visible_faces.begin();
                     it != visible_faces.end(); ++it)
                {
                    Face3D f = *it;

                    // Add new faces and remove the "visible" face.
                    insert_tetrahedron(f.A(), f.B(), f.C(), p);
                }
            }
        }
    }

    Point3D centroid()
    {
        Point3D center(0);
        std::size_t count = 0;

        for (typename FaceSet3D::const_iterator it = faces_.begin(); it != faces_.end(); ++it)
        {
            center += (it->A() + it->B() + it->C());
            count += 3;
        }

        return center / count;
    }

    inline Point3D normal(const Face3D &f)
    {
        return (f.B() - f.A()).cross_product(f.C() - f.A());
    }

    inline RealType area(const Face3D &f)
    {
        return normal(f).euclidean_norm() / 2;
    }

    Points3D points_;
    FaceSet3D faces_;
};

} // namespace surfaces
} // namespace bo

#endif // CONVEX_HULL_3D_HPP_1CB416FB_1F43_4159_A19B_202B43247622_


///////////////////////////////////////////////////////////////////////////////
//  Copyright 2014 Anton Bikineev
//  Copyright 2014 Christopher Kormanyos
//  Copyright 2014 John Maddock
//  Copyright 2014 Paul Bristow
//  Distributed under 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_HYPERGEOMETRIC_2F0_HPP
#define BOOST_MATH_HYPERGEOMETRIC_2F0_HPP

#include <boost/math/policies/policy.hpp>
#include <boost/math/policies/error_handling.hpp>
#include <boost/math/special_functions/detail/hypergeometric_series.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/tools/fraction.hpp>

namespace boost { namespace math { namespace detail {

   template <class T>
   struct hypergeometric_2F0_cf
   {
      //
      // We start this continued fraction at b on index -1
      // and treat the -1 and 0 cases as special cases.
      // We do this to avoid adding the continued fraction result
      // to 1 so that we can accurately evaluate for small results
      // as well as large ones.  See  http://functions.wolfram.com/07.31.10.0002.01
      //
      T a1, a2, z;
      int k;
      hypergeometric_2F0_cf(T a1_, T a2_, T z_) : a1(a1_), a2(a2_), z(z_), k(-2) {}
      typedef std::pair<T, T> result_type;

      result_type operator()()
      {
         ++k;
         if (k <= 0)
            return std::make_pair(z * a1 * a2, 1);
         return std::make_pair(-z * (a1 + k) * (a2 + k) / (k + 1), 1 + z * (a1 + k) * (a2 + k) / (k + 1));
      }
   };

   template <class T, class Policy>
   T hypergeometric_2F0_cf_imp(T a1, T a2, T z, const Policy& pol, const char* function)
   {
      using namespace boost::math;
      hypergeometric_2F0_cf<T> evaluator(a1, a2, z);
      boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
      T cf = tools::continued_fraction_b(evaluator, policies::get_epsilon<T, Policy>(), max_iter);
      policies::check_series_iterations<T>(function, max_iter, pol);
      return cf;
   }

   template <class T, class Policy>
   inline T hypergeometric_2F0_imp(T a1, T a2, const T& z, const Policy& pol, bool asymptotic = false)
   {
      //
      // The terms in this series go to infinity unless one of a1 and a2 is a negative integer.
      //
      using std::swap;
      BOOST_MATH_STD_USING

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

      if (z == 0)
         return 1;

      bool is_a1_integer = (a1 == floor(a1));
      bool is_a2_integer = (a2 == floor(a2));

      if (!asymptotic && !is_a1_integer && !is_a2_integer)
         return boost::math::policies::raise_overflow_error<T>(function, 0, pol);
      if (!is_a1_integer || (a1 > 0))
      {
         swap(a1, a2);
         swap(is_a1_integer, is_a2_integer);
      }
      //
      // At this point a1 must be a negative integer:
      //
      if(!asymptotic && (!is_a1_integer || (a1 > 0)))
         return boost::math::policies::raise_overflow_error<T>(function, 0, pol);
      //
      // Special cases first:
      //
      if (a1 == 0)
         return 1;
      if ((a1 == a2 - 0.5f) && (z < 0))
      {
         // http://functions.wolfram.com/07.31.03.0083.01
         int n = static_cast<int>(static_cast<boost::uintmax_t>(boost::math::lltrunc(-2 * a1)));
         T smz = sqrt(-z);
         return pow(2 / smz, -n) * boost::math::hermite(n, 1 / smz);
      }

      if (is_a1_integer && is_a2_integer)
      {
         if ((a1 < 1) && (a2 <= a1))
         {
            const unsigned int n = static_cast<unsigned int>(static_cast<boost::uintmax_t>(boost::math::lltrunc(-a1)));
            const unsigned int m = static_cast<unsigned int>(static_cast<boost::uintmax_t>(boost::math::lltrunc(-a2 - n)));

            return (pow(z, T(n)) * boost::math::factorial<T>(n, pol)) *
               boost::math::laguerre(n, m, -(1 / z), pol);
         }
         else if ((a2 < 1) && (a1 <= a2))
         {
            // function is symmetric for a1 and a2
            const unsigned int n = static_cast<unsigned int>(static_cast<boost::uintmax_t>(boost::math::lltrunc(-a2)));
            const unsigned int m = static_cast<unsigned int>(static_cast<boost::uintmax_t>(boost::math::lltrunc(-a1 - n)));

            return (pow(z, T(n)) * boost::math::factorial<T>(n, pol)) *
               boost::math::laguerre(n, m, -(1 / z), pol);
         }
      }

      if ((a1 * a2 * z < 0) && (a2 < -5) && (fabs(a1 * a2 * z) > 0.5))
      {
         // Series is alternating and maybe divergent at least for the first few terms
         // (until a2 goes positive), try the continued fraction:
         return hypergeometric_2F0_cf_imp(a1, a2, z, pol, function);
      }

      return detail::hypergeometric_2F0_generic_series(a1, a2, z, pol);
   }

} // namespace detail

template <class T1, class T2, class T3, class Policy>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_2F0(T1 a1, T2 a2, T3 z, const Policy& /* pol */)
{
   BOOST_FPU_EXCEPTION_GUARD
      typedef typename tools::promote_args<T1, T2, T3>::type result_type;
   typedef typename policies::evaluation<result_type, Policy>::type value_type;
   typedef typename policies::normalise<
      Policy,
      policies::promote_float<false>,
      policies::promote_double<false>,
      policies::discrete_quantile<>,
      policies::assert_undefined<> >::type forwarding_policy;
   return policies::checked_narrowing_cast<result_type, Policy>(
      detail::hypergeometric_2F0_imp<value_type>(
         static_cast<value_type>(a1),
         static_cast<value_type>(a2),
         static_cast<value_type>(z),
         forwarding_policy()),
      "boost::math::hypergeometric_2F0<%1%>(%1%,%1%,%1%)");
}

template <class T1, class T2, class T3>
inline typename tools::promote_args<T1, T2, T3>::type hypergeometric_2F0(T1 a1, T2 a2, T3 z)
{
   return hypergeometric_2F0(a1, a2, z, policies::policy<>());
}

  } } // namespace boost::math

#endif // BOOST_MATH_HYPERGEOMETRIC_HPP


#include "faasm/faasm.h"

#include <Eigen/Dense>

#include "ndpapi.h"

#include <stdio.h>
#include <string.h>

#include <regex>
#include <string_view>
#include <vector>

#include "util/kmeansrex.cpp"

using std::string_view;

string_view objKey;

uint32_t endswap32(uint32_t x)
{
    return ((x & 0xFFu) << 24u) | ((x & 0xFF00u) << 8u) |
           ((x & 0xFF0000u) >> 8u) | ((x & 0xFF000000u) >> 24u);
}

Eigen::MatrixXf pcaReducedData;

int work()
{
    uint32_t fetchedLength{};
    uint8_t* objData =
      __faasmndp_getMmap(reinterpret_cast<const uint8_t*>(objKey.data()),
                         objKey.size(),
                         1 * 1024 * 1024 * 1024,
                         &fetchedLength);
    if (objData == nullptr || fetchedLength < 64) {
        const string_view output{
            "FAILED - no object found with the given key"
        };
        faasmSetOutput(reinterpret_cast<const uint8_t*>(output.data()),
                       output.size());
        return 1;
    }
    const uint32_t magic = endswap32(*reinterpret_cast<uint32_t*>(objData));
    const uint32_t nImages =
      endswap32(*reinterpret_cast<uint32_t*>(objData + 4));
    const uint32_t nRows = endswap32(*reinterpret_cast<uint32_t*>(objData + 8));
    const uint32_t nCols =
      endswap32(*reinterpret_cast<uint32_t*>(objData + 12));
    const uint8_t* pixelDataStart = objData + 16;
    if (magic != 0x00000803) {
        const string_view output{ "FAILED - bad magic number" };
        faasmSetOutput(reinterpret_cast<const uint8_t*>(output.data()),
                       output.size());
        return 2;
    }
    const uint32_t pixelsPerImage = nRows * nCols;
    if (pixelsPerImage * nImages > fetchedLength - 16) {
        const string_view output{ "FAILED - missing image data" };
        faasmSetOutput(reinterpret_cast<const uint8_t*>(output.data()),
                       output.size());
        return 3;
    }
    Eigen::MatrixXf imageData(nImages, pixelsPerImage);
    const float fmul = 1.0f / 255.0f;
    for (uint32_t image = 0; image < nImages; image++) {
        const uint8_t* imageStart = pixelDataStart + (nRows * nCols) * image;
        for (uint32_t pixel = 0; pixel < nRows * nCols; pixel++) {
            imageData(image, pixel) = float(imageStart[pixel]) * fmul;
        }
    }
    // normalize data
    {
        Eigen::VectorXf mins = imageData.colwise().minCoeff();
        Eigen::VectorXf maxs = imageData.colwise().maxCoeff();
        Eigen::VectorXf scaleCoeff = maxs - mins;
        for (auto& val : scaleCoeff) {
            val = (val == 0) ? 1.0f : 1.0f / val;
        }
        imageData.rowwise() -= mins.transpose();
        imageData.array().rowwise() *= scaleCoeff.transpose().array();
    }
    // center on means
    {
        Eigen::VectorXf means = imageData.colwise().mean();
        imageData.rowwise() -= means.transpose();
    }
    Eigen::MatrixXf covarianceMat = imageData.adjoint() * imageData;
    covarianceMat = covarianceMat / (imageData.rows() - 1);
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eigensolver(covarianceMat);
    Eigen::VectorXf eigenvalues = eigensolver.eigenvalues();
    eigenvalues /= eigenvalues.sum();
    Eigen::MatrixXf eigenvectors = eigensolver.eigenvectors();
    Eigen::MatrixXf pcaTransform = eigenvectors.rightCols(32);
    pcaReducedData = imageData * pcaTransform;
    return 0;
}

int main(int argc, char* argv[])
{
    long inputSz = faasmGetInputSize();
    std::vector<uint8_t> inputBuf(inputSz);
    faasmGetInput(inputBuf.data(), inputBuf.size());
    string_view inputStr(reinterpret_cast<char*>(inputBuf.data()),
                         inputBuf.size());
    if (inputStr.size() < 1) {
        const string_view output{
            "FAILED - no key/value pair. Usage: grep with input 'key regex'"
        };
        faasmSetOutput(reinterpret_cast<const uint8_t*>(output.data()),
                       output.size());
        return 0;
    }
    objKey = inputStr;

    if (__faasmndp_storageCallAndAwait(work) != 0) {
        return 1;
    }

    const int K = 10;
    const int n_features = pcaReducedData.cols();
    const int n_iters = 25;
    const int seed = 1234567;
    const int n_examples = pcaReducedData.rows();

    Eigen::ArrayXXd dblData = pcaReducedData.array().cast<double>();
    Eigen::ArrayXXd mu = Eigen::ArrayXXd::Zero(K, n_features);
    Eigen::ArrayXd z = Eigen::ArrayXd::Zero(n_examples);
    RunKMeans(dblData.data(),
              n_examples,
              n_features,
              K,
              n_iters,
              seed,
              "plusplus",
              mu.data(),
              z.data());

    std::string output;
    output.reserve(z.rows() * 4);
    for (auto& el : z) {
        output += std::to_string(int(el));
        output.push_back(' ');
    }
    output.push_back('\n');

    faasmSetOutput(reinterpret_cast<uint8_t*>(output.data()), output.size());
    return 0;
}


/*
 * @Author: tom: https://github.com/TOMsworkspace
 * @Date: 2021-09-03 15:32:52
 * @Last Modified by: tom: https://github.com/TOMsworkspace
 * @Last Modified time: 2021-09-03 19:17:13
 */

#include <algorithm>
#include <sstream>
#include <iostream>

#include <Eigen\\Dense>
using namespace std;
using namespace Eigen;

//#include <gl/GL.h>
//#include <glad/glad.h>
//#include <GLFW/glfw3.h>

//#include <irrklang/irrKlang.h>
//using namespace irrklang;

#include "dtkFemSimulation.h"

#include "GL/freeglut.h"

//#include "resource_manager.h"
//#include "sprite_renderer.h"

int dim = 2;
int n_node_x = 50;
int n_node_y = 6;
float node_mass = 1.0f;
int n_node = n_node_x * n_node_y;
int n_fem_element = (n_node_x - 1) * (n_node_y - 1) * 2;
float deltat = 3e-4;
float deltax = (1.0 / 32);

float Young_E = 1000.0f; /**< 杨氏模量 */
float Poisson_r = 0.3f; /**< 泊松比 [0 - 0.5] */
float Lame_parameter_1 = Young_E / (2 * (1 + Poisson_r));
float Lame_parameter_2 = Young_E * Poisson_r / ((1 + Poisson_r) * (1 - 2 * Poisson_r));
float element_v = 0.01f; /**< 微元体积 */

float radius = 0.05;

int iterate_time = 3;

inline int mesh(int i, int j) {
    return i * n_node_y + j;
}

dtkFemSimulation::dtkFemSimulation(unsigned int width, unsigned int height)
: dtkScene(width, height),
points(n_node),pre_points(n_node),points_v(n_node),points_force(n_node),B(n_fem_element),
MeshTable(n_fem_element,std::vector<int>(3,0)),sphere(dtk::dtkGraphicsKernel::Point2(0.5,0.25), radius),
total_energy(0),pre_total_energy(0),spherecenter(0.5,0.25)
{

}

Matrix2f dtkFemSimulation::compute_D(int i){
    int a = MeshTable[i][0];
    int b = MeshTable[i][1];
    int c = MeshTable[i][2];

    Matrix2f ans;
    ans(0,0) = points[a][0] - points[c][0];
    ans(0,1) = points[b][0] - points[c][0];
    ans(1,0) = points[a][1] - points[c][1];
    ans(1,1) = points[b][1] - points[c][1];
    return ans;
}

void dtkFemSimulation::compute_B(){
    for(int i = 0; i < n_fem_element; ++i){
        this->B[i] = compute_D(i).inverse();
        //cout << setw(6) << B[i] << endl;
    }
}

Matrix2f dtkFemSimulation::compute_P(int i){
    Matrix2f D = compute_D(i);
    Matrix2f F = D * B[i];

    Matrix2f F_T = F.transpose().inverse();

    float J = max(0.5f, F.determinant()); /**< 形变率 */

    return Lame_parameter_1 * (F - F_T) + Lame_parameter_2 * log(J) * F_T ;
    //Matrix2f ans = D * this->B[i];
}

void dtkFemSimulation::compute_total_energy(){

    //this->total_energy = 0.0f;
    for(int i = 0; i < n_fem_element; ++i){
        Matrix2f D = compute_D(i);
        Matrix2f F = D * B[i];

        //NeoHooken
        float I1 = (F * F.transpose()).trace();
        float J = max(0.2f, (float)F.determinant()); /**< 形变率 */

        //cout << J << endl;

        float element_energy_density = 0.5 * Lame_parameter_1 * (I1 - dim) - Lame_parameter_1 * log(J) + 0.5 * Lame_parameter_2 * log(J) * log(J);
        this->total_energy += element_energy_density * element_v;
    }
}

dtkFemSimulation::~dtkFemSimulation()
{
}

void dtkFemSimulation::Init()
{
    dtkScene::Init();
    //TODO: load shaders

    //TODO: configure shaders

    //TODO: load textures

    //TODO: set render-specific controls

    //TODO: configure Scene objects

    //build mesh
    for(int i = 0; i < n_node_x; ++i){
        for(int j = 0; j < n_node_y; ++j){
            int idx = mesh(i,j);
            //this->points[idx][0] = -14 + i * deltax * 0.5;
            //this->points[idx][1] = 8 + j * deltax * 0.5 + i * deltax * 0.05;

            this->points[idx][0] = 0.1f + i * deltax * 0.5f;
            this->points[idx][1] = 0.5f + j * deltax * 0.5f + i * deltax * 0.1f;
            this->points_v[idx][0] = 0.0f;
            this->points_v[idx][1] = -1.0f;
        }
    }

    //this->pre_points = points;

    for(int i = 0; i < n_node_x - 1; ++i ){
        for(int j = 0; j < n_node_y - 1; ++j){
            //element id
            int eidx = (i * (n_node_y - 1) + j) * 2;
            this->MeshTable[eidx][0] = mesh(i,j);
            this->MeshTable[eidx][1] = mesh(i + 1, j);
            this->MeshTable[eidx][2] = mesh(i,j + 1);

            eidx = (i * (n_node_y - 1) + j) * 2 + 1;
            this->MeshTable[eidx][0] = mesh(i,j + 1);
            this->MeshTable[eidx][1] = mesh(i + 1,j + 1);
            this->MeshTable[eidx][2] = mesh(i + 1,j);
        }
    }

    compute_B();

    //TODO: audio
}

void dtkFemSimulation::compute_force(){
    for(int i = 0; i < n_node ; ++i){
        this->points_force[i] = Vector2f(0.0f, - 10.0f * node_mass);
    }

    for(int i = 0; i < n_fem_element; ++i){

        Matrix2f P = compute_P(i);
        Matrix2f H = - element_v * (P * (this->B[i].transpose()));

        Vector2f h1 = Vector2f(H(0,0), H(1,0));
        Vector2f h2 = Vector2f(H(0,1), H(1,1));

        int a = this->MeshTable[i][0];
        int b = this->MeshTable[i][1];
        int c = this->MeshTable[i][2];

        this->points_force[a] += h1;
        this->points_force[b] += h2;
        this->points_force[c] += -(h1 + h2);
    }
}

void dtkFemSimulation::Update(float dt)
{
    //TODO: update objects
    //TODO: check for object collisions
    if(this->State == SCENE_ACTIVE){

        // 迭代多轮, 防止穿透
        for(int i = 0; i < iterate_time ; ++i){
            //this->pre_total_energy = total_energy;
            compute_total_energy();
            DoCollisions();
            compute_force();

            //cout << total_energy << endl;

            //float deltaU = this->total_energy - this->pre_total_energy;
            //deltaU = abs(deltaU) < 1e-9 ? 0 : deltaU;
            for(int i = 0; i < n_node; ++i){
                // update points

                //Vector2f deltaX = this->points[i] - this->pre_points[i];

                //Vector2f diffUtoX = Vector2f(deltaU / deltaX[0], deltaU / deltaX[1]);

               // Vector2f diffUtoX = Vector2f(abs(deltaX[0]) > 1e-4 ? deltaU / deltaX[0] : 0.0f, abs(deltaX[1]) > 1e-4 ? deltaU / deltaX[1] : 0.0f);

                //diffUtoX = Vector2f(0.0f,0.0f);

                //cout << diffUtoX << endl;

               // this->points_v[i] = (this->points_v[i] + ((- diffUtoX / node_mass) + Vector2f(0.0f, -10.0f)) * deltat) * exp(deltat * -6);

                this->points_v[i] = (this->points_v[i] + (this->points_force[i] / node_mass) * deltat) * exp(deltat * -3);
                //this->pre_points[i] = this->points[i];

                this->points[i] += deltat * this->points_v[i];
            }

           // this->pre_points = this->points;
           // this->pre_total_energy = this->total_energy;
        }
    }
}

void dtkFemSimulation::ProcessInput(float dt)
{
    dtkScene::ProcessInput(dt);
    //TODO: process input(keys)

}

void dtkFemSimulation::Render()
{
    //if(this->State == SCENE_ACTIVE){
        //TODO: draw circle

        Vector2f center = spherecenter;
        //Vector2f(this->sphere.center()[0], this->sphere.center()[1]);

        glColor3f(0x06 * 1.0 / 0xff, 0x85 * 1.0 / 0xff, 0x87 * 1.0 / 0xff);
        glBegin(GL_POLYGON);

        int n = 100;
        for (int i = 0; i < n; i++)
        {
            glVertex2f(center[0] + radius * cos(2 * dtk::dtkPI / n * i), center[1] + radius * sin(2 * dtk::dtkPI / n * i));
        }
        glEnd();

        //TODO: draw fem element(triangles here)
        glColor3f(0x4f * 1.0 / 0xff, 0xb9 * 1.0 / 0xff, 0x9f * 1.0 / 0xff);
        glBegin(GL_LINES);
        for(int i = 0; i < n_fem_element; ++i){
            for(int j = 0 ; j < 3; ++j){
                int a = this->MeshTable[i][j];
                int b = this->MeshTable[i][(j  + 1) % 3];

                //draw line from a to b;
                glVertex2f(this->points[a][0] , this->points[a][1] );
                glVertex2f(this->points[b][0], this->points[b][1] );
            }
        }
        //glEnd();

        glColor3f(1.0, 1.0, 1.0);
        //glBegin(GL_LINES);
        glVertex2f(0.0f, 0.2f);
        glVertex2f(1.0f, 0.2f);

        glVertex2f(1.0f, 0.85f);
        glVertex2f(1.0f, 0.2f);

        glVertex2f(1.0f, 0.85f);
        glVertex2f(0.0f, 0.85f);

        glVertex2f(0.0f, 0.85f);
        glVertex2f(0.0f, 0.2f);
        glEnd();
   // }
}

// collision detection

void dtkFemSimulation::DoCollisions()
{
    if(this->State == SCENE_ACTIVE){
        Vector2f center = spherecenter;
        //Vector2f(this->sphere.center()[0], this->sphere.center()[1]);
        float radius = this->sphere.squared_radius();

        for(int i = 0; i < n_node; ++i){
            //# Collide with sphere

            Vector2f dis = this->points[i] - center;
            if((float)(dis.dot(dis)) < radius * radius)
            {
                Vector2f normal = dis.normalized();

                this->points[i] = center + radius * normal;
                this->points_v[i] -=  (this->points_v[i].dot(normal)) *  normal;
            }

            // Collide with ground

            if(this->points[i][1] < 0.2f) {
                this->points[i][1] = 0.2f;
                this->points_v[i][1] = 0.0f;
            }

            if(this->points[i][1] > 0.9f) {
                this->points[i][1] = 0.9f;
                this->points_v[i][1] = 0.0f;
            }

            if(this->points[i][0] < 0.0f) {
                this->points[i][0] = 0.0f;
                this->points_v[i][0] = 0.0f;
            }

            if(this->points[i][0] > 1.0f) {
                this->points[i][0] = 1.0f;
                this->points_v[i][0] = 0.0f;
            }
        }

    }
}

void dtkFemSimulation::moveBall(int x, int y){

    //this->spherecenter = Vector2f((x) * 1.0 / 800 , (600 - y) * 1.0 / 600 );
}

float dtkFemSimulation::getEnergy(){
    return this->total_energy;
}


#pragma once

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_cloud.h>
#include <Eigen/Dense>

namespace {

Eigen::Vector3f getRealSortedEigenValues(Eigen::Matrix<std::complex<float>, 3, 1> eigenValues)
{
  Eigen::Vector3f realEigenValues;
  for (std::size_t i = 0; i < 3; i++){
    realEigenValues(i) = std::real(eigenValues(i));
  }
  if (realEigenValues(0) > realEigenValues(1))
    std::swap(realEigenValues(0), realEigenValues(1));
  if (realEigenValues(1) > realEigenValues(2))
    std::swap(realEigenValues(1), realEigenValues(2));
  if (realEigenValues(0) > realEigenValues(1))
    std::swap(realEigenValues(0), realEigenValues(1));
  // std::cout << "realEigenValues: " << realEigenValues << "\n";
  return realEigenValues;
}

} // namespace

namespace pcl {

// L: linearity, P: planarity, S: sphericality
struct PointXYZLPS
{
	PCL_ADD_POINT4D;
	// union
	// {
	// 	float coordinate[3];
	// 	struct
	// 	{
	// 		float x;
	// 		float y;
	// 		float z;
	// 	};
	// };
	float linearity;
	float planarity;
	float sphericity;
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
} EIGEN_ALIGN16;

struct PointXYZILPS
{
    PCL_ADD_POINT4D;                  // preferred way of adding a XYZ+padding
    float intensity;
    float linearity;
	float planarity;
	float sphericity;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW   // make sure our new allocators are aligned
} EIGEN_ALIGN16;                    // enforce SSE padding for correct memory alignment

// struct PointXYZILPS
// {
// 	// PCL_ADD_POINT4D;
// 	union
// 	{
// 		float coordinate[3];
// 		struct
// 		{
// 			float x;
// 			float y;
// 			float z;
// 		};
// 	};
//     float intensity;
// 	float linearity;
// 	float planarity;
// 	float sphericity;
// 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
// } EIGEN_ALIGN16;

template <typename PointInT, typename PointOutT>
class DescriptorEstimation : public Feature<PointInT, PointOutT>
{
    typedef typename DescriptorEstimation<PointInT, PointOutT>::PointCloudOut PointCloudOut;
    using Feature<PointInT, PointOutT>::input_;
    using Feature<PointInT, PointOutT>::search_parameter_;
    using Feature<PointInT, PointOutT>::search_radius_;
    typedef boost::shared_ptr<NormalEstimation<PointInT, PointOutT> > Ptr;
    typedef boost::shared_ptr<const NormalEstimation<PointInT, PointOutT> > ConstPtr;

protected:

    void computeFeature (PointCloudOut &output)
    {
        for (int i = 0; i < input_->size(); i++){
            const PointInT& searchPoint = input_->points[i];
            std::vector<int> pointIdxRadiusSearch;
            std::vector<float> pointRadiusSquaredDistance;
            // kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);

            this->searchForNeighbors (i, search_parameter_, pointIdxRadiusSearch, pointRadiusSquaredDistance);

            if (pointIdxRadiusSearch.size() < 5) {
                continue;
            }
            typename pcl::PointCloud<PointInT>::Ptr neighbours (new pcl::PointCloud<PointInT>);
            for (std::size_t j = 0; j < pointIdxRadiusSearch.size (); ++j){
                neighbours->points.push_back(input_->points[ pointIdxRadiusSearch[j]]);
            }

            Eigen::Matrix3f covariance_matrix;

            // 16-bytes aligned placeholder for the XYZ centroid of a surface patch
            Eigen::Vector4f xyz_centroid;

            pcl::compute3DCentroid (*neighbours, xyz_centroid);
            // Compute the 3x3 covariance matrix
            pcl::computeCovarianceMatrix (*neighbours, xyz_centroid, covariance_matrix);
            // std::cout << "CCCC" << std::endl;
            // std::cout << "covariance_matrix: \n" << covariance_matrix << "\n";
            // std::cout << "xyz_centroid: \n" << xyz_centroid << "\n";

            Eigen::EigenSolver<Eigen::Matrix3f> eig(covariance_matrix);     // [vec val] = eig(A)
            // std::cout << "DDDD" << std::endl;
            Eigen::Matrix<std::complex<float>, 3, 1> eigenComplexValues = eig.eigenvalues();
            // std::cout << "EEEE" << std::endl;
            // Eigen::Matrix3f D = eig.pseudoEigenvalueMatrix();

            // int col_index, row_index;
            // std::cout << D.maxCoeff(&row_index, &col_index) << endl;
            // std::cout << row_index << " " << col_index << endl;
            // std::cout << "eigen value matrix: " << getRealSortedEigenValues(eigenValues) << "\n";
            Eigen::Vector3f eigenValues = getRealSortedEigenValues(eigenComplexValues);
            // std::cout << "FFFF" << std::endl;

            PointOutT pointWithFeature;

            pointWithFeature.x = searchPoint.x;
            pointWithFeature.y = searchPoint.y;
            pointWithFeature.z = searchPoint.z;
            pointWithFeature.linearity = 1 - eigenValues[1] / eigenValues[2];
            pointWithFeature.planarity = (eigenValues[1] - eigenValues[0]) / eigenValues[2];
            pointWithFeature.sphericity = eigenValues[0] / eigenValues[2];
            output.points.push_back(pointWithFeature);

            // float linearity = 1 - eigenValues[1] / eigenValues[2];
            // float planarity = (eigenValues[1] - eigenValues[0]) / eigenValues[2];
            // float sphericity = eigenValues[0] / eigenValues[2];
            // std::cout << "(" << linearity << ", " << planarity << ", " << sphericity << ")" << std::endl;
            // cloud->points[i].linearity = linearity;
            // cloud->points[i].planarity = planarity;
            // cloud->points[i].sphericity = sphericity;

        }
    }

}; // class DescriptorEstimation

} // namespace pcl

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZLPS,// 注册点类型宏
	(float, x, x)
	(float, y, y)
	(float, z, z)
	(float, linearity, linearity)
	(float, planarity, planarity)
	(float, sphericity, sphericity)
)

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZILPS,
	(float, x, x)
	(float, y, y)
	(float, z, z)
    (float, intensity, intensity)
	(float, linearity, linearity)
	(float, planarity, planarity)
	(float, sphericity, sphericity)
)

// pcl::NormalEstimation<PointTypeIO, PointTypeFull> ne;
// ne.setInputCloud (cloud_out);
// ne.setSearchMethod (search_tree);
// ne.setRadiusSearch (1);
// ne.compute (*cloud_with_normals);

typedef pcl::PointXYZ PointIn;
typedef pcl::PointXYZLPS PointOut;

int test_new(int argc, char** argv)
{
    pcl::PointCloud<PointIn>::Ptr cloud(new pcl::PointCloud<PointIn>);
    pcl::PointCloud<PointOut>::Ptr cloud_out(new pcl::PointCloud<PointOut>);

    pcl::io::loadPCDFile<PointIn> (argv[1], *cloud);
    // pcl::io::loadPLYFile<PointT> (argv[1], *cloud);

    pcl::VoxelGrid<PointIn> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.1f, 0.1f, 0.1f);
	sor.filter(*cloud);

    // pcl::KdTreeFLANN<PointIn> kdtree;
    pcl::search::KdTree<PointIn>::Ptr search_tree (new pcl::search::KdTree<PointIn>);
    search_tree->setInputCloud (cloud);

    pcl::DescriptorEstimation<PointIn, PointOut> de;
    de.setInputCloud(cloud);
    de.setSearchMethod(search_tree);
    de.setRadiusSearch(1);
    de.compute (*cloud_out);

    cloud_out->width = 1;
    cloud_out->height = cloud_out->points.size();
    pcl::io::savePCDFile ("descriptors_output.pcd", *cloud_out);

    return 0;

}

int test_gt(int argc, char** argv)
{
    pcl::PointCloud<PointIn>::Ptr cloud (new pcl::PointCloud<PointIn>);
    pcl::PointCloud<PointOut>::Ptr cloudOut (new pcl::PointCloud<PointOut>);

    pcl::io::loadPCDFile<PointIn> (argv[1], *cloud);
    // pcl::io::loadPLYFile<PointT> (argv[1], *cloud);

    pcl::VoxelGrid<PointIn> sor;
	sor.setInputCloud(cloud);
	sor.setLeafSize(0.1f, 0.1f, 0.1f);
	sor.filter(*cloud);

    // Placeholder for the 3x3 covariance matrix at each surface patch
    Eigen::Matrix3f covariance_matrix;
    // 16-bytes aligned placeholder for the XYZ centroid of a surface patch
    Eigen::Vector4f xyz_centroid;

    // Estimate the XYZ centroid
    pcl::compute3DCentroid (*cloud, xyz_centroid);

    // Compute the 3x3 covariance matrix
    pcl::computeCovarianceMatrix (*cloud, xyz_centroid, covariance_matrix);

    // std::cout << "covariance_matrix: \n" << covariance_matrix << "\n";
    // std::cout << "xyz_centroid: \n" << xyz_centroid << "\n";

    pcl::KdTreeFLANN<PointIn> kdtree;

    kdtree.setInputCloud (cloud);

    float radius = 0.5f;

    std::cout << "Done" << std::endl;

    for (int i = 0; i < cloud->size(); i++){
        PointIn searchPoint = cloud->points[i];
        std::vector<int> pointIdxRadiusSearch;
        std::vector<float> pointRadiusSquaredDistance;
        kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance);

        pcl::PointCloud<PointIn>::Ptr neighbours (new pcl::PointCloud<PointIn>);
        for (std::size_t j = 0; j < pointIdxRadiusSearch.size (); ++j){
        neighbours->points.push_back(cloud->points[ pointIdxRadiusSearch[j]]);

        }
        if (neighbours->points.size() < 5) {
            continue;
        }
        pcl::compute3DCentroid (*neighbours, xyz_centroid);
        // std::cout << "BBBB" << std::endl;
        // Compute the 3x3 covariance matrix
        pcl::computeCovarianceMatrix (*neighbours, xyz_centroid, covariance_matrix);
        // std::cout << "CCCC" << std::endl;
        // std::cout << "covariance_matrix: \n" << covariance_matrix << "\n";
        // std::cout << "xyz_centroid: \n" << xyz_centroid << "\n";

        Eigen::EigenSolver<Eigen::Matrix3f> eig(covariance_matrix);     // [vec val] = eig(A)
        // std::cout << "DDDD" << std::endl;
        Eigen::Matrix<std::complex<float>, 3, 1> eigenComplexValues = eig.eigenvalues();
        // std::cout << "EEEE" << std::endl;
        // Eigen::Matrix3f D = eig.pseudoEigenvalueMatrix();

        // int col_index, row_index;
        // std::cout << D.maxCoeff(&row_index, &col_index) << endl;
        // std::cout << row_index << " " << col_index << endl;
        // std::cout << "eigen value matrix: " << getRealSortedEigenValues(eigenValues) << "\n";
        Eigen::Vector3f eigenValues = getRealSortedEigenValues(eigenComplexValues);
        // std::cout << "FFFF" << std::endl;

        PointOut pointWithFeature;

        pointWithFeature.x = searchPoint.x;
        pointWithFeature.y = searchPoint.y;
        pointWithFeature.z = searchPoint.z;
        pointWithFeature.linearity = 1 - eigenValues[1] / eigenValues[2];
        pointWithFeature.planarity = (eigenValues[1] - eigenValues[0]) / eigenValues[2];
        pointWithFeature.sphericity = eigenValues[0] / eigenValues[2];
        cloudOut->points.push_back(pointWithFeature);

        // float linearity = 1 - eigenValues[1] / eigenValues[2];
        // float planarity = (eigenValues[1] - eigenValues[0]) / eigenValues[2];
        // float sphericity = eigenValues[0] / eigenValues[2];
        // std::cout << "(" << linearity << ", " << planarity << ", " << sphericity << ")" << std::endl;
        // cloud->points[i].linearity = linearity;
        // cloud->points[i].planarity = planarity;
        // cloud->points[i].sphericity = sphericity;

    }

    cloudOut->width = 1;
    cloudOut->height = cloudOut->points.size();
    pcl::io::savePCDFile ("descriptors_output.pcd", *cloudOut);

    return 0;
}


#include "eigen-runtime.h"
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/LU>

using namespace Eigen;

template <class T>
Map< Matrix<T,Dynamic,Dynamic> > matrix(void* p, int r, int c) {
    return Map< Matrix<T,Dynamic,Dynamic> >((T*)p, r, c);
}

template <class T>
Map< Matrix<T,Dynamic,Dynamic> > matrix(const void* p, int r, int c) {
    return Map< Matrix<T,Dynamic,Dynamic> >((T*)p, r, c);
}

template <class T>
Map< SparseMatrix<T> > smatrix(void* val, void* outer,
    void* inner, int r, int c, int s) {
    return Map< SparseMatrix<T> >(r, c, s, (int*)outer, (int*)inner, (T*)val);
}

template <class T>
Map<SparseMatrix<T> > smatrix(const void* val, const void* outer,
    const void* inner, int r, int c, int s) {
    return Map< SparseMatrix<T> >(r, c, s, (int*)outer, (int*)inner, (T*)val);
}

// Matrix product
template <class T>
RET dd_mul( void* p, int r, int c,
    const void* p1, int r1, int c1,
    const void* p2, int r2, int c2)
{
    matrix<T>(p,r,c) = matrix<T>(p1,r1,c1) * matrix<T>(p2,r2,c2);
    return 0;
}
API(dd_mul, (int code,
    void* p, int r, int c,
    const void* p1, int r1, int c1,
    const void* p2, int r2, int c2), (p,r,c,p1,r1,c1,p2,r2,c2));

template <class T>
RET ds_mul( void* p, int r, int c,
    const void* p1, int r1, int c1,
    const void* val, const void* outer, const void* inner,
    int r2, int c2, int s)
{
    matrix<T>(p,r,c) = matrix<T>(p1,r1,c1) * smatrix<T>(val, outer, inner, r2, c2, s);
    return 0;
}
API(ds_mul, (int code,
    void* p, int r, int c,
    const void* p1, int r1, int c1,
    const void* val, const void* outer, const void* inner,
    int r2, int c2, int s), (p,r,c,p1,r1,c1,val,outer,inner,r2,c2,s));

template <class T>
RET sd_mul( void* p, int r, int c,
    const void* val, const void* outer, const void* inner,
    int r2, int c2, int s,
    const void* p1, int r1, int c1)
{
    matrix<T>(p,r,c) = smatrix<T>(val, outer, inner, r2, c2, s) * matrix<T>(p1,r1,c1);
    return 0;
}
API(sd_mul, (int code,
    void* p, int r, int c,
    const void* val, const void* outer, const void* inner, int r2, int c2, int s,
    const void* p1, int r1, int c1), (p,r,c,val,outer,inner,r2,c2,s,p1,r1,c1));

template <class T>
RET ss_mul( void** v, void* o, void** i, int r, int c, int* s,
    const void* v1, const void* o1, const void* i1, int r1, int c1, int s1,
    const void* v2, const void* o2, const void* i2, int r2, int c2, int s2)
{
    typedef Map<SparseMatrix<T> > MapSparseMatrix;
    MapSparseMatrix a(r1, c1, s1, (int*)o1, (int*)i1, (T*)v1);
    MapSparseMatrix b(r2, c2, s2, (int*)o2, (int*)i2, (T*)v2);
    SparseMatrix<T> M = (a * b).pruned();

    memcpy(o, M.outerIndexPtr(), (c+1) * sizeof(int));

    *s = M.nonZeros();
    T* p1 = (T*) malloc((*s) * sizeof(T));
    memcpy(p1, M.valuePtr(), (*s) * sizeof(T));
    *v = p1;
    int* p2 = (int*) malloc((*s) * sizeof(int));
    memcpy(p2, M.innerIndexPtr(), (*s) * sizeof(int));
    *i = p2;
    return 0;
}
API(ss_mul, (int code,
    void** v, void* o, void** i, int r, int c, int* s,
    const void* v1, const void* o1, const void* i1, int r1, int c1, int s1,
    const void* v2, const void* o2, const void* i2, int r2, int c2, int s2),
    (v,o,i,r,c,s,v1,o1,i1,r1,c1,s1,v2,o2,i2,r2,c2,s2));

template <class T>
RET ss_plus( void** v, void* o, void** i, int r, int c, int* s,
    const void* v1, const void* o1, const void* i1, int r1, int c1, int s1,
    const void* v2, const void* o2, const void* i2, int r2, int c2, int s2)
{
    typedef Map<SparseMatrix<T> > MapSparseMatrix;
    MapSparseMatrix a(r1, c1, s1, (int*)o1, (int*)i1, (T*)v1);
    MapSparseMatrix b(r2, c2, s2, (int*)o2, (int*)i2, (T*)v2);
    SparseMatrix<T> M = a + b;

    memcpy(o, M.outerIndexPtr(), (c+1) * sizeof(int));

    *s = M.nonZeros();
    T* p1 = (T*) malloc((*s) * sizeof(T));
    memcpy(p1, M.valuePtr(), (*s) * sizeof(T));
    *v = p1;
    int* p2 = (int*) malloc((*s) * sizeof(int));
    memcpy(p2, M.innerIndexPtr(), (*s) * sizeof(int));
    *i = p2;
    return 0;
}
API(ss_plus, (int code,
    void** v, void* o, void** i, int r, int c, int* s,
    const void* v1, const void* o1, const void* i1, int r1, int c1, int s1,
    const void* v2, const void* o2, const void* i2, int r2, int c2, int s2),
    (v,o,i,r,c,s,v1,o1,i1,r1,c1,s1,v2,o2,i2,r2,c2,s2));

template <class T>
RET ss_cmul( void** v, void* o, void** i, int r, int c, int* s,
    const void* v1, const void* o1, const void* i1, int r1, int c1, int s1,
    const void* v2, const void* o2, const void* i2, int r2, int c2, int s2)
{
    typedef Map<SparseMatrix<T> > MapSparseMatrix;
    MapSparseMatrix a(r1, c1, s1, (int*)o1, (int*)i1, (T*)v1);
    MapSparseMatrix b(r2, c2, s2, (int*)o2, (int*)i2, (T*)v2);

    SparseMatrix<T> M = a.cwiseProduct(b);

    memcpy(o, M.outerIndexPtr(), (c+1) * sizeof(int));

    *s = M.nonZeros();
    T* p1 = (T*) malloc((*s) * sizeof(T));
    memcpy(p1, M.valuePtr(), (*s) * sizeof(T));
    *v = p1;
    int* p2 = (int*) malloc((*s) * sizeof(int));
    memcpy(p2, M.innerIndexPtr(), (*s) * sizeof(int));
    *i = p2;
    return 0;
}
API(ss_cmul, (int code,
    void** v, void* o, void** i, int r, int c, int* s,
    const void* v1, const void* o1, const void* i1, int r1, int c1, int s1,
    const void* v2, const void* o2, const void* i2, int r2, int c2, int s2),
    (v,o,i,r,c,s,v1,o1,i1,r1,c1,s1,v2,o2,i2,r2,c2,s2));

template <class T>
RET sd_plus( void* p, int r, int c,
    const void* val, const void* outer, const void* inner,
    int r2, int c2, int s,
    const void* p1, int r1, int c1)
{
    typedef Map< Matrix<T,Dynamic,Dynamic> > MapMatrix;
    MapMatrix x((T*)p, r, c);
    x = matrix<T>(p1,r1,c1);
    x += smatrix<T>(val, outer, inner, r2, c2, s);
    return 0;
}
API(sd_plus, (int code,
    void* p, int r, int c,
    const void* val, const void* outer, const void* inner, int r2, int c2, int s,
    const void* p1, int r1, int c1), (p,r,c,val,outer,inner,r2,c2,s,p1,r1,c1));

#define UNOP(name) \
extern "C" RET __attribute__((noinline)) eigen_##name(int code, void* p, int r, int c, const void* p1, int r1, int c1) {\
        GUARD_START\
        switch (code) {\
            case 0: matrix<T0>(p,r,c) = matrix<T0>(p1,r1,c1).name(); break;\
            case 1: matrix<T1>(p,r,c) = matrix<T1>(p1,r1,c1).name(); break;\
            case 2: matrix<T2>(p,r,c) = matrix<T2>(p1,r1,c1).name(); break;\
            case 3: matrix<T3>(p,r,c) = matrix<T3>(p1,r1,c1).name(); break;\
        }\
        GUARD_END\
    }

UNOP(inverse);


/**
 * Copyright (c) 2018, University Osnabrück
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University Osnabrück nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL University Osnabrück BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * EigenSVDPointAlign.cpp
 *
 *  @date Feb 21, 2014
 *  @author Thomas Wiemann
 */

#include <Eigen/SVD>

using namespace Eigen;

namespace lvr2
{

template<typename T, typename PointT>
T EigenSVDPointAlign<T, PointT>::alignPoints(
    SLAMScanPtr scan,
    Point3** neighbors,
    const Vec3& centroid_m,
    const Vec3& centroid_d,
    Mat4& align) const
{
    T error = 0.0;
    size_t pairs = 0;

    // Fill H matrix
    Mat3 H = Matrix3d::Zero();

    for (size_t i = 0; i < scan->numPoints(); i++)
    {
        if (neighbors[i] == nullptr)
        {
            continue;
        }

        Vec3 m = neighbors[i]->template cast<T>() - centroid_m;
        Vec3 d = scan->point(i).template cast<T>() - centroid_d;

        error += (m - d).squaredNorm();
        pairs++;

        // same as "H += m * d.transpose();" but faster
        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                H(j, k) += d[j] * m[k];
            }
        }
    }

    error = sqrt(error / (T)pairs);

    JacobiSVD<Mat3> svd(H, ComputeFullU | ComputeFullV);

    Mat3 U = svd.matrixU();
    Mat3 V = svd.matrixV();

    Mat3 R = V * U.transpose();

    align = Mat4::Identity();
    align.template block<3, 3>(0, 0) = R;

    // Calculate translation
    Vec3 translation = centroid_m - R * centroid_d;
    align.template block<3, 1>(0, 3) = translation;

    return error;
}

template<typename T, typename PointT>
T EigenSVDPointAlign<T, PointT>::alignPoints(
    PointPairVector& pairs,
    const Vec3& centroid_m,
    const Vec3& centroid_d,
    Mat4& align) const
{
    T error = 0.0;
    size_t n = pairs.size();

    // Fill H matrix
    Mat3 H = Mat3::Zero();

    for (size_t i = 0; i < n; i++)
    {
        Vec3 m = pairs[i].first.template cast<T>() - centroid_m;
        Vec3 d = pairs[i].second.template cast<T>() - centroid_d;

        error += (m - d).squaredNorm();

        // same as "H += m * d.transpose();" but faster
        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                H(j, k) += d[j] * m[k];
            }
        }
    }

    error = sqrt(error / (T)n);

    JacobiSVD<Mat3> svd(H, ComputeFullU | ComputeFullV);

    Mat3 U = svd.matrixU();
    Mat3 V = svd.matrixV();

    Mat3 R = V * U.transpose();

    align = Mat4::Identity();
    align.template block<3, 3>(0, 0) = R;

    // Calculate translation
    Vec3 translation = centroid_m - R * centroid_d;
    align.template block<3, 1>(0, 3) = translation;

    return error;
}

} // namespace lvr2


#include <boost/algorithm/string.hpp>
#include <boost/format.hpp>
#include <algorithm>
#include <cmath>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
#include <array>

#include "train_features.h"

using namespace std;

double expit(double x) {
    if (x > 0) {
        return 1. / (1. + exp(-x));
    } else {
        return 1. - 1. / (1. + exp(x));
    }
}

double log1exp(double x) {
    if (x > 0) {
        return x + log1p(exp(-x));
    } else {
        return log1p(exp(x));
    }
}

void train_with_features(string data_file_path,
                         string features_file_path,
                         string noise_file_path,
                         int n_steps, double eta_0, double iter_power,
                         size_t l_dimensions,
                         string output_file_path) {
    cout << data_file_path << endl;
    cout << features_file_path << endl;
    cout << noise_file_path << endl;

    // Data loading
    fstream data_file_input;
    data_file_input.open(data_file_path, ios::in);
    string line;
    vector<string> tokens;
    getline(data_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    size_t data_size = stoul(tokens[0]);
    size_t n_samples = stoul(tokens[1]);

    vector<size_t> start_indexes(n_samples);
    vector<size_t> permutation(n_samples);
    vector<int> data(data_size + n_samples);
    long n_data = 0;
    long n_noise = 0;
    for (size_t i = 0, data_index = 0; i < n_samples; ++i) {
        getline(data_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        start_indexes[i] = data_index;
        permutation[i] = i;
        for (size_t j = 0; j < tokens.size(); ++j) {
            data[data_index + j] = stoi(tokens[j]);
        }
        if (data[data_index] == 0) {
            ++n_noise;
        } else {
            ++n_data;
        }
        data[data_index + tokens.size()] = -1;
        data_index += tokens.size() + 1;
    }

    fstream features_file_input;
    features_file_input.open(features_file_path, ios::in);
    getline(features_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    size_t n_items = stoul(tokens[0]);
    size_t m_features = stoul(tokens[1]);

    auto index_features = [m_features](size_t i, size_t j) -> size_t {
        return (i * m_features) + j;
    };
    vector<double> features(n_items * m_features);
    for (size_t i = 0; i < n_items; ++i) {
        getline(features_file_input, line);
        boost::split(tokens, line, boost::is_any_of(","));
        for (size_t j = 0; j < m_features; ++j) {
            features[index_features(i, j)] = stod(tokens[j]);
        }
    }

    fstream noise_file_input;
    noise_file_input.open(noise_file_path, ios::in);
    getline(noise_file_input, line);
    boost::split(tokens, line, boost::is_any_of(","));
    vector<double> noise_weights(m_features);
    for (size_t i = 0; i < m_features; ++i) {
        noise_weights[i] = stod(tokens[i]);
    }
    vector<double> noise_utilities(n_items);
    for (size_t i = 0; i < n_items; ++i) {
        noise_utilities[i] = 0;
        for (size_t j = 0; j < m_features; ++j) {
            noise_utilities[i] += features[index_features(i, j)] * noise_weights[j];
        }
    }

    // Calculate constant quantities.
    double log_nu = log(n_noise / n_data);
    double logz_noise = 0;
    for (size_t i = 0; i < n_items; ++i) {
        logz_noise += log1exp(noise_utilities[i]);
    }
    cout << "Log nu: " << log_nu << endl;
    cout << "Log noise: " << logz_noise << endl;

    // Initialize parameters.
    auto random_engine = default_random_engine{};
    random_engine.seed(10000000);
    uniform_real_distribution<double_t> udouble_dist(0, 1e-3);
    vector<double> b_weights(m_features * l_dimensions);
    vector<double> a_weights(m_features);
    double n_logz = 0;

    auto index_b_weights = [l_dimensions](size_t i, size_t j) -> size_t {
        return (i * l_dimensions) + j;
    };

    for (size_t i = 0; i < m_features; ++i) {
        a_weights[i] = noise_weights[i];
    }
    for (size_t i = 0; i < m_features; ++i) {
        for (size_t j = 0; j < l_dimensions; ++j) {
            b_weights[index_b_weights(i, j)] = udouble_dist(random_engine);
        }
    }
    for (size_t i = 0; i < n_items; ++i) {
        n_logz -= log1exp(noise_utilities[i]);
    }

    vector<double> a_gradient(m_features);
    for (size_t iter = 0; iter < n_steps; ++iter) {
        shuffle(begin(permutation), end(permutation), random_engine);
        for (size_t sub_iter = 0; sub_iter < n_samples; ++sub_iter) {
            size_t start_idx = start_indexes[permutation[sub_iter]];
            size_t end_idx;
            if (permutation[sub_iter] == n_samples - 1) {
                end_idx = data_size + n_samples - 1;
            } else {
                end_idx = start_indexes[permutation[sub_iter] + 1] - 1;
            }
            int label = data[start_idx];
            double p_model = n_logz;
            double p_noise = -logz_noise;
            for (size_t j = 0; j < m_features; ++j) {
                a_gradient[j] = 0;
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    a_gradient[j] += features[index_features(data[i], j)];
                    p_model += features[index_features(data[i], j)] *
                            a_weights[j];
                    p_noise += features[index_features(data[i], j)] *
                            noise_weights[j];
                }
            }

            vector<double> max_weights(l_dimensions);
            vector<int> max_weight_indexes(l_dimensions);
            for (size_t j = 0; j < l_dimensions; ++j) {
                max_weights[j] = -1;
                max_weight_indexes[j] = -1;
                for (size_t i = start_idx + 1; i < end_idx; ++i) {
                    double weight = 0;
                    for (size_t k = 0; k < m_features; ++k) {
                        weight += features[index_features(data[i], k)] *
                                b_weights[index_b_weights(k, j)];
                    }
                    if (weight > max_weights[j]) {
                        max_weights[j] = weight;
                        max_weight_indexes[j] = data[i];
                    }
                    p_model -= weight;
                }
                p_model += max_weights[j];
            }

            double learning_rate = eta_0 *
                    pow((iter * n_samples) + sub_iter + 1, -iter_power);
            double factor = learning_rate *
                    (label - expit(p_model - p_noise - log_nu));

            for (size_t i = 0; i < m_features; ++i) {
                a_weights[i] += factor * a_gradient[i];
                for (size_t j = 0; j < l_dimensions; ++j) {
                    b_weights[index_b_weights(i, j)] += factor *
                        (features[index_features(max_weight_indexes[j], i)] -
                                a_gradient[i]);
                    if (b_weights[index_b_weights(i, j)] <= 0) {
                        b_weights[index_b_weights(i, j)] =
                                udouble_dist(random_engine);
                    }
                }
            }
            n_logz += factor;
        }
    }

    fstream output_file(output_file_path, ios::out);

    cout << n_logz << endl;
    output_file << n_logz << endl;

    for (size_t i = 0; i < m_features - 1; ++i) {
        cout << a_weights[i] << ",";
        output_file << a_weights[i] << ",";
    }
    cout << a_weights[m_features - 1] << endl;
    output_file << a_weights[m_features - 1] << endl;

    for (size_t i = 0; i < m_features; ++i) {
        for (size_t j = 0; j < l_dimensions - 1; ++j) {
            cout << b_weights[index_b_weights(i, j)] << ",";
            output_file << b_weights[index_b_weights(i, j)] << ",";
        }
        output_file << b_weights[index_b_weights(i, l_dimensions - 1)] << endl;
        cout << endl;
    }

}

int main(int argc, char* argv[]) {
    int fold_number = stoi(argv[1]);
    int dim_number = stoi(argv[2]);
    int feature_set = stoi(argv[3]);
    for (int d = 1; d <= dim_number; ++d) {
        for (int i = 1; i <= fold_number; ++i) {
            train_with_features(
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/path_set_nce_data_features_%1%_fold_%2%.csv") % feature_set % i).str(),
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/path_set_nce_features_%1%.csv") % feature_set).str(),
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/path_set_nce_noise_features_%1%_fold_%2%.csv") % feature_set % i).str(),
                    10, 0.01, 0.1, d,
                    (boost::format("/home/diegob/workspace/master-thesis-2015/data/models/path_set_nce_out_features_%1%_dim_%2%_fold_%3%.csv") % feature_set % d % i).str());
        }
    }

}


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

#pragma once
#include <cassert>
#include <algorithm>
#include <utility>
#include <complex>
#include <random>
#include <cmath>

#include <Eigen/Dense>
#include "syk_types.hpp"

// https://web.eecs.umich.edu/~rajnrao/Acta05rmt.pdf

namespace syk {
using namespace std::complex_literals;

template<typename rng_type>
MatrixType RandomRealIidGauss(rng_type* rand_gen, int n, int m) {
    MatrixType mat = MatrixType::Zero(n, m);
    std::normal_distribution normal;

    for(int j = 0; j < m; ++j) {
        for(int i = 0; i < n; ++i) {
            mat(i, j) = static_cast<MatrixType::Scalar>(normal(*rand_gen));
        }
    }
    return mat;
}

template<typename rng_type>
MatrixType RandomGOE(rng_type* rand_gen, int n) {
    MatrixType A = RandomRealIidGauss(rand_gen, n, n);
    return (A + A.transpose()) / (2 * std::sqrt(n));
}

template<typename rng_type>
MatrixType RandomGUE(rng_type* rand_gen, int n) {
    MatrixType A = RandomRealIidGauss(rand_gen, n, n) + 1.0i * RandomRealIidGauss(rand_gen, n, n);
    return (A + A.adjoint()) / (2 * std::sqrt(n));
}
}


/**  \file trace_ublas.hpp \brief Trace computation for uBlas */
/*
-----------------------------------------------------------------------------
   Copyright (C) 2011 Ruben Martinez-Cantin <rmcantin@unizar.es>

   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
-----------------------------------------------------------------------------
*/
#ifndef __TRACE_UBLAS_HPP__
#define __TRACE_UBLAS_HPP__

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

namespace bayesopt
{
  namespace utils
  {

    template<class E>
    typename E::value_type trace(const E &A)
    {
      const size_t n = (std::min)(A.size1(),A.size2());
      typename E::value_type sum = 0;
      for (size_t i=0; i<n; ++i)
	sum += A(i,i);

      return sum;
    }

    template<class E>
    typename E::value_type log_trace(const E &A)
    {
      const size_t n = (std::min)(A.size1(),A.size2());
      typename E::value_type sum = 0;
      for (size_t i=0; i<n; ++i)
	sum += std::log(A(i,i));

      return sum;
    }

    template<class E1, class E2>
    typename E1::value_type trace_prod(const E1 & A, const E2 & B )
    {
      namespace ublas = boost::numeric::ublas;

      const size_t n = (std::min)(A.size1(),B.size2());
      typename E1::value_type sum = 0;
      for (size_t i=0; i<n; ++i)
	sum += ublas::inner_prod(ublas::row(A,i),ublas::column(B,i));

      return sum;
    }

  } //  namespace utils
} //namespace bayesopt

#endif


// Copyright Nick Thompson, 2021
// 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)

// This implements bilinear interpolation on a uniform grid.
// If dx and dy are both positive, then the (x,y) = (x0, y0) is associated with data index 0 (herein referred to as f[0])
// The point (x0 + dx, y0) is associated with f[1], and (x0 + i*dx, y0) is associated with f[i],
// i.e., we are assuming traditional C row major order.
// The y coordinate increases *downward*, as is traditional in 2D computer graphics.
// This is *not* how people generally think in numerical analysis (although it *is* how they lay out matrices).
// Providing the capability of a grid rotation is too expensive and not ergonomic; you'll need to perform any rotations at the call level.

// For clarity, the value f(x0 + i*dx, y0 + j*dy) must be stored in the f[j*cols + i] position.

#ifndef BOOST_MATH_INTERPOLATORS_BILINEAR_UNIFORM_HPP
#define BOOST_MATH_INTERPOLATORS_BILINEAR_UNIFORM_HPP

#include <utility>
#include <memory>
#include <boost/math/interpolators/detail/bilinear_uniform_detail.hpp>

namespace boost::math::interpolators {

template <class RandomAccessContainer>
class bilinear_uniform
{
public:
    using Real = typename RandomAccessContainer::value_type;
    using Z = typename RandomAccessContainer::size_type;

    bilinear_uniform(RandomAccessContainer && fieldData, Z rows, Z cols, Real dx = 1, Real dy = 1, Real x0 = 0, Real y0 = 0)
    : m_imp(std::make_shared<detail::bilinear_uniform_imp<RandomAccessContainer>>(std::move(fieldData), rows, cols, dx, dy, x0, y0))
    {
    }

    Real operator()(Real x, Real y) const
    {
        return m_imp->operator()(x,y);
    }

    friend std::ostream& operator<<(std::ostream& out, bilinear_uniform<RandomAccessContainer> const & bu) {
        out << *bu.m_imp;
        return out;
    }

private:
    std::shared_ptr<detail::bilinear_uniform_imp<RandomAccessContainer>> m_imp;
};

}
#endif