#define BOOST_TEST_MODULE SparseLatticeMultTestsSpecial

#include "SparseLattice.h"
#include "MIAConfig.h"

#ifdef MIA_USE_HEADER_ONLY_TESTS
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

//!Relies on normal SparseLattice*SparseLattice operator functioning correctly, i.e., that the SparseLatticeMultTests passed
template<typename data_type>
void multwork(size_t m1, size_t n1, size_t n2, size_t p,double hypersparsity){

    typedef LibMIA::SparseLattice<data_type> sparseType;
    sparseType A,B;
    A=sparseType(m1,n1,p);
    A.resize(std::ceil(p*m1*hypersparsity));
    A.randu(1,10);
    A.rand_indices();
    A.collect_duplicates();

    B=sparseType(n1,n2,p);
    B.resize(std::ceil(p*n2*hypersparsity));
    B.randu(1,10);
    B.rand_indices();
    B.collect_duplicates();

    sparseType C,C_test;

    C=A*B;

    C_test=A.template csc_times<false>(B);
    BOOST_CHECK_MESSAGE(C.fuzzy_equals(C_test,test_precision<data_type>()),std::string("CSC Mult Test Accum for ")+typeid(data_type).name());

    C_test=A.template csc_no_accum<false>(B);
    BOOST_CHECK_MESSAGE(C.fuzzy_equals(C_test,test_precision<data_type>()),std::string("CSC Mult Test No Accum for ")+typeid(data_type).name());

    C_test=A.template csc_times<true>(B);
    BOOST_CHECK_MESSAGE(C.fuzzy_equals(C_test,test_precision<data_type>()),std::string("DCSC Mult Test Accum for ")+typeid(data_type).name());

    C_test=A.template csc_no_accum<true>(B);
    BOOST_CHECK_MESSAGE(C.fuzzy_equals(C_test,test_precision<data_type>()),std::string("DCSC Mult Test No Accum for ")+typeid(data_type).name());

    C_test=A.outer_times(B);
    BOOST_CHECK_MESSAGE(C.fuzzy_equals(C_test,test_precision<data_type>()),std::string("Outer Mult Test for ")+typeid(data_type).name());

}

BOOST_AUTO_TEST_CASE( SparseLatticeMultTestsSpecial )
{

    //multwork<double>(5,5,5,5,1);
    multwork<double>(20,20,20,20,1);
    multwork<float>(20,20,20,20,1);
    multwork<int>(20,20,20,20,1);
    multwork<long>(20,20,20,20,1);

    multwork<double>(20,20,20,20,.5);
    multwork<float>(20,20,20,20,.5);
    multwork<int>(20,20,20,20,.5);
    multwork<long>(20,20,20,20,.5);

    multwork<double>(40,20,20,20,1);
    multwork<float>(40,20,20,20,1);
    multwork<int>(40,20,20,20,1);
    multwork<long>(40,20,20,20,1);

    multwork<double>(20,20,40,20,.5);
    multwork<float>(20,20,40,20,.5);
    multwork<int>(20,20,20,40,.5);
    multwork<long>(20,20,20,40,.5);

}


#include <deal.II/base/function_parser.h>
#include <deal.II/base/point.h>
#include <iostream>
#include <map>
#include <unordered_map>
#include <boost/functional/hash.hpp>

using namespace dealii;

namespace std {
template<>
class hash<dealii::Point<2>>
{
 public:
  std::size_t operator()(const dealii::Point<2>& x) const
  {
    std::size_t current = std::hash<double>()(x[0]);
    boost::hash_combine(current, std::hash<double>()(x[1]));

    return current;
  }

};
}  // std

int main(int argc, char *argv[])
{
  // set up problem:
  std::string variables = "x,y";
  std::string expression = "cos(pi*x)+sqrt(y)";
  std::map<std::string,double> constants;
  constants["pi"] = 3.14159265358979323846264338328;
  // FunctionParser with 2 variables and 1 component:
  FunctionParser<2> fp(1);
  fp.initialize(variables,
                expression,
                constants);
  // Point at which we want to evaluate the function
  Point<2> point(1.0, 4.0);
  // evaluate the expression at 'point':
  double result = fp.value(point);
  std::cout << "result " << result << "\n";
  std::unordered_map<Point<2>, double> huch;

  return 0;
}


#ifndef TRIUMF_BNMR_SLR_CBRT_EXP_HPP
#define TRIUMF_BNMR_SLR_CBRT_EXP_HPP

#include <boost/math/quadrature/tanh_sinh.hpp>
#include <cmath>
#include <triumf/bnmr/slr/common.hpp>

// TRIUMF: Canada's particle accelerator centre
namespace triumf {

// β-detected nuclear magnetic resonance (β-NMR)
namespace bnmr {

// spin-lattice relaxation (SLR)
namespace slr {

/// pulsed cube root exponential integral (from 0 to time_p <= time)
template <typename T = double>
T pulsed_cbrt_exp_integral(T time, T time_p, T nuclear_lifetime, T slr_rate) {
  // make sure that
  assert(time >= time_p);
  // integrand for the numeric integral
  auto integrand = [=](T t_p) {
    return std::exp(-(time - t_p) / nuclear_lifetime) *
           std::exp(-std::cbrt(slr_rate * (time - t_p)));
  };
  // create the integrator for tanh-sinh quadrature
  static boost::math::quadrature::tanh_sinh<T> integrator;
  // evaluate the integral from 0 to time_p
  T Q = integrator.integrate(integrand, 0.0, time_p);
  return Q;
}

/// pulsed cube root exponential
template <typename T = double>
T pulsed_cbrt_exp(T time, T nuclear_lifetime, T pulse_length, T asymmetry,
                  T slr_rate) {
  if (time == 0.0) {
    return asymmetry;
  } else if (time > 0.0 and time <= pulse_length) {
    return asymmetry *
           pulsed_cbrt_exp_integral(time, time, nuclear_lifetime, slr_rate) /
           normalization(time, nuclear_lifetime);
  } else if (time > pulse_length) {
    return (asymmetry *
            pulsed_cbrt_exp_integral(time, pulse_length, nuclear_lifetime,
                                     slr_rate) /
            normalization(pulse_length, nuclear_lifetime)) /
           std::exp(-(time - pulse_length) / nuclear_lifetime);
  } else {
    return 0.0;
  }
}

/// pulsed cube root exponential (ROOT)
template <typename T = double> T pulsed_cbrt_exp(const T *x, const T *par) {
  return pulsed_cbrt_exp<T>(*x, par[0], par[1], par[2], par[3]);
}

} // namespace slr

} // namespace bnmr

} // namespace triumf

#endif // TRIUMF_BNMR_SLR_CBRT_EXP_HPP


#pragma once

#include <cmath>
#include <ros/ros.h>
#include <ct/optcon/optcon.h>
#include <lqr_controller/declarations_quaternion.hpp>
#include <nav_msgs/Odometry.h>
#include <Eigen/Geometry>
#include <mavros/frame_tf.h>
#include <mav_msgs/eigen_mav_msgs.h>
#include <mav_trajectory_generation/polynomial_optimization_linear.h>
//#include <mav_trajectory_generation/polynomial_optimization_nonlinear.h>
#include <mav_trajectory_generation/trajectory.h>
#include <mav_trajectory_generation/trajectory_sampling.h>
#include <mav_trajectory_generation_ros/ros_visualization.h>
#include <visualization_msgs/MarkerArray.h>
#include <ros/package.h>

namespace LQR {
class LQR_Quaternion {
  public:
    /*!
     * Constructor.
     * @param nodeHandle the ROS node handle.
     */
    LQR_Quaternion(ros::NodeHandle& nodeHandle);

    /*!
     * Destructor.
     */
    virtual ~LQR_Quaternion();

    control_vector_t getTrajectoryControl();
    state_vector_t getError();
    ct::core::FeedbackMatrix<nStates, nControls> getGain();
    void setOutput(double output, int j);
    void setOutput(control_vector_t output);
    control_vector_t getOutput();
    state_vector_t getRefStates();

   private:

    /*!
     * ROS topic callback method.
     * @param message the received message.
     */

    void topicCallback(const nav_msgs::Odometry::ConstPtr& msg);
    void setStates(const nav_msgs::Odometry::ConstPtr& msg, state_vector_t& x);
    void setError(const state_vector_t& xref, const state_vector_t& x, state_vector_t& xerror);
    bool setTrajectoryReference(state_vector_t& xref,control_vector_t& uref);
    bool setStaticReference(state_vector_t& xref,control_vector_t& uref, Eigen::Vector4d& flat_states);
    Eigen::Vector3d quaternion_to_rpy_wrap(const Eigen::Quaterniond &q);
    void generateTrajectory(mav_msgs::EigenTrajectoryPoint::Vector& states);

    //! ROS node handle.
    ros::NodeHandle& nodeHandle_;

    //! ROS topic subscriber.
    ros::Subscriber odom_sub_;

    //Marker publisher
    ros::Publisher marker_pub_;

    //! State and control matrix dimensions
    const size_t state_dim = nStates;
    const size_t control_dim = nControls;

    //Trajectory
    double sampling_interval = 0.1;
    const double v_max = 2.0;
    const double a_max = 5.0;
    const int dimension = 3;
    int traj_index;
    bool initiated;
    mav_msgs::EigenTrajectoryPoint::Vector states_;
    visualization_msgs::MarkerArray markers_;

    ros::Time init_time_;
    Eigen::Vector3d position_enu_;
    Eigen::Vector3d velocity_enu_;
    Eigen::Quaterniond q_enu_;
    state_matrix_t A_;
    control_gain_matrix_t B_;
    ct::core::FeedbackMatrix<nStates, nControls> Kold_;
    ct::core::FeedbackMatrix<nStates, nControls> Knew_;
    ros::Time callBack_;
    state_vector_t x_;
    control_vector_t u_;
    state_vector_t xref_;
    control_vector_t uref_;
    state_vector_t xerror_;
    control_vector_t output_;

    ct::optcon::TermQuadratic<nStates, nControls> quadraticCost_;
    ct::optcon::TermQuadratic<nStates, nControls>::state_matrix_t Q_;
    ct::optcon::TermQuadratic<nStates, nControls>::control_matrix_t R_;
    ct::optcon::LQR<nStates, nControls> lqrSolver_;
    //states
    state_matrix_t A_quadrotor(const state_vector_t& x, const control_vector_t& u);
    control_gain_matrix_t B_quadrotor(const state_vector_t& x, const control_vector_t& u);
  };

} /* namespace */


/**
 * @file convolutional_network_test.cpp
 * @author Marcus Edel
 * @author Abhinav Moudgil
 *
 * Tests the convolutional neural network.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>
#include <mlpack/methods/ann/init_rules/gaussian_init.hpp>

#include <ensmallen.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ConvolutionalNetworkTest);

/**
 * Train the vanilla network on a larger dataset.
 */
BOOST_AUTO_TEST_CASE(VanillaNetworkTest)
{
  arma::mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");

  // Normalize each point since these are images.
  arma::uword nPoints = X.n_cols;
  for (arma::uword i = 0; i < nPoints; i++)
  {
    X.col(i) /= norm(X.col(i), 2);
  }

  // Build the target matrix.
  arma::mat Y = arma::zeros<arma::mat>(1, nPoints);
  for (size_t i = 0; i < nPoints; i++)
  {
    if (i < nPoints / 2)
    {
      // Assign label "1" to all samples with digit = 4
      Y(i) = 1;
    }
    else
    {
      // Assign label "2" to all samples with digit = 9
      Y(i) = 2;
    }
  }

  /*
   * Construct a convolutional neural network with a 28x28x1 input layer,
   * 24x24x8 convolution layer, 12x12x8 pooling layer, 8x8x12 convolution layer
   * and a 4x4x12 pooling layer which is fully connected with the output layer.
   * The network structure looks like:
   *
   * Input    Convolution  Pooling      Convolution  Pooling      Output
   * Layer    Layer        Layer        Layer        Layer        Layer
   *
   *          +---+        +---+        +---+        +---+
   *          | +---+      | +---+      | +---+      | +---+
   * +---+    | | +---+    | | +---+    | | +---+    | | +---+    +---+
   * |   |    | | |   |    | | |   |    | | |   |    | | |   |    |   |
   * |   +--> +-+ |   +--> +-+ |   +--> +-+ |   +--> +-+ |   +--> |   |
   * |   |      +-+   |      +-+   |      +-+   |      +-+   |    |   |
   * +---+        +---+        +---+        +---+        +---+    +---+
   */
  // It isn't guaranteed that the network will converge in the specified number
  // of iterations using random weights. If this works 1 of 5 times, I'm fine
  // with that. All I want to know is that the network is able to escape from
  // local minima and to solve the task.
  bool success = false;
  for (size_t trial = 0; trial < 5; ++trial)
  {
    FFN<NegativeLogLikelihood<>, RandomInitialization> model;

    model.Add<Convolution<> >(1, 8, 5, 5, 1, 1, 0, 0, 28, 28);
    model.Add<ReLULayer<> >();
    model.Add<MaxPooling<> >(8, 8, 2, 2);
    model.Add<Convolution<> >(8, 12, 2, 2);
    model.Add<ReLULayer<> >();
    model.Add<MaxPooling<> >(2, 2, 2, 2);
    model.Add<Linear<> >(192, 20);
    model.Add<ReLULayer<> >();
    model.Add<Linear<> >(20, 10);
    model.Add<ReLULayer<> >();
    model.Add<Linear<> >(10, 2);
    model.Add<LogSoftMax<> >();

    // Train for only 8 epochs.
    ens::RMSProp opt(0.001, 1, 0.88, 1e-8, 8 * nPoints, -1);

    double objVal = model.Train(X, Y, opt);

    // Test that objective value returned by FFN::Train() is finite.
    BOOST_REQUIRE_EQUAL(std::isfinite(objVal), true);

    arma::mat predictionTemp;
    model.Predict(X, predictionTemp);
    arma::mat prediction = arma::zeros<arma::mat>(1, predictionTemp.n_cols);

    for (size_t i = 0; i < predictionTemp.n_cols; ++i)
    {
      prediction(i) = arma::as_scalar(arma::find(
            arma::max(predictionTemp.col(i)) == predictionTemp.col(i), 1)) + 1;
    }

    size_t correct = arma::accu(prediction == Y);
    double classificationError = 1 - double(correct) / X.n_cols;
    if (classificationError <= 0.25)
    {
      success = true;
      break;
    }
  }

  BOOST_REQUIRE_EQUAL(success, true);
}

BOOST_AUTO_TEST_SUITE_END();


#ifndef CT_ICP_TYPES_HPP
#define CT_ICP_TYPES_HPP

#include <map>
#include <unordered_map>
#include <list>

#include <tsl/robin_map.h>

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <glog/logging.h>

#include "utils.hpp"

#define _USE_MATH_DEFINES

#include <math.h>

namespace ct_icp {

    // A Point3D
    struct Point3D {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        Eigen::Vector3d raw_pt; // Raw point read from the sensor
        Eigen::Vector3d pt; // Corrected point taking into account the motion of the sensor during frame acquisition
        double alpha_timestamp = 0.0; // Relative timestamp in the frame in [0.0, 1.0]
        double timestamp = 0.0; // The absolute timestamp (if applicable)
        int index_frame = -1; // The frame index

        Point3D() = default;
    };

    inline double AngularDistance(const Eigen::Matrix3d &rota,
                                  const Eigen::Matrix3d &rotb) {
        double norm = ((rota * rotb.transpose()).trace() - 1) / 2;
        norm = std::acos(norm) * 180 / M_PI;
        return norm;
    }

    // A Trajectory Frame
    struct TrajectoryFrame {
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        bool success = true;
        double begin_timestamp = 0.0;
        double end_timestamp = 1.0;
        Eigen::Matrix3d begin_R;
        Eigen::Vector3d begin_t;
        Eigen::Matrix3d end_R;
        Eigen::Vector3d end_t;

        inline double EgoAngularDistance() const {
            return AngularDistance(begin_R, end_R);
        }

        double TranslationDistance(const TrajectoryFrame &other) {
            return (begin_t - other.begin_t).norm() + (end_t - other.end_t).norm();
        }

        double RotationDistance(const TrajectoryFrame &other) {
            return (begin_R * other.begin_R.inverse() - Eigen::Matrix3d::Identity()).norm() +
                   (end_R * other.end_R.inverse() - Eigen::Matrix3d::Identity()).norm();
        }

        TrajectoryFrame() = default;

        [[nodiscard]] inline Eigen::Matrix4d MidPose() const {
            Eigen::Matrix4d mid_pose = Eigen::Matrix4d::Identity();
            auto q_begin = Eigen::Quaterniond(begin_R);
            auto q_end = Eigen::Quaterniond(end_R);
            Eigen::Vector3d t_begin = begin_t;
            Eigen::Vector3d t_end = end_t;
            Eigen::Quaterniond q = q_begin.slerp(0.5, q_end);
            q.normalize();
            mid_pose.block<3, 3>(0, 0) = q.toRotationMatrix();
            mid_pose.block<3, 1>(0, 3) = 0.5 * t_begin + 0.5 * t_end;
            return mid_pose;
        }
    };

    // Voxel
    // Note: Coordinates range is in [-32 768, 32 767]
    struct Voxel {

        Voxel() = default;

        Voxel(short x, short y, short z) : x(x), y(y), z(z) {}

        bool operator==(const Voxel &vox) const { return x == vox.x && y == vox.y && z == vox.z; }

        inline bool operator<(const Voxel &vox) const {
            return x < vox.x || (x == vox.x && y < vox.y) || (x == vox.x && y == vox.y && z < vox.z);
        }

        inline static Voxel Coordinates(const Eigen::Vector3d &point, double voxel_size) {
            return {short(point.x() / voxel_size),
                    short(point.y() / voxel_size),
                    short(point.z() / voxel_size)};
        }

        short x;
        short y;
        short z;
    };

    typedef std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d>> ArrayVector3d;
    typedef std::vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d>> ArrayMatrix4d;
    typedef ArrayMatrix4d ArrayPoses;

    struct VoxelBlock {

        explicit VoxelBlock(int num_points = 20) : num_points_(num_points) { points.reserve(num_points); }

        ArrayVector3d points;

        bool IsFull() const { return num_points_ == points.size(); }

        void AddPoint(const Eigen::Vector3d &point) {
            CHECK(num_points_ >= points.size()) << "Voxel Is Full";
            points.push_back(point);
        }

        inline int NumPoints() const { return points.size(); }

        inline int Capacity() { return num_points_; }

    private:
        int num_points_;
    };

    typedef tsl::robin_map<Voxel, VoxelBlock> VoxelHashMap;

} // namespace Elastic_ICP

// Specialization of std::hash for our custom type Voxel
namespace std {

    template<>
    struct hash<ct_icp::Voxel> {
        std::size_t operator()(const ct_icp::Voxel &vox) const {
#ifdef CT_ICP_IS_WINDOWS
            const std::hash<int32_t> hasher;
            return ((hasher(vox.x) ^ (hasher(vox.y) << 1)) >> 1) ^ (hasher(vox.z) << 1) >> 1;
#else
            const size_t kP1 = 73856093;
            const size_t kP2 = 19349669;
            const size_t kP3 = 83492791;
            return vox.x * kP1 + vox.y * kP2 + vox.z * kP3;
#endif
        }
    };
}

#endif //CT_ICP_TYPES_HPP


#include <iostream>
#include <algorithm>
#include <iterator>
#include <cmath>
#include <fstream>
#include <valarray>
#include <sstream>
#include <filesystem>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>

#include "miMaS/field.h"
#include "miMaS/weno.h"
#include "miMaS/fft.h"
#include "miMaS/array_view.h"
#include "miMaS/poisson.h"
#include "miMaS/rk.h"
#include "miMaS/config.h"
#include "miMaS/signal_handler.h"

namespace o2 {
  template < typename _T , std::size_t NumDimsV >
  auto
  trp_v ( field<_T,NumDimsV> const & u , ublas::vector<_T> const& E )
  {
    field<_T,NumDimsV> trp(tools::array_view<const std::size_t>(u.shape(),NumDimsV+1));

    { auto k=0, km1=trp.size(0)-1;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[km1][i])/(2.*u.step.dv) );
      }
    }
    for ( auto k=1 ; k<trp.size(0)-1 ; ++k ) {
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[k+1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }
    { auto k=trp.size(0)-1, kp1=0;
      for ( auto i=0 ; i<trp.size(1) ; ++i ) {
        trp[k][i] = ( E(i)*(u[kp1][i]-u[k-1][i])/(2.*u.step.dv) );
      }
    }

    return trp;
  }
}

namespace math = boost::math::constants;
const std::complex<double> & I = std::complex<double>(0.,1.);

#define SQ(X) ((X)*(X))
#define Xi(i) (i*f.step.dx+f.range.x_min)
#define Vk(k) (k*f.step.dv+f.range.v_min)

auto
maxwellian ( double rho , double u , double T ) {
  return [=](double x,double v){ return rho/(std::sqrt(2.*math::pi<double>()*T))*std::exp( -0.5*SQ(v-u)/T ); };
}

int
main(int argc, char const *argv[])
{
  std::filesystem::path p("config.init");
  if ( argc > 1 ) {
    p = argv[1];
  }
  auto c = config(p);
  std::cout << " " << c.output_dir << std::endl;
  c.name = "";

  c.create_output_directory();
  std::ofstream ofconfig( c.output_dir / "config.init" );
  ofconfig << c << "\n";
  ofconfig.close();

/* --------------------------------------------------------------- */
  field<double,1> f(boost::extents[c.Nv][c.Nx]);
  double Kx = 0.5;

  f.range.v_min = -8.; f.range.v_max = 8.;
  //f.range.x_min =  0.; f.range.x_max = 2./Kx*math::pi<double>();
  f.range.x_min = 0.; f.range.x_max = 20.*math::pi<double>();
  f.compute_steps();

  ublas::vector<double> v (c.Nv,0.);
  for ( std::size_t k=0 ; k<c.Nv ; ++k ) { v[k] = Vk(k); }

  ublas::vector<double> kx(c.Nx);
  {
    double l = f.range.len_x();
    for ( auto i=0 ; i<c.Nx/2 ; ++i ) { kx[i]      = 2.*math::pi<double>()*i/l; }
    for ( int i=-c.Nx/2 ; i<0 ; ++i ) { kx[c.Nx+i] = 2.*math::pi<double>()*i/l; }
  }

  //double alpha = 0.2 , ui = 2.0;
  auto landau_M = maxwellian( 1.        ,  0.   , 1.   );
  auto db_M1    = maxwellian( 0.5       , -c.ui , 1.   ) , db_M2  = maxwellian( 0.5       ,  c.ui , 1.  );
  //auto bot_M1   = maxwellian( 1.-alpha  ,  0. , 1.   ) , bot_M2 = maxwellian( alpha     ,  ui , 0.25);
  auto bot_M1   = maxwellian(0.9,0.,1.) , bot_M2 = maxwellian(0.2,4.5,0.25);
  auto tb_MC    = maxwellian( 1.-c.alpha  ,  0. , c.Tc ) ,
       tb_M1    = maxwellian( 0.5*c.alpha ,  c.ui , 1.   ) , tb_M2  = maxwellian( 0.5*c.alpha , -c.ui , 1.  );
  auto v10_MC   = maxwellian( 1.-c.alpha  ,  0. , c.Tc ) , v10_Mh = maxwellian( c.alpha     ,  0. , 1.  );

  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      //f[k][i] = ((1.-alpha)*M0c(Xi(i),Vk(k)) + 0.5*alpha*( Mpu(Xi(i),Vk(k)) + Mmu(Xi(i),Vk(k)) ))*(1.+0.01*std::cos(0.5*Xi(i)));

      //// landau damping : Kx=0.5
      //f[k][i] = landau_M(Xi(i),Vk(k))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// strong landau damping : Kx=0.5
      //f[k][i] = landau_M(Xi(i),Vk(k))*(1.+0.6*std::cos(Kx*Xi(i)));
      //// double beam Kx=0.2, ui=2.4 ou Kx=0.2, ui=4.5
      //f[k][i] = (db_M1(Xi(i),Vk(k))+db_M2(Xi(i),Vk(k)))*(1.+0.001*std::cos(Kx*Xi(i)));
      //// bot Kx=0.5 , alpha=0.2 , ui=4.5
      //f[k][i] = (bot_M1(Xi(i),Vk(k)) + bot_M2(Xi(i),Vk(k)))*(1.+0.04*std::cos(0.3*Xi(i)));
      //// tb Kx=0.5 , ui=4. , alpha=0.2 , Tc=0.01
      f[k][i] = tb_MC(Xi(i),Vk(k)) + (tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
      //// v10 Kx=0.5 , alpha=0.2
      //f[k][i] = v10_MC(Xi(i),Vk(k)) + ( std::pow(Vk(k),10)*v10_Mh(Xi(i),Vk(k))/945. )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
  }
  f.write( c.output_dir / "init.dat" );

  unsigned int i_t = 0;
  double current_time = 0.;
  double dt =  0.1*f.step.dv/0.6;
  //double dt = 1.433*f.step.dv;

  std::vector<double> ee;   ee.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> Emax; Emax.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> H;    H.reserve(int(std::ceil(c.Tf/dt))+1);
  std::vector<double> Ec;   Ec.reserve(int(std::ceil(c.Tf/dt))+1);

  std::vector<double> times; times.reserve(int(std::ceil(c.Tf/dt))+1);

  // lambda to save data, any time you want, where you want
  auto save_data = [&] ( std::string && suffix ) -> void {
    suffix = c.name + suffix;

    // save temporel data
    auto dt_y = [&,count=0] (auto const& y) mutable {
      std::stringstream ss; ss<<times[count++]<<" "<<y;
      return ss.str();
    };
    c << monitoring::data( "ee_"+suffix+".dat"   , ee   , dt_y );
    c << monitoring::data( "Emax_"+suffix+".dat" , Emax , dt_y );
    c << monitoring::data( "H_"+suffix+".dat"    , H    , dt_y );

    // save distribution function
    f.write( c.output_dir / ("vp_"+suffix+".dat") );
  };

  // to stop simulation at any time (and save data)
  signal_handler::signal_handler<SIGINT,SIGILL>::handler( [&]( int signal ) -> void {
    std::cerr << "\n\033[41;97m ** End of execution after signal " << signal << " ** \033[0m\n";
    std::cerr << "\033[38;5;202msave data...\033[0m\n";
    save_data("_SIGINT");
  });

  // space scheme
  auto wenol = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return wenolin::trp_v(f,E); };
  auto weno  = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return weno::trp_v(f,E); };
  auto cd2   = [&](field<double,1>const& f , ublas::vector<double> const& E )->field<double,1> { return o2::trp_v(f,E); };

  // time scheme init
  //expRK::HochbruckOstermann<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,cd2);
  //expRK::Krogstad<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,cd2);
  //lawson::RK33<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,weno);
  lawson::RK44<poisson<double>> rk(c.Nx,c.Nv,f.range.len_x(),f.shape(),v,kx,weno);

  rk.E = rk.poisson_solver(f.density());
  {
    Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    double electric_energy = 0.;
    for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
    H.push_back( energy(f,rk.E) );
    Ec.push_back( kinetic_energy(f) );
    times.push_back( 0. );
  }

  while (  current_time < c.Tf ) {
    std::cout<<" ["<<std::setw(5)<<i_t<<"] "<< current_time <<"\r"<<std::flush;

    f = rk(f,dt);

    // end of time loop

    // MONITORING
    rk.E = rk.poisson_solver(f.density());
    Emax.push_back( std::abs(*std::max_element( rk.E.begin() , rk.E.end() , [](double a,double b){return (std::abs(a) < std::abs(b));} )) );
    double electric_energy = 0.;
    for ( const auto & ei : rk.E ) { electric_energy += ei*ei*f.step.dx; }
    ee.push_back( std::sqrt(electric_energy) );
    H.push_back( energy(f,rk.E) );
    Ec.push_back( kinetic_energy(f) );

    //dt = std::min( 0.1 , SIGMA*f.step.dv/Emax[i_t] );

    // increment time
    ++i_t;
    current_time += dt;
    times.push_back( current_time );
  } // while (  i_t*dt < Tf )
  std::cout<<" ["<<std::setw(5)<<i_t<<"] "<<i_t*dt <<std::endl;

  //f.write( c.output_dir / "vp.dat" );

  save_data("");
/*
  of.open( c.output_dir / "ee.dat" );
  std::transform( ee.begin() , ee.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "Emax.dat" );
  std::transform( Emax.begin() , Emax.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "H.dat" );
  std::transform( H.begin() , H.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();

  of.open( c.output_dir / "Ec.dat" );
  std::transform( Ec.begin() , Ec.end() , std::ostream_iterator<std::string>(of,"\n") , dt_y );
  of.close();
*/
  std::ofstream of;
  auto dx_y = [&,count=0](auto const& y) mutable { std::stringstream ss; ss<< f.step.dx*(count++) <<" "<<y; return ss.str(); };

  of.open( c.output_dir / "E.dat" );
  std::transform( rk.E.begin() , rk.E.end() , std::ostream_iterator<std::string>(of,"\n") , dx_y );
  of.close();

  auto rho = f.density();
  of.open( c.output_dir / "rho.dat" );
  std::transform( rho.begin() , rho.end() , std::ostream_iterator<std::string>(of,"\n") , dx_y );
  of.close();

  auto J = f.courant();
  of.open( c.output_dir / "J.dat" );
  std::transform( J.begin() , J.end() , std::ostream_iterator<std::string>(of,"\n") , dx_y );
  of.close();

  of.open( c.output_dir / "energy.dat" );
  for ( auto i=0 ; i<times.size() ; ++i ) {
    of << times[i] << " " << ee[i] << " " << Ec[i] << " " << H[i] << " " << ee[i] + Ec[i] << "\n";
  }
  of.close();

  for (field<double,2>::size_type k=0 ; k<f.size(0) ; ++k ) {
    for (field<double,2>::size_type i=0 ; i<f.size(1) ; ++i ) {
      f[k][i] -= tb_MC(Xi(i),Vk(k)) + (tb_M1(Xi(i),Vk(k)) + tb_M2(Xi(i),Vk(k)) )*(1. + 0.01*std::cos(Kx*Xi(i)));
    }
  }
  f.write( c.output_dir / "diff.dat" );

  return 0;
}


#include "Math/gfp.h"
#include "Math/gf2n.h"
#include "Networking/sockets.h"
#include "Tools/int.h"
#include "Math/Setup.h"
#include "Auth/fake-stuff.h"
#include "Eigen/Dense"
#include "json/json.hpp"
#include<gmp.h>
#include <NTL/ZZ.h>
#include <NTL/ZZ_pXFactoring.h>

#include <sodium.h>
#include <iostream>
#include <cstdio>
#include <sstream>
#include <fstream>
#include <cstdlib>

using namespace NTL;

// Add the private input value to triple[0] and send to each spdz engine.
void send_private_inputs(vector<gfp>& values, vector<int>& sockets, int nparties)
{
    int num_inputs = values.size();
    octetStream os;
    vector< vector<gfp> > triples(num_inputs, vector<gfp>(3));
    vector<gfp> triple_shares(3);

    // Receive num_inputs triples from SPDZ
    for (int j = 0; j < nparties; j++)
    {
        os.reset_write_head();
        os.Receive(sockets[j]);

        for (int j = 0; j < num_inputs; j++)
        {
            for (int k = 0; k < 3; k++)
            {
                triple_shares[k].unpack(os);
                triples[j][k] += triple_shares[k];
            }
        }
    }

    // Check triple relations (is a party cheating?)
    for (int i = 0; i < num_inputs; i++)
    {
        if (triples[i][0] * triples[i][1] != triples[i][2])
        {
            cout << triples[i][0] << " , " << triples[i][1] << " , " << triples[i][2] << endl;
            cerr << "Incorrect triple at " << i << ", aborting\n";
            exit(1);
        }
    }

    os.reset_write_head();
    // Send inputs + triple[0], so SPDZ can compute shares of each value
    for (int i = 0; i < num_inputs; i++)
    {
        gfp y = values[i] + triples[i][0];
        y.pack(os);
        cout << y << " , ";
    }
    for (int j = 0; j < nparties; j++)
        os.Send(sockets[j]);
}

// Assumes that Scripts/setup-online.sh has been run to compute prime
void initialise_fields(const string& dir_prefix)
{
  int lg2;
  bigint p;

  string filename = dir_prefix + "Params-Data";
  cout << "loading params from: " << filename << endl;

  ifstream inpf(filename.c_str());
  if (inpf.fail()) { throw file_error(filename.c_str()); }
  inpf >> p;
  inpf >> lg2;
  inpf.close();

  gfp::init_field(p);
  gf2n::init_field(lg2);
}

// Based off of
vector<gfp> keygen(int primeLength) {
    ZZ p, q;
    GenPrimePair(p, q, primeLength);
    ZZ n = p * q;
    ZZ g = n + 1;
    ZZ phi = (p - 1) * (q - 1);
    // LCM(p, q) = p * q / GCD(p, q);
   	// ZZ lambda = phi / GCD(p - 1, q - 1);
   	ZZ lambda = phi;
    ZZ mu = InvMod(lambda, modulus);

    vector<gfp> keys(4);
    keys[0] = n;
    keys[1] = g;
    keys[2] = lambda;
    keys[3] = mu;
    return keys;
}

/*
gfp receive_one_result(vector<int>& sockets, int nparties)
{
    vector<gfp> output_values(3);
    octetStream os;
    for (int i = 0; i < nparties; i++)
    {
        os.reset_write_head();
        os.Receive(sockets[i]);
        for (unsigned int j = 0; j < 3; j++)
        {
            gfp value;
            value.unpack(os);
            output_values[j] += value;
        }
    }

    if (output_values[0] * output_values[1] != output_values[2])
    {
        cerr << "Unable to authenticate output value as correct, aborting." << endl;
        exit(1);
    }
    return output_values[0];
}

vector<double> receive_result(vector<int>& sockets, int nparties, int NUM_COLUMNS)
{
    cout << "Receiving matrix" << endl;
    vector<double> output_values(NUM_COLUMNS);
    octetStream os;
    for (int i = 0; i < NUM_COLUMNS; i++)
    {
        //gfp gfp_val;
        gfp gfp_val = receive_one_result(sockets, nparties);

        const gfp gfp_regular = gfp_val;
        bigint val;
        to_bigint(val, gfp_regular);

        bigint val_negate;
        gfp_val.negate();
        to_bigint(val_negate, gfp_val);

        double converted_double = mpz_get_d(val.get_mpz_t()) / pow(2, 20);
        double converted_double_negate = -1 * mpz_get_d(val_negate.get_mpz_t()) / pow(2, 20);
        cout << "Converted double " << converted_double << endl;
        cout << "Converted double negative " << converted_double_negate << endl;
        if (abs(converted_double) < 10) {
            output_values[i] = converted_double;
        } else {
            output_values[i] = converted_double_negate;
        }
        //cout << "received " << output_values[i] << endl;
    }

    return output_values;
}
*/

void func(int argc, char** argv) {
    cout << argc;
    cout << argv;
    return;
}

int main(int argc, char** argv) {

    //srand(time(NULL));
    clock_t start;
    double duration;
    int port_base = 14000;
    //int nparties = 2;

    //Shift all numbers over by 20 bits
    int numShift = 20;

    //string host_names[] = {"ec2-52-39-162-238.us-west-2.compute.amazonaws.com", "ec2-34-223-215-198.us-west-2.compute.amazonaws.com", "ec2-23-20-124-131.compute-1.amazonaws.com", "ec2-52-73-142-253.compute-1.amazonaws.com"};

	string host_names[] = {"localhost", "localhost", "localhost", "localhost"};
    if (argc < 1) {
        cout << "Please provide client id" << endl;
        exit(0);
    }

    int nparties = atoi(argv[2]);
    // Default prime length to 128 bits
    int prime_length = 128;
    // Init
    string prep_data_prefix = get_prep_dir(nparties, 128, 128);
    initialise_fields(prep_data_prefix);

    vector<int> sockets(nparties);

    for (int i = 0; i < nparties; i++)
    {
        set_up_client_socket(sockets[i], host_names[i].c_str(), port_base + i);
    }
    cout << "Finish setup socket connections to SPDZ engines." << endl;

    start = clock();

    vector<gfp> keys = keygen(prime_length);
    send_private_inputs(values, sockets, nparties);
    cout << "Sent private inputs to each SPDZ engine, waiting for result..." << endl;

    // Get the result back (client_id of winning client)
    //vector<double> result = receive_result(sockets, nparties, cols);

    duration = (clock() - start ) / (double) CLOCKS_PER_SEC;
    printf(" Took %f seconds for Paillier Keygen", duration);

    for (int i = 0; i < nparties; i++) {
        close_client_socket(sockets[i]);
    }

    func(argc, argv);
}


/*!
* \file GrapheneFloquet.cpp
*
*
* \author Author: D. Gagnon <denisg6@hotmail.com>
*/

// Include some headers
#include <iostream>
#include <fstream>
#include <armadillo>
#include <cmath>
#include <complex>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>

#include "GrapheneFloquet.hpp"
#include "Utils.hpp"

/// Main function
int main(int argc, char *argv[])
{

    // Parse parameter file
    boost::property_tree::ptree pt;
    boost::property_tree::ini_parser::read_ini("GrapheneFloquet.ini", pt);

    // Problem parameters
    double freq  =          std::stof(pt.get<std::string>("Parameters.frequency"));  // Angular frequency in Hz
    double E0 =             std::stof(pt.get<std::string>("Parameters.E0"));         // Peak electric field in V/m
    size_t nblocks =  std::stoi(pt.get<std::string>("Parameters.nblocks"));    // Envelope frequency, in units of freq

    // Parameter sweep
    double xmin =   std::stof(pt.get<std::string>("Sweep.xmin")); // Minimum frequency in sweep
    double xmax =   std::stof(pt.get<std::string>("Sweep.xmax")); // Minimum frequency in sweep
    size_t x_elem =    std::stoi(pt.get<std::string>("Sweep.xnum"));

    // Parameter sweep
    double ymin =   std::stof(pt.get<std::string>("Sweep.ymin")); // Minimum frequency in sweep
    double ymax =   std::stof(pt.get<std::string>("Sweep.ymax")); // Minimum frequency in sweep
    size_t y_elem =    std::stoi(pt.get<std::string>("Sweep.ynum"));

    // Process input variables (angular frequency units)
    double omega = 2.0*M_PI*freq;     // Angular frequency

    // Meshgrid (vectors of parameters)
    auto xvec = arma::linspace(xmin, xmax, x_elem);
    auto yvec = arma::linspace(ymin, ymax, y_elem);

    // Prepare output file
    // std::ofstream outfile;
    // outfile.open("quasienergies.dat");

    // Prepare output file for transition probabilities
    std::ofstream outfile2;
    // outfile2.open("kmap.dat");

    // Variables to store probability values
    auto probvec = arma::mat(y_elem,x_elem);
    auto probvec_sigma = arma::mat(y_elem,x_elem);

    // Variables to store quasienergy values
    auto quasi0 = arma::mat(y_elem,x_elem);
    auto quasi1 = arma::mat(y_elem,x_elem);
    auto quasi2 = arma::mat(y_elem,x_elem);

    // Variables for loops
    size_t id, id2;
    double prob = 0.0;
    double prob_sigma = 0.0; // Trans. prob. between sigma_z eigenstates

    // Loop for each K value and compute probability
    # pragma omp parallel for default(shared) private (id, id2, prob, prob_sigma)
    for (id=0; id < y_elem; id++)
    {
        // Initialize probabilities (will be passed by reference)
        for (id2=0; id2 < x_elem; id2++)
        {
            // Compute energies and transition probability
            auto Energies = QuasiEnergies(xvec[id2],yvec[id],omega,E0,nblocks, prob, prob_sigma);

            // Store in arrays
            probvec(id,id2) = prob;
            probvec_sigma(id,id2) = prob_sigma;

            quasi0(id,id2) = Energies[Energies.n_elem/2 - 1]; // Quasi-energ.
            quasi1(id,id2) = Energies[Energies.n_elem/2];
            quasi2(id,id2) = Energies[Energies.n_elem/2 + 1];
        }

    }

    // Save Floquet Data
    probvec.save("probability.dat", arma::raw_ascii);

    // Save parameters vector
    xvec.save("xvec.dat", arma::raw_ascii);
    yvec.save("yvec.dat", arma::raw_ascii);

    quasi0.save("quasi0.dat", arma::raw_ascii);
    quasi1.save("quasi1.dat", arma::raw_ascii);
    quasi2.save("quasi2.dat", arma::raw_ascii);

    // Save parameters vector
    xvec.save("xvec.dat", arma::raw_ascii);
    yvec.save("yvec.dat", arma::raw_ascii);

    return 0;

    /*
    auto Gamma = GammaValues(2.30,0.0,316227766016837.94,1e10,1);

    for(auto it = Gamma.begin(); it != Gamma.end(); it++)
    {
        std::cout << *it << std::endl;
    }

    double prob = 0.0;
    double prob_sigma = 0.0;
    auto Quasi = QuasiEnergies(2.30,0.0,316227766016837.94,1e10,1, prob, prob_sigma);
    */
}


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_ATANH_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ATANH_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-hyperbolic
    This function object returns the hyperbolic tangent argument \f$\frac12\log\frac{1+x}{1-x}\f$

    @par Header <boost/simd/function/atanh.hpp>

    @see cosh, sinh, acosh, asinh, atanh, asech, acoth, acsch

    @par Example:

      @snippet atanh.cpp atanh

    @par Possible output:

      @snippet atanh.txt atanh

  **/
  IEEEValue atanh(IEEEValue const& x);
} }
#endif

#include <boost/simd/function/scalar/atanh.hpp>
#include <boost/simd/function/simd/atanh.hpp>

#endif


// Copyright (c) 2015-2018, CNRS
// Authors: Justin Carpentier <jcarpent@laas.fr>

#ifndef __multicontact_api_math_nrand_hpp__
#define __multicontact_api_math_nrand_hpp__

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

namespace multicontact_api
{
  namespace math
  {

    template<typename Scalar>
    inline Scalar nrand()
    {
      const Scalar two_pi = 2 * boost::math::constants::pi<Scalar>();
      const Scalar eps = std::numeric_limits<Scalar>::min();
      using std::rand;
      using std::cos;
      using std::sqrt;
      using std::log;

      Scalar u1, u2;
      const Scalar RAND_MAX_INV = (1.0 / RAND_MAX);
      do
      {
        u1 = (Scalar)rand() * RAND_MAX_INV;
        u2 = (Scalar)rand() * RAND_MAX_INV;
      }
      while(u1 <= eps);

      Scalar z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);

      return z0;

    }
  }
}

#endif // ifndef __multicontact_api_math_nrand_hpp__


//
// Created by dominik on 25.06.21.
//

#ifndef SQSGENERATOR_STRUCTURE_UTILS_HPP
#define SQSGENERATOR_STRUCTURE_UTILS_HPP

#include "types.hpp"
#include "utils.hpp"
#include <vector>
#include <limits>
#include <algorithm>
#include <boost/multi_array.hpp>
#include <boost/log/trivial.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>

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

namespace sqsgenerator::utils {

        template<typename T>
        multi_array<T, 3> pbc_shortest_vectors(const matrix<T> &lattice, const matrix<T> &coords, bool frac_coords = false){
            const matrix<T> cart_coords(frac_coords ? prod(coords, lattice): coords);
            auto num_atoms {static_cast<index_t>(coords.size1())};
            auto a {row(lattice, 0)};
            auto b {row(lattice, 1)};
            auto c {row(lattice, 2)};

            std::vector<int> axis {-1, 0, 1};
            multi_array<T, 3> vecs(boost::extents[num_atoms][num_atoms][3]);
            // pi1 = position_index_1
            // pi2 = position_index_2
            for (index_t pi1 = 0; pi1 < num_atoms; pi1++) {
                auto p1 {row(cart_coords, pi1)};
                for (index_t pi2 = pi1+1; pi2 < num_atoms; pi2++) {
                    auto p2 {row(cart_coords, pi2)};
                    T norm {std::numeric_limits<T>::max()};
                    for (auto &i : axis) {
                        for (auto &j: axis) {
                            for (auto &k: axis) {
                                auto t = i * a + j * b + k * c;
                                auto diff = p1 - (t + p2);
                                T image_norm {norm_2(diff)};
                                if (image_norm < norm) {
                                    norm = image_norm;
                                    for (index_t dim = 0; dim < 3; dim++) {
                                        vecs[pi1][pi2][dim] = diff(dim);
                                        vecs[pi2][pi1][dim] = -diff(dim);
                                    }
                                }
                            }
                        }
                    }
                }
            }

            return vecs;
        }

        template<typename T>
        multi_array<T, 3> pbc_shortest_vectors(const std::vector<T> &lattice, const std::vector<T> &coords,  bool frac_coords = false){
            assert(lattice.size() == 9);
            assert(coords.size() % 3 == 0);
            return pbc_shortest_vectors(matrix_from_vector(3, 3, lattice),
                                        matrix_from_vector(coords.size() / 3, 3, coords), frac_coords);
        }

        template<typename MultiArray>
        multi_array<typename MultiArray::element, 2> distance_matrix(const MultiArray &vecs){
            typedef typename MultiArray::index index_t;
            typedef typename MultiArray::element T;
            auto shape(shape_from_multi_array(vecs));
            auto num_atoms {static_cast<index_t>(shape[0])};
            multi_array<T, 2> d2(boost::extents[num_atoms][num_atoms]);
            for (index_t i = 0; i < num_atoms; i++) {
                for (index_t j = i; j < num_atoms; j++) {
                    T norm = std::sqrt(
                            vecs[i][j][0]*vecs[i][j][0] +
                            vecs[i][j][1]*vecs[i][j][1] +
                            vecs[i][j][2]*vecs[i][j][2]);
                    d2[i][j] = norm;
                    d2[j][i] = norm;
                }
            }
            return d2;
        }

        template<typename MultiArray>
        pair_shell_matrix_t shell_matrix(const MultiArray &distance_matrix, const std::vector<typename MultiArray::element> &distances, typename MultiArray::element atol = 1.0e-5, typename MultiArray::element rtol=1.0e-8) {

            typedef typename MultiArray::index index_t;
            typedef typename MultiArray::element T;
            auto shape(shape_from_multi_array(distance_matrix));
            auto num_atoms {static_cast<index_t>(shape[0])};
            pair_shell_matrix_t shells(boost::extents[num_atoms][num_atoms]);
            auto is_close_tol = [&atol, &rtol] (T a, T b) {
                return is_close(a, b, atol, rtol);
            };

            auto find_shell = [&distances, &is_close_tol] (T distance) {
                if (distance < 0 ) return -1;
                if (is_close_tol(distance, 0.0)) return 0;
                else {
                    for (size_t i = 0; i < distances.size() - 1; i++) {
                        T lower_bound {distances[i]}, upper_bound {distances[i+1]};
                        if ((is_close_tol(distance, lower_bound) or distance > lower_bound) and (is_close_tol(distance, upper_bound) or upper_bound > distance)) {
                            return static_cast<int>(i+1);
                        }
                    }
                }
                return static_cast<int>(distances.size());
            };

            for (index_t i = 0; i < num_atoms; i++) {
                for (index_t j = i + 1; j < num_atoms; j++) {
                    int shell {find_shell(distance_matrix[i][j])};
                    if (shell < 0) throw std::runtime_error("A shell was detected which I am not aware of");
                    else if (shell == 0 and i != j) {
                        BOOST_LOG_TRIVIAL(warning) << "Atoms " + std::to_string(i) + " and " + std::to_string(j) + " are overlapping! (distance = " + std::to_string(distance_matrix[i][j])<< ", shell = " << shell <<")!";
                    }
                    shells[i][j] = shell;
                    shells[j][i] = shell;
                }
            }
            return shells;
        }

    template<typename MultiArray>
    std::vector<typename MultiArray::element> default_shell_distances(const MultiArray &distance_matrix, typename MultiArray::element atol = 1.0e-5, typename MultiArray::element rtol=1.0e-8) {
        typedef typename MultiArray::index index_t;
        typedef typename MultiArray::element T;
        auto shape(shape_from_multi_array(distance_matrix));
        auto num_atoms {static_cast<index_t>(shape[0])};
        std::vector<T> all_distances(distance_matrix.data(), distance_matrix.data() + distance_matrix.num_elements());
        std::sort(all_distances.begin(), all_distances.end());
        std::vector<T> shell_dists;

        auto is_close_tol = [=] (T a, T b) {
            return is_close(a, b, atol, rtol);
        };

        std::function<int(T)> get_shell_index  = [&](T distance){
            for (auto i = 0; i < shell_dists.size(); i++)  if (is_close_tol(distance, shell_dists[i])) return i;
            return -1;
        };

        for (const auto& distance : all_distances) {
            auto shell_dist_index {get_shell_index(distance)};
            // We average the distances
            if (shell_dist_index >= 0) shell_dists[shell_dist_index] = 0.5 * (shell_dists[shell_dist_index] + distance);
            else shell_dists.push_back(distance);
            std::sort(shell_dists.begin(), shell_dists.end());
        }

        // make a sanity check -> we check here that each of the computed coordination shell occurs at least once
        pair_shell_matrix_t current_shell_matrix(shell_matrix(distance_matrix, shell_dists, atol, rtol));
        std::set<shell_t> unique_shells(current_shell_matrix.data(), current_shell_matrix.data() + current_shell_matrix.num_elements());
        if (unique_shells.size() < shell_dists.size()) {
            auto message = "The number of of computed (default) shells does not match the occuring shells in the shell matrix (computed: " + std::to_string(shell_dists.size()) + " !=" + " shell_matrix: " + std::to_string(unique_shells.size()) + ")";
            BOOST_LOG_TRIVIAL(warning) << message;
            throw std::runtime_error(message);
        }
        return shell_dists;
    }

    std::map<shell_t, index_t> shell_index_map(const pair_shell_weights_t &weights);
    std::vector<AtomPair> create_pair_list(const pair_shell_matrix_t &shell_matrix, const std::map<shell_t, double> &weights);
    std::tuple<std::vector<shell_t>, std::vector<double>> compute_shell_indices_and_weights(const pair_shell_weights_t &shell_weights);
    array_3d_t compute_prefactors(const_pair_shell_matrix_ref_t shell_matrix, const pair_shell_weights_t &shell_weights, const configuration_t &configuration);
}

#endif //SQSGENERATOR_STRUCTURE_UTILS_HPP


#include <iostream>
#include <cinttypes>
#include <vector>
#include <numeric>
#include <iomanip>

using namespace std;

#include <boost/math/distributions/normal.hpp>

double epsilon = 1e-7;
double epsilon2 = 1e-24;

double solve(double a, int64_t till) {
    double answer(0.5);
    vector<pair<double, double>> vals{make_pair(0.0, 0.5)};
    double remaining((M_PI * M_PI) / 6 - 1);
#if 0
    double prev_answer = 0;
#endif
    for (int64_t i = 2; i <= till; ++i) {
        double component = ((double)1.0) / ((double)i) / i;
        vector<pair<double, double>> new_vals;
        for (auto v: vals) {
            if (v.first + remaining < a) {
                continue;
            }

            if (v.first + component > a) {
                answer += v.second / 2.0;
                new_vals.push_back(make_pair(v.first, v.second / 2.0));
            } else {
                new_vals.push_back(make_pair(v.first, v.second / 2.0));
                new_vals.push_back(make_pair(v.first + component, v.second / 2.0));
            }
        }
        remaining -= component;

        sort(new_vals.begin(), new_vals.end(), greater<pair<double, double>>());
        vals.clear();
        for (auto v: new_vals) {
            if (v.second < epsilon2) {
                continue;
            }
            if (vals.empty() || (vals.back().first - v.first) > epsilon) {
                vals.push_back(v);
            } else {
                vals.back().first = vals.back().first * vals.back().second + v.first * v.second;
                vals.back().second += v.second;
                vals.back().first /= vals.back().second;
            }
        }

        if (vals.empty()) {
            break;
        }
#if 0
        if (answer - prev_answer > 1e-8) {
            cout << i << ": " << answer << ", " << answer - prev_answer << " (" << vals.size() << ")" << endl;
            prev_answer = answer;
        }
#endif
    }

    // Approximate by normal distribution.
    boost::math::normal_distribution<double> nd(remaining / 2.0, remaining / boost::math::constants::pi<double>() / 2);
    if (!vals.empty()) {
        for (auto v: vals) {
            answer += v.second * (1.0 - boost::math::cdf(nd, a - v.first));
        }
    }

    return answer;
}

int main() {
    cout << setprecision(15);

    //double answer = solve(0.5, 10000000);
    double answer = solve(0.5, 3000);
    cout << setprecision(8);
    cout << answer << endl;
}


#include <simulation.hpp>

#include <boost/numeric/odeint.hpp>

#include "eigenIntegration.hpp"

namespace scpp
{

class ODE
{
public:
    ODE(Model::ptr_t model, double dt, const Model::input_vector_t &u0, const Model::input_vector_t &u1);
    void operator()(const Model::state_vector_t &f, Model::state_vector_t &dfdt, const double t);

private:
    Model::ptr_t model;
    Model::input_vector_t u0, u1;
    double dt;
};

ODE::ODE(Model::ptr_t model, double dt, const Model::input_vector_t &u0, const Model::input_vector_t &u1)
    : model(model), u0(u0), u1(u1), dt(dt) {}

void ODE::operator()(const Model::state_vector_t &f, Model::state_vector_t &dfdt, const double t)
{
    Model::input_vector_t u = u0 + t / dt * (u1 - u0);
    model->computef(f, u, dfdt);
}

void simulate(Model::ptr_t model, double dt,
              const Model::input_vector_t &u0,
              const Model::input_vector_t &u1,
              Model::state_vector_t &x)
{
    using namespace boost::numeric::odeint;
    runge_kutta_fehlberg78<Model::state_vector_t, double, Model::state_vector_t, double, vector_space_algebra> stepper;

    ODE ode(model, dt, u0, u1);

    integrate_adaptive(stepper, ode, x, 0., dt, dt / 20.);
}

} // namespace scpp


/******************************************************************************
 * Copyright (C) 2013 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file simulate-online.cpp
    \brief This file runs a simulation of the calibration problem in iterative
           mode.
  */

#include <vector>

#include <boost/make_shared.hpp>

#include <Eigen/Core>

#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/three_point_methods.hpp>

#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/Optimizer2Options.hpp>
#include <aslam/backend/SparseQrLinearSystemSolver.hpp>
#include <aslam/backend/SparseQRLinearSolverOptions.h>
#include <aslam/backend/GaussNewtonTrustRegionPolicy.hpp>
#include <aslam/backend/Optimizer2.hpp>

#include <aslam/calibration/statistics/UniformDistribution.h>
#include <aslam/calibration/statistics/NormalDistribution.h>
#include <aslam/calibration/data-structures/VectorDesignVariable.h>
#include <aslam/calibration/geometry/Transformation.h>
#include <aslam/calibration/base/Timestamp.h>
#include <truncated-svd-solver/marginalization.h>

#include "aslam/calibration/2dlrf/utils.h"
#include "aslam/calibration/2dlrf/ErrorTermMotion.h"
#include "aslam/calibration/2dlrf/ErrorTermObservation.h"

using namespace aslam::calibration;
using namespace aslam::backend;
using namespace sm::kinematics;

int main(int argc, char** argv) {
  // steps to simulate
  const size_t steps = 5000;

  // timestep size
  const double T = 0.1;

  // true state
  std::vector<Eigen::Matrix<double, 3, 1> > x_true;
  x_true.reserve(steps);

  // integrated odometry
  std::vector<Eigen::Matrix<double, 3, 1> > x_odom;
  x_odom.reserve(steps);

  // true control input
  std::vector<Eigen::Matrix<double, 3, 1> > u_true;
  const double sineWaveAmplitude = 1.0;
  const double sineWaveFrequency = 0.01;
  genSineWavePath(u_true, steps, sineWaveAmplitude, sineWaveFrequency, T);

  // measured control input
  std::vector<Eigen::Matrix<double, 3, 1> > u_noise;
  u_noise.reserve(steps);

  // number of landmarks
  const size_t nl = 17;

  // playground size
  Eigen::Matrix<double, 2, 1> min(0, 0);
  Eigen::Matrix<double, 2, 1> max(30, 30);

  // bearing measurements
  std::vector<std::vector<double> > b;
  b.reserve(steps);
  b.push_back(std::vector<double>(nl, 0));

  // range measurements
  std::vector<std::vector<double> > r;
  r.reserve(steps);
  r.push_back(std::vector<double>(nl, 0));

  // covariance matrix for motion model
  Eigen::Matrix<double, 3, 3> Q = Eigen::Matrix<double, 3, 3>::Zero();
  Q(0, 0) = 0.00044;
  Q(1, 1) = 1e-6;
  Q(2, 2) = 0.00082;

  // covariance matrix for observation model
  Eigen::Matrix<double, 2, 2> R = Eigen::Matrix<double, 2, 2>::Zero();
  R(0, 0) = 0.00090;
  R(1, 1) = 0.00067;

  // landmark positions
  std::vector<Eigen::Matrix<double, 2, 1> > x_l;
  UniformDistribution<double, 2>(min, max).getSamples(x_l, nl);

  // true calibration parameters
  Eigen::Matrix<double, 3, 1> Theta(0.219, 0.1, 0.78);

  // guessed calibration parameters
//  Eigen::Matrix<double, 3, 1> Theta_hat = Theta +
//    aslam::calibration::NormalDistribution<3>(
//    Eigen::Matrix<double, 3, 1>::Zero(),
//    Eigen::Matrix<double, 3, 3>::Identity() * 1e-2).getSample();
  Eigen::Matrix<double, 3, 1> Theta_hat(0.23, 0.11, 0.8);

  // initial state
  Eigen::Matrix<double, 3, 1> x_0(1.0, 1.0, M_PI / 4);
  x_true.push_back(x_0);
  x_odom.push_back(x_0);
  u_noise.push_back(Eigen::Matrix<double, 3, 1>::Zero());

  // simulate
  for (size_t i = 1; i < steps; ++i) {
    Eigen::Matrix<double, 3, 3> B = Eigen::Matrix<double, 3, 3>::Identity();
    B(0, 0) = cos(x_true[i - 1](2));
    B(0, 1) = -sin(x_true[i - 1](2));
    B(1, 0) = sin(x_true[i - 1](2));
    B(1, 1) = cos(x_true[i - 1](2));
    Eigen::Matrix<double, 3, 1> xk = x_true[i - 1] + T * B * u_true[i];
    xk(2) = angleMod(xk(2));
    x_true.push_back(xk);
    u_noise.push_back(u_true[i] + NormalDistribution<3>(
      Eigen::Matrix<double, 3, 1>::Zero(), Q).getSample());
    B(0, 0) = cos(x_odom[i - 1](2));
    B(0, 1) = -sin(x_odom[i - 1](2));
    B(1, 0) = sin(x_odom[i - 1](2));
    B(1, 1) = cos(x_odom[i - 1](2));
    xk = x_odom[i - 1] + T * B * u_noise[i];
    xk(2) = angleMod(xk(2));
    x_odom.push_back(xk);
    const double ct = cos(x_true[i](2));
    const double st = sin(x_true[i](2));
    std::vector<double> rk(nl, 0);
    std::vector<double> bk(nl, 0);
    for (size_t j = 0; j < nl; ++j) {
      const double aa = x_l[j](0) - x_true[i](0) - Theta(0) * ct +
        Theta(1) * st;
      const double bb = x_l[j](1) - x_true[i](1) - Theta(0) * st -
        Theta(1) * ct;
      const double range = sqrt(aa * aa + bb * bb) + NormalDistribution<1>(
        0, R(0, 0)).getSample();
      rk[j] = range;
      bk[j] = angleMod(atan2(bb, aa) - x_true[i](2) - Theta(2) +
        NormalDistribution<1>(0, R(1, 1)).getSample());
    }
    r.push_back(rk);
    b.push_back(bk);
  }

  // landmark guess
  std::vector<Eigen::Matrix<double, 2, 1> > x_l_hat;
  initLandmarks(x_l_hat, x_odom, Theta_hat, r, b);

  // create state design variables
  std::vector<boost::shared_ptr<VectorDesignVariable<3> > > dv_x;
  dv_x.reserve(steps);
  for (size_t i = 0; i < steps; ++i) {
    dv_x.push_back(
      boost::make_shared<VectorDesignVariable<3> >(x_odom[i]));
    dv_x[i]->setActive(true);
  }

  // create landmarks design variables
  std::vector<boost::shared_ptr<VectorDesignVariable<2> > > dv_x_l;
  dv_x_l.reserve(nl);
  for (size_t i = 0; i < nl; ++i) {
    dv_x_l.push_back(
      boost::make_shared<VectorDesignVariable<2> >(x_l_hat[i]));
    dv_x_l[i]->setActive(true);
  }

  // create calibration parameters design variable
  auto dv_Theta = boost::make_shared<VectorDesignVariable<3> >(Theta_hat);
  dv_Theta->setActive(true);

  // batch size
  const size_t batchSize = 200;

  // mutual information threshold
  const double miTol = 0.5;

  // batch idx used for optimization
  std::vector<size_t> batchIdx;
  batchIdx.reserve(steps / batchSize);

  // calibration parameters record
  Eigen::Matrix <double, 3, 1> calibParams = dv_Theta->getValue();

  // Sigma record
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> SigmaRecord;

  // Sigma determinant record
  double SigmaDetRecord = 0;

  // iterative optimization
  for (size_t i = 0; i < steps; i += batchSize) {
    const double timeStart = Timestamp::now();

    // insert current batch
    batchIdx.push_back(i);

    // create optimization problem
    auto problem = boost::make_shared<OptimizationProblem>();

    // add design variables to the problem
    for (size_t j = 0; j < batchIdx.size(); ++j)
      for (size_t k = batchIdx[j]; k < batchIdx[j] + batchSize; ++k)
        problem->addDesignVariable(dv_x[k]);
    for (size_t j = 0; j < nl; ++j)
      problem->addDesignVariable(dv_x_l[j]);
    problem->addDesignVariable(dv_Theta);

    // add error terms to the problem
    for (size_t j = 0; j < batchIdx.size(); ++j) {
      for (size_t k = batchIdx[j] + 1; k < batchIdx[j] + batchSize; ++k) {
        auto e_mot = boost::make_shared<ErrorTermMotion>(dv_x[k - 1].get(),
          dv_x[k].get(), T, u_noise[k], Q);
        problem->addErrorTerm(e_mot);
        for (size_t l = 0; l < nl; ++l) {
          auto e_obs = boost::make_shared<ErrorTermObservation>(dv_x[k].get(),
            dv_x_l[l].get(), dv_Theta.get(), r[k][l], b[k][l], R);
          problem->addErrorTerm(e_obs);
        }
      }
    }

    std::cout << "Calibration before: " << calibParams.transpose() << std::endl;

    // optimization round
    Optimizer2Options options;
    options.verbose = true;
    options.linearSystemSolver =
      boost::make_shared<SparseQrLinearSystemSolver>();
    options.trustRegionPolicy =
      boost::make_shared<GaussNewtonTrustRegionPolicy>();
    SparseQRLinearSolverOptions linearSolverOptions;
    linearSolverOptions.colNorm = true;
    linearSolverOptions.qrTol = 0.02;
    Optimizer2 optimizer(options);
    optimizer.getSolver<SparseQrLinearSystemSolver>()->setOptions(
      linearSolverOptions);
    optimizer.setProblem(problem);
    optimizer.optimize();

    // Sigma computation
    aslam::backend::CompressedColumnMatrix<std::ptrdiff_t> Jt =
        optimizer.getSolver<SparseQrLinearSystemSolver>()->
        getJacobianTranspose();

    const size_t dim = 3;
    const size_t numCols = Jt.rows();

    Eigen::MatrixXd NS, CS, Sigma, SigmaP, Omega;
    cholmod_sparse Jt_cholmod;
    Jt.getView(&Jt_cholmod);
    truncated_svd_solver::marginalize(&Jt_cholmod, numCols - dim, NS, CS,
                                      Sigma, SigmaP, Omega);
    const double SigmaDet = Sigma.determinant();

    std::cout << "Calibration after: " << *dv_Theta << std::endl;
    std::cout << "Sigma: " << std::endl << Sigma << std::endl;

    // decision round
    if (batchIdx.size() == 1) { // keep if only one batch
      calibParams = dv_Theta->getValue();
      SigmaRecord = Sigma;
      SigmaDetRecord = SigmaDet;
    }
    else {
      // compute mutual information
      const double mi = 0.5 * log2(SigmaDetRecord / SigmaDet);

      // keep batch if needed
      if (mi > miTol) {
        calibParams = dv_Theta->getValue();
        SigmaRecord = Sigma;
        SigmaDetRecord = SigmaDet;
      }
      else
        batchIdx.pop_back();
    }

    const double timeStop = Timestamp::now();
    std::cout << "Batch processing time [s]: " << timeStop - timeStart
      << std::endl;
  }

  std::cout << "Final calibration: " << calibParams.transpose() << std::endl;
  std::cout << "Sigma: " << std::endl << SigmaRecord << std::endl;
  std::cout << "Data used: " << (batchIdx.size() * batchSize) / (double)steps
    * 100 << " %" << std::endl;

  // output results to file
  std::ofstream x_true_log("x_true.txt");
  for (size_t i = 0; i < steps; ++i)
    x_true_log << x_true[i].transpose() << std::endl;
  std::ofstream x_odom_log("x_odom.txt");
  for (size_t i = 0; i < steps; ++i)
    x_odom_log << x_odom[i].transpose() << std::endl;
  std::ofstream x_est_log("x_est.txt");
  for (size_t i = 0; i < batchIdx.size(); ++i)
    for (size_t j = batchIdx[i]; j < batchIdx[i] + batchSize; ++j)
      x_est_log << *(dv_x[j]) << std::endl;
  std::ofstream l_log("l.txt");
  for (size_t i = 0; i < nl; ++i)
    l_log << x_l[i].transpose() << std::endl;
  std::ofstream l_est_log("l_est.txt");
  for (size_t i = 0; i < nl; ++i)
    l_est_log << *(dv_x_l[i]) << std::endl;

  // align landmarks
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> l =
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(3, nl);
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> l_est =
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i) {
    l(0, i) = x_l[i](0);
    l(1, i) = x_l[i](1);
    l_est(0, i) = dv_x_l[i]->getValue()(0);
    l_est(1, i) = dv_x_l[i]->getValue()(1);
  }
  Transformation<double, 3> trans(threePointSvd(l, l_est));
  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> l_est_trans =
    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>::Zero(3, nl);
  for (size_t i = 0; i < nl; ++i)
    l_est_trans.col(i) = trans(l_est.col(i));
  std::ofstream l_est_trans_log("l_est_trans.txt");
  for (size_t i = 0; i < nl; ++i)
    l_est_trans_log << l_est_trans.col(i).head<2>().transpose() << std::endl;

  // align poses
  std::vector<Eigen::Matrix<double, 3, 1> > x_est_trans;
  x_est_trans.reserve(steps);
  std::ofstream x_est_trans_log("x_est_trans.txt");
  for (size_t i = 0; i < batchIdx.size(); ++i)
    for (size_t j = batchIdx[i]; j < batchIdx[i] + batchSize; ++j) {
      Eigen::Matrix<double, 3, 1> pose((Eigen::Matrix<double, 3, 1>()
        << dv_x[j]->getValue().head<2>(), 0).finished());
      trans.transform(pose, pose);
      pose(2) = dv_x[j]->getValue()(2);
      x_est_trans.push_back(pose);
      x_est_trans_log << pose.transpose() << std::endl;
    }

  return 0;
}


#include "point_transform_function.hpp"
#include "cgal_typedefs.hpp"
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Regular_triangulation_3.h>
//#include <CGAL/Regular_triangulation_euclidean_traits_3.h>
#include <CGAL/Fixed_alpha_shape_3.h>
#include <CGAL/Triangulation_vertex_base_with_info_3.h>
#include <CGAL/Fixed_alpha_shape_vertex_base_3.h>
#include <CGAL/Fixed_alpha_shape_cell_base_3.h>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <utility>
#include <iterator>
#include <stdexcept>

namespace busv{

template <typename T>
T* init_mem(size_t size, const T& init_val){
	T* mem = (T*)malloc(sizeof(T)*size);
	if(!mem){
		throw std::runtime_error("Insufficient memory");
	}
	for(size_t i = 0; i < size; ++i){
		mem[i] = init_val;
	}
	return mem;
}

template <typename it_t, typename AS, typename T>
size_t countNotType(it_t it, it_t end, const AS& as, T type){
	size_t num = 0;
	for(;it!=end;++it){
		if(as.classify(*it) != type){
			++num;
		}
	}
	return num;
}

template <typename it_t, typename AS, typename T>
size_t countNotType2(it_t it, it_t end, const AS& as, T type){
	size_t num = 0;
	for(;it!=end;++it){
		if(as.classify(it) != type){
			++num;
		}
	}
	return num;
}

template <typename T, typename U>
void alpha_shapes(
	U num_points, T alpha,
	const T* points, const T* weights,
	U* num_edges, U** edges,
	U* num_triangles, U** triangles,
	U* num_tetrahedra, U** tetrahedra
){
	//typedef cgal_typedefs<T, U> cgt;
	//typedef typename cgt::Fixed_alpha_shape_3 Fixed_alpha_shape_3;
	//typedef typename cgt::Triangulation_3 Triangulation_3;
	typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

	//fixed weighted example from docs
	/*typedef CGAL::Regular_triangulation_vertex_base_3<K>      Vbb;
	typedef CGAL::Fixed_alpha_shape_vertex_base_3<K,Vbb>      Vb;
	typedef CGAL::Regular_triangulation_cell_base_3<K>        Rcb;
	typedef CGAL::Fixed_alpha_shape_cell_base_3<K,Rcb>        Cb;
	typedef CGAL::Triangulation_data_structure_3<Vb,Cb>       Tds;
	typedef CGAL::Regular_triangulation_3<K,Tds>              Triangulation_3;
	typedef CGAL::Fixed_alpha_shape_3<Triangulation_3>        Fixed_alpha_shape_3;*/

	typedef CGAL::Regular_triangulation_vertex_base_3<K> Vbb;
	typedef CGAL::Triangulation_vertex_base_with_info_3<U, K, Vbb> Tb;
	typedef CGAL::Fixed_alpha_shape_vertex_base_3<K, Tb> Vb;

	typedef CGAL::Regular_triangulation_cell_base_3<K> Rcb;
	typedef CGAL::Fixed_alpha_shape_cell_base_3<K, Rcb> Fb;

	typedef CGAL::Triangulation_data_structure_3<Vb,Fb> Tds;
	typedef CGAL::Regular_triangulation_3<K,Tds> Triangulation_3;
	typedef CGAL::Fixed_alpha_shape_3<Triangulation_3> Fixed_alpha_shape_3;
	//typedef typename Fixed_alpha_shape_3::Cell_handle Cell_handle;
	//typedef typename Fixed_alpha_shape_3::Vertex_handle Vertex_handle;
	typedef typename Fixed_alpha_shape_3::Cell Cell;
	typedef typename Fixed_alpha_shape_3::Facet Facet;
	typedef typename Fixed_alpha_shape_3::Edge Edge;
	//typedef typename Triangulation_3::Weighted_point Weighted_point;
	//typedef typename Triangulation_3::Bare_point Bare_point;

	busv::PointTransformFunction<Triangulation_3, U, T> tfunc(points, weights);
	//unsigned int i;

	//build one alpha_shape with alpha=0
	Fixed_alpha_shape_3 as(
		boost::make_transform_iterator(boost::counting_iterator<U>(0), tfunc),
		boost::make_transform_iterator(boost::counting_iterator<U>(num_points), tfunc),
		alpha
	);

	*num_edges = countNotType(as.finite_edges_begin(), as.finite_edges_end(), as, Fixed_alpha_shape_3::EXTERIOR);
	U* t = *edges = init_mem<U>((*num_edges)*2, 0);
	typedef typename Fixed_alpha_shape_3::Finite_edges_iterator edge_it_t;
	for(edge_it_t it = as.finite_edges_begin(), eit = as.finite_edges_end(); it!= eit; ++it){
		const Edge& e = *it;
		if(as.classify(e) != Fixed_alpha_shape_3::EXTERIOR){
			t[0] = e.first->vertex(e.second)->info();
			t[1] = e.first->vertex(e.third)->info();
			t += 2;
		}
	}

	*num_triangles = countNotType(as.finite_facets_begin(), as.finite_facets_end(), as, Fixed_alpha_shape_3::EXTERIOR);
	t = *triangles = init_mem<U>((*num_triangles)*3, 0);
	typedef typename Fixed_alpha_shape_3::Finite_facets_iterator facet_it_t;
	for(facet_it_t it = as.finite_facets_begin(), eit = as.finite_facets_end(); it!= eit; ++it){
		const Facet& f = *it;
		//if(as.classify(f) == Fixed_alpha_shape_3::SINGULAR){
		if(as.classify(f) != Fixed_alpha_shape_3::EXTERIOR){
			t[0] = f.first->vertex((f.second+1)%4)->info();
			t[1] = f.first->vertex((f.second+2)%4)->info();
			t[2] = f.first->vertex((f.second+3)%4)->info();
			t += 3;
		}
	}

	*num_tetrahedra = countNotType2(as.finite_cells_begin(), as.finite_cells_end(), as, Fixed_alpha_shape_3::EXTERIOR);
	t = *tetrahedra = init_mem<U>((*num_tetrahedra)*4, 0);
	typedef typename Fixed_alpha_shape_3::Finite_cells_iterator cell_it_t;
	for(cell_it_t it = as.finite_cells_begin(), eit = as.finite_cells_end(); it!= eit; ++it){
		const Cell& c = *it;
//		if(as.classify(it) == Fixed_alpha_shape_3::INTERIOR){
		if(as.classify(it) != Fixed_alpha_shape_3::EXTERIOR){
			t[0] = c.vertex(0)->info();
			t[1] = c.vertex(1)->info();
			t[2] = c.vertex(2)->info();
			t[3] = c.vertex(3)->info();
			t += 4;
		}
	}
}

} //namespace busv


//==============================================================================
//         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_TRIGONOMETRIC_FUNCTIONS_COMPLEX_GENERIC_SINCOS_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_FUNCTIONS_COMPLEX_GENERIC_SINCOS_HPP_INCLUDED
#include <nt2/trigonometric/functions/sincos.hpp>
#include <nt2/include/functions/sincos.hpp>
#include <nt2/include/functions/sinhcosh.hpp>
#include <nt2/include/functions/real.hpp>
#include <nt2/include/functions/imag.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_real.hpp>
#include <nt2/include/functions/logical_or.hpp>
#include <nt2/include/functions/if_zero_else.hpp>
#include <boost/fusion/include/std_pair.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sincos_, tag::cpu_,
                             (A0),
                             (generic_ < complex_<floating_ < A0> > > )
                             (generic_ < complex_<floating_ < A0> > > )
                             (generic_ < complex_<floating_ < A0> > > )
                            )
  {
    typedef void result_type;
    inline void operator()(A0 const& a0,A0 & a1,A0 & a2) const
    {
      typedef typename meta::as_real<A0>::type rtype;
      rtype c, s, ch, sh;
      sincos(nt2::real(a0), s, c);
      sinhcosh(nt2::imag(a0), sh, ch);
      rtype r1 = if_zero_else(is_imag(a0), s*ch);
      rtype i1 = if_zero_else(is_real(a0), c*sh);
      rtype r2 = c*ch;
      rtype i2 = if_zero_else(logical_or(is_imag(a0), is_real(a0)), -s*sh);
      a1 =  A0(r1, i1);
      a2 =  A0(r2, i2);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sincos_, tag::cpu_,
                             (A0)(A1),
                             (generic_ < imaginary_<floating_ < A0> > > )
                             (generic_ < dry_<floating_ < A1> > > )
                             (generic_ < imaginary_<floating_ < A0> > > )
                            )
  {
    typedef void result_type;
    inline void operator()(A0 const& a0,A1 & a1,A0 & a2) const
    {
      typedef typename meta::as_real<A0>::type rtype;
      rtype ch, sh;
      sinhcosh(nt2::imag(a0), sh, ch);
      a1 =  bitwise_cast<A1>(ch);
      a2 =  bitwise_cast<A0>(-sh);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sincos_, tag::cpu_,
                             (A0),
                             (generic_ < dry_<floating_ < A0> > > )
                             (generic_ < dry_<floating_ < A0> > > )
                             (generic_ < dry_<floating_ < A0> > > )
                            )
  {
    typedef void result_type;
    inline void operator()(A0 const& a0,A0 & a1,A0 & a2) const
    {
      typedef typename meta::as_real<A0>::type rtype;
      rtype c, s;
      sincos(nt2::real(a0), s, c);
      a1 =  bitwise_cast<A0>(c);
      a2 =  bitwise_cast<A0>(s);
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sincos_, tag::cpu_,
                             (A0),
                             (generic_ < complex_<floating_ < A0> > > )
                             (generic_ < complex_<floating_ < A0> > > )
                            )
  {
    typedef A0 result_type;
    inline A0 operator()(A0 const& a0,A0 & a2) const
    {
      result_type a1;
      sincos(a0, a1, a2);
      return a1;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sincos_, tag::cpu_,
                             (A0),
                             (generic_ < complex_<floating_<A0> > > )
                            )
  {
    typedef std::pair<A0, A0>           result_type;
    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      sincos(a0, res.first, res.second);
      return res;
    }
  };
} }

#endif


#include <octomap/GaussionOcTree.h>
#include <octomap/octomap.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <Eigen/Dense>
#include <unordered_map>
#include <unordered_set>

#define maxdepth 16   // unit: layer
#define resolution 2  // unit: m

int main(int argc, char** argv) {
  std::string octomap_name = "simple_tree_gussion.ot";

  // Part1: Read origin point cloud
  Eigen::Vector4f centroid;
  Eigen::Matrix3f covariance_matrix;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(
      new pcl::PointCloud<pcl::PointXYZ>);

  std::cout << " --------------- "
            << " initial         "
            << " --------------- " << std::endl;

  if (argc == 2) {
    std::cout << "use input data" << std::endl;

    if (pcl::io::loadPCDFile(argv[1], *cloud_in)) {
      std::cerr << "failed to open " << argv[1] << std::endl;
      return 1;
    }

    pcl::computeMeanAndCovarianceMatrix(*cloud_in, covariance_matrix, centroid);

    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    transform(0, 3) = -centroid(0);
    transform(1, 3) = -centroid(1);
    transform(2, 3) = -centroid(2);

    pcl::transformPointCloud(*cloud_in, *cloud_in, transform);
  } else {
    std::cout << "use builtin data" << std::endl;
    cloud_in->push_back(pcl::PointXYZ(1, 1.5, 0));
    cloud_in->push_back(pcl::PointXYZ(1, 0.5, 0));
    cloud_in->push_back(pcl::PointXYZ(0.5, 1, 0));
    cloud_in->push_back(pcl::PointXYZ(1.5, 1, 0));

    cloud_in->push_back(pcl::PointXYZ(3, 1.5, 0));
    cloud_in->push_back(pcl::PointXYZ(3, 0.5, 0));
    cloud_in->push_back(pcl::PointXYZ(2.5, 1, 0));
    cloud_in->push_back(pcl::PointXYZ(3.5, 1, 0));

    cloud_in->push_back(pcl::PointXYZ(1, 2.5, 0));
    cloud_in->push_back(pcl::PointXYZ(1, 3.5, 0));
    cloud_in->push_back(pcl::PointXYZ(0.5, 3, 0));
    cloud_in->push_back(pcl::PointXYZ(1.5, 3, 0));

    cloud_in->push_back(pcl::PointXYZ(1, 2.5, 2));
    cloud_in->push_back(pcl::PointXYZ(1, 3.5, 2));
    cloud_in->push_back(pcl::PointXYZ(0.5, 3, 2));
    cloud_in->push_back(pcl::PointXYZ(1.5, 3, 2));

    cloud_in->push_back(pcl::PointXYZ(3, 2.5, 0));
    cloud_in->push_back(pcl::PointXYZ(3, 3.5, 0));
    cloud_in->push_back(pcl::PointXYZ(2.5, 3, 0));
    cloud_in->push_back(pcl::PointXYZ(3.5, 3, 0));

    cloud_in->push_back(pcl::PointXYZ(1, -2, 0));
    cloud_in->push_back(pcl::PointXYZ(2, -1, 0));
    cloud_in->push_back(pcl::PointXYZ(2, -2, 0));
    cloud_in->push_back(pcl::PointXYZ(2, -3, 0));
    cloud_in->push_back(pcl::PointXYZ(3, -2, 0));
  }

  std::cout << "Input " << cloud_in->size() << " pts. " << std::endl;

  // Part2: Construct Gaussion Octomap
  std::cout << " --------------- "
            << " compute tree    "
            << " --------------- " << std::endl;

  octomap::GaussionOcTree save_tree(resolution);
  std::unordered_multimap<octomap::OcTreeKey, pcl::PointXYZ,
                          octomap::OcTreeKey::KeyHash>
      unorderedMultiMap;
  std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> set;

  for (auto p : (*cloud_in).points) {
    auto key = save_tree.coordToKey(p.x, p.y, p.z, 16);
    unorderedMultiMap.emplace(key, p);
    set.emplace(key);
  }

  for (auto iter = set.begin(); iter != set.end(); ++iter) {
    auto key = *iter;
    auto range = unorderedMultiMap.equal_range(key);
    int i = 0;

    pcl::PointCloud<pcl::PointXYZ> cloud;
    for (auto it = range.first; it != range.second; ++it) {
      cloud.push_back(it->second);
      i++;
    }

    if (cloud.size() < 20) continue;
    pcl::computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid);
    covariance_matrix = covariance_matrix * i / (i - 1);
    auto n = save_tree.updateNode(key, true);
    n->setGaussionDistribution(i, centroid.head<3>(), covariance_matrix);
    // std::cout << "This part has: " << cloud.size() << " pts. " << std::endl
    //           << "centroid: " << centroid << std::endl;
  }

  save_tree.updateInnerOccupancy();

  std::cout << " --------------- "
            << " itr test before "
            << " --------------- " << std::endl;

  std::cout << "tree size: " << save_tree.size() << std::endl;

  int write_count_leaf_[maxdepth] = {0};

  for (int ite = 0; ite < maxdepth; ite++) {
    bool firstin_ = true;
    for (auto it = save_tree.begin_leafs(ite), end = save_tree.end_leafs();
         it != end; ++it) {
      if (firstin_) {
        std::cout << " ------- " << it.getDepth() << "/"
                  << save_tree.getTreeDepth() << " layer ------- " << std::endl;
        std::cout << " coordinate: " << it.getCoordinate()
                  << "  size: " << it.getSize() << std::endl;
        firstin_ = false;
      }

      if (ite == 0) {  // 打印最低层的所有分布
        std::cout << "第" << write_count_leaf_[ite] << "个分布， "
                  << " " << it->getGaussionDistribution() << std::endl;
      }

      if (ite == maxdepth - 1) {  // 打印次低层的所有分布
        std::cout << "第" << write_count_leaf_[ite] << "个分布， "
                  << " " << it->getGaussionDistribution() << std::endl;
      }

      if (ite == maxdepth - 2) {  // 打印次低层的所有分布
        std::cout << "第" << write_count_leaf_[ite] << "个分布， "
                  << " " << it->getGaussionDistribution() << std::endl;

        std::cout << "search [3.5,3,0]" << std::endl;
        int x_ = 2;
        int y_ = 2;
        int z_ = 0;
        auto key = save_tree.coordToKey(x_, y_, z_, maxdepth - 2);
        octomap::GaussionOcTreeNode* node = save_tree.search(key, maxdepth - 2);
        std::cout << node->getGaussionDistribution() << std::endl;
      }

      if (ite == maxdepth - 3) {  // 打印次低层的所有分布
        std::cout << "第" << write_count_leaf_[ite] << "个分布， "
                  << " " << it->getGaussionDistribution() << std::endl;

        std::cout << "search [3.5,3,0]" << std::endl;
        int x_ = 2;
        int y_ = 2;
        int z_ = 0;
        auto key = save_tree.coordToKey(x_, y_, z_, maxdepth - 2);

        octomap::GaussionOcTreeNode* node = save_tree.search(key, maxdepth - 2);
        std::cout << node->getGaussionDistribution();
      }
      // if (ite == 1) { // 打印最高分布
      //   std::cout << "第" << write_count_leaf_[ite] << "个分布， "
      //             << " " << it->getGaussionDistribution() << std::endl;
      // }
      write_count_leaf_[ite]++;
    }

    std::cout << " num of leafs :: " << write_count_leaf_[ite] << std::endl
              << std::endl;
  }

  // Part3: Save Gaussion Octomap
  save_tree.write(octomap_name);

  // Part4: Read Gaussion Octomap
  cloud_in->clear();

  octomap::AbstractOcTree* read_tree =
      octomap::AbstractOcTree::read(octomap_name);
  octomap::GaussionOcTree* readtree =
      dynamic_cast<octomap::GaussionOcTree*>(read_tree);

  std::cout << " --------------- "
            << " itr test  after "
            << " --------------- " << std::endl;

  std::cout << "tree size: " << readtree->size() << std::endl;

  int read_count_leaf_[maxdepth] = {0};
  for (int ite = 0; ite < maxdepth; ite++) {
    bool firstin_ = true;
    for (auto it = readtree->begin_leafs(ite), end = readtree->end_leafs();
         it != end; ++it) {
      if (firstin_) {
        std::cout << " ------- " << it.getDepth() << "/"
                  << readtree->getTreeDepth() << " layer ------- " << std::endl;
        std::cout << " coordinate: " << it.getCoordinate()
                  << "  size: " << it.getSize() << std::endl;
        firstin_ = false;
      }

      // if (ite == 0) {  // 打印最低层的所有分布
      //   std::cout << "第" << read_count_leaf_[ite] << "个分布， "
      //             << " " << it->getGaussionDistribution() << std::endl;
      // }

      // if (ite == maxdepth - 1) {  // 打印次低层的所有分布
      //   std::cout << "第" << read_count_leaf_[ite] << "个分布， "
      //             << " " << it->getGaussionDistribution() << std::endl;
      // }

      // if (ite == 1) {
      //   std::cout << "第" << read_count_leaf_[ite] << "个分布， "
      //             << " " << it->getGaussionDistribution() << std::endl;
      // }
      read_count_leaf_[ite]++;
    }

    std::cout << " num of leafs :: " << read_count_leaf_[ite] << std::endl
              << std::endl;
  }

  std::cout << "read file " << octomap_name << " done" << std::endl
            << std::endl;
}


#include <CGAL/Simple_cartesian.h>

#include <CGAL/boost/graph/graph_traits_Linear_cell_complex_for_combinatorial_map.h>
#include <CGAL/boost/graph/IO/polygon_mesh_io.h>

#include <boost/graph/breadth_first_search.hpp>

#include <fstream>

typedef CGAL::Simple_cartesian<double>              Kernel;
typedef Kernel::Point_3                             Point;
typedef CGAL::Linear_cell_complex_traits<3, Kernel> LCC_traits;

typedef CGAL::Linear_cell_complex_for_bgl_combinatorial_map_helper
         <2, 3, LCC_traits>::type LCC;

typedef boost::graph_traits<LCC>::vertex_descriptor vertex_descriptor;
typedef boost::graph_traits<LCC>::vertex_iterator   vertex_iterator;

int main(int argc, char** argv)
{
  LCC lcc;
  CGAL::IO::read_polygon_mesh((argc>1)?argv[1]:"cube.off", lcc);

  // This is the vector where the distance gets written to
  std::vector<int> distance(lcc.vertex_attributes().size());

  // Here we start at an arbitrary vertex
  // Any other vertex could be the starting point
  vertex_iterator vb, ve;
  boost::tie(vb,ve)=vertices(lcc);
  vertex_descriptor  vd = *vb;

  std::cout << "We compute distances to " << vd->point() << std::endl;

  // bfs = breadth first search explores the graph
  // Just as the distance_recorder there is a way to record the predecessor of a vertex
  boost::breadth_first_search(lcc,
                              vd,
                              visitor(boost::make_bfs_visitor
                                      (boost::record_distances
                                       (make_iterator_property_map
                                        (distance.begin(),
                                         get(boost::vertex_index, lcc)),
                                        boost::on_tree_edge()))));

  // Traverse all vertices and show at what distance they are
  for(boost::tie(vb,ve)=vertices(lcc); vb!=ve; ++vb)
  {
    vd = *vb;
    std::cout<<vd->point()<<"  is "<<distance[vd->id()]<<" hops away."<<std::endl;
  }

  return 0;
}