/**
 * @file calc-characteristic.cpp
 *
 * @brief calculate characteristic polynomial.
 *
 * @author Mutsuo Saito (Hiroshima University)
 * @author Makoto Matsumoto (The University of Tokyo)
 *
 * Copyright (C) 2012 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University and The University of Tokyo.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <iostream>
#include <inttypes.h>
#include <stdint.h>
#include <time.h>
#include "dSFMText.hpp"
#include "dSFMT-calc-jump.hpp"
#include <NTL/GF2X.h>
#include <NTL/vec_GF2.h>
#include <NTL/GF2XFactoring.h>

using namespace dsfmt;
using namespace NTL;
using namespace std;

void calc_minimal(GF2X& minimal, int maxdegree, w128_t outseq[], int pos)
{
    uint64_t mask = 0;
    vec_GF2 seq;
    seq.SetLength(2 * maxdegree);
    int idx = 0;

    if (pos >= 104 || pos < 0) {
	cerr << "pos error:" << dec << pos << endl;
	exit(1);
    } else if (pos >= 52) {
	idx = 1;
	mask = UINT64_C(1) << (pos - 52);
    } else {
	idx = 0;
	mask = UINT64_C(1) << pos;
    }
    if (idx < 0 || idx >= 2) {
	cerr << "idx error:" << dec << idx << endl;
	exit(1);
    }
    for (int i = 0; i < 2 * maxdegree; i++) {
	if ((outseq[i].u[idx] & mask) != 0) {
	    seq[i] = 1;
	} else {
	    seq[i] = 0;
	}
    }
    MinPolySeq(minimal, seq, maxdegree);
#if defined(DEBUG)
    cout << "deg(minimal) = " << dec << deg(minimal) << endl;
#endif
}

void LCM(GF2X& lcm, const GF2X& x, const GF2X& y) {
    GF2X gcd;
    mul(lcm, x, y);
    GCD(gcd, x, y);
//void DivRem(GF2X& q, GF2X& r, const GF2X& a, const GF2X& b);
// q = a/b, r = a%b
    GF2X r;
    DivRem(lcm, r, lcm, gcd);
    if (deg(r) != -1) {
	cerr << "LCM error: deg(r) = " << dec << deg(r);
	cerr << "r:" << r << endl;
	exit(1);
    }
}

static void get_lcm_sub(GF2X& lcmpoly, dSFMText& dsfmt) {
    GF2X tmp(1,1);
    int maxdegree = dsfmt.get_maxdegree();
    w128_t out_seq[2 * maxdegree];

    for (int i = 0; i < 2 * maxdegree; i++) {
	out_seq[i] = dsfmt.next();
    }
    GF2X minimal;
    GF2X t1;
    SetCoeff(t1, 0, 1);
    SetCoeff(t1, 1, 1);
    for (int pos = 0; pos < 104; pos++) {
	calc_minimal(minimal, maxdegree, out_seq, pos);
	LCM(tmp, lcmpoly, minimal);
	lcmpoly = tmp;
	if (divide(lcmpoly, t1)) {
	    return;
	}
    }
#if defined(DEBUG)
    cout << "deg(lcmpoly) = " << dec << deg(lcmpoly) << endl;
#endif
}

void get_lcm(GF2X& lcmpoly, dSFMText& dsfmt) {
    int maxdegree = dsfmt.get_maxdegree();
    GF2X t1;
    SetCoeff(t1, 0, 1);
    SetCoeff(t1, 1, 1);

    //uint32_t time = (uint32_t)clock();
    dsfmt.seeding(1234);
    get_lcm_sub(lcmpoly, dsfmt);
    if (deg(lcmpoly) >= maxdegree) {
	return;
    }
    if (divide(lcmpoly, t1)) {
	return;
    }
    for(int i = 0; i < maxdegree; i++) {
	dsfmt.init_basis(i);
	get_lcm_sub(lcmpoly, dsfmt);
	if (deg(lcmpoly) >= maxdegree) {
	    return;
	}
	if (divide(lcmpoly, t1)) {
	    return;
	}
    }
}

static int has_large_irreducible(GF2X& fpoly, int degree) {
    static const GF2X t2(2, 1);
    static const GF2X t1(1, 1);
    GF2X t2m;
    GF2X t;
    GF2X alpha;
    int m;

    t2m = t2;
    if (deg(fpoly) < degree) {
	return 0;
    }
    t = t1;
    t += t2m;

    for (m = 1; deg(fpoly) > degree; m++) {
	for(;;) {
	    GCD(alpha, fpoly, t);
	    if (IsOne(alpha)) {
		break;
	    }
	    fpoly /= alpha;
	    if (deg(fpoly) < degree) {
		return 0;
	    }
	}
	t2m *= t2m;
	t2m %= fpoly;
	add(t, t2m, t1);
    }
    if (deg(fpoly) != degree) {
	return 0;
    }
    return IterIrredTest(fpoly);
}

void check_fix(dSFMText & fix)
{
    fix.setup_high();
    cout << "before:";
    fix.print(cout);
    fix.next();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next();
    cout << "after 3 step:";
    fix.print(cout);
}

void check_fix2(dSFMText & fix)
{
    cout << "before:";
    fix.print(cout);
    fix.next_add();
    cout << "after 1 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 2 step:";
    fix.print(cout);
    fix.next_add();
    cout << "after 3 step:";
    fix.print(cout);
}

void calc_fix(GF2X& inv, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText fix(mexp, pos1, sl1, mask1, mask2);
    //fix.setup_high();
    //fix.next();
    //cout << "setup0:";
    //fix.print(cout);
    fix.setup_constants();
#if defined(DEBUG)
    cout << "setup:";
    fix.print(cout);
#endif
    dSFMText work(mexp, pos1, sl1, mask1, mask2);
#if defined(DEBUG)
    cout << "zero:";
    work.print(cout);
#endif
    for (long i = 0; i <= deg(inv); i++) {
	if (IsOne(coeff(inv, i))) {
	    work.add(fix);
#if defined(DEBUG) && 0
    cout << "work:";
    work.print(cout);
#endif
	}
	fix.next();
    }
    cout << "fix:";
    work.print(cout);
    check_fix(work);
}

void check_const(GF2X& lcm, int mexp, int pos1, int sl1,
	      uint64_t mask1, uint64_t mask2)
{
    dSFMText normal(mexp, pos1, sl1, mask1, mask2);
    dSFMText add(mexp, pos1, sl1, mask1, mask2);
    normal.seeding(123);
    add.seeding(123);

    normal.setup_high();
    normal.next();
    add.next_add();
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
    for (int i = 0; i < 5; i++) {
	normal.next();
	add.next_add();
    }
    cout << "normal:";
    normal.print(cout);
    cout << "add:";
    add.print(cout);
}

void check_gcd(GF2X& d, GF2X& s, GF2X& inv, GF2X& work, GF2X& t1)
{
//    XGCD(d, s, inv, work, t1);
//    check_gcd(d, s, inv, work, t1);
//void XGCD(GF2X& d, GF2X& s, GF2X& t, const GF2X& a, const GF2X& b);
// d = gcd(a,b), a s + b t = d
    if (deg(d) != 0) {
	cout << "check_gcd d != 1:" << dec << deg(d) << endl;
	return;
    }
    GF2X p;
    GF2X q;
    mul(p, s, work);
    mul(q, inv, t1);
    p += q;
    if (p != d) {
	cout << "check_gcd p != d, p = " << p << endl;
	cout << "check_gcd p != d, d = " << d << endl;
    } else {
	cout << "check_gcd OK d:" << d << endl;
    }
}
int main(int argc, char *argv[]) {
    if (argc < 6) {
	cout << argv[0] << " mexp pos1 sl1 mask1 mask2" << endl;
	return -1;
    }
    int mexp = strtol(argv[1], NULL, 10);
    int pos1 = strtol(argv[2], NULL, 10);
    int sl1 = strtol(argv[3], NULL, 10);
    uint64_t mask[2];
    mask[0] = strtoull(argv[4], NULL, 16);
    mask[1] = strtoull(argv[5], NULL, 16);
#if defined(DEBUG)
    cout << "mexp:" << dec << mexp << endl;
    cout << "pos1:" << dec << pos1 << endl;
    cout << "sl1:" << dec << sl1 << endl;
    cout << "mask1:" << hex<< mask[0] << endl;
    cout << "mask2:" << hex << mask[1] << endl;
#endif
    dSFMText dsfmt(mexp, pos1, sl1, mask[0], mask[1]);
    GF2X characteristic(0,1);
    get_lcm(characteristic, dsfmt);
#if defined(DEBUG)
    cout << "degree:" << dec << deg(characteristic) << endl;
    cout << "maxdegree = " << dec << dsfmt.get_mamaxdegree() << endl;
    cout << characteristic << endl;
#endif
    GF2X work;
    work = characteristic;
#if defined(DEBUG)
    cout << "degree:" << deg(characteristic) << endl;
    cout << characteristic << endl;
#endif
    if (!has_large_irreducible(characteristic, mexp)) {
        cout << "error?" << endl;
        return -1;
    }
#if 0
    GF2X remain = work / characteristic;
    vec_pair_GF2X_long factors;
    CanZass(factors, remain);
    cout << "degree of work:" << dec << deg(work) << endl;
    cout << "degree of remain:" << dec << deg(remain) << endl;
    cout << "=== factor of remain ===" << endl;
    for (int i = 0; i < factors.length(); i++) {
	cout << factors[i].a;
	cout << ":";
	cout << dec << deg(factors[i].a);
	cout << ":";
	cout << dec << factors[i].b << endl;
    }
#endif
    GF2X d, s, inv;
    GF2X t1;
    SetCoeff(t1, 0, 1);
    SetCoeff(t1, 1, 1);
    XGCD(d, s, inv, work, t1);
    check_gcd(d, s, inv, work, t1);
// d = gcd(a,b), a s + b t = d
//    MulMod(s, inv, t1, work);
#if defined(DEBUG)
    cout << "deg work:" << dec << deg(work) << endl;
    cout << "deg inv:" << dec << deg(inv) << endl;
#endif
    string x;
//    cout << "# deg = " << dec << deg(work) << endl;
    polytostring(x, work);
    cout << "#" << dec << mexp;
    cout << "," << dec << pos1;
    cout << "," << dec << sl1;
    cout << "," << hex << mask[0];
    cout << "," << hex << mask[1];
    cout << dec << endl;
    cout << x << endl;
    cout << dec << flush;
    if (deg(d) != 0) {
	cout << "doesn't have fixpoint." << endl;
	return 0;
    }
#if 0
    string y;
    polytostring(y, inv);
    if (deg(d) == 0) {
	cout << y << endl;
    } else {
	cout << "can't finf inv d=" << d << endl;
    }
#endif
    calc_fix(inv, mexp, pos1, sl1, mask[0], mask[1]);
    //check_const(inv, mexp, pos1, sl1, mask[0], mask[1]);
}


/**
 * @file pointcloud_test.cpp
 * @brief Point Cloud CLIPPER tests
 * @author Parker Lusk <plusk@mit.edu>
 * @date 3 October 2020
 */

#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <clipper/clipper.h>
#include <clipper/find_dense_cluster.h>
#include <clipper/invariants/builtins.h>

TEST(PointCloud, KnownScaleInvariant) {

  //
  // Algorithm setup
  //

  // instantiate the invariant function that will be used to score associations
  clipper::invariants::EuclideanDistance::Params iparams;
  clipper::invariants::EuclideanDistance invariant(iparams);

  //
  // Data setup
  //

  // create a target/model point cloud of data
  Eigen::Matrix3Xd model(3, 4);
  model.col(0) << 0, 0, 0;
  model.col(1) << 2, 0, 0;
  model.col(2) << 0, 3, 0;
  model.col(3) << 2, 2, 0;

  // transform of data w.r.t model
  Eigen::Affine3d T_MD;
  T_MD = Eigen::AngleAxisd(M_PI/8, Eigen::Vector3d::UnitZ());
  T_MD.translation() << 5, 3, 0;

  // create source/data point cloud
  Eigen::Matrix3Xd data = T_MD.inverse() * model;

  // remove one point from the tgt (model) cloud---simulates a partial view
  data.conservativeResize(3, 3);

  //
  // Identify data association
  //

  // an empty association set is assumed to be all-to-all (12 total)
  clipper::Association A;

  Eigen::MatrixXd M, C;
  std::tie(M, C) = clipper::scorePairwiseConsistency(invariant, model, data, A);

  // A should be an all-to-all hypothesis
  const int n = model.cols() * data.cols();
  EXPECT_EQ(A.rows(), n);
  EXPECT_EQ(A.cols(), 2); // CLIPPER is a pair-wise data association algo

  // Ensure that an all-to-all hypothesis was correctly created
  for (size_t i=0; i<model.cols(); i++) {
    for (size_t j=0; j<data.cols(); j++) {
      const size_t k = i * data.cols() + j;
      EXPECT_EQ(A(k,0), i);
      EXPECT_EQ(A(k,1), j);
    }
  }

  // affinity matrix made up of all-to-all hypothesis between 4 and 3 items
  EXPECT_EQ(M.rows(), A.rows());
  EXPECT_EQ(M.cols(), A.rows());

  // diagonal of the affinity matrix should be all ones
  EXPECT_EQ(M.diagonal(), Eigen::VectorXd::Ones(M.rows()));

  // matrices should be symmetric
  EXPECT_EQ(M, M.transpose());
  EXPECT_EQ(C, C.transpose());

  // in this case with perfect data, affinity matrix is binary and so
  // affinity matrix == constraint matrix
  EXPECT_EQ(M, C);

  // expected affinity matrix, from MATLAB
  Eigen::MatrixXd Mtrue = Eigen::MatrixXd(M.rows(), M.cols());
  Mtrue << 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0,
           0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
           0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0,
           0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
           1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0,
           0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0,
           0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0,
           0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0,
           1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0,
           0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0,
           0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
           0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1;
  EXPECT_EQ(M, Mtrue);
}

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

TEST(PointCloud, KnownScale) {

  //
  // Algorithm setup
  //

  // instantiate the invariant function that will be used to score associations
  clipper::invariants::EuclideanDistance::Params iparams;
  clipper::invariants::EuclideanDistance invariant(iparams);

  //
  // Data setup
  //

  // create a target/model point cloud of data
  Eigen::Matrix3Xd model(3, 4);
  model.col(0) << 0, 0, 0;
  model.col(1) << 2, 0, 0;
  model.col(2) << 0, 3, 0;
  model.col(3) << 2, 2, 0;

  // transform of data w.r.t model
  Eigen::Affine3d T_MD;
  T_MD = Eigen::AngleAxisd(M_PI/8, Eigen::Vector3d::UnitZ());
  T_MD.translation() << 5, 3, 0;

  // create source/data point cloud
  Eigen::Matrix3Xd data = T_MD.inverse() * model;

  // remove one point from the tgt (model) cloud---simulates a partial view
  data.conservativeResize(3, 3);

  //
  // Identify data association
  //

  // an empty association set will be assumed to be all-to-all
  clipper::Association A;

  // create affinity matrix
  Eigen::MatrixXd M, C;
  std::tie(M, C) = clipper::scorePairwiseConsistency(invariant, model, data, A);

  // find the "densest clique"
  clipper::Solution soln = clipper::findDenseCluster(M, C);

  clipper::Association Ainliers = clipper::selectInlierAssociations(soln, A);

  ASSERT_EQ(Ainliers.rows(), 3);
  for (size_t i=0; i<Ainliers.rows(); ++i) {
    EXPECT_EQ(Ainliers(i, 0), Ainliers(i, 1));
  }

}

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

TEST(PointCloud, KnownScaleConvenience) {

  //
  // Algorithm setup
  //

  clipper::Params params;
  clipper::invariants::EuclideanDistance::Params iparams;

  // instantiate the clipper object that will process incoming pairs of sensor
  // data to determine the outlier-free set of pairwise associations
  clipper::CLIPPER<clipper::invariants::EuclideanDistance> clipper(params, iparams);

  //
  // Data setup
  //

  // create a target/model point cloud of data
  Eigen::Matrix3Xd model(3, 4);
  model.col(0) << 0, 0, 0;
  model.col(1) << 2, 0, 0;
  model.col(2) << 0, 3, 0;
  model.col(3) << 2, 2, 0;

  // transform of data w.r.t model
  Eigen::Affine3d T_MD;
  T_MD = Eigen::AngleAxisd(M_PI/8, Eigen::Vector3d::UnitZ());
  T_MD.translation() << 5, 3, 0;

  // create source/data point cloud
  Eigen::Matrix3Xd data = T_MD.inverse() * model;

  // remove one point from the tgt (model) cloud---simulates a partial view
  data.conservativeResize(3, 3);

  //
  // Identify data association
  //

  // note that data types are specified by invariant class
  clipper::Association Ainliers = clipper.findCorrespondences(model, data);

  ASSERT_EQ(Ainliers.rows(), 3);
  for (size_t i=0; i<Ainliers.rows(); ++i) {
    EXPECT_EQ(Ainliers(i, 0), Ainliers(i, 1));
  }

}

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

TEST(PointCloud, LargePointCloud) {

  //
  // Algorithm setup
  //

  clipper::Params params;
  clipper::invariants::EuclideanDistance::Params iparams;
  iparams.sigma = 0.015;
  iparams.epsilon = 0.02;
  clipper::CLIPPER<clipper::invariants::EuclideanDistance> clipper(params, iparams);

  //
  // Data setup
  //

  // create a target/model point cloud of data
  static constexpr int N = 32;
  Eigen::Matrix3Xd model = 5*Eigen::MatrixXd::Random(3, N);

  // transform of data w.r.t model
  Eigen::Affine3d T_MD;
  T_MD = Eigen::AngleAxisd(M_PI/8, Eigen::Vector3d::UnitZ());
  T_MD.translation() << 5, 3, 0;

  // create source/data point cloud
  Eigen::Matrix3Xd data = T_MD.inverse() * model;

  //
  // Identify data association
  //

  // note that data types are specified by invariant class
  clipper::Association Ainliers = clipper.findCorrespondences(model, data);

  ASSERT_EQ(Ainliers.rows(), N);
  for (size_t i=0; i<Ainliers.rows(); ++i) {
    EXPECT_EQ(Ainliers(i, 0), Ainliers(i, 1));
  }

}

TEST(PointCloud, LargePointCloudSparseClipper) {

  //
  // Algorithm setup
  //

  clipper::Params params;
  clipper::invariants::EuclideanDistance::Params iparams;
  iparams.sigma = 0.015;
  iparams.epsilon = 0.02;
  clipper::invariants::EuclideanDistance invariant(iparams);
  //clipper::CLIPPER<clipper::invariants::EuclideanDistance> clipper(params, iparams);

  //
  // Data setup
  //

  // create a target/model point cloud of data
  static constexpr int N = 32;
  Eigen::Matrix3Xd model = 5*Eigen::MatrixXd::Random(3, N);

  // transform of data w.r.t model
  Eigen::Affine3d T_MD;
  T_MD = Eigen::AngleAxisd(M_PI/8, Eigen::Vector3d::UnitZ());
  T_MD.translation() << 5, 3, 0;

  // create source/data point cloud
  Eigen::Matrix3Xd data = T_MD.inverse() * model;

  //
  // Identify data association
  //

  // note that data types are specified by invariant class
  clipper::Association A = clipper::Association();
  Eigen::SparseMatrix<double> sM, sC;
  std::tie(sM, sC) =
      clipper::scoreSparsePairwiseConsistency(invariant, model, data, A, true);
  auto soln = clipper::findDenseClusterOfSparseGraph(sM, sC, params);
  clipper::Association Ainliers = clipper::selectInlierAssociations(soln, A);

  ASSERT_EQ(Ainliers.rows(), N);
  for (size_t i=0; i<Ainliers.rows(); ++i) {
    EXPECT_EQ(Ainliers(i, 0), Ainliers(i, 1));
  }

}


//meshビューア

#include <ros/ros.h>
#include <stdio.h>
#include <iostream>
#include <string>

#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/vtk_io.h>
#include <pcl/PolygonMesh.h>

#include <boost/thread/thread.hpp>

#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/Point.h>
#include <pcl_conversions/pcl_conversions.h>

#include <tf/transform_broadcaster.h>

#include <local_tool/filters.hpp>
#include <local_tool/mathematics.hpp>
#include <local_tool/registration.hpp>

using namespace std;

class Mesh
{
	private:
		ros::NodeHandle n;
		ros::Rate r;
		ros::Publisher input_pub;
		ros::Publisher output_pub;

		double MU;
		double M_NEIGHBORS;//max
		double G_RADIUS;
		double K_SEARCH;

		string file_input;

		bool NORMAL_C;

		pcl::PolygonMesh::Ptr triangles;
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud ;

	public:
		Mesh(ros::NodeHandle& n);

		void create_polygon(void);
		void vis_polygon(void);

};

Mesh::Mesh(ros::NodeHandle &n) :
	r(10)
{

	n.getParam("mesh_mu",MU);
	n.getParam("mesh_max_neighbors",M_NEIGHBORS);
	n.getParam("mesh_radius",G_RADIUS);
	n.getParam("mesh_normalconsistency",NORMAL_C);
	n.getParam("normal_ksearch",K_SEARCH);
	n.getParam("input/cloud",file_input);

	input_pub = n.advertise<sensor_msgs::PointCloud2>("/input_cloud", 10);
	output_pub = n.advertise<sensor_msgs::PointCloud2>("/output_cloud", 10);

	triangles.reset (new pcl::PolygonMesh());
	cloud.reset (new pcl::PointCloud<pcl::PointXYZ>);

}

void
Mesh::create_polygon(void){

	pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZI>);
	// Load input file into a PointCloud<T> with an appropriate type
	pcl::PCLPointCloud2 cloud_blob;
	pcl::io::loadPCDFile(file_input, cloud_blob);
	pcl::fromPCLPointCloud2 (cloud_blob, *cloud);
	//* the data should be available in cloud

	// Normal estimation*
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> nor;
	pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud (cloud);
	nor.setInputCloud (cloud);
	nor.setSearchMethod (tree);
	nor.setKSearch (K_SEARCH);
	nor.compute (*normals);
	//* normals should not contain the point normals + surface curvatures

	// Concatenate the XYZ and normal fields*
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
	pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
	//* cloud_with_normals = cloud + normals

	// Create search tree*
	pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
	tree2->setInputCloud (cloud_with_normals);

	// Initialize objects
	pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;

	// Set the maximum distance between connected points (maximum edge length)
	// gp3.setSearchRadius (0.025);
	gp3.setSearchRadius (G_RADIUS);

	// Set typical values for the parameters
	gp3.setMu (MU);//defalut 2.5
	gp3.setMaximumNearestNeighbors (M_NEIGHBORS);
	gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
	gp3.setMinimumAngle(M_PI/18); // 10 degrees
	gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
	gp3.setNormalConsistency(NORMAL_C);
	// gp3.setNormalConsistency(true);

	// Get result
	gp3.setInputCloud (cloud_with_normals);
	gp3.setSearchMethod (tree2);
	gp3.reconstruct (*triangles);

	// Additional vertex information
	std::vector<int> parts = gp3.getPartIDs();
	std::vector<int> states = gp3.getPointStates();

	cout <<triangles->polygons.size() <<"triangles created" << endl;

	//ros
	// pcl::fromPCLPointCloud2 (triangles->cloud, *output_cloud);
	// sensor_msgs::PointCloud2 pc, pc2;
	// pcl_msgs::PolygonMesh pc3;
	// pcl::toROSMsg(*cloud, pc);
	// pcl::toROSMsg(*output_cloud, pc2);
	// pc3.cloud = pc2;

}

void
Mesh::vis_polygon(void)
{
	// 点群のビューア
	pcl::visualization::PCLVisualizer viewer("Cloud Viewer");

	viewer.setBackgroundColor (0.0, 0.2, 0.6);

	viewer.addPolygonMesh(*triangles);

	while (!viewer.wasStopped ())
	{
		viewer.spinOnce(10);

		boost::this_thread::sleep (boost::posix_time::microseconds (100000));
	}

}

int main(int argc, char** argv){
	ros::init(argc, argv, "mesh");
	ros::NodeHandle n;

	cout<<"-------mesh ok--------"<<endl;

	Mesh mesh(n);

	mesh.create_polygon();
	mesh.vis_polygon();

	ros::spin();

	return 0;
}


/**********************************************************************
*  Copyright (c) 2008-2013, Alliance for Sustainable Energy.
*  All rights reserved.
*
*  This library is free software; you can redistribute it and/or
*  modify it under the terms of the GNU Lesser General Public
*  License as published by the Free Software Foundation; either
*  version 2.1 of the License, or (at your option) any later version.
*
*  This library is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
*  Lesser General Public License for more details.
*
*  You should have received a copy of the GNU Lesser General Public
*  License along with this library; if not, write to the Free Software
*  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
**********************************************************************/

#ifndef UTILITIES_GEOMETRY_GEOMETRY_HPP
#define UTILITIES_GEOMETRY_GEOMETRY_HPP

#include <utilities/UtilitiesAPI.hpp>

#include <utilities/geometry/Point3d.hpp>
#include <utilities/geometry/Vector3d.hpp>

#include <vector>
#include <boost/optional.hpp>

namespace openstudio{

  /// convert degrees to radians
  UTILITIES_API double degToRad(double degrees);

  /// convert radians to degrees
  UTILITIES_API double radToDeg(double radians);

  /// compute area from surface as Point3dVector
  UTILITIES_API boost::optional<double> getArea(const Point3dVector& points);

  /// compute Newall vector from surface as Point3dVector, direction is same as outward normal
  /// magnitude is twice the area
  UTILITIES_API boost::optional<Vector3d> getNewallVector(const Point3dVector& points);

  /// compute outward normal from surface as Point3dVector
  UTILITIES_API boost::optional<Vector3d> getOutwardNormal(const Point3dVector& points);

  /// compute centroid from surface as Point3dVector
  UTILITIES_API boost::optional<Point3d> getCentroid(const Point3dVector& points);

  /// reorder points to upper-left-corner convention
  UTILITIES_API std::vector<Point3d> reorderULC(const Point3dVector& points);

  /// removes colinear points, tolerance is for length of cross product after normalizing each line segment
  UTILITIES_API std::vector<Point3d> removeColinear(const Point3dVector& points, double tol = 0.001);

  /// return distance between two points
  UTILITIES_API double getDistance(const Point3d& point1, const Point3d& point2);

  /// return angle (in radians) between two vectors
  UTILITIES_API double getAngle(const Vector3d& vector1, const Vector3d& vector2);

  /// compute distance in meters between two points on the Earth's surface
  /// lat and lon are specified in degrees
  UTILITIES_API double getDistanceLatLon(double lat1, double lon1, double lat2, double lon2);

  /// check if two vectors of points are equal (within tolerance) irregardless of initial ordering.
  UTILITIES_API bool circularEqual(const Point3dVector& points1, const Point3dVector& points2, double tol = 0.001);

} // openstudio

#endif //UTILITIES_GEOMETRY_GEOMETRY_HPP


#include <exception>
#include <vector>
#include <Eigen/Core>

Eigen::MatrixXd cholesky_swig_eigen(const Eigen::MatrixXd &M);


// #pragma GCC optimize ("O0")

#include <boost/log/trivial.hpp>

#include "common.hpp"
#include "constants.hpp"

double svaToUra(int sva)
{
	/*
		GLOBAL POSITIONING SYSTEM
		STANDARD POSITIONING SERVICE
		SIGNAL SPECIFICATION
		2nd Ed, June 2,1995
		see section - 2.5.3 User Range Accuracy
	*/
	double ura = 0;
	if (sva <= 6)
	{
		ura = 10 * pow(2, 1 + ((double)sva / 2.0));
		ura = round(ura) / 10.0;
	}
	else if (sva != 15)
		ura = pow(2, (double)sva - 2.0);
	else
		ura = -1;
	return ura;
}

double svaToSisa(int sva)
{
	/*
		EUROPEAN GNSS (GALILEO) OPEN SERVICE
		SIGNAL-IN-SPACE
		INTERFACE CONTROL
		DOCUMENT
		Issue 2.0, January 2021
		See Section, 5.1.12. Signal In Space Accuracy (SISA)
	*/

	double sisa;
	if (sva <= 49)			sisa = 0.0 + (sva - 0) * 0.01;
	else if (sva <= 74)		sisa = 0.5 + (sva - 50) * 0.02;
	else if (sva <= 99)		sisa = 1.0 + (sva - 75) * 0.04;
	else if (sva <= 125)	sisa = 2.0 + (sva - 100) * 0.16;
	else					sisa = -1;
	return sisa;
}

int sisaToSva(double sisa)
{
	if (sisa < 0)
	{
		BOOST_LOG_TRIVIAL(error) << "Error converting SISA to SVA, value is less than zero.";
		return -1;
	}

	if (sisa <= 0.49)	    return (int)((((sisa - 0.0) / 0.01) + 0) + 0.5);
	else if (sisa <= 0.98)  return (int)((((sisa - 0.5) / 0.02) + 50) + 0.5);
	else if (sisa <= 1.96)  return (int)((((sisa - 1.0) / 0.04) + 75) + 0.5);
	else if (sisa <= 6.00)  return (int)((((sisa - 2.0) / 0.16) + 100) + 0.5);
	else
	{
		BOOST_LOG_TRIVIAL(warning) << "SISA is too large SVA undefined.";
		return -1;
	}
}

/* crc-24q parity --------------------------------------------------------------
* compute crc-24q parity for sbas, rtcm3
* args   : unsigned char *buff I data
*          int    len    I      data length (bytes)
* return : crc-24Q parity
* notes  : see reference [2] A.4.3.3 Parity
*-----------------------------------------------------------------------------*/
unsigned int crc24q(
	const unsigned char *buff,
	int len)
{
//	trace(4,"%s: len=%d\n",__FUNCTION__, len);

	unsigned int crc = 0;

	for (int i=0;i<len;i++)
		crc = ((crc<<8) & 0xFFFFFF) ^ tbl_CRC24Q[(crc >> 16) ^ buff[i]];

	return crc;
}

void setbitu(
	unsigned char*	buff,
	int 			pos,
	int				len,
	unsigned int	data)
{
	unsigned int mask=1u<<(len-1);

	if	( len<=0
		||len>32)
	{
		return;
	}

	unsigned long int invalid = (1ul<<len);

	if (data >= invalid)
	{
		std::cout << "Warning: " << __FUNCTION__ << " has data outside range\n";
	}

	for (int i = pos; i < pos + len; i++, mask >>= 1)
	{
		if (data&mask)	buff[i/8] |=  (1u<<(7-i%8));
		else			buff[i/8] &= ~(1u<<(7-i%8));
	}
}

void setbits(
	unsigned char*	buff,
	int				pos,
	int				len,
	int				data)
{
	unsigned int mask=1u<<(len-1);

	if	( len<=0
		||len>32)
	{
		return;
	}

	long int invalid = (1ul<<(len-1));

	if	( +data >= invalid
		||-data >= invalid)
	{
		std::cout << "Warning: " << __FUNCTION__ << " has data outside range, setting invalid\n";
		data = -invalid;
	}

	for (int i = pos; i < pos + len; i++, mask >>= 1)
	{
		if (data&mask)	buff[i/8] |=  (1u<<(7-i%8));
		else			buff[i/8] &= ~(1u<<(7-i%8));
	}
}

int setbituInc(
	unsigned char*	buff,
	int				pos,
	int				len,
	unsigned int	var)
{
	setbitu(buff, pos, len, var);
	return pos + len;
}

int setbitsInc(
	unsigned char*	buff,
	int				pos,
	int				len,
	int				var)
{
	setbits(buff, pos, len, var);
	return pos + len;
}

/* extract unsigned/signed bits ------------------------------------------------
* extract unsigned/signed bits from byte data
* args   : unsigned char *buff I byte data
*          int    pos    I      bit position from start of data (bits)
*          int    len    I      bit length (bits) (len<=32)
* return : extracted unsigned/signed bits
*-----------------------------------------------------------------------------*/
unsigned int getbitu(
	const unsigned char*	buff,
	int						pos,
	int						len)
{
	unsigned int bits = 0;
	for (int i=pos;i<pos+len;i++)
		bits=(bits<<1)+((buff[i/8]>>(7-i%8))&1u);

	return bits;
}

int getbits(
	const unsigned char*	buff,
	int						pos,
	int						len)
{
	unsigned int bits = getbitu(buff, pos, len);

	long int invalid = (1ul<<(len-1));

	if (bits == -invalid)
	{
		std::cout << "warning: invalid number received on " << __FUNCTION__ << " " << invalid << " " << len << std::endl;
	}

	if	( len<=0
		||len>=32
		||!(bits&(1u<<(len-1))))
	{
		return (int)bits;
	}
	return (int)(bits|(~0u<<len)); /* extend sign */
}

unsigned int getbituInc(
	const unsigned char*	buff,
	int&					pos,
	int						len)
{
	unsigned int ans = getbitu(buff, pos, len);
	pos += len;
	return ans;
}

int getbitsInc(
	const unsigned char*	buff,
	int&					pos,
	int						len)
{
	int ans = getbits(buff, pos, len);
	pos += len;
	return ans;
}


//---------------------------------------------------------------------------//
// Copyright (c) 2013 Kyle Lutz <kyle.r.lutz@gmail.com>
//
// 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
//
// See http://kylelutz.github.com/compute for more information.
//---------------------------------------------------------------------------//

#define BOOST_TEST_MODULE TestStableSort
#include <boost/test/unit_test.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/algorithm/stable_sort.hpp>
#include <boost/compute/algorithm/is_sorted.hpp>
#include <boost/compute/container/vector.hpp>

#include "check_macros.hpp"
#include "context_setup.hpp"

BOOST_AUTO_TEST_CASE(sort_int_vector)
{
    int data[] = { -4, 152, -5000, 963, 75321, -456, 0, 1112 };
    boost::compute::vector<int> vector(data, data + 8);
    BOOST_CHECK_EQUAL(vector.size(), size_t(8));
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end()) == false);

    boost::compute::stable_sort(vector.begin(), vector.end());
    BOOST_CHECK(boost::compute::is_sorted(vector.begin(), vector.end()) == true);
    CHECK_RANGE_EQUAL(int, 8, vector, (-5000, -456, -4, 0, 152, 963, 1112, 75321));
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.GIL (Generic Image Library) - tests
//
// Copyright 2020 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//

#include <algorithm>
#include <boost/core/lightweight_test.hpp>
#include <boost/gil/point.hpp>
#include <boost/gil/rasterization/line.hpp>

#include <cmath>
#include <cstddef>
#include <iterator>
#include <random>
#include <vector>

namespace gil = boost::gil;

namespace boost
{
namespace gil
{
std::ostream& operator<<(std::ostream& os, const point_t p)
{
    os << "{x=" << p.x << ", y=" << p.y << "}";
    return os;
}
}} // namespace boost::gil

using line_type = std::vector<gil::point_t>;

struct endpoints
{
    gil::point_t start;
    gil::point_t end;
};

endpoints create_endpoints(std::mt19937& twister,
                           std::uniform_int_distribution<std::ptrdiff_t>& distr)
{
    gil::point_t start{distr(twister), distr(twister)};
    gil::point_t end{distr(twister), distr(twister)};
    return {start, end};
}

line_type create_line(endpoints points)
{
    gil::bresenham_line_rasterizer rasterizer;
    line_type forward_line(rasterizer.point_count(points.start, points.end));
    rasterizer(points.start, points.end, forward_line.begin());
    return forward_line;
}

void test_start_end(const line_type& line_points, endpoints points)
{
    BOOST_TEST_EQ(line_points.front(), points.start);
    BOOST_TEST_EQ(line_points.back(), points.end);
}

// Look at TODO below
// void test_two_way_equivalence(const line_type& forward, line_type backward)
// {
//     std::reverse(backward.begin(), backward.end());
//     BOOST_TEST_ALL_EQ(forward.begin(), forward.end(), backward.begin(), backward.end());
// }

void test_connectivity(line_type const& line_points)
{
    for (std::size_t i = 1; i < line_points.size(); ++i)
    {
        const auto x_diff = std::abs(line_points[i].x - line_points[i - 1].x);
        const auto y_diff = std::abs(line_points[i].y - line_points[i - 1].y);
        BOOST_TEST_LE(x_diff, 1);
        BOOST_TEST_LE(y_diff, 1);
    }
}

void test_bresenham_rasterizer_follows_equation(line_type line_points)
{
    auto start = line_points.front();
    auto end = line_points.back();

    auto width = std::abs(end.x - start.x) + 1;
    auto height = std::abs(end.y - start.y) + 1;
    if (width < height)
    {
        std::swap(width, height);
        std::transform(line_points.begin(), line_points.end(), line_points.begin(),
                       [](gil::point_t p)
                       {
                           return gil::point_t{p.y, p.x};
                       });
        // update start and end
        start = line_points.front();
        end = line_points.back();
    }
    const double sign = [start, end]()
    {
        auto const width_sign = end.x < start.x;
        auto const height_sign = end.y < start.y;
        auto const slope_sign = width_sign != height_sign;
        return slope_sign ? -1 : 1;
    }();
    const double slope = static_cast<double>(height) / static_cast<double>(width);
    const double intercept =
        static_cast<double>(start.y) - sign * slope * static_cast<double>(start.x);
    for (const auto& point : line_points)
    {
        double const expected_y = sign * slope * static_cast<double>(point.x) + intercept;
        auto const difference =
            std::abs(point.y - static_cast<std::ptrdiff_t>(std::round(expected_y)));
        BOOST_TEST_LE(difference, static_cast<std::ptrdiff_t>(slope + 1));
    }
}

int main()
{
    const std::ptrdiff_t size = 256;
    for (std::size_t seed = 0; seed <= 100; ++seed)
    {
        std::mt19937 twister(seed);
        std::uniform_int_distribution<std::ptrdiff_t> distr(0, size - 1);
        const std::size_t sample_count = 100;
        for (std::size_t sample_index = 0; sample_index < sample_count; ++sample_index)
        {
            auto endpoints = create_endpoints(twister, distr);
            auto forward_line = create_line(endpoints);
            test_start_end(forward_line, endpoints);
            // TODO: figure out if forward/backward equivalence is possible to provide
            // auto backward_line = create_line({endpoints.end, endpoints.start});
            // test_two_way_equivalence(forward_line, backward_line);
            test_connectivity(forward_line);
            // test_connectivity(backward_line);
            test_bresenham_rasterizer_follows_equation(forward_line);
            // test_bresenham_rasterizer_follows_equation(backward_line);
        }
    }

    return boost::report_errors();
}


#include "catch.hpp"

#include <libIntegrate/_1D/RiemannRule.hpp>

#include <boost/units/systems/si/force.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/quantity.hpp>
using namespace boost::units;
using namespace boost::units::si;

namespace BoostUnitsTests
{

TEST_CASE( "Testing Riemann rule with boost units." ) {

  _1D::RiemannRule<quantity<energy>> integrate;
  quantity<energy> I;

  std::vector<quantity<length>> x(3);
  std::vector<quantity<force>> F(3);
  x[0] = 0.*meter; F[0] = 1.*newton;
  x[1] = 1.*meter; F[1] = 2.*newton;
  x[2] = 2.*meter; F[2] = 3.*newton;

  I = integrate( x, F );
  REQUIRE( quantity_cast<double>(I) == Approx( 3 ) );

}

}


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

#include "vstab_nodes.hpp"

#include <Eigen/SVD>

static const char KERNEL_HOMOGRAPHY_FILTER_NAME[VX_MAX_KERNEL_NAME] = "example.nvx.homography_filter";

// Kernel implementation
static vx_status VX_CALLBACK homographyFilter_kernel(vx_node, const vx_reference *parameters, vx_uint32 num)
{
    if (num != 4)
        return VX_FAILURE;

    vx_status status = VX_SUCCESS;

    vx_matrix input = (vx_matrix)parameters[0];
    vx_matrix homography = (vx_matrix)parameters[1];
    vx_image image = (vx_image)parameters[2];
    vx_array mask = (vx_array)parameters[3];

    // Copy input to homography
    vx_float32 intputData[9] = {0};
    status |= vxCopyMatrix(input, intputData, VX_READ_ONLY, VX_MEMORY_TYPE_HOST);
    status |= vxCopyMatrix(homography, intputData, VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST);

    vx_uint32 width = 0, height = 0;
    status |= vxQueryImage(image, VX_IMAGE_ATTRIBUTE_WIDTH, &width, sizeof(width));
    status |= vxQueryImage(image, VX_IMAGE_ATTRIBUTE_HEIGHT, &height, sizeof(height));

    vx_size nPoints;
    status |= vxQueryArray(mask, VX_ARRAY_ATTRIBUTE_NUMITEMS, &nPoints, sizeof(nPoints));

    vx_int32 nInliers = 0;
    if (nPoints > 0)
    {
        vx_map_id map_id;
        vx_size stride;
        void* ptr;
        status |= vxMapArrayRange(mask, 0, nPoints, &map_id, &stride, &ptr, VX_READ_ONLY, VX_MEMORY_TYPE_HOST, 0);

        for (vx_size i = 0; i < nPoints; i++)
        {
            vx_uint8 v = vxArrayItem(vx_uint8, ptr, i, stride);
            if (v != 0)
                ++nInliers;
        }

        status |= vxUnmapArrayRange(mask, map_id);
    }

    int inlierThresh = std::max(15, static_cast<int>(0.1 * nPoints));
    Matrix3x3f_rm eye3x3 = Matrix3x3f_rm::Identity();

    if (nInliers < inlierThresh)
    {
        status |= vxCopyMatrix(homography, eye3x3.data(), VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST);
        return status;
    }

    vx_float32 data[9];
    status |= vxCopyMatrix(homography, data, VX_READ_ONLY, VX_MEMORY_TYPE_HOST);

    Matrix3x3f_rm M = Matrix3x3f_rm::Map(data, 3, 3);
    M.transposeInPlace();

    // restrictions on the lenghts of the diagonals of the warped image
    Matrix3x4f_rm vertices = Matrix3x4f_rm::Zero();

    for(int i=0; i<4; ++i)
        vertices(2, i) = 1.0f;

    vertices(0, 1) = static_cast<float>(width);
    vertices(0, 2) = static_cast<float>(width);
    vertices(1, 2) = static_cast<float>(height);
    vertices(1, 3) = static_cast<float>(height);

    Matrix3x4f_rm dstVertices = M * vertices;
    for(int i=0; i<4; ++i)
    {
        dstVertices(0,i) /= dstVertices(2,i);
        dstVertices(1,i) /= dstVertices(2,i);
        dstVertices(2,i) = 1.0f;
    }

    float diagLenGold = std::sqrt(static_cast<float>(width*width + height*height));

    float dx = dstVertices(0,0) - dstVertices(0,2);
    float dy = dstVertices(1,0) - dstVertices(1,2);
    float lenDiag1 = sqrt(dx*dx + dy*dy);

    dx = dstVertices(0,1) - dstVertices(0,3);
    dy = dstVertices(1,1) - dstVertices(1,3);
    float lenDiag2 = sqrt(dx*dx + dy*dy);

    float averDiagLen = (lenDiag1 + lenDiag2) / 2;
    float diagRatio1 = std::min(diagLenGold, averDiagLen) / std::max(diagLenGold, averDiagLen);
    if (diagRatio1 < 0.5f)
    {
        status |= vxCopyMatrix(homography, eye3x3.data(), VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST);
        return status;
    }

    float maxDiag = std::max(lenDiag1, lenDiag2);
    if (maxDiag > 0.0f)
    {
        float diagRatio2 = std::min(lenDiag1, lenDiag2) / maxDiag;
        if (diagRatio2 < 0.25f)
        {
            status |= vxCopyMatrix(homography, eye3x3.data(), VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST);
            return status;
        }
    }
    else
    {
        status |= vxCopyMatrix(homography, eye3x3.data(), VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST);
        return status;
    }

    // restriction on min eigen value
    typedef Eigen::JacobiSVD<Matrix3x3f_rm> JacobiSVD;

    JacobiSVD svd(M);
    JacobiSVD::SingularValuesType singValues = svd.singularValues();

    if (singValues(2) < 1e-4f)
    {
        status |= vxCopyMatrix(homography, eye3x3.data(), VX_WRITE_ONLY, VX_MEMORY_TYPE_HOST);
        return status;
    }

    return status;
}

// Parameter validator
static vx_status VX_CALLBACK homographyFilter_validate(vx_node, const vx_reference parameters[],
                                                       vx_uint32 numParams, vx_meta_format metas[])
{
    if (numParams != 4) return VX_ERROR_INVALID_PARAMETERS;

    vx_matrix input = (vx_matrix)parameters[0];
    vx_array mask = (vx_array)parameters[3];

    vx_status status = VX_SUCCESS;

    vx_enum inputDataType = 0;
    vx_size inputRows = 0ul, inputCols = 0ul;
    vxQueryMatrix(input, VX_MATRIX_ATTRIBUTE_TYPE, &inputDataType, sizeof(inputDataType));
    vxQueryMatrix(input, VX_MATRIX_ATTRIBUTE_ROWS, &inputRows, sizeof(inputRows));
    vxQueryMatrix(input, VX_MATRIX_ATTRIBUTE_COLUMNS, &inputCols, sizeof(inputCols));

    vx_enum maskType = 0;
    vxQueryArray(mask, VX_ARRAY_ATTRIBUTE_ITEMTYPE, &maskType, sizeof(maskType));

    if (inputDataType != VX_TYPE_FLOAT32 || inputCols != 3 || inputRows != 3)
    {
        status = VX_ERROR_INVALID_PARAMETERS;
    }

    if (maskType != VX_TYPE_UINT8)
    {
        status = VX_ERROR_INVALID_TYPE;
    }

    vx_meta_format homographyMeta = metas[1];

    vx_enum homographyType = VX_TYPE_FLOAT32;
    vx_size homographyRows = 3;
    vx_size homographyCols = 3;

    vxSetMetaFormatAttribute(homographyMeta, VX_MATRIX_ATTRIBUTE_TYPE, &homographyType, sizeof(homographyType));
    vxSetMetaFormatAttribute(homographyMeta, VX_MATRIX_ATTRIBUTE_ROWS, &homographyRows, sizeof(homographyRows));
    vxSetMetaFormatAttribute(homographyMeta, VX_MATRIX_ATTRIBUTE_COLUMNS, &homographyCols, sizeof(homographyCols));

    return status;
}

// Register user defined kernel in OpenVX context
vx_status registerHomographyFilterKernel(vx_context context)
{
    vx_status status = VX_SUCCESS;

    vx_enum id;
    status = vxAllocateUserKernelId(context, &id);
    if (status != VX_SUCCESS)
    {
        vxAddLogEntry((vx_reference)context, status, "[%s:%u] Failed to allocate an ID for the HomographyFilter kernel",
                      __FUNCTION__, __LINE__);
        return status;
    }

    vx_kernel kernel = vxAddUserKernel(context, KERNEL_HOMOGRAPHY_FILTER_NAME,
                                       id,
                                       homographyFilter_kernel,
                                       4,
                                       homographyFilter_validate,
                                       NULL,
                                       NULL
                                       );

    status = vxGetStatus((vx_reference)kernel);
    if (status != VX_SUCCESS)
    {
        vxAddLogEntry((vx_reference)context, status, "[%s:%u] Failed to create HomographyFilter Kernel", __FUNCTION__, __LINE__);
        return status;
    }

    status |= vxAddParameterToKernel(kernel, 0, VX_INPUT, VX_TYPE_MATRIX, VX_PARAMETER_STATE_REQUIRED); // input
    status |= vxAddParameterToKernel(kernel, 1, VX_OUTPUT, VX_TYPE_MATRIX, VX_PARAMETER_STATE_REQUIRED); // homography
    status |= vxAddParameterToKernel(kernel, 2, VX_INPUT, VX_TYPE_IMAGE, VX_PARAMETER_STATE_REQUIRED); // image
    status |= vxAddParameterToKernel(kernel, 3, VX_INPUT, VX_TYPE_ARRAY, VX_PARAMETER_STATE_REQUIRED); // mask

    if (status != VX_SUCCESS)
    {
        vxReleaseKernel(&kernel);
        vxAddLogEntry((vx_reference)context, status, "[%s:%u] Failed to initialize HomographyFilter Kernel parameters", __FUNCTION__, __LINE__);
        return VX_FAILURE;
    }

    status = vxFinalizeKernel(kernel);
    vxReleaseKernel(&kernel);

    if (status != VX_SUCCESS)
    {
        vxAddLogEntry((vx_reference)context, status, "[%s:%u] Failed to finalize HomographyFilter Kernel", __FUNCTION__, __LINE__);
        return VX_FAILURE;
    }

    return status;
}

vx_node homographyFilterNode(vx_graph graph, vx_matrix input, vx_matrix homography, vx_image image, vx_array mask)
{
    vx_node node = NULL;

    vx_kernel kernel = vxGetKernelByName(vxGetContext((vx_reference)graph), KERNEL_HOMOGRAPHY_FILTER_NAME);

    if (vxGetStatus((vx_reference)kernel) == VX_SUCCESS)
    {
        node = vxCreateGenericNode(graph, kernel);
        vxReleaseKernel(&kernel);

        if (vxGetStatus((vx_reference)node) == VX_SUCCESS)
        {
            vxSetParameterByIndex(node, 0, (vx_reference)input);
            vxSetParameterByIndex(node, 1, (vx_reference)homography);
            vxSetParameterByIndex(node, 2, (vx_reference)image);
            vxSetParameterByIndex(node, 3, (vx_reference)mask);
        }
    }

    return node;
}


#define DEBUG 1
/**
 * File    : A.cpp
 * Author  : Kazune Takahashi
 * Created : 2020/1/25 11:32:12
 * Powered by Visual Studio Code
 */
#include <algorithm>
#include <bitset>
#include <cassert>
#include <cctype>
#include <chrono>
#include <cmath>
#include <complex>
#include <cstdint>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <iomanip>
#include <iostream>
#include <map>
#include <queue>
#include <random>
#include <set>
#include <stack>
#include <string>
#include <tuple>
#include <unordered_map>
#include <unordered_set>
#include <vector>
// ----- boost -----
#include <boost/rational.hpp>
#include <boost/multiprecision/cpp_int.hpp>
// ----- using directives and manipulations -----
using namespace std;
using boost::rational;
using boost::multiprecision::cpp_int;
using ll = long long;
template <typename T>
using max_heap = priority_queue<T>;
template <typename T>
using min_heap = priority_queue<T, vector<T>, greater<T>>;
// ----- constexpr for Mint and Combination -----
constexpr ll MOD{1000000007LL};
// constexpr ll MOD{998244353LL}; // be careful
constexpr ll MAX_SIZE{3000010LL};
// constexpr ll MAX_SIZE{30000010LL}; // if 10^7 is needed
// ----- ch_max and ch_min -----
template <typename T>
void ch_max(T &left, T right)
{
  if (left < right)
  {
    left = right;
  }
}
template <typename T>
void ch_min(T &left, T right)
{
  if (left > right)
  {
    left = right;
  }
}
// ----- Mint -----
template <ll MOD = MOD>
class Mint
{
public:
  ll x;
  Mint() : x{0LL} {}
  Mint(ll x) : x{(x % MOD + MOD) % MOD} {}
  Mint operator-() const { return x ? MOD - x : 0; }
  Mint &operator+=(const Mint &a)
  {
    if ((x += a.x) >= MOD)
    {
      x -= MOD;
    }
    return *this;
  }
  Mint &operator-=(const Mint &a) { return *this += -a; }
  Mint &operator*=(const Mint &a)
  {
    (x *= a.x) %= MOD;
    return *this;
  }
  Mint &operator/=(const Mint &a)
  {
    Mint b{a};
    return *this *= b.power(MOD - 2);
  }
  Mint operator+(const Mint &a) const { return Mint(*this) += a; }
  Mint operator-(const Mint &a) const { return Mint(*this) -= a; }
  Mint operator*(const Mint &a) const { return Mint(*this) *= a; }
  Mint operator/(const Mint &a) const { return Mint(*this) /= a; }
  bool operator<(const Mint &a) const { return x < a.x; }
  bool operator<=(const Mint &a) const { return x <= a.x; }
  bool operator>(const Mint &a) const { return x > a.x; }
  bool operator>=(const Mint &a) const { return x >= a.x; }
  bool operator==(const Mint &a) const { return x == a.x; }
  bool operator!=(const Mint &a) const { return !(*this == a); }
  const Mint power(ll N)
  {
    if (N == 0)
    {
      return 1;
    }
    else if (N % 2 == 1)
    {
      return *this * power(N - 1);
    }
    else
    {
      Mint half = power(N / 2);
      return half * half;
    }
  }
};
template <ll MOD>
Mint<MOD> operator+(ll lhs, const Mint<MOD> &rhs)
{
  return rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator-(ll lhs, const Mint<MOD> &rhs)
{
  return -rhs + lhs;
}
template <ll MOD>
Mint<MOD> operator*(ll lhs, const Mint<MOD> &rhs)
{
  return rhs * lhs;
}
template <ll MOD>
Mint<MOD> operator/(ll lhs, const Mint<MOD> &rhs)
{
  return Mint<MOD>{lhs} / rhs;
}
template <ll MOD>
istream &operator>>(istream &stream, Mint<MOD> &a)
{
  return stream >> a.x;
}
template <ll MOD>
ostream &operator<<(ostream &stream, const Mint<MOD> &a)
{
  return stream << a.x;
}
// ----- Combination -----
template <ll MOD = MOD, ll MAX_SIZE = MAX_SIZE>
class Combination
{
public:
  vector<Mint<MOD>> inv, fact, factinv;
  Combination() : inv(MAX_SIZE), fact(MAX_SIZE), factinv(MAX_SIZE)
  {
    inv[1] = 1;
    for (auto i = 2LL; i < MAX_SIZE; i++)
    {
      inv[i] = (-inv[MOD % i]) * (MOD / i);
    }
    fact[0] = factinv[0] = 1;
    for (auto i = 1LL; i < MAX_SIZE; i++)
    {
      fact[i] = Mint<MOD>(i) * fact[i - 1];
      factinv[i] = inv[i] * factinv[i - 1];
    }
  }
  Mint<MOD> operator()(int n, int k)
  {
    if (n >= 0 && k >= 0 && n - k >= 0)
    {
      return fact[n] * factinv[k] * factinv[n - k];
    }
    return 0;
  }
  Mint<MOD> catalan(int x, int y)
  {
    return (*this)(x + y, y) - (*this)(x + y, y - 1);
  }
};
// ----- for C++14 -----
using mint = Mint<MOD>;
using combination = Combination<MOD, MAX_SIZE>;
template <typename T>
T gcd(T x, T y) { return y ? gcd(y, x % y) : x; }
template <typename T>
T lcm(T x, T y) { return x / gcd(x, y) * y; }
template <typename T>
int popcount(T x) // C++20
{
  int ans{0};
  while (x != 0)
  {
    ans += x & 1;
    x >>= 1;
  }
  return ans;
}
// ----- frequently used constexpr -----
// constexpr double epsilon{1e-10};
// constexpr ll infty{1000000000000000LL};
// constexpr int dx[4] = {1, 0, -1, 0};
// constexpr int dy[4] = {0, 1, 0, -1};
// ----- Yes() and No() -----
void Yes()
{
  cout << "Yes" << endl;
  exit(0);
}
void No()
{
  cout << "No" << endl;
  exit(0);
}

class Sieve
{
  static constexpr ll MAX_SIZE{10000010LL};
  ll N;
  vector<ll> f;
  vector<ll> prime_nums;

public:
  Sieve(ll N = MAX_SIZE) : N{N}, f(N, 0), prime_nums{}
  {
    f[0] = f[1] = -1;
    for (auto i = 2; i < N; i++)
    {
      if (f[i])
      {
        continue;
      }
      prime_nums.push_back(i);
      f[i] = i;
      for (auto j = 2 * i; j < N; j += i)
      {
        if (!f[j])
        {
          f[j] = i;
        }
      }
    }
  }

  bool is_prime(ll x) const
  { // 2 \leq x \leq MAX_SIZE^2
    if (x < N)
    {
      return f[x];
    }
    for (auto e : prime_nums)
    {
      if (x % e == 0)
      {
        return false;
      }
    }
    return true;
  }

  vector<ll> const &primes() const
  {
    return prime_nums;
  }

  vector<ll> factor_list(ll x) const
  {
    if (x < 2)
    {
      return {};
    }
    vector<ll> res;
    auto it{prime_nums.begin()};
    if (x < N)
    {
      while (x != 1)
      {
        res.push_back(f[x]);
        x /= f[x];
      }
    }
    else
    {
      while (x != 1 && it != prime_nums.end())
      {
        if (x % *it == 0)
        {
          res.push_back(*it);
          x /= *it;
        }
        else
        {
          ++it;
        }
      }
      if (x != 1)
      {
        res.push_back(x);
      }
    }
    return res;
  }

  vector<tuple<ll, ll>> factor(ll x) const
  {
    if (x < 2)
    {
      return {};
    }
    auto factors{factor_list(x)};
    vector<tuple<ll, ll>> res{make_tuple(factors[0], 0)};
    for (auto x : factors)
    {
      if (x == get<0>(res.back()))
      {
        get<1>(res.back())++;
      }
      else
      {
        res.emplace_back(x, 1);
      }
    }
    return res;
  }
};

// ----- main() -----

using ld = long double;
using point = complex<ld>;
constexpr ld PI{3.14159265358979323846};

class Solve
{
  ld La, Lb, Lc, Na, Nb, Nc, Ne, Ma, Mb, Mc, Tc;
  int alpha;
  Sieve sieve;
  ld Ta = 0;
  ld Tb;
  point Oa{0, 900}, Ob{900, 0}, Oc{900, 900}, Oe{90, 50};
  point Aa{1, 0}, Ab{0, 1}, Ac{-1, 0};
  ld S{500}, A{3000}, D{60};

public:
  Solve(ld La, ld Lb, ld Lc, ld Na, ld Nb, ld Nc, ld Ne, ld Ma, ld Mb, ld Mc, ld Tc) : La{La}, Lb{Lb}, Lc{Lc}, Na{Na}, Nb{Nb}, Nc{Nc}, Ne{Ne}, Ma{Ma}, Mb{Mb}, Mc{Mc}, Tc{Tc} {}

  void flush()
  {
    for (auto i = 0; i < 20; ++i)
    {
      ld now{0};
      ld W{Ma};
      if (now + Ma * Na > 100)
      {
        W = 500;
      }
      flush_A(W);
      now += Na * W;
      W = Mb;
      if (now + Mb * Nb > 100)
      {
        W = 500;
      }
      flush_B(W);
      now += Nb * W;
      W = Mc;
      if (now + Mc * Nc > 100)
      {
        W = 500;
      }
      flush_C(W);
      now += Nc * W;
      flush_E(now / Ne);
    }
  }

private:
  void update_Tb()
  {
    if (alpha == 0)
    {
      Tb = 0;
    }
    else
    {
      ll p{sieve.primes()[alpha + 1]};
      ll r{p % 180};
      Tb = (r <= 90 ? r : 180 - r);
    }
  }

  point arg(ld theta) { return theta / 180 * PI * point(0, 1); }
  point Pa() { return exp(-arg(Ta)) * La + Oa; }
  point Pb() { return exp(arg(Tb)) * Lb + Ob; }
  point Pc() { return exp(arg(Tc)) * Lc + Oc; }

  void flush_simple(ld X, ld Sx, ld Ax, ld Dx, ld Y, ld Sy, ld Ay, ld Dy, ld Ta, ld Wa, ld Wb, ld Wc, ld We)
  {
    cout << fixed << setprecision(0) << X << ", " << Sx << ", " << Ax << ", " << Dx << ", " << Y << ", " << Sy << ", " << Ay << ", " << Dy << ", " << Ta << ", " << Wa << ", " << Wb << ", " << Wc << ", " << We << endl;
  }

  void flush_A(ld W)
  {
    flush_simple(Pa().real(), S, A, D, Pa().imag(), S, A, D, Ta, W, 0, 0, 0);
    alpha += W;
  }

  void flush_B(ld W)
  {
    flush_simple(Pb().real(), S, A, D, Pb().imag(), S, A, D, Ta, 0, W, 0, 0);
  }

  void flush_C(ld W)
  {
    flush_simple(Pc().real(), S, A, D, Pc().imag(), S, A, D, Ta, 0, 0, W, 0);
  }

  void flush_E(ld W)
  {
    flush_simple(Oe.real(), S, A, D, Oe.imag(), S, A, D, Ta, 0, 0, 0, W);
  }
};

int main()
{
  ld La, Lb, Lc, Na, Nb, Nc, Ne, Ma, Mb, Mc, Tc;
  cin >> La >> Lb >> Lc >> Na >> Nb >> Nc >> Ne >> Ma >> Mb >> Mc >> Tc;
  Solve solve(La, Lb, Lc, Na / 1000, Nb / 1000, Nc / 1000, Ne / 1000, Ma, Mb, Mc, Tc);
  solve.flush();
}


#include <iostream>
#include <limits>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>>
    graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

class edge_adder
{
    graph &G;

public:
    explicit edge_adder(graph &G) : G(G) {}

    void add_edge(int from, int to, long capacity)
    {
        auto c_map = boost::get(boost::edge_capacity, G);
        auto r_map = boost::get(boost::edge_reverse, G);
        const auto e = boost::add_edge(from, to, G).first;
        const auto rev_e = boost::add_edge(to, from, G).first;
        c_map[e] = capacity;
        c_map[rev_e] = 0; // reverse edge has no capacity!
        r_map[e] = rev_e;
        r_map[rev_e] = e;
    }
};

using namespace std;

void solve()
{
    int n, m;
    cin >> n >> m;

    graph G(n);
    edge_adder adder(G);

    int u, v, c;
    for (int i = 0; i < m; ++i) {
        cin >> u >> v >> c;
        adder.add_edge(u, v, c);
    }

    long minCut = numeric_limits<long>::max();

    int source = 0;
    for (int target = 1; target < n; ++target) {
        long maxFlow = boost::push_relabel_max_flow(G, source, target);
        minCut = min(minCut, maxFlow);
        maxFlow = boost::push_relabel_max_flow(G, target, source);
        minCut = min(minCut, maxFlow);
    }
    cout << minCut << endl;
}

int main()
{
    ios_base::sync_with_stdio(false);
    int t;
    cin >> t;
    for (int i = 0; i < t; ++i)
    {
        solve();
    }
    return 0;
}


#include <geometry.h>
#include <tiny_math_types.h>

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

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(raycast_tetrahedron)
{

  using std::sqrt;

  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;
  typedef MT::value_traits          VT;

  V const p0 = V::make(0.0, 0.0, 0.0);
  V const p1 = V::make(1.0, 0.0, 0.0);
  V const p2 = V::make(0.0, 1.0, 0.0);
  V const p3 = V::make(0.0, 0.0, 1.0);

  geometry::Tetrahedron<V> const tetrahedron = geometry::make_tetrahedron(p0,p1,p2,p3);

  // Ray hitting on bottom of tetrahedron
  {
    V                const p   = V::make( 0.0, 0.0, -1.0);
    V                const r   = V::make( 0.2, 0.2,  1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool const hit = geometry::compute_raycast_tetrahedron(ray, tetrahedron, q, length );

    BOOST_CHECK( hit );
    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }
  // Ray missing tetrahedron
  {
    V                const p   = V::make( 0.0, 0.0, -1.0);
    V                const r   = V::make( -0.2, -0.2,  1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool const hit = geometry::compute_raycast_tetrahedron(ray, tetrahedron, q, length );

    BOOST_CHECK( !hit );
  }
  // Ray hitting on bottom of tetrahedron only bottom is surface
  {
    std::vector<bool> surface_map(4, false);

    surface_map[3] = true;

    V                const p   = V::make( 0.0, 0.0, -1.0);
    V                const r   = V::make( 0.2, 0.2,  1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool const hit = geometry::compute_raycast_tetrahedron(ray, tetrahedron, q, length, surface_map );

    BOOST_CHECK( hit );
    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }
  // Ray from above hitting bottom of tetrahedron only bottom is surface
  {
    std::vector<bool> surface_map(4, false);

    surface_map[3] = true;

    V                const p   = V::make( 0.0, 0.0,  1.0);
    V                const r   = V::make( 0.2, 0.2, -1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool const hit = geometry::compute_raycast_tetrahedron(ray, tetrahedron, q, length, surface_map );

    BOOST_CHECK( !hit );
  }

  // Stress testing... generating a bunch of random rays that all are hitting the oblique top plane
  for (unsigned int samples = 0u; samples < 1000u; ++samples)
  {
    V                const noise           = V::random(0.0, 1.0);
    T                const v1              = noise(0);
    T                const v2              = noise(1);
    T                const v3              = 1.0 - v1 - v2;
    T                const w1              = (v3 < 0.0) ?  v1/(1.0-v3) : v1;
    T                const w2              = (v3 < 0.0) ?  v2/(1.0-v3) : v2;
    T                const w3              = (v3 < 0.0) ?  0.0 : v3;

    if( w3==0.0) // We do not wish to test for exact edge cases --- they are sensitive to finite precision errors
      continue;
    if( w3==0.0)  // We do not wish to test for exact edge cases --- they are sensitive to finite precision errors
      continue;
    if( w3==0.0)  // We do not wish to test for exact edge cases --- they are sensitive to finite precision errors
      continue;

    BOOST_CHECK_CLOSE( (w1+w2+w3),  1.0, 0.01);

    BOOST_CHECK( w1 >= 0.0);
    BOOST_CHECK( w2 >= 0.0);
    BOOST_CHECK( w3 >= 0.0);

    BOOST_CHECK( w1 <= 1.0);
    BOOST_CHECK( w2 <= 1.0);
    BOOST_CHECK( w3 <= 1.0);

    V                const hit_point       = w1*p1 + w2*p2 + w3*p3;
    V                const ray_origin      = noise*10.0;
    V                const ray_direction   = hit_point - ray_origin;
    geometry::Ray<V> const ray             = geometry::make_ray(ray_origin, ray_direction);

    T       length = VT::zero();
    V       q      = V::zero();

    bool const hit = geometry::compute_raycast_tetrahedron(ray, tetrahedron, q, length );

    BOOST_CHECK( hit );
    BOOST_CHECK_CLOSE( length, tiny::norm(ray_direction), 0.01);

    for(unsigned int i = 0;i<3u;++i)
    {
      if(fabs(hit_point(i)) < tiny::working_precision<T>() )
        BOOST_CHECK_SMALL( q(i),  tiny::working_precision<T>());
      else
        BOOST_CHECK_CLOSE( q(i),  hit_point(i), 0.01);
    }
  }

}

BOOST_AUTO_TEST_SUITE_END();


/*
@copyright Louis Dionne 2015
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/assert.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/product.hpp>
#include <boost/hana/range.hpp>
#include <boost/hana/tuple.hpp>
namespace hana = boost::hana;

int main() {
    BOOST_HANA_CONSTANT_CHECK(
        hana::product<>(hana::make_range(hana::int_c<1>, hana::int_c<6>)) == hana::int_c<1 * 2 * 3 * 4 * 5>
    );

    BOOST_HANA_CONSTEXPR_CHECK(
        hana::product<>(hana::make_tuple(1, hana::int_c<3>, hana::long_c<-5>, 9)) == 1 * 3 * -5 * 9
    );

    BOOST_HANA_CONSTEXPR_CHECK(
        hana::product<unsigned long>(hana::make_tuple(2ul, 3ul)) == 6ul
    );
}


#include <algorithm>
#include <numeric>
#include <cmath>
#include <iostream>
#include <iomanip>
#include <limits>

#include <boost/timer.hpp>

#include "CompositeMinimization.h"

namespace Grante {

double CompositeMinimization::FISTAMinimize(
	CompositeMinimizationProblem& prob, std::vector<double>& x_opt,
	double conv_tol, unsigned int max_iter, bool verbose) {
	double Lub = 0.25;	// Lipschitz upper bound estimate for the gradient
	double eta = 2.0;	// Lipschitz bound scale factor

	// Initialize iterates
	unsigned int dim = prob.Dimensions();
	x_opt.resize(dim);
	prob.ProvideStartingPoint(x_opt);
	std::vector<double> xprev(x_opt);
	std::vector<double> x_fgrad_dummy;
	std::vector<double> y(x_opt);
	std::vector<double> y_fgrad(dim, 0.0);
	std::vector<double> u(dim, 0.0);

	// Convergence criterion
	double conv = std::numeric_limits<double>::infinity();
	double t = 1.0;

	// Objective F(x)=f(x)+g(x)
	double Fval = std::numeric_limits<double>::signaling_NaN();
	boost::timer total_timer;
	unsigned int lip_iter = 0;
	for (unsigned int iter = 0; max_iter == 0 || iter < max_iter; ++iter) {
		double y_fval = prob.EvalF(y, y_fgrad);
		double obj = y_fval + prob.EvalG(y, x_fgrad_dummy);

		// Verbose output
		if (verbose && (iter % 20 == 0)) {
			std::cout << std::endl;
			std::cout << "  iter     time      objective        conv  lipiter"
				<< std::endl;
		}
		if (verbose) {
			std::ios_base::fmtflags original_format = std::cout.flags();
			std::streamsize original_prec = std::cout.precision();

			// Iteration
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << iter << "  ";
			// Total runtime
			std::cout << std::setiosflags(std::ios::left)
				<< std::resetiosflags(std::ios::scientific)
				<< std::setiosflags(std::ios::fixed)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setprecision(1)
				<< std::setw(6) << total_timer.elapsed() << "s  ";
			std::cout << std::resetiosflags(std::ios::fixed);

			// Objective function
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(5)
				<< std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::showpos)
				<< std::setw(7) << obj << "   ";
			// Convergence criterion
			std::cout << std::setiosflags(std::ios::scientific)
				<< std::setprecision(2)
				<< std::resetiosflags(std::ios::showpos)
				<< std::setiosflags(std::ios::left) << conv << "   ";
			// Lipschitz iterations
			std::cout << std::setiosflags(std::ios::left)
				<< std::setiosflags(std::ios::adjustfield)
				<< std::setw(6) << lip_iter;

			std::cout << std::endl;

			std::cout.precision(original_prec);
			std::cout.flags(original_format);
		}

		// Backtracking
		lip_iter = 0;
		do {
			lip_iter += 1;

			// Solve (2.6) in Beck and Teboulle
			std::transform(y.begin(), y.end(), y_fgrad.begin(), u.begin(),
				[Lub](double y_e, double y_fgrad_e) -> double {
					return (y_e - y_fgrad_e/Lub); });
			// update x_opt
			prob.EvalGProximalOperator(u, Lub, x_opt);

			// Evaluate F(x)
			Fval = prob.EvalF(x_opt, x_fgrad_dummy);
			Fval += prob.EvalG(x_opt, x_fgrad_dummy);

			// Evaluate Q_L(x,y)
			double Qval = y_fval + prob.EvalG(x_opt, x_fgrad_dummy);
			for (unsigned int di = 0; di < dim; ++di) {
				double xsuby = x_opt[di] - y[di];
				Qval += xsuby*y_fgrad[di];
				Qval += 0.5*Lub*xsuby*xsuby;
			}

			// Sufficient upper bound on the Lipschitz constant?
			if (Fval <= Qval)
				break;

			Lub *= eta;	// Increase upper bound estimate
		} while (true);

		// Update t sequence
		double tprev = t;
		t = 0.5*(1.0 + std::sqrt(1.0 + 4.0*tprev*tprev));

		// Perform averaged step: update y
		std::transform(x_opt.begin(), x_opt.end(), xprev.begin(),
			y.begin(), [t,tprev](double xe, double xle) -> double {
				return (xe + ((tprev-1.0)/t)*(xe-xle)); });
		// conv=norm(x_opt-xprev)
		conv = 0.0;
		for (unsigned int di = 0; di < dim; ++di)
			conv += (x_opt[di]-xprev[di]) * (x_opt[di]-xprev[di]);
		conv = std::sqrt(conv);
		if (conv <= conv_tol) {
			if (verbose) {
				std::cout << "Converged with tolerance " << conv
					<< " (<= " << conv_tol << ")." << std::endl;
			}
			return (Fval);
		}

		std::copy(x_opt.begin(), x_opt.end(), xprev.begin());
	}
	return (Fval);	// objective of x_opt
}

}


//
// Created by bobin on 17-12-13.
//
#include <MapPoint.h>
#include <vector>
#include <Eigen/Dense>
#include <random>
#include <chrono>

using namespace ORB_SLAM2;
using namespace std;
using namespace Eigen;

int main(int argc, char **argv){
    int point_num = 100;
    vector<Vector3d> points;
    points.resize(point_num);

    for (int i = 0; i < point_num; ++i) {
        double x,y,z;
        x =  std::rand() % 100;
        y = std::rand() % 100;
        z = 0.1 * (1 - 5 * x - 10 * y) + std::rand() % 100 / 100.0 ;
        points[i] = Vector3d(x, y, z)  / 10.0;
    }

    SurfacePieceWise surfacePieceWise(0);
    surfacePieceWise.FitPlane(true, points);
    std::cout << surfacePieceWise.GetNormal() << std::endl;
    std::cout << surfacePieceWise.GetCentroid() << std::endl;
}


#include <iostream>
#include <Eigen/Dense>

Eigen::VectorXd dipoleval(const Eigen::VectorXd & x, const Eigen::VectorXd & t, const Eigen::VectorXd & y){
	int x_size= x.size();
	int w=1;
	int n= t.size();
	Eigen::VectorXd p=y;
	Eigen::VectorXd dP=Eigen::MatrixXd::Zero(1,n);
	for (int i=1; i<n; i++){//im
		for (int j=i-1; j>0;j--){//i0
			//% compute dp(i)'s
            dP(j) = p(j+1) + (x(w)-t(j))*dP(j+1) - p(j) - (x(w)-t(i))*dP(j);
            dP(j) = dP(j) / ( t(i) - t(j) );
            //% compute p(i)'s
            p(j)  = (x(w)-t(j)*p(j+1) - (x(w)-t(i))*p(j));
            p(j)  = p(j) / ( t(i) - t(j) );
            }
			std::cout <<"test\n";

	}
	return p.row(0);
}

int main(){
	int n=6;
	Eigen::VectorXd x=Eigen::MatrixXd::Random(n,1);
	Eigen::VectorXd y=Eigen::MatrixXd::Random(n,1);
	Eigen::VectorXd t=Eigen::MatrixXd::Random(n,1);

	Eigen::VectorXd v;
	v=dipoleval(x,t,y);
	std::cout << v << std::endl;
}

Nummerical Methods for CSE/PS6/dipoleval.cpp


/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*                                                                           */
/*  This file is part of the library KASKADE 7                               */
/*    see http://www.zib.de/projects/kaskade7-finite-element-toolbox         */
/*                                                                           */
/*  Copyright (C) 2002-2011 Zuse Institute Berlin                            */
/*                                                                           */
/*  KASKADE 7 is distributed under the terms of the ZIB Academic License.    */
/*    see $KASKADE/academic.txt                                              */
/*                                                                           */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

#ifndef NORMS_HH
#define NORMS_HH

/**
 * @file
 * @brief  Some norms for FunctionSpaceElement s or FunctionViews
 * @author Anton Schiela
 */

#include <boost/fusion/include/at_c.hpp>
#include <fem/integration.hh>
#include <fem/functionviews.hh>

namespace Kaskade
{
/// L_2-norms
struct L2Norm
{
/// Evaluation of square norm
  template<typename Function>
  double square(Function const& f) const
  {
    Integral<typename Function::Space> integral;
    FunctionViews::AbsSquare<Function> asf(f);
    return integral(asf);
  }

/// Evaluation of norm
  template<typename Function>
  double operator()(Function const& f) const
  {
    return std::sqrt(square(f));
  }
};

/// L_2-norms
// struct L2ScalarProduct
// {
// /// Evaluation of square norm
//   template<typename Function>
//   double operator()(Function f,Function g)
//   {
//     Integral<typename Function::Space> integral;
//     FunctionViews::Dot<Function> dotfg(f,g);
//     return integral(dotfg);
//   }
//
// /// Evaluation of norm
//   template<typename Function>
//   double operator()(Function f)
//   {
//     return std::sqrt(this->operator()(f,f));
//   }
// };

/// H1-semi-norms
struct H1SemiNorm
{
/// Evaluation of square norm
  template<typename Function>
  double square(Function const& f) const
  {
    Integral<typename Function::Space> integral;
    FunctionViews::GradientAbsSquare<Function> asf(f);
    return integral(asf);
//    FunctionViews::Gradient<Function> gf(f);
//    L2Norm l2Norm;
//    return l2Norm.square(gf);
  }

/// Evaluation of norm
  template<typename Function>
  double operator()(Function const& f) const
  {
    return std::sqrt(square(f));
  }
};

// THE FOLLOWING CODE IS UNDOCUMENTED, APPEARS TO BE RATHER PROJECT-SPECIFIC, AND IS NOWHERE USED IN
// THE SVN REPOSITORY. UNLESS SOME UNEXPECTED PROBLEMS ARISE, THIS CODE WILL BE DELETED AS OF
// 2016-03-31.
//
// template <int val>
// struct Decrement
// {
//   static constexpr int value = val-1;
// };
//
//
//
// template <int variableId, int numberOfVariables>
// struct MyNormEvaluator
// {
//   template <class DomainElement, class Norm>
//   static double square(DomainElement const& x, Norm const& norm)
//   {
//     return norm.square( boost::fusion::at_c<variableId>(x.data) ) + MyNormEvaluator<variableId-1,numberOfVariables>::square(x,norm);
//   }
// };
//
// template <int numberOfVariables>
// struct MyNormEvaluator<0,numberOfVariables>
// {
//   template <class DomainElement, class Norm>
//   static double square(DomainElement const& x, Norm const& norm)
//   {
//     return norm.square( boost::fusion::at_c<0>(x.data) );
//   }
// };
//
// struct MyH1SemiNorm
// {
//   template <typename DomainElement>
//   static double square(DomainElement const& x)
//   {
//     return MyNormEvaluator<DomainElement::Descriptions::noOfVariables-1, DomainElement::Descriptions::noOfVariables>::square(x,H1SemiNorm());
//   }
//
//   template <typename DomainElement>
//   double operator()(DomainElement const& x) const
//   {
//     return sqrt(square(x));
//   }
// };

// struct MyL2Norm
// {
//   template <typename DomainElement>
//   static double square(DomainElement const& x)
//   {
//     return MyNormEvaluator<DomainElement::Descriptions::noOfVariables-1, DomainElement::Descriptions::noOfVariables>::square(x,L2Norm());
//   }
//
//   template <typename DomainElement>
//   double operator()(DomainElement const& x) const
//   {
//     return sqrt(square(x));
//   }
// };

/// H1-norms
struct H1Norm
{
/// Evaluation of square norm
  template<typename Function>
  double square(Function f)
  {
    H1SemiNorm h1Norm;
    L2Norm l2Norm;
    return h1Norm.square(f)+l2Norm.square(f);
  }

/// Evaluation of norm
  template<typename Function>
  double operator()(Function f)
  {
    return std::sqrt(square(f));
  }
};

template<class Space> class LocalIntegral;
template<class Grid, class T> class CellData;

/// local (cellwise) H1-semi-norms
template<class Function>
typename CellData<typename Function::Space::Grid, typename Function::ValueType>::CellDataVector localH1SemiNorm(Function const& f)
{
  typedef typename Function::Space Space;
  typedef typename Space::Grid Grid;
  LocalIntegral<Space> localIntegral;
  typename CellData<Grid,typename Function::ValueType>::CellDataVector
    errorIndicator(localIntegral(
                     makeView<FunctionViews::AbsSquare>(makeView<FunctionViews::Gradient>(f))));
  return errorIndicator;
}

/// local (cellwise) L2-norms
template<class Function>
typename CellData<typename Function::Space::Grid, typename Function::ValueType>::CellDataVector localL2Norm(Function const& f)
{
  typedef typename Function::Space Space;
  typedef typename Space::Grid Grid;
  LocalIntegral<Space> localIntegral;
  typename CellData<Grid,typename Function::ValueType>::CellDataVector
    errorIndicator(localIntegral(
                     makeView<FunctionViews::AbsSquare>(f)));
  return errorIndicator;
}

/**
 * @brief boundaryL2Norm computes the L2-norm of an FE function on the whole boundary of the underlying grid.
 *
 * @param function is the FE function to be integrated.
 * @tparam FEFunction is the type of the integrand.
 * @return value of integral
 *
 */
template <class FEFunction>
typename FEFunction::Space::Scalar boundaryL2Norm(FEFunction const& function) {
  FunctionViews::AbsSquare<FEFunction> functionSquared(function);
  return std::sqrt(integrateOverBoundary(functionSquared));
}
} // end of namespace Kaskade
#endif


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

int main()
{
  EVE_VALUE mid = sizeof(EVE_VALUE) == 4 ? 2 : 5;
  {
    auto lmin = EVE_VALUE(0);
    auto lmax = EVE_VALUE(mid);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdj0 = [](auto x){return std::cyl_bessel_j(0, x);};
    auto boostj0= [](auto x){return boost::math::detail::bessel_j0(x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_j0_small) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_j0_small) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdj0_small) , xp, stdj0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boostj0_small), xp, boostj0 , arg0);
  }
  {
    auto lmin = EVE_VALUE(mid);
    auto lmax = EVE_VALUE(8);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdj0 = [](auto x){return std::cyl_bessel_j(0, x);};
    auto boostj0= [](auto x){return boost::math::detail::bessel_j0(x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_j0_medium) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_j0_medium) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdj0_medium) , xp, stdj0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boostj0_medium), xp, boostj0 , arg0);
  }
  {
    auto lmin = EVE_VALUE(8);
    auto lmax = EVE_VALUE(10000);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdj0 = [](auto x){return std::cyl_bessel_j(0, x);};
    auto boostj0= [](auto x){return boost::math::detail::bessel_j0(x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_j0_large) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_j0_large) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdj0_large) , xp, stdj0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boostj0_large), xp, boostj0 , arg0);
  }
  {
    auto lmin = EVE_VALUE(0);
    auto lmax = EVE_VALUE(11);

    auto arg0 = eve::bench::random_<EVE_VALUE>(lmin,lmax);
    auto stdj0 = [](auto x){return std::cyl_bessel_j(0, x);};
    auto boostj0= [](auto x){return boost::math::detail::bessel_j0(x);};
    eve::bench::experiment xp;
    run<EVE_TYPE> (EVE_NAME(cyl_bessel_j0_mixed) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(cyl_bessel_j0_mixed) , xp, eve::cyl_bessel_j0 , arg0);
    run<EVE_VALUE>(EVE_NAME(stdj0_mixed) , xp, stdj0 , arg0);
    run<EVE_VALUE>(EVE_NAME(boostj0_mixed), xp, boostj0 , arg0);
  }
}


#ifndef STAN_MATH_PRIM_MAT_FUN_QR_R_HPP
#define STAN_MATH_PRIM_MAT_FUN_QR_R_HPP

#include <stan/math/prim/arr/err/check_nonzero_size.hpp>
#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/scal/err/check_greater_or_equal.hpp>
#include <Eigen/QR>

namespace stan {
  namespace math {

    template <typename T>
    Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
    qr_R(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& m) {
      typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
      check_nonzero_size("qr_R", "m", m);
      check_greater_or_equal("qr_R",
                             "m.rows()",
                             static_cast<size_t>(m.rows()),
                             static_cast<size_t>(m.cols()));
      Eigen::HouseholderQR<matrix_t> qr(m.rows(), m.cols());
      qr.compute(m);
      matrix_t R = qr.matrixQR();
      if (m.rows() > m.cols())
        R.bottomRows(m.rows() - m.cols()).setZero();
      for (int i = 0; i < R.cols(); i++) {
        for (int j = 0; j < i; j++)
          R.coeffRef(i, j) = 0.0;
        if (R(i, i) < 0)
          R.row(i) *= -1.0;
      }
      return R;
    }
  }
}
#endif


/*
 * Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

/*
 * File:   SideScanGeoreferencing.hpp
 * Author: Jordan McManus <jordan.mcmanus@cidco.ca>
 *
 * Created on March 11, 2020, 12:40 PM
 */

#ifndef SIDESCANGEOREFERENCING_HPP
#define SIDESCANGEOREFERENCING_HPP

#include <Eigen/Dense>
#include "../Position.hpp"
#include "../math/CoordinateTransform.hpp"

class SideScanGeoreferencing {
public:

    static void georeferenceSideScanEcef(
            Eigen::Vector3d & antennaEcef,
            Eigen::Vector3d & antenna2TowPointLeverArmEcef,
            Eigen::Vector3d & layBackEcef,
            Eigen::Vector3d & sideDistanceEcef,
            Position & georeferencedPosition
            ) {

        Eigen::Vector3d objectPositionEcef =
                antennaEcef +
                antenna2TowPointLeverArmEcef +
                layBackEcef +
                sideDistanceEcef;

        CoordinateTransform::convertECEFToLongitudeLatitudeElevation(objectPositionEcef, georeferencedPosition);
    }
};

#endif /* SIDESCANGEOREFERENCING_HPP */


// build in NTL src/ folder
//g++ -I../include -I.  -g -O2 -std=c++11 -pthread -march=native  -o myLLLtest myLLLtest.cpp ntl.a  -lgmp    -lm #LSTAT

#include <NTL/LLL.h>

NTL_CLIENT

int main()
{
    mat_ZZ B;

    cin >> B;

    ZZ d;

    LLL(d, B, 90, 100);

    cout << "det = " << d << endl;
    cout << "B = " << B << endl;
    cout << "Finished" << endl;
}


#define BOOST_TEST_MODULE "test_gaussian_potential"

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

#include <test/util/check_potential.hpp>
#include <mjolnir/forcefield/local/GaussianPotential.hpp>

BOOST_AUTO_TEST_CASE(Gaussian_double)
{
    using real_type = double;
    constexpr std::size_t N = 1000;
    constexpr real_type   h = 1e-6;
    constexpr real_type tol = 1e-6;
    const real_type e  = 2.0;
    const real_type w  = 0.15;
    const real_type r0 = 7.0;

    mjolnir::GaussianPotential<real_type> gaussian(e, w, r0);

    const real_type x_min = r0 - 5.0 * w;
    const real_type x_max = r0 + 5.0 * w;

    mjolnir::test::check_potential(gaussian, x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(Gaussian_float)
{
    using real_type = float;
    constexpr std::size_t N = 100;
    constexpr real_type   h = 1e-3f;
    constexpr real_type tol = 1e-3f;
    const real_type e  = 2.0;
    const real_type w  = 0.15;
    const real_type r0 = 7.0;

    mjolnir::GaussianPotential<real_type> gaussian(e, w, r0);

    const real_type x_min = r0 - 5.0f * w;
    const real_type x_max = r0 + 5.0f * w;
    mjolnir::test::check_potential(gaussian, x_min, x_max, tol, h, N);
}

BOOST_AUTO_TEST_CASE(Gaussian_cutoff_double)
{
    using real_type = double;
    constexpr real_type   h = 1e-6;

    {
        const real_type e  = 2.0;
        const real_type w  = 0.15;
        const real_type r0 = 7.0;

        mjolnir::GaussianPotential<real_type> gaussian(e, w, r0);

        const auto rc = gaussian.cutoff();

        BOOST_TEST(std::isfinite(rc));
        BOOST_TEST(std::abs(gaussian.potential(rc)) ==
                   mjolnir::math::abs_tolerance<real_type>(),
                   boost::test_tools::tolerance(h));
    }
    {
        const real_type e  = -2.0;
        const real_type w  = 0.15;
        const real_type r0 = 7.0;

        mjolnir::GaussianPotential<real_type> gaussian(e, w, r0);

        const auto rc = gaussian.cutoff();

        BOOST_TEST(std::isfinite(rc));
        BOOST_TEST(std::abs(gaussian.potential(rc)) ==
                   mjolnir::math::abs_tolerance<real_type>(),
                   boost::test_tools::tolerance(h));
    }
}

BOOST_AUTO_TEST_CASE(Gaussian_cutoff_float)
{
    using real_type = float;
    constexpr real_type   h = 1e-3;

    {
        const real_type e  = 2.0;
        const real_type w  = 0.15;
        const real_type r0 = 7.0;

        mjolnir::GaussianPotential<real_type> gaussian(e, w, r0);

        const auto rc = gaussian.cutoff();

        BOOST_TEST(std::isfinite(rc));
        BOOST_TEST(gaussian.potential(rc) ==
                   mjolnir::math::abs_tolerance<real_type>(),
                   boost::test_tools::tolerance(h));
    }
    {
        const real_type e  = -2.0;
        const real_type w  = 0.15;
        const real_type r0 = 7.0;

        mjolnir::GaussianPotential<real_type> gaussian(e, w, r0);

        const auto rc = gaussian.cutoff();

        BOOST_TEST(std::isfinite(rc));
        BOOST_TEST(std::abs(gaussian.potential(rc)) ==
                   mjolnir::math::abs_tolerance<real_type>(),
                   boost::test_tools::tolerance(h));
    }
}


/**
 * MULTIDRONE Project:
 *
 * Geographic to Cartesian coordinate conversion and vice versa, with an intermediate conversion to UTM.
 *
 * This library suppose that the path of the robot cannot cross more than 2 different UTM zones.
 *
 */

#ifndef GEOGRAPHIC_TO_CARTESIAN
#define GEOGRAPHIC_TO_CARTESIAN

#include <math.h>
#include <algorithm>
#include <stdlib.h>
#include <geometry_msgs/Point32.h>
#include <geographic_msgs/GeoPoint.h>

#include <geodesy/utm.h>                // IMPORTANT TO INSTALL GEODESY: sudo apt-get install ros-kinetic-geographic-info

// #define USING_IN_PYTHON

// #ifdef USING_IN_PYTHON
// #include <boost/python.hpp>
// #endif

namespace multidrone {

inline geometry_msgs::Point32 geographic_to_cartesian (const geographic_msgs::GeoPoint& _actual_coordinate_geo, const geographic_msgs::GeoPoint& _origin_geo) {

    geodesy::UTMPoint actual_coordinate_UTM(_actual_coordinate_geo); // Conversion from geographic coordinates to UTM.
    geodesy::UTMPoint origin_UTM(_origin_geo);                       // Conversion from geographic coordinates to UTM.

    geometry_msgs::Point32 actual_coordinate_cartesian;              // Cartesian coordinate that this function will return.

    // The problem with UTM is that if there are coordinates in more than one zone, it's difficult to merge the coordinates of the different zones.
    // This is because each zone has 6 degrees of longitude, and the width in meters of the zone is variable from equator to the poles.
    // Something similar when the coordinates are in different hemispheres, separated by the equator. Each hemisphere has a different origin for the y axis.
    // These are the reasons why the x and y assignation for the Cartesian conversion isn't a simple UTM substraction.

    // Assignating "actual_coordinate_cartesian.x" is tricky when the actual coordinate and the origin of the Cartesian coordinates are on different zones.
    if ( int(actual_coordinate_UTM.zone)==60 && origin_UTM.zone==1 ) {         // Coordinate and origin separated by the +-180º longitude
        geographic_msgs::GeoPoint geo_180_w;
        geographic_msgs::GeoPoint geo_180_e;
        geo_180_w.longitude = 179.9999999;
        geo_180_w.latitude = _actual_coordinate_geo.latitude;
        geo_180_w.altitude = _actual_coordinate_geo.altitude;
        geo_180_e.longitude = -179.9999999;
        geo_180_e.latitude = _actual_coordinate_geo.latitude;
        geo_180_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_180_w(geo_180_w);
        geodesy::UTMPoint utm_180_e(geo_180_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_180_w.easting + utm_180_e.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
    } else if ( origin_UTM.zone==60 && int(actual_coordinate_UTM.zone)==1 ) {  // Coordinate and origin separated by the +-180º longitude
        geographic_msgs::GeoPoint geo_180_w;
        geographic_msgs::GeoPoint geo_180_e;
        geo_180_w.longitude = 179.9999999;
        geo_180_w.latitude = _actual_coordinate_geo.latitude;
        geo_180_w.altitude = _actual_coordinate_geo.altitude;
        geo_180_e.longitude = -179.9999999;
        geo_180_e.latitude = _actual_coordinate_geo.latitude;
        geo_180_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_180_w(geo_180_w);
        geodesy::UTMPoint utm_180_e(geo_180_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_180_e.easting + utm_180_w.easting - origin_UTM.easting; // Transformation of the x coordinate taking into account the different zones. " - utm_180_w.easting + utm_180_e.easting " computes the x step in both sides of the border longitude.
    } else if ( int(actual_coordinate_UTM.zone) < origin_UTM.zone ) {
        int quotient_from_int_division = (int) ( std::max(std::abs(_actual_coordinate_geo.longitude),std::abs(_origin_geo.longitude))/6 );                    // int division of the max longitude (absolute, without sign)
        double border_longitude = quotient_from_int_division * 6.0 *pow(-1,std::signbit(std::max(_actual_coordinate_geo.longitude,_origin_geo.longitude)));   // border_longitude = quotient_from_int_division * 6 *(-1)^(1 if negative longitude, 0 if positive)
        geographic_msgs::GeoPoint geo_w;
        geographic_msgs::GeoPoint geo_e;
        geo_w.longitude = border_longitude - 0.0000001;
        geo_w.latitude = _actual_coordinate_geo.latitude;
        geo_w.altitude = _actual_coordinate_geo.altitude;
        geo_e.longitude = border_longitude + 0.0000001;
        geo_e.latitude = _actual_coordinate_geo.latitude;
        geo_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_w(geo_w);
        geodesy::UTMPoint utm_e(geo_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_w.easting + utm_e.easting - origin_UTM.easting;         // Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
    } else if ( origin_UTM.zone < int(actual_coordinate_UTM.zone) ) {
        int quotient_from_int_division = (int) ( std::max(std::abs(_actual_coordinate_geo.longitude),std::abs(_origin_geo.longitude))/6 );                    // int division of the max longitude (absolute, without sign)
        double border_longitude = quotient_from_int_division * 6.0 *pow(-1,std::signbit(std::max(_actual_coordinate_geo.longitude,_origin_geo.longitude)));   // border_longitude = quotient_from_int_division * 6 *(-1)^(1 if negative longitude, 0 if positive)
        geographic_msgs::GeoPoint geo_w;
        geographic_msgs::GeoPoint geo_e;
        geo_w.longitude = border_longitude - 0.0000001;
        geo_w.latitude = _actual_coordinate_geo.latitude;
        geo_w.altitude = _actual_coordinate_geo.altitude;
        geo_e.longitude = border_longitude + 0.0000001;
        geo_e.latitude = _actual_coordinate_geo.latitude;
        geo_e.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_w(geo_w);
        geodesy::UTMPoint utm_e(geo_e);
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting - utm_e.easting + utm_w.easting - origin_UTM.easting;    // Transformation of the x coordinate taking into account the different zones. " - utm_w.easting + utm_e.easting " computes the x step in both sides of the border longitude.
    } else {
        // The actual coordinate is in the same zone that the origin (the first station). This is the normal situation, the assigntation of the x axis value is trivial (simple subtraction).
        actual_coordinate_cartesian.x = actual_coordinate_UTM.easting -origin_UTM.easting;
    }

    // Assignating "actual_coordinate_cartesian.y" is also tricky when the actual coordinate and the origin of the Cartesian coordinates are on different hemispheres:
    if ( origin_UTM.band=='N' && actual_coordinate_UTM.band=='M' ) {         // Coordinate and origin separated by the 0º latitude
        geographic_msgs::GeoPoint geo_0_n;
        geographic_msgs::GeoPoint geo_0_s;
        geo_0_n.longitude = _actual_coordinate_geo.longitude;
        geo_0_n.latitude = 0.0000001;
        geo_0_n.altitude = _actual_coordinate_geo.altitude;
        geo_0_s.longitude = _actual_coordinate_geo.longitude;
        geo_0_s.latitude = -0.0000001;
        geo_0_s.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_0_n(geo_0_n);
        geodesy::UTMPoint utm_0_s(geo_0_s);
        actual_coordinate_cartesian.y = actual_coordinate_UTM.northing - utm_0_s.northing + utm_0_n.northing - origin_UTM.northing;    // Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_s.northing + utm_0_n.northing " computes the y step in both sides of the equator.
    } else if ( actual_coordinate_UTM.band=='N' && origin_UTM.band=='M' ) {  // Coordinate and origin separated by the 0º latitude
        geographic_msgs::GeoPoint geo_0_n;
        geographic_msgs::GeoPoint geo_0_s;
        geo_0_n.longitude = _actual_coordinate_geo.longitude;
        geo_0_n.latitude = 0.0000001;
        geo_0_n.altitude = _actual_coordinate_geo.altitude;
        geo_0_s.longitude = _actual_coordinate_geo.longitude;
        geo_0_s.latitude = -0.0000001;
        geo_0_s.altitude = _actual_coordinate_geo.altitude;
        geodesy::UTMPoint utm_0_n(geo_0_n);
        geodesy::UTMPoint utm_0_s(geo_0_s);
        actual_coordinate_cartesian.y = actual_coordinate_UTM.northing - utm_0_n.northing + utm_0_s.northing - origin_UTM.northing;    // Transformation of the y coordinate taking into account the different hemispheres. " - utm_0_n.northing + utm_0_s.northing " computes the y step in both sides of the equator.
    } else {
        // The actual coordinate is in the same hemisphere that the origin (the first station). This is the normal situation, the assigntation of the y axis value is trivial (simple subtraction).
        actual_coordinate_cartesian.y = actual_coordinate_UTM.northing-origin_UTM.northing;
    }

    // Assignating "actual_coordinate_cartesian.z" is trivial always, simple subtraction.
    actual_coordinate_cartesian.z = actual_coordinate_UTM.altitude-origin_UTM.altitude;

    return actual_coordinate_cartesian;
}   // end geographic_to_cartesian

inline geographic_msgs::GeoPoint cartesian_to_geographic (const geometry_msgs::Point32& _actual_coordinate_cartesian, const geographic_msgs::GeoPoint& _origin_geo) {
    geodesy::UTMPoint actual_coordinate_aux(_origin_geo);                                               // Conversion from geographic coordinates to UTM.

    actual_coordinate_aux.easting  += _actual_coordinate_cartesian.x;
    actual_coordinate_aux.northing += _actual_coordinate_cartesian.y;
    actual_coordinate_aux.altitude += _actual_coordinate_cartesian.z;

    // Convert to geographic coordinates and return the coordinate. This conversion is easier as it doesn't matter if the points are in different zones or hemispheres, but in that case the error increases if the points are very far from each other.
    return geodesy::toMsg (actual_coordinate_aux);

}   // end cartesian_to_geographic

// #ifdef USING_IN_PYTHON
// BOOST_PYTHON_MODULE(geographic_to_cartesian_and_vice_verse) {
//     using namespace boost::python;
//     def("geographic_to_cartesian", geographic_to_cartesian);
//     def("cartesian_to_geographic", cartesian_to_geographic);
// }
// #endif

}   // end namespace multidrone

#endif // GEOGRAPHIC_TO_CARTESIAN


#include <iostream>
#include <cmath>

using ::std::string;

#include <vector>

using ::std::vector;

#include <pcl/point_types.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/cloud_viewer.h>

using ::pcl::PointCloud;
using ::pcl::PointXYZ;
using ::pcl::computeCentroid;
using ::pcl::transformPointCloud;
using ::pcl::io::loadPLYFile;
using ::pcl::visualization::CloudViewer;

#include <Eigen/Dense>

using ::Eigen::Matrix4d;

#include "my_point_cloud.hpp"

#define PI 3.14159265

MyPCL::MyPointCloud::MyPointCloud(string path) {
    int pcl_load_status;

    cloud.reset(new PointCloud<PointXYZ>());
    pcl_load_status = loadPLYFile(path, *cloud);

    if (pcl_load_status == -1) {
        PCL_ERROR("Reading error!\n");
        exit(pcl_load_status);
    }
}

MyPCL::MyPointCloud::~MyPointCloud() {}

void MyPCL::MyPointCloud::view() {
    CloudViewer viewer("My PointCloud viewer");

    viewer.showCloud(cloud);
    while (!viewer.wasStopped()) {} // Busy waiting
}

PointXYZ MyPCL::MyPointCloud::centroid() {
    size_t centroid_dimension;
    PointXYZ centroid;

    centroid_dimension = computeCentroid(*cloud, centroid);

    if (centroid_dimension < 3)
        exit(-1);

    return centroid;
}

void MyPCL::MyPointCloud::apply_offset(PointXYZ offset) {
    apply_transformation(offset, PointXYZ(1, 1, 1), PointXYZ(0, 0, 0));
}

void MyPCL::MyPointCloud::apply_scale(double x_s, double y_s, double z_s) {
    apply_transformation(PointXYZ(0, 0, 0), PointXYZ(x_s, y_s, z_s), PointXYZ(0, 0, 0));
}

void MyPCL::MyPointCloud::apply_scale(double scale) {
    apply_scale(scale, scale, scale);
}

void MyPCL::MyPointCloud::apply_rotation(double x_r, double y_r, double z_r) {
    apply_transformation(PointXYZ(0, 0, 0), PointXYZ(1, 1, 1), PointXYZ(x_r, y_r, z_r));
}

void MyPCL::MyPointCloud::apply_transformation(PointXYZ offset, PointXYZ scale, PointXYZ rotation) {
    Matrix4d m_x, m_y, m_z;
    m_x = Matrix4d::Identity();
    m_y = Matrix4d::Identity();
    m_z = Matrix4d::Identity();

    auto to_rad = [] (double degree) {
        return degree / 180 * PI;
    };

    double rad_x, rad_y, rad_z;
    rad_x = to_rad(rotation.x);
    rad_y = to_rad(rotation.y);
    rad_z = to_rad(rotation.z);

    m_x(1, 1) = cos(rad_x);
    m_x(1, 2) = -sin(rad_x);
    m_x(2, 1) = sin(rad_x);
    m_x(2, 2) = cos(rad_x);

    m_y(0, 0) = cos(rad_y);
    m_y(2, 0) = sin(rad_y);
    m_y(0, 2) = -sin(rad_y);
    m_y(2, 2) = cos(rad_y);

    m_z(0, 0) = cos(rad_z);
    m_z(0, 1) = -sin(rad_z);
    m_z(1, 0) = sin(rad_z);
    m_z(1, 1) = cos(rad_z);

    Matrix4d m_rotation;
    m_rotation = m_x * m_y * m_z;

    Matrix4d m_scale;
    m_scale = Matrix4d::Identity();

    // Setting the scaling matrix
    /*
     * x_s 0   0   0
     * 0   y_s 0   0
     * 0   0   z_s 0
     * 0   0   0   1
     */

    m_scale(0, 0) = scale.x;
    m_scale(1, 1) = scale.y;
    m_scale(2, 2) = scale.z;

    Matrix4d m_translation;
    m_translation = Matrix4d::Identity();

    // Setting the translation matrix
    /*
     * 1 0 0 x
     * 0 1 0 y
     * 0 0 1 z
     * 0 0 0 1
     */
    m_translation(0, 3) = offset.x;
    m_translation(1, 3) = offset.y;
    m_translation(2, 3) = offset.z;

    PointCloud<PointXYZ>::Ptr transformed;
    transformed.reset(new PointCloud<PointXYZ>());

    Matrix4d m_transform;
    m_transform = m_scale * m_rotation * m_translation;

    transformPointCloud(*cloud, *transformed, m_transform);
    cloud = transformed;
}