#include <igl/per_vertex_normals.h>
#include <igl/principal_curvature.h>
#include <igl/avg_edge_length.h>
#include <igl/massmatrix.h>
#include <igl/adjacency_list.h>
#include <igl/per_face_normals.h>
#include <igl/barycenter.h>
#include <igl/pinv.h>
#include <igl/edges.h>
#include <Eigen/SparseCore>
#include <igl/adjacency_list.h>
#include <igl/adjacency_matrix.h>
#include <igl/per_face_normals.h>
#include <igl/per_vertex_normals.h>
#include <igl/avg_edge_length.h>
#include <igl/edge_flaps.h>
#include <igl/unique_edge_map.h>
#include <igl/vertex_triangle_adjacency.h>
#include <igl/principal_curvature.h>
#include <igl/collapse_edge.h>
#include <igl/writeOBJ.h>
#include <igl/C_STR.h>
#include <igl/circulation.h>
#include <igl/is_edge_manifold.h>
#include <igl/decimate.h>
#include <igl/shortest_edge_and_midpoint.h>
#include <igl/infinite_cost_stopping_condition.h>
#include "split_edges.h"
using namespace std;

void split_edges_until_bound(Eigen::MatrixXd & V,Eigen::MatrixXi & F, Eigen::VectorXi & feature, Eigen::VectorXd & high, Eigen::VectorXd & low){

    using namespace Eigen;
    int m = F.rows();
    int n = V.rows();
    int num_feat = feature.size();
    std::vector<std::vector<int>> A;
    std::vector<bool> is_feature_vertex;
    is_feature_vertex.resize(n);
    Eigen::VectorXi is_feature_vertex_vec;
    is_feature_vertex_vec.setZero(n);
    igl::adjacency_list(F,A);
    Eigen::MatrixXi E,uE;
    Eigen::VectorXi EMAP;
    std::vector<std::vector<int>> uE2E;
    igl::unique_edge_map(F,E,uE,EMAP,uE2E);
    int k = uE.rows();
    //std::cout << "Start split_edges_until_bound" << std::endl;

    for (int s = 0; s < num_feat; s++) {
        is_feature_vertex[feature(s)] = true;
       // is_feature_vertex_vec(feature(s)) = 1;
    }

    bool keep_splitting = true;
    std::vector<int> edges_to_split;

    while (keep_splitting) {
        //std::cout << "A" << std::endl;
        edges_to_split.resize(0);

        for (int i = 0; i < uE.rows(); i++) {
            //std::cout << "B" << std::endl;
            if (!is_feature_vertex[uE(i,0)] && !is_feature_vertex[uE(i,1)] && uE2E[i].size()==2) {
            //if (is_feature_vertex_vec(uE(i,0))==0 && is_feature_vertex_vec(uE(i,1))==0 && uE2E[i].size()==2) {
                if ( (V.row(uE(i,0))-V.row(uE(i,1))).norm()>((high(uE(i,0))+high(uE(i,1)))/2)  ){
                    edges_to_split.push_back(i);
                    //std::cout << "C" << std::endl;
                }
            }
        }

        //std::cout << "B" << std::endl;

        //std::cout << "D" << std::endl;
        if(edges_to_split.size()==0){
            keep_splitting = false;
        }else{
            // SPLIT EDGES IN VECTOR edges_to_split
            //

            //std::cout << "Before call to split_edges" << std::endl;
            //std::cout << edges_to_split.size() << std::endl;
            split_edges(V,F,E,uE,EMAP,uE2E,high,low,edges_to_split);
            //igl::writeOBJ("test.obj",V,F);
            //igl::unique_edge_map(F,E,uE,EMAP,uE2E);
            //std::cout << igl::is_edge_manifold(F) << std::endl;
            //std::cout << "After call to split_edges" << std::endl;

        }

       keep_splitting = false; // THIS IS A PATCH, NOT GOOD
    }

}

// g++ -I/usr/local/libigl/external/eigen -I/usr/local/libigl/include -std=c++11 -framework Accelerate main.cpp remesh_botsch.cpp -o main


#include <stan/math/mix.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <vector>

TEST(ProbDistributions, fvar_var) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::fvar;
  using stan::math::var;

  Matrix<fvar<var>, Dynamic, 1> theta(3, 1);
  theta << 0.2, 0.3, 0.5;
  Matrix<fvar<var>, Dynamic, 1> alpha(3, 1);
  alpha << 1.0, 1.0, 1.0;
  for (int i = 0; i < 3; i++) {
    theta(i).d_ = 1.0;
    alpha(i).d_ = 1.0;
  }

  EXPECT_FLOAT_EQ(0.6931472,
                  stan::math::dirichlet_log(theta, alpha).val_.val());
  EXPECT_FLOAT_EQ(0.99344212, stan::math::dirichlet_log(theta, alpha).d_.val());

  Matrix<fvar<var>, Dynamic, 1> theta2(4, 1);
  theta2 << 0.01, 0.01, 0.8, 0.18;
  Matrix<fvar<var>, Dynamic, 1> alpha2(4, 1);
  alpha2 << 10.5, 11.5, 19.3, 5.1;
  for (int i = 0; i < 3; i++) {
    theta2(i).d_ = 1.0;
    alpha2(i).d_ = 1.0;
  }

  EXPECT_FLOAT_EQ(-43.40045,
                  stan::math::dirichlet_log(theta2, alpha2).val_.val());
  EXPECT_FLOAT_EQ(2017.2858,
                  stan::math::dirichlet_log(theta2, alpha2).d_.val());
}

TEST(ProbDistributions, fvar_varVectorised) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::dirichlet_log;
  using stan::math::fvar;
  using stan::math::var;

  Matrix<fvar<var>, Dynamic, 1> theta1(3, 1), theta2(3, 1), theta3(3, 1);
  theta1 << 0.2, 0.3, 0.5;
  theta2 << 0.1, 0.8, 0.1;
  theta3 << 0.6, 0.1, 0.3;

  Matrix<fvar<var>, Dynamic, 1> alpha1(3, 1), alpha2(3, 1), alpha3(3, 1);
  alpha1 << 1.0, 1.0, 1.0;
  alpha2 << 6.2, 3.5, 9.1;
  alpha3 << 2.5, 7.4, 6.1;

  std::vector<Matrix<fvar<var>, Dynamic, 1>> theta_vec(3);
  theta_vec[0] = theta1;
  theta_vec[1] = theta2;
  theta_vec[2] = theta3;

  std::vector<Matrix<fvar<var>, Dynamic, 1>> alpha_vec(3);
  alpha_vec[0] = alpha1;
  alpha_vec[1] = alpha2;
  alpha_vec[2] = alpha3;

  Matrix<fvar<var>, Dynamic, 1> result(3);
  result[0] = dirichlet_log(theta1, alpha1);
  result[1] = dirichlet_log(theta2, alpha2);
  result[2] = dirichlet_log(theta3, alpha3);

  fvar<var> out = dirichlet_log(theta_vec, alpha_vec);

  EXPECT_FLOAT_EQ(result.val().val().sum(), out.val_.val());
  EXPECT_FLOAT_EQ(result.d().val().sum(), out.d_.val());

  result[0] = dirichlet_log(theta1, alpha1);
  result[1] = dirichlet_log(theta2, alpha1);
  result[2] = dirichlet_log(theta3, alpha1);

  out = dirichlet_log(theta_vec, alpha1);

  EXPECT_FLOAT_EQ(result.val().val().sum(), out.val_.val());
  EXPECT_FLOAT_EQ(result.d().val().sum(), out.d_.val());

  result[0] = dirichlet_log(theta1, alpha1);
  result[1] = dirichlet_log(theta1, alpha2);
  result[2] = dirichlet_log(theta1, alpha3);

  out = dirichlet_log(theta1, alpha_vec);

  EXPECT_FLOAT_EQ(result.val().val().sum(), out.val_.val());
  EXPECT_FLOAT_EQ(result.d().val().sum(), out.d_.val());
}

TEST(ProbDistributions, fvar_fvar_var) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::fvar;
  using stan::math::var;

  Matrix<fvar<fvar<var>>, Dynamic, 1> theta(3, 1);
  theta << 0.2, 0.3, 0.5;
  Matrix<fvar<fvar<var>>, Dynamic, 1> alpha(3, 1);
  alpha << 1.0, 1.0, 1.0;
  for (int i = 0; i < 3; i++) {
    theta(i).d_ = 1.0;
    alpha(i).d_ = 1.0;
  }

  EXPECT_FLOAT_EQ(0.6931472,
                  stan::math::dirichlet_log(theta, alpha).val_.val_.val());
  EXPECT_FLOAT_EQ(0.99344212,
                  stan::math::dirichlet_log(theta, alpha).d_.val_.val());

  Matrix<fvar<fvar<var>>, Dynamic, 1> theta2(4, 1);
  theta2 << 0.01, 0.01, 0.8, 0.18;
  Matrix<fvar<fvar<var>>, Dynamic, 1> alpha2(4, 1);
  alpha2 << 10.5, 11.5, 19.3, 5.1;
  for (int i = 0; i < 3; i++) {
    theta2(i).d_ = 1.0;
    alpha2(i).d_ = 1.0;
  }

  EXPECT_FLOAT_EQ(-43.40045,
                  stan::math::dirichlet_log(theta2, alpha2).val_.val_.val());
  EXPECT_FLOAT_EQ(2017.2858,
                  stan::math::dirichlet_log(theta2, alpha2).d_.val_.val());
}

TEST(ProbDistributions, fvar_fvar_varVectorised) {
  using Eigen::Dynamic;
  using Eigen::Matrix;
  using stan::math::dirichlet_log;
  using stan::math::fvar;
  using stan::math::var;

  Matrix<fvar<fvar<var>>, Dynamic, 1> theta1(3, 1), theta2(3, 1), theta3(3, 1);
  theta1 << 0.2, 0.3, 0.5;
  theta2 << 0.1, 0.8, 0.1;
  theta3 << 0.6, 0.1, 0.3;

  Matrix<fvar<fvar<var>>, Dynamic, 1> alpha1(3, 1), alpha2(3, 1), alpha3(3, 1);
  alpha1 << 1.0, 1.0, 1.0;
  alpha2 << 6.2, 3.5, 9.1;
  alpha3 << 2.5, 7.4, 6.1;

  std::vector<Matrix<fvar<fvar<var>>, Dynamic, 1>> theta_vec(3);
  theta_vec[0] = theta1;
  theta_vec[1] = theta2;
  theta_vec[2] = theta3;

  std::vector<Matrix<fvar<fvar<var>>, Dynamic, 1>> alpha_vec(3);
  alpha_vec[0] = alpha1;
  alpha_vec[1] = alpha2;
  alpha_vec[2] = alpha3;

  Matrix<fvar<fvar<var>>, Dynamic, 1> result(3);
  result[0] = dirichlet_log(theta1, alpha1);
  result[1] = dirichlet_log(theta2, alpha2);
  result[2] = dirichlet_log(theta3, alpha3);

  fvar<fvar<var>> out = dirichlet_log(theta_vec, alpha_vec);

  EXPECT_FLOAT_EQ(result.val().val().val().sum(), out.val_.val_.val());
  EXPECT_FLOAT_EQ(result.d().val().val().sum(), out.d_.val_.val());

  result[0] = dirichlet_log(theta1, alpha1);
  result[1] = dirichlet_log(theta2, alpha1);
  result[2] = dirichlet_log(theta3, alpha1);

  out = dirichlet_log(theta_vec, alpha1);

  EXPECT_FLOAT_EQ(result.val().val().val().sum(), out.val_.val_.val());
  EXPECT_FLOAT_EQ(result.d().val().val().sum(), out.d_.val_.val());

  result[0] = dirichlet_log(theta1, alpha1);
  result[1] = dirichlet_log(theta1, alpha2);
  result[2] = dirichlet_log(theta1, alpha3);

  out = dirichlet_log(theta1, alpha_vec);

  EXPECT_FLOAT_EQ(result.val().val().val().sum(), out.val_.val_.val());
  EXPECT_FLOAT_EQ(result.d().val().val().sum(), out.d_.val_.val());
}


//==================================================================================================
/*
  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)
*/
//==================================================================================================
//! [reduce-phase]
#include <boost/simd/algorithm/reduce.hpp>
#include <iostream>
#include <numeric>

int main()
{
  float values[] = {1.f,2.f,3.f,4.f,5.f,6.f,7.f,8.f,9.f,};

  std::cout << "SIMD reduce    :"
            << boost::simd::reduce( &values[0], &values[0]+9, 1.f
                                  , boost::simd::multiplies
                                  )
            << std::endl;

  return 0;
}
//! [reduce-phase]


#define BOOST_TEST_MODULE "Test General Distance Function"

#include <boost/test/unit_test.hpp>
#include <cmath>
#include <iostream>

#include "distance/Euclidean.hpp"
#include "distance/Chebyshev.hpp"
#include "distance/Manhattan.hpp"
#include "distance/Cosine.hpp"
#include "distance/Distance.hpp"
#include "Exception.hpp"
#include "TimeSeries.hpp"

using namespace genex;
using std::isinf;

#define TOLERANCE 1e-9
struct MockData
{
  dist_t euclidean_dist = pairwiseDistance<Euclidean, double>;
  dist_t manhattan_dist = pairwiseDistance<Manhattan, double>;
  dist_t chebyshev_dist = pairwiseDistance<Chebyshev, double>;

  dist_t euclidean_warped_dist = warpedDistance<Euclidean, double>;
  dist_t manhattan_warped_dist = warpedDistance<Manhattan, double>;
  dist_t chebyshev_warped_dist = warpedDistance<Chebyshev, double>;

  data_t dat_1[5] = {1, 2, 3, 4, 5};
  data_t dat_2[5] = {11, 2, 3, 4, 5};

  data_t dat_3[2] = {2, 4};
  data_t dat_4[5] = {2, 2, 2, 4, 4};

  data_t dat_5[4] = {1, 2, 2, 4};
  data_t dat_6[4] = {1, 2, 4, 5};

  data_t dat_7[4] = {2, 2, 2, 2};
  data_t dat_8[4] = {20, 20, 20, 15};

  data_t dat_9[6]  = {2, 2, 2, 2, 2, 2};
  data_t dat_10[6] = {4, 3, 3, 3, 3, 3};

  data_t dat_11[7] = {4, 3, 5, 3, 5, 3, 4};
  data_t dat_12[7] = {4, 3, 3, 1, 1, 3, 4};

  data_t dat_13[10] = {0, 2, 3, 5, 8, 6, 3, 2, 3, 5};
  data_t dat_14[7] =  {8, 4, 6, 1, 5, 10, 9};
};

BOOST_AUTO_TEST_CASE( general_distance, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  TimeSeries ts_1(data.dat_1, 0, 0, 5);
  TimeSeries ts_2(data.dat_2, 0, 0, 5);

  data_t total_1 = data.euclidean_dist(ts_1, ts_2, INF, gNoMatching);
  BOOST_TEST( total_1, 2.0 );

  data_t total_2 = data.manhattan_dist(ts_1, ts_2, INF, gNoMatching);
  BOOST_TEST( total_2, 2.0 );

  data_t total_3 = data.chebyshev_dist(ts_1, ts_2, INF, gNoMatching);
  BOOST_TEST( total_3, 10.0 );
}

BOOST_AUTO_TEST_CASE( easy_general_warped_distance, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  TimeSeries ts_1{data.dat_1, 0, 0, 2};
  TimeSeries ts_2{data.dat_2, 0, 0, 2};

  TimeSeries ts_3{data.dat_3, 0, 0, 2};
  TimeSeries ts_4{data.dat_4, 0, 0, 5};

  TimeSeries ts_5{data.dat_5, 0, 0, 4};
  TimeSeries ts_6{data.dat_6, 0, 0, 4};

  TimeSeries ts_11{data.dat_11, 0, 0, 7};
  TimeSeries ts_12{data.dat_12, 0, 0, 7};

  setWarpingBandRatio(1.0);

  data_t total_0 = data.euclidean_warped_dist(ts_1, ts_2, INF, gNoMatching);
  BOOST_TEST( total_0 == sqrt(100.0) / (2 * 2) );

  data_t total_1 = data.euclidean_warped_dist(ts_3, ts_4, INF, gNoMatching);
  BOOST_TEST( total_1 == 0.0 );

  data_t total_2 = data.manhattan_warped_dist(ts_3, ts_4, INF, gNoMatching);
  BOOST_TEST( total_2 == 0.0 );

  data_t total_3 = data.chebyshev_warped_dist(ts_3, ts_4, INF, gNoMatching);
  BOOST_TEST( total_3 == 0.0 );

  data_t total_4 = data.euclidean_warped_dist(ts_5, ts_6, INF, gNoMatching);
  BOOST_TEST( total_4 == sqrt(1.0) / (2 * 4.0) );

  data_t total_5 = data.manhattan_warped_dist(ts_5, ts_6, INF, gNoMatching);
  BOOST_TEST( total_5 == 1.0/ (2 * 4.0) );

  data_t total_6 = data.chebyshev_warped_dist(ts_5, ts_6, INF, gNoMatching);
  BOOST_TEST( total_6 == 1.0 );

  data_t total_7 = data.euclidean_warped_dist(ts_11, ts_12, INF, gNoMatching);
  data_t result_7 = sqrt(12.0)/ (2 * 7);
  BOOST_TEST( total_7 == result_7 );

  data_t total_8 = data.manhattan_warped_dist(ts_11, ts_12, INF, gNoMatching);
  BOOST_TEST( total_8 == 8.0 / (2 * 7) );

  data_t total_9 = data.chebyshev_warped_dist(ts_11, ts_12, INF, gNoMatching);
  BOOST_TEST( total_9 == (2.0) );

  matching_t matching_1 = {};
  matching_t matching_1_test = {{0, 0}, {1, 1}};
  data_t total_10 = data.euclidean_warped_dist(ts_1, ts_2, INF, matching_1);
  BOOST_TEST( total_10 == sqrt(100.0) / (2 * 2) );
  BOOST_TEST( matching_1 == matching_1_test);

  matching_t matching_2 = {};
  matching_t matching_2_test = {{0, 0}, {0, 1}, {0, 2}, {1, 3}, {1, 4}};
  data_t total_11 = data.euclidean_warped_dist(ts_3, ts_4, INF, matching_2);
  BOOST_TEST( total_11 == 0 );
  BOOST_TEST( matching_2 == matching_2_test);

  matching_t matching_3 = {};
  matching_t matching_3_test = {{0, 0}, {0, 1}, {0, 2}, {1, 3}, {1, 4}};
  data_t total_12 = data.manhattan_warped_dist(ts_3, ts_4, INF, matching_3);
  BOOST_TEST( total_12 == 0 );
  BOOST_TEST( matching_3 == matching_3_test);

  matching_t matching_4 = {};
  matching_t matching_4_test = {{0, 0}, {0, 1}, {0, 2}, {1, 3}, {1, 4}};
  data_t total_13 = data.chebyshev_warped_dist(ts_3, ts_4, INF, matching_4);
  BOOST_TEST( total_13 == 0 );
  BOOST_TEST( matching_4 == matching_4_test);

  matching_t matching_5 = {};
  matching_t matching_5_test = {{0, 0}, {1, 1}, {2, 1}, {3, 2}, {3, 3}};
  data_t total_14 = data.euclidean_warped_dist(ts_5, ts_6, INF, matching_5);
  BOOST_TEST( total_14 == sqrt(1.0) / (2 * 4.0) );
  BOOST_TEST( matching_5 == matching_5_test);

  matching_t matching_6 = {};
  matching_t matching_6_test = {{0, 0}, {1, 1}, {2, 1}, {3, 2}, {3, 3}};
  data_t total_15 = data.manhattan_warped_dist(ts_5, ts_6, INF, matching_6);
  BOOST_TEST( total_15 == sqrt(1.0) / (2 * 4.0) );
  BOOST_TEST( matching_6 == matching_6_test);

  matching_t matching_7 = {};
  matching_t matching_7_test = {{0, 0}, {1, 1}, {2, 1}, {3, 2}, {3, 3}};
  data_t total_16 = data.chebyshev_warped_dist(ts_5, ts_6, INF, matching_7);
  BOOST_TEST( total_16 == 1.0 );
  BOOST_TEST( matching_7 == matching_7_test);
}

BOOST_AUTO_TEST_CASE( easy_gwd_dropout, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  TimeSeries ts_3{data.dat_3, 0, 0, 2};
  TimeSeries ts_4{data.dat_4, 0, 0, 5};

  TimeSeries ts_7{data.dat_7, 0, 0, 4};
  TimeSeries ts_8{data.dat_8, 0, 0, 4};

  data_t total_1 = data.euclidean_warped_dist(ts_3, ts_4, 5, gNoMatching);
  BOOST_TEST( total_1 == 0.0 );

  data_t total_2 = data.manhattan_warped_dist(ts_3, ts_4, 5, gNoMatching);
  BOOST_TEST( total_2 == 0.0 );

  data_t total_3 = data.chebyshev_warped_dist(ts_3, ts_4, 5, gNoMatching);
  BOOST_TEST( total_3 == 0.0 );

  data_t total_5 = data.manhattan_warped_dist(ts_7, ts_8, 5, gNoMatching);
  BOOST_TEST( isinf(total_5) );

  data_t total_6 = data.chebyshev_warped_dist(ts_7, ts_8, 5, gNoMatching);
  BOOST_TEST( isinf(total_6) );
}

BOOST_AUTO_TEST_CASE( gwd_different_distances, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  TimeSeries ts_9{data.dat_9, 0, 0, 6};
  TimeSeries ts_10{data.dat_10, 0, 0, 6};

  data_t total_1 = data.euclidean_warped_dist(ts_9, ts_10, INF, gNoMatching);
  BOOST_TEST( total_1 == sqrt(9.0)/(2 * 6) );

  data_t total_2 = data.manhattan_warped_dist(ts_9, ts_10, INF, gNoMatching);
  BOOST_TEST( total_2 == 7.0/ (2 * 6) );

  data_t total_3 = data.chebyshev_warped_dist(ts_9, ts_10, INF, gNoMatching);
  BOOST_TEST( total_3 == 2.0 );
}

BOOST_AUTO_TEST_CASE( get_distance_metric, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  TimeSeries ts_for_function_call{data.dat_3, 0, 0, 2};
  const dist_t d = getDistanceFromName("euclidean");
  BOOST_CHECK(d);
}

BOOST_AUTO_TEST_CASE( distance_not_found )
{
  BOOST_CHECK_THROW( getDistanceFromName("oracle"), GenexException );
}

BOOST_AUTO_TEST_CASE( keogh_lower_bound, *boost::unit_test::tolerance(TOLERANCE) )
{
  MockData data;
  TimeSeries a{data.dat_13, 10};
  TimeSeries b{data.dat_14, 7};

  setWarpingBandRatio(0.2);
  data_t klb = keoghLowerBound(a, b, 10);

  BOOST_TEST( klb == sqrt(31.0) / (2 * 10) );
}


#include <iostream>
#include <cmath>
#include <algorithm>
#include <vector>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/lexical_cast.hpp>
using namespace boost::multiprecision;
using namespace std;
void solve(cpp_int x, vector<cpp_int> &v) {
    if (x > 3234566667) return;
    v.push_back(x);
    cpp_int base = x * 10, m = x % 10;
    for (int i = -1; i <= 1; i++) {
        if (m + i >= 0 && m + i <= 9) solve(base + (m + i), v);
    }
}
int main() {
    int k; cin >> k;
    vector<cpp_int> v;
    for (int i = 1; i <= 9; i++) solve(i, v);
    sort(v.begin(), v.end());
    cout << v[k - 1] << endl;
}


//------------------------------------------------------------------------------
/*
    This file is part of rippled: https://github.com/ripple/rippled
    Copyright (c) 2012-2015 Ripple Labs Inc.

    Permission to use, copy, modify, and/or distribute this software for any
    purpose  with  or without fee is hereby granted, provided that the above
    copyright notice and this permission notice appear in all copies.

    THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
    WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
    MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
    ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
    WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
    ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

#include <ripple/basics/mulDiv.h>
#include <ripple/basics/contract.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <limits>
#include <utility>

namespace ripple
{

std::pair<bool, std::uint64_t>
mulDiv(std::uint64_t value, std::uint64_t mul, std::uint64_t div)
{
    using namespace boost::multiprecision;

    uint128_t result;
    result = multiply(result, value, mul);

    result /= div;

    auto constexpr limit = std::numeric_limits<std::uint64_t>::max();

    if (result > limit)
        return { false, limit };

    return { true, static_cast<std::uint64_t>(result) };
}

} // ripple


#include <CGAL/Polyhedron_3.h>
#include <CGAL/Polyhedron_items_with_id_3.h>
#include <CGAL/boost/graph/graph_traits_Polyhedron_3.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/extract_mean_curvature_flow_skeleton.h>
#include <CGAL/Polygon_mesh_processing/connected_components.h>
#include <CGAL/IO/Polyhedron_iostream.h>

#include <boost/foreach.hpp>

#include <fstream>
#include <set>
#include <queue>

typedef CGAL::Simple_cartesian<double>                               Kernel;
typedef Kernel::Point_3                                              Point;
typedef Kernel::Vector_3                                             Vector;
typedef CGAL::Polyhedron_3<Kernel, CGAL::Polyhedron_items_with_id_3> Polyhedron;
typedef boost::graph_traits<Polyhedron>::vertex_descriptor           vertex_descriptor;

typedef CGAL::Mean_curvature_flow_skeletonization<Polyhedron>        Mean_curvature_skeleton;
typedef Mean_curvature_skeleton::Skeleton                            Skeleton;
typedef boost::graph_traits<Skeleton>::vertex_descriptor             vertex_desc;
typedef boost::graph_traits<Skeleton>::edge_descriptor             edge_desc;

// The input of the skeletonization algorithm must be a pure triangular closed
// mesh and has only one component.
bool is_mesh_valid(Polyhedron& pMesh)
{
  if (!pMesh.is_closed())
  {
    std::cerr << "The mesh is not closed.";
    return false;
  }
  if (!pMesh.is_pure_triangle())
  {
    std::cerr << "The mesh is not a pure triangle mesh.";
    return false;
  }

  // the algorithm is only applicable on a mesh
  // that has only one connected component
  std::size_t num_component;
  CGAL::Counting_output_iterator output_it(&num_component);
  CGAL::internal::corefinement::extract_connected_components(pMesh, output_it);
  ++output_it;
  if (num_component != 1)
  {
    std::cerr << "The mesh is not a single closed mesh. It has "
              << num_component << " components.";
    return false;
  }
  return true;
}

int main()
{
  Polyhedron mesh;
  std::ifstream input("data/elephant.off");

  if ( !input || !(input >> mesh) || mesh.empty() ) {
    std::cerr << "Cannot open data/elephant.off" << std::endl;
    return EXIT_FAILURE;
  }
  if (!is_mesh_valid(mesh)) {
    return EXIT_FAILURE;
  }

  Skeleton skeleton;

  CGAL::extract_mean_curvature_flow_skeleton(mesh, skeleton);

  if (num_vertices(skeleton) == 0)
  {
    std::cerr << "The number of skeletal points is zero!\n";
    return EXIT_FAILURE;
  }

// check all vertices are seen exactly once
{
  std::set<vertex_descriptor> visited;
  BOOST_FOREACH(vertex_desc v, vertices(skeleton))
  {
    BOOST_FOREACH(vertex_descriptor vd, skeleton[v].vertices)
      if (!visited.insert(vd).second)
      {
        std::cerr << "A vertex was seen twice!\n";
        return EXIT_FAILURE;
      }
  }

  BOOST_FOREACH(vertex_descriptor vd, vertices(mesh))
  {
    if (!visited.count(vd))
    {
      std::cerr << "A vertex was not seen!\n";
      return EXIT_FAILURE;
    }
  }
}

// check the skeleton is connected
{
  std::queue<vertex_desc> qu;
  std::set<vertex_desc> visited;

  qu.push(*vertices(skeleton).first);
  visited.insert(qu.back());

  while (!qu.empty())
  {
    vertex_desc cur = qu.front();
    qu.pop();

    BOOST_FOREACH(edge_desc ed, in_edges(cur, skeleton))
    {
      vertex_desc next = source(ed, skeleton);
      if (visited.insert(next).second)
        qu.push(next);
    }
  }

  BOOST_FOREACH(vertex_desc vd, vertices(skeleton))
  {
    if (!visited.count(vd))
    {
      std::cerr << "Skeleton curve is not fully connected!\n";
      return EXIT_FAILURE;
    }
  }
}
  std::cout << "Pass connectivity test.\n";
  return EXIT_SUCCESS;
}


/******************************************************
    Author : shipeng_liu
    Email : 1196075299@qq.com
    Description: Adaptive dynamic programming
******************************************************/
#include <cmath>
#include<iostream>
#include <fstream>
#include <iomanip>
#include <Eigen/Dense>
#include "rambot_controller/src/controller/adp_learning/critic_network.cpp"
using namespace std;

/******************************************************************
    Action Network
    Input: S, diff_s
    number of hidden_layer: 1
    number of active_function: hidden_number
    output: steer_cmd
    parameter_layer1: parameters from input to hidden_layer
    parameter_layer2: parameters from hidden_layer to output
******************************************************************/

class action_network {

  private:

    double learning_rate = 0.00001 ; //learning rate
    double reward_count = 1;
    Eigen::MatrixXd parameter_layer1;
    Eigen::MatrixXd parameter_layer2;
    Eigen::MatrixXd input_variable;
    Eigen::MatrixXd hidden_layer;
    Eigen::MatrixXd p_hidden_layer;
    double output;
    int hidden_number;
    double deriva_J_u;
    static double active_function(double input) {
        // 1 - exp( - qi(t) ) / 1 + exp( - qi(t) ) )
        double ret = std::exp(- input);
        double tmp = (2)/(1 + ret) - 1;
        return tmp;
    }

  public:

    action_network()
    {
        hidden_number = 4;
        init();
    }

    void init()
    {
        parameter_layer1 = Eigen::MatrixXd::Random(2, hidden_number);
        parameter_layer2 = Eigen::MatrixXd::Random(hidden_number, 1);

        hidden_layer = Eigen::MatrixXd::Random(1, hidden_number);
        p_hidden_layer = Eigen::MatrixXd::Random(1, hidden_number);
        input_variable = Eigen::MatrixXd::Random(1,2);
        output = 0;

    }

    double output_cmd(double s, double diff_s)
    {
        input_variable(0,0) = s;
        input_variable(0,1) = diff_s;
        //cout << parameter_layer1 << endl;
        //cout << parameter_layer2 << endl;
        hidden_layer = input_variable * parameter_layer1;

        for (int i = 0; i < hidden_number; i++)
        {
            p_hidden_layer(0,i) = active_function(hidden_layer(0,i));
            // limit the p_Hidden_layer
            if (p_hidden_layer(0,i) < 0.000001 && p_hidden_layer(0,i) > 0 )
                p_hidden_layer(0,i) = 0.000001;
            else if (p_hidden_layer(0,i) > -0.000001 && p_hidden_layer(0,i) < 0)
                p_hidden_layer(0,i) = - 0.000001;
        }
        Eigen::MatrixXd test = p_hidden_layer * parameter_layer2;
        output = test(0,0);
        output = active_function(output);
        return output;
    }

    void update_weight(double J_cost, critic_network *critic_network)
    {

        double reference_signal = 0;
        double eat = J_cost - reference_signal;
        cout << "\naction_network_information:\n";
        cout << left << setw(20) << "[ refer: " << right
             << setw(20) << reference_signal << "]" << endl;
        cout << left << setw(20) << "[ J_cost: " << right
             << setw(20) << J_cost << "]" << endl;
        cout << left << setw(20) << "[ eat: " << right
             << setw(20) << eat << "]" << endl;
        cout << left << setw(20) << "[ p_hidden_layer: " << right
             << setw(20) << p_hidden_layer << "]" << endl;
        cout << "[ p_hidden_layer: " << endl;
        cout << p_hidden_layer << endl;

        // cout <<  "[ parameter_layer1: " << endl;
        // cout << parameter_layer1 << endl;

        // cout << "[ parameter_layer2: " << endl;
        // cout << parameter_layer2 << endl;

        // compute the derivative of J to U
        for (int l = 0; l < 4; l++)
        {
            deriva_J_u += ( critic_network->return_parameter_layer2()(l,0) ) * 0.5 * (1 - pow(critic_network->return_p_hidden_layer()(0,l),2)) * critic_network->return_parameter_layer1()(2,l);
        }
        for (int j = 0; j < hidden_number; j++)
        {
            //compute the parameter_layer1
            for (int k = 0; k < 2; k++)
            {

                double derivative_1 = reward_count * eat *  0.5 * (1-pow(output,2)) * parameter_layer2(j,0) * 0.5 * (1 - pow(p_hidden_layer(0,j),2)) * input_variable(0,k) * deriva_J_u;
                double delta_weight1 = learning_rate * (- derivative_1);

                parameter_layer1(k,j) += delta_weight1;
            }

            // compute the parameter_layer2

            double derivative =  reward_count * eat * 0.5 * (1-pow(output,2)) *(p_hidden_layer(0,j)) * deriva_J_u;

            double delta_weight = learning_rate * (- derivative);
            parameter_layer2(j,0) += delta_weight;

        }

    }
};


#pragma once

#include <Eigen/Core>
#include <Eigen/Dense>
#include <vector>
#include <random>
#include <numeric>
#include <iostream>

using namespace std;
using namespace Eigen;

template <typename state>
struct ParticleFilter{
    ParticleFilter(int number_of_particles, int states, function<void (int id)> process, function<double (int id)> cost,
                   VectorXd &start, double std):
            number_of_particles(number_of_particles), process(process), cost(cost), start(start){
        particles.resize(number_of_particles, VectorXd(states));
        weights.resize(number_of_particles, 1.0/number_of_particles);
        costs.resize(number_of_particles,RAND_MAX);
        cumsum.resize(number_of_particles);
        indx.resize(number_of_particles);

        distribution = normal_distribution<double>(0,std);
        state_length = particles.begin()->size();

        resample(true);
    }

    void step(){
        double sum_of_elems = 0.0;
        for(uint i=0;i<number_of_particles;i++){
            process(i);
            costs[i] = cost(i);
            sum_of_elems += costs[i];
        }

        double Neff = 0.0, weight_sum = 0.0;
        for(uint i=0;i<number_of_particles;i++){
            weights[i] = costs[i]/sum_of_elems;
            weight_sum += weights[i];
            Neff += pow(weights[i],2.0);
        }

        Neff = 1.0/Neff;

        if(Neff < number_of_particles/2.0) {
            cout <<"resample, Neff: " << Neff << endl;
            resample(false, Neff);
        }
    }

    double result(state &winner, int *index){
        double val;
        minimum<double>(costs, val, index);
        winner = particles[*index];
        return val;
    }

    function<double (int id)> cost;
    function<void (int id)> process;

    void resample(bool all, int N = 0){
        if(all){ //resample all
            for(state &particle:particles){
                particle = start;
                for(int i=0;i<state_length;i++){
                    particle(i) += distribution(generator);
                }
            }
        }else{ // Sampling Importance Resampling
            vector<size_t> indexes = sort_indexes<double>(weights);
            for(uint i=0;i<N;i++){
                particles[indexes[i]] = particles[indexes[number_of_particles-1-i]];
                double w0 = weights[indexes[i]], w1 = indexes[number_of_particles-1-i];

            }

        }
    }

    template<typename T>
    void minimum(vector<T> &v, T &val, int *index ){
        *index = 0;
        val = v[0];
        for(uint i=1; i<v.size(); i++){
            if(v[i]<val){
                val = v[i];
                *index = i;
            }
        }
    }

    template <typename T>
    vector<size_t> sort_indexes(const vector<T> &v) {

        // initialize original index locations
        vector<size_t> idx(v.size());
        iota(idx.begin(), idx.end(), 0);

        // sort indexes based on comparing values in v
        sort(idx.begin(), idx.end(),
             [&v](size_t i1, size_t i2) {return v[i1] > v[i2];});

        return idx;
    }

    vector<state> particles;
    vector<double> weights, costs;
    vector<double> cumsum;
    vector<int> indx;
    int number_of_particles;
    default_random_engine generator;
    normal_distribution<double> distribution;
    int state_length;
    VectorXd start;
};


/*
 * Path-related utility functions.
 *
 * Copyright (C) 2014-2015 DubinsPathPlanner.
 * Created by David Goodman <dagoodma@gmail.com>
 * Redistribution and use of this file is allowed according to the terms of the MIT license.
 * For details see the LICENSE file distributed with DubinsPathPlanner.
 */
#include <stdio.h>
#include <math.h>
#include <algorithm>

#include <Eigen/Dense>

#include <DubinsCurves.h>

#include <dpp/basic/basic.h>
#include <dpp/basic/Logger.h>
#include <dpp/basic/Path.h>
#include <dpp/basic/Util.h>

using Eigen::Vector3d;
using Eigen::Vector2d;

#define HEADING_TOLERANCE          1E-10 // upperbound for Euclidean metric, radius safe?

namespace dpp {

/**
 * Calculate the shortest Dubins' path distance to the node. Note that all angles
 * used in this function are heading angles from 0 at the y-axis, and
 * counter-clockwise is positive.
 * @FIXME Add RLR and LRL curves. Does this fix dist > 3*r?
 */
double dubinsPathLength(VehicleConfiguration &Cs, VehicleConfiguration &Ce,
    double turnRadius) {
    double r = turnRadius; // shorter name
    Vector2d Ps = Vector2d(Cs.x(), Cs.y()),
        Pe = Vector2d(Ce.x(), Ce.y());
    double Xs = Cs.m_heading,
        Xe = Ce.m_heading;
    double dist = (Ps - Pe).norm();

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Given Cs=" << Cs << ", Ce=" << Ce << ", r=" << r << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Got dist=" << dist << " compared to r=" << r << "." << std::endl;

    // Added tolerance to avoid numerical instability for paths with no curviture
    // TODO compute curviture as ratio w.r.t. turn radius?
    // FIXME this does not take into account the feasability of the path!
    //        Should add an isReachable( func)
    if (fabs(Xs - Xe) <= HEADING_TOLERANCE) {
        Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Using straight line L=" << (Pe - Ps).norm() << ". |Xs - Xe| = "
            << fabs(Xs - Xe) << " <= " << HEADING_TOLERANCE << std::endl;
        return (Pe - Ps).norm();
    }

    //DPP_ASSERT(dist >= 3.0 * r);
    // FIXME: return an unfeasible path macro, eg -1
    // FIXME: improve this function so that we don't have to use the dubins-curves library
    //         this is needed for boustrophedon algorithm which does not care if the
    //         point dist is >= 3.0 * r
    // note: Detect if the final heading is close to the opposite of the initial,
    //       to allow for implementing CCC-type paths (ie. U-turns) via Dubins
    //       Corollarry to theorem 1. Though this may not satisfy all cases where
    //       no feasible paths exist. "U-turn" is either: {RLR or LRL}?
    if (dist < 3.0 * r) {
        //return DPP_MAX_EDGE_COST;
        double *q0, *q1;
        Cs.asArray(&q0);
        Ce.asArray(&q1);
        q0[2] = dpp::headingToAngle(q0[2]);
        q1[2] = dpp::headingToAngle(q1[2]);
        DubinsCurves::DubinsPath path;
        DubinsCurves::dubins_init( q0, q1, turnRadius, &path);
        double expectedLength = DubinsCurves::dubins_path_length(&path);
        return expectedLength;
    }
    //if (dist < 3.0 * r) {
    //  std::domain_error("distance must be larger than 3*r");
    //}

    // Convert headings to circular angles
    double alpha = headingToAngle(Xs),
           beta = headingToAngle(Xe);

    // Find circle center points for each case
    Vector3d R_rs(cos(alpha - M_PI/2.0), sin(alpha - M_PI/2.0), 0.0),
        R_ls(cos(alpha + M_PI/2.0), sin(alpha + M_PI/2.0), 0.0),
        R_re(cos(beta - M_PI/2.0), sin(beta - M_PI/2.0), 0.0),
        R_le(cos(beta + M_PI/2.0), sin(beta + M_PI/2.0), 0.0);

    Vector3d PC_rs(Cs.x(), Cs.y(), 0.0),
        PC_ls(Cs.x(), Cs.y(), 0.0),
        PC_re(Ce.x(), Ce.y(), 0.0),
        PC_le(Ce.x(), Ce.y(), 0.0);

    PC_rs = PC_rs.transpose() + r*R_rs.transpose();
    PC_ls = PC_ls.transpose() + r*R_ls.transpose();
    PC_re = PC_re.transpose() + r*R_re.transpose();
    PC_le = PC_le.transpose() + r*R_le.transpose();

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_rs: (" << PC_rs.x() << "," << PC_rs.y()
        << "," << PC_rs.z() << "," <<")." << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_ls: (" << PC_ls.x() << "," << PC_ls.y()
        << "," << PC_ls.z() << "," <<")." << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_re: (" << PC_re.x() << "," << PC_re.y()
        << "," << PC_re.z() << "," <<")." << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "PC_le: (" << PC_le.x() << "," << PC_le.y()
        << "," << PC_le.z() << "," <<")." << std::endl;

    // Case I, R-S-R
    double x = headingBetween(PC_rs, PC_re);
    double L1 = (PC_rs - PC_re).norm()
        + r*wrapAngle(2.0 * M_PI + wrapAngle(x - M_PI/2.0) - wrapAngle(Xs - M_PI/2.0))
        + r*wrapAngle(2.0 * M_PI + wrapAngle(Xe - M_PI/2.0) - wrapAngle(x - M_PI/2.0));

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L1: " << L1 << ", with x=" << x << std::endl;

    // Case II, R-S-L
    double ls = (PC_le - PC_rs).norm();
    x = headingBetween(PC_rs, PC_le);
    double x2 = x - M_PI/2.0 + asin(2.0*r/ls);
    double L2 = sqrt(ls*ls - 4*r*r) + r*wrapAngle(2.0*M_PI + wrapAngle(x2)
        - wrapAngle(Xs - M_PI/2.0)) + r*wrapAngle(2.0*M_PI + wrapAngle(x2 + M_PI)
        - wrapAngle(Xe + M_PI/2.0));
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L2: " << L2 << " with ls=" << ls << ", x=" << x << ", x2=" << x2 << std::endl;

    // Case III, L-S-R
    ls = (PC_re - PC_ls).norm();
    x = headingBetween(PC_ls, PC_re);
    double ratioOA = 2.0*r/ls;
    // Bound the ratio from -1 to 1
    ratioOA = std::max<double> (-1.0, ratioOA);
    ratioOA = std::min<double> (1.0, ratioOA);
    DPP_ASSERT(ratioOA <= 1.0 && ratioOA >= -1.0);
    x2 = acos(ratioOA);
    double L3 = sqrt(ls*ls - 4*r*r) + r*wrapAngle(2.0*M_PI + wrapAngle(Xs + M_PI/2.0)
        - wrapAngle(x + x2)) + r*wrapAngle(2.0*M_PI + wrapAngle(Xe - M_PI/2.0)
        - wrapAngle(x + x2 - M_PI));

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L3: " << L3 << " with ls=" << ls << ", x=" << x << ", x2="
        << x2 << std::endl;

    // Case IV, L-S-L
    x = headingBetween(PC_ls, PC_le);
    double L4 = (PC_ls - PC_le).norm() + r*wrapAngle(2.0*M_PI + wrapAngle(Xs + M_PI/2.0)
        - wrapAngle(x + M_PI/2.0)) + r*wrapAngle(2.0*M_PI + wrapAngle(x + M_PI/2.0)
        - wrapAngle(Xe + M_PI/2.0));

    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "L4: " << L4 << ", with x=" << x << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Comparing L1=" << L1 << " L2=" << L2 << " L3=" << L3 << " L4="
        << L4 << std::endl;

    return std::min({L1, L2, L3, L4});
}

/**
 * Finds the cost of the shortest dubins path over the given tour with headings.
 * If returnCost is true, the cost of returning back to the first node in the
 * tour will be included.
 * @param[in] G graph with nodes to tour
 * @param[in] GA attributes of the graph
 * @param[in] Tour ordered list of nodes to visit
 * @param[in] Headings for vehicle at each node
 * @param[in] turnRadius of the vehicle
 * @param[in] returnEdge whether to add a return edge back to the first node
 */
double dubinsTourCost(ogdf::Graph &G, ogdf::GraphAttributes &GA,
    ogdf::List<ogdf::node> &Tour, ogdf::NodeArray<double> &Headings,
    double turnRadius, bool returnCost) {
    ogdf::ListIterator<ogdf::node> iter;
    double cost = 0.0;

    if (Tour.size() < 2) {
        Logger::logWarn(DPP_LOGGER_VERBOSE_1) << "Zero cost for an empty tour." << std::endl;
        return 0.0;
    }

    // Add the return edge if necessary
    ogdf::List<ogdf::node> modTour(Tour);
    ogdf::node lastNode = modTour.back();
    if (returnCost && lastNode != *(modTour.begin())) {
        modTour.pushBack(*(Tour.begin()));
    }
    else if (!returnCost && lastNode == *(modTour.begin())) {
        modTour.popBack();
    }

    int m = modTour.size() - 1;
    int i = 0; // edge index
    for ( iter = modTour.begin(); (i < m && iter != modTour.end()); iter++ ) {
        ogdf::node u = *iter, v = *(iter.succ());

        VehicleConfiguration Cu(GA.x(u), GA.y(u), Headings(u)),
                      Cv(GA.x(v), GA.y(v), Headings(v));
        cost += dubinsPathLength(Cu, Cv, turnRadius);
        i++;

        Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Found cost " << cost << " from node "
            << GA.idNode(u) << "->" << GA.idNode(v) << ", where headings "
            << GA.idNode(u) << ": " << Headings(u) << ", " << GA.idNode(v) << ": "
            << Headings(v) << std::endl;
    }
    Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Found total cost "
    << cost << " for tour." << std::endl;

    return cost;
}

/**
 * Adds weighted edges to the graph with the cost of the shortest dubins path
 * between each node in the tour. If returnEdge is true, an edge returning to the
 * origin is added. Added edges are saved into the list of edges.
 * @param[in] G graph with nodes to tour
 * @param[in] GA attributes of the graph
 * @param[in] Tour ordered list of nodes to visit
 * @param[in] Headings for vehicle at each node
 * @param[in] turnRadius of the vehicle
 * @param[out] Edges ordered list of edges to build
 * @param[in] returnEdge whether to add a return edge back to the first node
 */
double createDubinsTourEdges(ogdf::Graph &G, ogdf::GraphAttributes &GA,
    ogdf::List<ogdf::node> &Tour, ogdf::NodeArray<double> &Headings,
    double turnRadius, ogdf::List<ogdf::edge> &Edges, bool returnEdge) {
    ogdf::ListIterator<ogdf::node> iter;
    double total_cost = 0.0;

    if (Tour.size() < 2) return 0.0;
    DPP_ASSERT(G.numberOfEdges() < 1);
    //    std::range_error("Cannot have existing edges in graph");
    //}

    // Add the return edge if necessary
    ogdf::List<ogdf::node> modTour(Tour);
    ogdf::node lastNode = modTour.back();
    if (returnEdge && lastNode != *(modTour.begin())) {
        modTour.pushBack(*(Tour.begin()));
    }
    else if (!returnEdge && lastNode == *(modTour.begin())) {
        modTour.popBack();
    }

    int m = modTour.size() - 1;
    int i = 0; // edge index
    for ( iter = modTour.begin(); (i < m && iter != modTour.end()); iter++ ) {
        ogdf::node u = *iter, v = *(iter.succ());

        VehicleConfiguration Cu(GA.x(u), GA.y(u), Headings(u)),
                      Cv(GA.x(v), GA.y(v), Headings(v));
        double cost = dubinsPathLength(Cu, Cv, turnRadius);

        Logger::logDebug(DPP_LOGGER_VERBOSE_3) << "Found cost " << cost << " from node "
            << GA.idNode(u) << "->" << GA.idNode(v) << ", where headings "
            << GA.idNode(u) << ": " << Headings(u) << ", " << GA.idNode(v) << ": "
            << Headings(v) << std::endl;
        //printf("Found cost %0.1f from node %d->%d, where headings %d: %0.1f, %d: %0.1f\n",
        //    cost, GA.idNode(u), GA.idNode(v), GA.idNode(u), Headings(u), GA.idNode(v), Headings(v));

        // Add the edge
        ogdf::edge e = G.newEdge(u,v);
        GA.doubleWeight(e) = cost;
        Edges.pushBack(e);
        total_cost += cost;
        i++;
    }
    Logger::logDebug(DPP_LOGGER_VERBOSE_2) << "Created tour edges with total cost "
    << total_cost << ": " << std::endl;
    Logger::logDebug(DPP_LOGGER_VERBOSE_2) << printEdges(G, GA, Edges);

    return total_cost;
}

/**
 * Computes an adjacency matrix of Dubins path lengths between nodes with the
 * given headings (for ATSP).
 */
void buildDubinsAdjacencyMatrix(ogdf::Graph &G, ogdf::GraphAttributes &GA,
    NodeMatrix<double> &A, ogdf::NodeArray<double> &Headings, double turnRadius) {

    ogdf::node i, j;
    forall_nodes(i, G) {
        VehicleConfiguration Ci(GA.x(i), GA.y(i), Headings(i));

        forall_nodes(j, G) {
            if (i == j) {
                A[i][i] = DPP_MAX_EDGE_COST;
                continue;
            }
            VehicleConfiguration Cj(GA.x(j), GA.y(j), Headings(j));

            double w = dubinsPathLength(Ci, Cj, turnRadius);
            A[i][j] = w;
        }
    }
}

} // namespace DPP


///////////////////////////////////////////////////////////////////////////////
// weighted_variance.hpp
//
//  Copyright 2005 Daniel Egloff, Eric Niebler. 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_ACCUMULATORS_STATISTICS_WEIGHTED_VARIANCE_HPP_EAN_28_10_2005
#define BOOST_ACCUMULATORS_STATISTICS_WEIGHTED_VARIANCE_HPP_EAN_28_10_2005

#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/weighted_sum.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_moment.hpp>

namespace boost { namespace accumulators
{

namespace impl
{
    //! Lazy calculation of variance of weighted samples.
    /*!
        The default implementation of the variance of weighted samples is based on the second moment
        \f$\widehat{m}_n^{(2)}\f$ (weighted_moment<2>) and the mean\f$ \hat{\mu}_n\f$ (weighted_mean):
        \f[
            \hat{\sigma}_n^2 = \widehat{m}_n^{(2)}-\hat{\mu}_n^2,
        \f]
        where \f$n\f$ is the number of samples.
    */
    template<typename Sample, typename Weight, typename MeanFeature>
    struct lazy_weighted_variance_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, Weight>::result_type result_type;

        lazy_weighted_variance_impl(dont_care) {}

        template<typename Args>
        result_type result(Args const &args) const
        {
            extractor<MeanFeature> const some_mean = {};
            result_type tmp = some_mean(args);
            return weighted_moment<2>(args) - tmp * tmp;
        }
    };

    //! Iterative calculation of variance of weighted samples.
    /*!
        Iterative calculation of variance of weighted samples:
        \f[
            \hat{\sigma}_n^2 =
                \frac{\bar{w}_n - w_n}{\bar{w}_n}\hat{\sigma}_{n - 1}^2
              + \frac{w_n}{\bar{w}_n - w_n}\left(X_n - \hat{\mu}_n\right)^2
            ,\quad n\ge2,\quad\hat{\sigma}_0^2 = 0.
        \f]
        where \f$\bar{w}_n\f$ is the sum of the \f$n\f$ weights \f$w_i\f$ and \f$\hat{\mu}_n\f$
        the estimate of the mean of the weighted smaples. Note that the sample variance is not defined for
        \f$n <= 1\f$.
    */
    template<typename Sample, typename Weight, typename MeanFeature, typename Tag>
    struct weighted_variance_impl
      : accumulator_base
    {
        typedef typename numeric::functional::multiplies<Sample, Weight>::result_type weighted_sample;
        // for boost::result_of
        typedef typename numeric::functional::average<weighted_sample, Weight>::result_type result_type;

        template<typename Args>
        weighted_variance_impl(Args const &args)
          : weighted_variance(numeric::average(args[sample | Sample()], numeric::one<Weight>::value))
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            std::size_t cnt = count(args);

            if(cnt > 1)
            {
                extractor<MeanFeature> const some_mean = {};

                result_type tmp = args[parameter::keyword<Tag>::get()] - some_mean(args);

                this->weighted_variance =
                    numeric::average(this->weighted_variance * (sum_of_weights(args) - args[weight]), sum_of_weights(args))
                  + numeric::average(tmp * tmp * args[weight], sum_of_weights(args) - args[weight] );
            }
        }

        result_type result(dont_care) const
        {
            return this->weighted_variance;
        }

    private:
        result_type weighted_variance;
    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::weighted_variance
// tag::immediate_weighted_variance
//
namespace tag
{
    struct lazy_weighted_variance
      : depends_on<weighted_moment<2>, weighted_mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::lazy_weighted_variance_impl<mpl::_1, mpl::_2, weighted_mean> impl;
    };

    struct weighted_variance
      : depends_on<count, immediate_weighted_mean>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::weighted_variance_impl<mpl::_1, mpl::_2, immediate_weighted_mean, sample> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::weighted_variance
// extract::immediate_weighted_variance
//
namespace extract
{
    extractor<tag::lazy_weighted_variance> const lazy_weighted_variance = {};
    extractor<tag::weighted_variance> const weighted_variance = {};
}

using extract::lazy_weighted_variance;
using extract::weighted_variance;

// weighted_variance(lazy) -> lazy_weighted_variance
template<>
struct as_feature<tag::weighted_variance(lazy)>
{
    typedef tag::lazy_weighted_variance type;
};

// weighted_variance(immediate) -> weighted_variance
template<>
struct as_feature<tag::weighted_variance(immediate)>
{
    typedef tag::weighted_variance type;
};

////////////////////////////////////////////////////////////////////////////
//// droppable_accumulator<weighted_variance_impl>
////  need to specialize droppable lazy weighted_variance to cache the result at the
////  point the accumulator is dropped.
///// INTERNAL ONLY
/////
//template<typename Sample, typename Weight, typename MeanFeature>
//struct droppable_accumulator<impl::weighted_variance_impl<Sample, Weight, MeanFeature> >
//  : droppable_accumulator_base<
//        with_cached_result<impl::weighted_variance_impl<Sample, Weight, MeanFeature> >
//    >
//{
//    template<typename Args>
//    droppable_accumulator(Args const &args)
//      : droppable_accumulator::base(args)
//    {
//    }
//};

}} // namespace boost::accumulators

#endif


#ifndef QUADEIGS_H_
#define QUADEIGS_H_

#include <Eigen/Dense>
#include <memory>

class QuadEigs {
public:
  QuadEigs(const Eigen::Ref<const Eigen::MatrixXd> &matM,
           const Eigen::Ref<const Eigen::MatrixXd> &matD,
           const Eigen::Ref<const Eigen::MatrixXd> &matK);

  Eigen::VectorXcd eigenvalues(int m);

private:
  const int ndim_;
  Eigen::MatrixXd matM_, matD_, matK_;
  Eigen::MatrixXd matA_, matB_;
};

#endif


#include <Rcpp.h>
#include <boost/multi_array.hpp>

using namespace Rcpp;

// [[Rcpp::plugins("cpp11")]]
// [[Rcpp::depends(BH)]]

//'Estep in girt.
//'
//'@param xall Item response matrix.
//'@param t0 item parameter vector
//'@param Xm node of theta dist.
//'@param Wm weight of theta dist.
//'@param group a vector.
//'@param ind a design matrix for group.
//'@param resp a design matrix for person.
//'@param D factor constant.
//'@param MLL a vector
//'@export
// [[Rcpp::export]]

List Estep_irt(
  IntegerMatrix xall,
  NumericMatrix t0, // a, b, c
  NumericVector Xm,
  NumericMatrix Wm,
  IntegerVector group,
  IntegerMatrix ind, // design matrix
  IntegerMatrix resp, // design matrix
  double D,
  NumericVector MLL
){

  int nj = xall.ncol();// item n
  int ni = xall.nrow(); // subject n
  int N  = Xm.length(); // node n
  int ng = max(group); // group n

  double a,b,c,t,tt,u,x;
  int g,i,m,j;

  boost::multi_array <double, 3> Lim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Gim (boost::extents[ng][ni][N]);
  boost::multi_array <double, 3> Njm (boost::extents[ng][nj][N]);
  boost::multi_array <double, 3> rjm (boost::extents[ng][nj][N]);

  for(g=0; g<ng; g++){
    for(i=0; i<ni; i++){
      if(group[i] != g+1) continue; // 集団に属さない受験者の部分はスキップ
      for(m=0; m<N; m++){
        t = 1;
        x = Xm[m];
        tt = 0;
        for(j=0; j<nj; j++){
          //if(resp(i,j)==0) continue; // NAの反応パタンのところは計算ループから外れる。
          a = t0(j,0);
          if(a == 0) continue;
          b = t0(j,1);
          c = t0(j,2);
          u = xall(i,j);
          if(u == 1){ // 正答した場合の尤度
            tt = c+(1.0-c)/(1.0+exp(-D*a*(x-b)));
          } else if (u == 0) { // 誤答した場合の尤度
            tt = 1.0-(c+(1.0-c)/(1.0+exp(-D*a*(x-b))));
          } else { // 欠測値の場合は尤度を計算しないので，1
            tt = 1;
          }
          t = t * tt; // sum
        }
        Lim[g][i][m] = t;
      }
    }
  }

  // 各受検者のthetaごとに事後分布の重みを計算する。
  double uu;
  double f = 0; // 周辺対数尤度代入用
  double l,w;
  for(int g=0; g<ng; g++){
    for(int i=0; i<ni; i++){
      if(group[i] != g+1) continue; // 集団に属さない受験者の部分はスキップ
      u = 0;
      for(int m=0; m<N; m++){ // 総和を1にするための分母の計算 // sum
        l = Lim[g][i][m];
        w = Wm(m,g);
        uu = l*w;
        u += uu;
      }
      f += log(u);
      for(int m=0; m<N; m++){
        l = Lim[g][i][m];
        w = Wm(m,g);
        Gim[g][i][m] =  l * w / u;
      }

    }
  }
  MLL.push_back(f);
  if(traits::is_nan<REALSXP>(f)){
    // 対数尤度の計算に失敗したら，計算を中止する。
    stop("Can't calculate marginal log likelihood.");
  }

  //Rcout<<"expected frequency of subjects in each nodes calculation.\n";
  double k,kk;
  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の期待度数
      if(ind(g,j) == 0) continue;
      for(int m=0; m<N; m++){
        k = 0;
        for(int i=0; i<ni; i++){ // 欠測値がある場合，項目ごとに受検者数が異なる。
          if(resp(i,j)==0) continue;
          //double d = resp(i,j);
          kk = Gim[g][i][m];
          k += kk; //* d;
        }
        Njm[g][j][m]= k;
      }
    }
  }

  double h,gg,hh;
  for(int g=0; g<ng; g++){
    for(int j=0; j<nj; j++){ // 各分点の正答受検者の期待度数
      if(ind(g,j) == 0) continue;
      for(int m=0; m<N; m++){
        h = 0;
        for(int i=0; i<ni; i++){ // sum
          //double d = resp(i,j);
          if(resp(i,j) == 0) continue;
          u = xall(i,j); // そもそもその項目に回答していない場合は，度数に数え上げない
          gg = Gim[g][i][m];
          hh = u*gg;
          h += hh;
        }
        rjm[g][j][m] = h;
      }
    }
  }

  return List::create(_["Njm"]=Njm, _["rjm"]=rjm, _["Lim"]=Lim, _["Gim"]=Gim, _["MLL"]=MLL);

}


// Copyright Henrik Steffen Gaßmann 2020
//
// Distributed under the Boost Software License, Version 1.0.
//         (See accompanying file LICENSE or copy at
//           https://www.boost.org/LICENSE_1_0.txt)

#pragma once

#include <cstddef>

#include <bit>
#include <type_traits>

#include <boost/endian/arithmetic.hpp>

#include <dplx/cncr/math_supplement.hpp>
#include <dplx/predef/compiler.h>

#include <dplx/dp/detail/type_utils.hpp>
#include <dplx/dp/detail/utils.hpp>

#if defined DPLX_COMP_MSVC_AVAILABLE
#include <intrin.h>
#endif

namespace dplx::dp::detail
{

template <typename T>
constexpr auto find_last_set_bit(T value) noexcept -> int
{
    static_assert(std::is_integral_v<T>);
    static_assert(std::is_unsigned_v<T>);
    static_assert(sizeof(T) <= sizeof(unsigned long long));

    // #LangODR to be resolved after MSVC supports __cpp_lib_bitops
#if __cpp_lib_bitops >= 201907L && __cpp_lib_is_constant_evaluated >= 201811L

    if (std::is_constant_evaluated())
    {
        return (digits_v<T> - 1) - std::countl_zero(value);
    }

#endif

#if defined(DPLX_COMP_GCC_AVAILABLE) || defined(DPLX_COMP_CLANG_AVAILABLE)

    if constexpr (sizeof(T) <= sizeof(unsigned int))
    {
        return (digits_v<unsigned int> - 1)
             ^ __builtin_clz(static_cast<unsigned int>(value));
    }
    else if constexpr (sizeof(T) <= sizeof(unsigned long))
    {
        return (digits_v<unsigned long> - 1)
             ^ __builtin_clzl(static_cast<unsigned long>(value));
    }
    else /*if constexpr (sizeof(T) <= sizeof(unsigned long long))
            see static_assert above */
    {
        return (digits_v<unsigned long long> - 1)
             ^ __builtin_clzll(static_cast<unsigned long long>(value));
    }

#elif defined(DPLX_COMP_MSVC_AVAILABLE)

    unsigned long result;
    if constexpr (sizeof(T) <= sizeof(unsigned long))
    {
        _BitScanReverse(&result, static_cast<unsigned long>(value));
        return static_cast<int>(result);
    }
    else if constexpr (sizeof(T) <= sizeof(unsigned long long))
    {
#if defined(_M_ARM64) || defined(_M_AMD64)

        _BitScanReverse64(&result, static_cast<unsigned long long>(value));
        return static_cast<int>(result);

#else

        static_assert(sizeof(unsigned long) * 2 == sizeof(unsigned long long));

        if (_BitScanReverse(&result, static_cast<unsigned long>(
                                             value >> digits_v<unsigned long>)))
        {
            return static_cast<int>(result + digits_v<unsigned long>);
        }
        else
        {
            _BitScanReverse(&result, static_cast<unsigned long>(value));
            return static_cast<int>(result);
        }
#endif
    }

#else

    return (digits_v<T> - 1) ^ std::countl_zero(value);

#endif
}

template <cncr::unsigned_integer T>
constexpr auto rotl(T const v, int n) noexcept -> T
{
    return (v << n) | (v >> (digits_v<T> - n));
}

template <cncr::integer T, std::endian order>
constexpr auto load(std::byte const *const src) -> T
{
    // static_assert(order == std::endian::native);
    static_assert(order == std::endian::big || order == std::endian::little);

    if (std::is_constant_evaluated())
    {
        static_assert(sizeof(T) <= 8);
        using uT = std::make_unsigned_t<T>;
        if constexpr (order == std::endian::little)
        {
            uT acc = std::to_integer<uT>(src[0]);
            if constexpr (sizeof(acc) >= 2)
            {
                acc |= std::to_integer<uT>(src[1]) << 8;
            }
            if constexpr (sizeof(acc) >= 4)
            {
                acc |= std::to_integer<uT>(src[2]) << 16
                     | std::to_integer<uT>(src[3]) << 24;
            }
            if constexpr (sizeof(acc) == 8)
            {
                acc |= std::to_integer<uT>(src[4]) << 32
                     | std::to_integer<uT>(src[5]) << 40
                     | std::to_integer<uT>(src[6]) << 48
                     | std::to_integer<uT>(src[7]) << 56;
            }
            return static_cast<T>(acc);
        }
        else
        {
            uT acc = std::to_integer<uT>(src[0]) << 56;
            if constexpr (sizeof(acc) >= 2)
            {
                acc |= std::to_integer<uT>(src[1]) << 48;
            }
            if constexpr (sizeof(acc) >= 4)
            {
                acc |= std::to_integer<uT>(src[2]) << 40
                     | std::to_integer<uT>(src[3]) << 32;
            }
            if constexpr (sizeof(acc) == 8)
            {
                acc |= std::to_integer<uT>(src[4]) << 24
                     | std::to_integer<uT>(src[5]) << 16
                     | std::to_integer<uT>(src[6]) << 8
                     | std::to_integer<uT>(src[7]);
            }
            return static_cast<T>(acc >> (64 - digits_v<uT>));
        }
    }
    else
    {
        constexpr auto boostOrder = order == std::endian::little
                                          ? boost::endian::order::little
                                          : boost::endian::order::big;
        return boost::endian::endian_load<T, sizeof(T), boostOrder>(
                reinterpret_cast<unsigned char const *>(src));
        // T assembled;
        // std::memcpy(&assembled, src, sizeof(assembled));
        // return assembled;
    }
}

template <cncr::integer T, std::endian order>
constexpr auto load_partial(const std::byte *data, int num) -> T
{
    static_assert(sizeof(T) <= 8);
    static_assert(order == std::endian::big || order == std::endian::little);

    using uT = std::make_unsigned_t<T>;
    if constexpr (order == std::endian::little)
    {
        uT assembled = 0;
        switch (num)
        {
        case 7:
            if constexpr (sizeof(assembled) == 8)
            {
                assembled |= std::to_integer<uT>(data[6]) << 48;
            }
            [[fallthrough]];
        case 6:
            if constexpr (sizeof(assembled) == 8)
            {
                assembled |= std::to_integer<uT>(data[5]) << 40;
            }
            [[fallthrough]];
        case 5:
            if constexpr (sizeof(assembled) == 8)
            {
                assembled |= std::to_integer<uT>(data[4]) << 32;
            }
            [[fallthrough]];

        case 4:
            if constexpr (sizeof(assembled) >= 4)
            {
                assembled |= std::to_integer<uT>(data[3]) << 24;
            }
            [[fallthrough]];
        case 3:
            if constexpr (sizeof(assembled) >= 4)
            {
                assembled |= std::to_integer<uT>(data[2]) << 16;
            }
            [[fallthrough]];

        case 2:
            if constexpr (sizeof(assembled) >= 2)
            {
                assembled |= std::to_integer<uT>(data[1]) << 8;
            }
            [[fallthrough]];

        case 1:
            assembled |= std::to_integer<uT>(data[0]);
            [[fallthrough]];

        case 0:
            break;
        }
        return assembled;
    }
    else
    {
    }
} // namespace dplx::dp::detail

constexpr auto byte_swap_u32(std::uint32_t const x) noexcept -> std::uint32_t
{
    if (std::is_constant_evaluated())
    {
        // byte_swap adapted from Boost.Endian
        // Copyright 2019 Peter Dimov
        //
        // Distributed under the Boost Software License, Version 1.0.
        // http://www.boost.org/LICENSE_1_0.txt
        //
        //  -- portable approach suggested by tymofey, with avoidance of
        //     undefined behavior as suggested by Giovanni Piero Deretta,
        //     with a further refinement suggested by Pyry Jahkola.

        std::uint32_t const step16 = x << 16 | x >> 16;
        return ((step16 << 8) & 0xff00ff00) | ((step16 >> 8) & 0x00ff00ff);
    }
    else
    {
        return boost::endian::endian_reverse(x);
    }
}

} // namespace dplx::dp::detail


#ifndef KMEANS_HPP_
#define KMEANS_HPP_

#include <iostream>
#include <vector>
// #include <Eigen/Eigen>
#include <iostream>
#include <string>
#include <limits>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/Point_set_3.h>
#include <CGAL/IO/read_points.h>
#include <CGAL/IO/io.h>
#include <glog/logging.h>
#include <limits>
#include <CGAL/squared_distance_3.h>
#include <CGAL/centroid.h>

class Kmeans
{
	typedef CGAL::Simple_cartesian<double> K;
	typedef K::Point_3 Point_3;
    typedef CGAL::Point_set_3<Point_3> Point_set;

public:
	Kmeans();
	~Kmeans() {}
	Kmeans(std::string file);
	/**
	 * @brief Set the Input object
	 *
	 * @param source_path
	 */
	bool setInput(std::string source);

	void setSimilarityModel(int flag);

	/**
	 * @brief Set the Numberof Clusters and select init centers indices
	 *
	 * @param number_of_clusters the number of clusters
	 */
	void setNumberofClusters(int number_of_clusters);
	/**
	 * @brief Set the Init Cluster Center object
	 *
	 * @param init_centers the indices of init_centers point, if you should assign the init point,
	 * 					   you should use this function
	 */
	void setInitClusterCenter(const std::vector<Point_3> init_centers);

	/**
	 * @brief Set the Max Iterations
	 *
	 * @param maxiter the max number of iteration
	 */
	void setMaxIterations(int maxiter) {
		maxiter_ = maxiter;
	};

	/**
	 * @brief Set the Max Correspondence Distance
	 *
	 * @param maxcor
	 */
	void setMaxCorrespondenceDistance(float maxcor) {
		maxcor_ = maxcor;
	};

	/**
	 * @brief execute kmean
	 *
	 * @return true
	 * @return false
	 */
	bool compute();

	/**
	 * @brief get result
	 *
	 */
	bool save_clusters(std::string path, int minsize);

private:
	/**
	 * @brief input point cloud
	 */
	Point_set points_;

	/**
	 * @brief the indices of point which is center
	 *
	 */
	// std::vector<int> centers_indices_;
	std::vector<Point_3> centers_points_;

	/**
	 * @brief indices of each clusters
	 *
	 */
	std::vector<std::vector<int>> clusters_indices_;

	/**
	 * @brief assign centers
	 *
	 */
	void assign_centers();

	/**
	 * @brief compute threshold between centers and cluster center
	 *
	 * @return true meet the requirement
	 * @return false
	 */
	bool compute_threshold();

	/**
	 * @brief some parameters
	 *
	 */
	int maxiter_;
	int finaliter_;
	float maxcor_;
	int number_of_clusters_;
};

Kmeans::Kmeans()
{
	LOG(INFO) << "construct kmeans ...";
	finaliter_ = 0;
	maxiter_ = 100;
	maxcor_ = 10.0;
	number_of_clusters_ = 10;
	clusters_indices_.resize(number_of_clusters_);
}

Kmeans::Kmeans(std::string file)
{
	LOG(INFO) << "construct kmeans ...";
	setInput(file);

	finaliter_ = 0;
	maxiter_ = 100;
	maxcor_ = 1.0;
	number_of_clusters_ = 10;
	clusters_indices_.resize(number_of_clusters_);
}

bool Kmeans::setInput(std::string source)
{
	LOG(INFO) << "read data from file path";
	CGAL::IO::read_XYZ(source, points_);
	if (points_.size() == 0) {
		LOG(INFO) << "input data is zero";
		return false;
	}
	return true;
}

void Kmeans::setNumberofClusters(int number_of_clusters)
{
	LOG(INFO) << "set number of clusters";
	number_of_clusters_ = number_of_clusters;
    centers_points_.resize(number_of_clusters_);

	srand((int)time(0));
	// selsect number_of_clusters different points
	for (int i = 0; i < number_of_clusters_; i++) {
		centers_points_[i] = points_.point(rand() % points_.size());
	}
}

void Kmeans::setInitClusterCenter(const std::vector<Point_3> init_centers)
{
	number_of_clusters_ = init_centers.size();
    centers_points_.resize(number_of_clusters_);
	for (int i = 0; i < init_centers.size(); ++i)
		centers_points_[i] = init_centers[i];
}

bool Kmeans::compute_threshold() {
	// get points from points_
	LOG(INFO) << "computer center and updata centers";
	bool isUpdate = false;
	float x = 0, y = 0, z = 0;
	// Point_set temp_points;
	std::vector<Point_3> temp_points;

	for (int i = 0; i < clusters_indices_.size(); i++)  {
		Point_3 center = centers_points_[i];
		for (int j = 0; j < clusters_indices_[i].size(); j++) {
			Point_3 p = points_.point(j);
			temp_points.push_back(p);
		}
		Point_3 temp_centroid = CGAL::centroid(temp_points.begin(),
											   temp_points.end(),
											   CGAL::Dimension_tag<0>());

		float temp = CGAL::squared_distance(center, temp_centroid);
		if (temp > maxcor_) {
			centers_points_[i] = Point_3(x, y, z);
			isUpdate = true;
		}
	}
	LOG(INFO) << "compute_threshold && update center successful";
	return isUpdate;
}

void Kmeans::assign_centers() {
	LOG(INFO) << "assign points to target centers";
	for (int i = 0; i < points_.size(); ++i) {
		float min_distance = std::numeric_limits<float>::max();
		int min_dis_clusters = -1;
		Point_3 temp_p = points_.point(i);
		for (int j = 0; j < number_of_clusters_; ++j) {
			float temp = CGAL::squared_distance(temp_p, centers_points_[j]);
			if (temp < min_distance) {
				min_dis_clusters = j;
				min_distance = temp;
			}
		}
		clusters_indices_[min_dis_clusters].push_back(i);
	}
	LOG(INFO) << "assign_centers successful";
}

bool Kmeans::compute() {
	LOG(INFO) << "compute ... ";
	bool isUpdate = true;
	int iter = 0;
	while (isUpdate) {
		assign_centers();
		isUpdate = compute_threshold();
		iter ++;
		if (iter == maxiter_)
			break;
	}
	finaliter_ = iter;
    LOG(INFO) << "compute successful ";

	return true;
}

// @TODOS add
bool Kmeans::save_clusters(std::string path, int minsize = std::numeric_limits<unsigned int>::min()) {
    std::string output_file = "";
    for (int i = 0; i < clusters_indices_.size(); ++i) {
        output_file = path + "/" + std::to_string(i) + ".asc";
        LOG(INFO) << "output_file = " << output_file;
        auto temp = clusters_indices_[i];
        if (temp.size() > minsize) {
            std::ofstream out(output_file);
            for (int j = 0; j < temp.size(); j++)
                out << points_.point(temp[j]).x() << " "
					<< points_.point(temp[j]).y() << " "
					<< points_.point(temp[j]).z() << std::endl;
            out.close();
        }
    }

    return true;
}

#endif


#ifndef PYTHONIC_INCLUDE_NUMPY_EXPM1_HPP
#define PYTHONIC_INCLUDE_NUMPY_EXPM1_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/expm1.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{

  namespace wrapper
  {
    template <class T>
    std::complex<T> expm1(std::complex<T> const &val)
    {
      return exp(val) - 1;
    }
    template <class T>
    auto expm1(T const &val) -> decltype(boost::simd::expm1(val))
    {
      return boost::simd::expm1(val);
    }
  }

#define NUMPY_NARY_FUNC_NAME expm1
#define NUMPY_NARY_FUNC_SYM wrapper::expm1
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


#pragma once
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/variate_generator.hpp>

// Provides a number of classes that encapsulate non-uniform probability
// distributions (Normal, Poisson, etc).
class PoissonDistribution
{
  public:
    PoissonDistribution(const int mean);

    int next();

  private:
    boost::mt19937 mt;
    boost::poisson_distribution<int> pdist;
    boost::variate_generator<boost::mt19937, boost::poisson_distribution<int>> poisson_generator;
};


// Boost.Geometry
// Unit Test

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

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

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

#include "test_formula.hpp"
#include "intersection_cases.hpp"

#include <boost/geometry/formulas/andoyer_inverse.hpp>
#include <boost/geometry/formulas/gnomonic_intersection.hpp>
#include <boost/geometry/formulas/sjoberg_intersection.hpp>
#include <boost/geometry/formulas/thomas_direct.hpp>
#include <boost/geometry/formulas/thomas_inverse.hpp>
#include <boost/geometry/formulas/vincenty_direct.hpp>
#include <boost/geometry/formulas/vincenty_inverse.hpp>

void check_inverse(expected_result const& result, expected_result const& expected, expected_result const& reference, double reference_error)
{
    check_one(result.lon, expected.lon, reference.lon, reference_error);
    check_one(result.lat, expected.lat, reference.lat, reference_error);
}

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

    double lona1r = results.p1.lon * d2r;
    double lata1r = results.p1.lat * d2r;
    double lona2r = results.p2.lon * d2r;
    double lata2r = results.p2.lat * d2r;
    double lonb1r = results.q1.lon * d2r;
    double latb1r = results.q1.lat * d2r;
    double lonb2r = results.q2.lon * d2r;
    double latb2r = results.q2.lat * d2r;

    expected_result result;

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

    bg::formula::gnomonic_intersection<double, bg::formula::vincenty_inverse, bg::formula::vincenty_direct>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.gnomonic_vincenty, results.gnomonic_karney, 0.00000001);

    bg::formula::gnomonic_intersection<double, bg::formula::thomas_inverse, bg::formula::thomas_direct>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.gnomonic_thomas, results.gnomonic_karney, 0.0000001);

    bg::formula::sjoberg_intersection<double, bg::formula::vincenty_inverse, 4>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.sjoberg_vincenty, results.sjoberg_karney, 0.00000001);

    bg::formula::sjoberg_intersection<double, bg::formula::thomas_inverse, 2>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.sjoberg_thomas, results.sjoberg_karney, 0.0000001);

    bg::formula::sjoberg_intersection<double, bg::formula::andoyer_inverse, 1>
        ::apply(lona1r, lata1r, lona2r, lata2r, lonb1r, latb1r, lonb2r, latb2r, result.lon, result.lat, spheroid);
    result.lon *= r2d;
    result.lat *= r2d;
    check_inverse(result, results.sjoberg_andoyer, results.sjoberg_karney, 0.0001);
}

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

    return 0;
}