/*
  Copyright (c) 2010 Toru Tamaki

  Permission is hereby granted, free of charge, to any person
  obtaining a copy of this software and associated documentation
  files (the "Software"), to deal in the Software without
  restriction, including without limitation the rights to use,
  copy, modify, merge, publish, distribute, sublicense, and/or sell
  copies of the Software, and to permit persons to whom the
  Software is furnished to do so, subject to the following
  conditions:

  The above copyright notice and this permission notice shall be
  included in all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
  EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
  OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
  NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
  HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
  WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
  OTHER DEALINGS IN THE SOFTWARE.
*/

#include <iostream>

#include <boost/shared_array.hpp>

#include <flann/flann.hpp>

// uncomment if you do not use the viewer.
//#define NOVIEWER

#include "3dregistration.h"

extern "C" {
  int sgemm_(const char *transa, const char *transb, const int *m, const int *n, const int *k,
             const float *alpha, const float *a, const int *lda, const float *b, const int *ldb,
             const float *beta, float *c, const int *ldc);
}

inline static float
distanceSquare(float x1, float y1, float z1,
	       float x2, float y2, float z2){

  float tmpx = (x1 - x2);
  float tmpy = (y1 - y2);
  float tmpz = (z1 - z2);

  return tmpx*tmpx + tmpy*tmpy + tmpz*tmpz;
}

inline static float
distanceSquareRT(float x1, float y1, float z1,
		 float x2, float y2, float z2, float* h_R, float* h_t){

  return distanceSquare(x1, y1, z1,
			(h_R[0]*x2 + h_R[1]*y2 + h_R[2]*z2) + h_t[0],
			(h_R[3]*x2 + h_R[4]*y2 + h_R[5]*z2) + h_t[1],
			(h_R[6]*x2 + h_R[7]*y2 + h_R[8]*z2) + h_t[2]);

}

#if 1
static void
findCenter(const float* h_X, const int Xsize,
	   float* h_Xc){

  const float* h_Xx = &h_X[Xsize*0];
  const float* h_Xy = &h_X[Xsize*1];
  const float* h_Xz = &h_X[Xsize*2];

  double Xcx = 0.0f, Xcy = 0.0f, Xcz = 0.0f;

#pragma omp parallel for reduction (+:Xcx,Xcy,Xcz)
  for(int i = 0; i < Xsize; i++){
    Xcx += h_Xx[i];
    Xcy += h_Xy[i];
    Xcz += h_Xz[i];
  }

  h_Xc[0] = Xcx / Xsize;
  h_Xc[1] = Xcy / Xsize;
  h_Xc[2] = Xcz / Xsize;

}

#else
static void
findCenter(const float* h_X, const int Xsize,
	   float* h_Xc){

  const float* h_Xx = &h_X[Xsize*0];
  const float* h_Xy = &h_X[Xsize*1];
  const float* h_Xz = &h_X[Xsize*2];

  h_Xc[0] = h_Xc[1] = h_Xc[2] = 0.0f;

  for(int i = 0; i < Xsize; i++){
    h_Xc[0] += h_Xx[i];
    h_Xc[1] += h_Xy[i];
    h_Xc[2] += h_Xz[i];
  }

  h_Xc[0] /= Xsize;
  h_Xc[1] /= Xsize;
  h_Xc[2] /= Xsize;

}
#endif

void icp(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target,
	 const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_source,
	 float* h_R, float* h_t,
	 const registrationParameters &param)
{

  int Xsize, Ysize;
  boost::shared_array<float> h_X, h_Y;
  cloud2data(cloud_target, h_X, Xsize);
  cloud2data(cloud_source, h_Y, Ysize);

  //
  // initialize paramters
  //
  int maxIteration = param.maxIteration;

  //
  // memory allocation
  //

  float* h_Xx = h_X.get() + Xsize*0;
  float* h_Xy = h_X.get() + Xsize*1;
  float* h_Xz = h_X.get() + Xsize*2;

  float* h_Yx = h_Y.get() + Ysize*0;
  float* h_Yy = h_Y.get() + Ysize*1;
  float* h_Yz = h_Y.get() + Ysize*2;

  boost::shared_array<float> h_Xcorr ( new float[Ysize*3] ); // points in X corresponding to Y

  float h_S[9];
  float h_Xc[3];
  float h_Yc[3];

  findCenter(h_Y.get(), Ysize, h_Yc);

  // building flann index
  boost::shared_array<float> m_X ( new float [Xsize*3] );
  for (int i = 0; i < Xsize; i++)
  {
    m_X[i*3 + 0] = h_Xx[i];
    m_X[i*3 + 1] = h_Xy[i];
    m_X[i*3 + 2] = h_Xz[i];
  }
  flann::Matrix<float> mat_X(m_X.get(), Xsize, 3); // Xsize rows and 3 columns
  flann::Index< flann::L2<float> > index( mat_X, flann::KDTreeIndexParams() );
  index.buildIndex();

  boost::shared_array<float> m_Y ( new float [Ysize*3] );
  float* h_Xcorrx = h_Xcorr.get() + Ysize*0;
  float* h_Xcorry = h_Xcorr.get() + Ysize*1;
  float* h_Xcorrz = h_Xcorr.get() + Ysize*2;

  boost::shared_array<float> h_Ycentered ( new float [Ysize*3] );
  {
    float* h_Ycenteredx = h_Ycentered.get() + Ysize*0;
    float* h_Ycenteredy = h_Ycentered.get() + Ysize*1;
    float* h_Ycenteredz = h_Ycentered.get() + Ysize*2;
    for(int i = 0; i < Ysize; i++){
      h_Ycenteredx[i] = h_Yx[i] - h_Yc[0];
      h_Ycenteredy[i] = h_Yy[i] - h_Yc[1];
      h_Ycenteredz[i] = h_Yz[i] - h_Yc[2];
    }
  }

  // ICP main loop

  for(int iter=0; iter < maxIteration; iter++){

    // find closest points

    #pragma omp parallel for
    for (int i = 0; i < Ysize; i++)
    {
      m_Y[i*3 + 0] = (h_R[0]*h_Yx[i] + h_R[1]*h_Yy[i] + h_R[2]*h_Yz[i]) + h_t[0];
      m_Y[i*3 + 1] = (h_R[3]*h_Yx[i] + h_R[4]*h_Yy[i] + h_R[5]*h_Yz[i]) + h_t[1];
      m_Y[i*3 + 2] = (h_R[6]*h_Yx[i] + h_R[7]*h_Yy[i] + h_R[8]*h_Yz[i]) + h_t[2];
    }
    flann::Matrix<float> mat_Y(m_Y.get(), Ysize, 3); // Ysize rows and 3 columns

    std::vector< std::vector<size_t> > indices(Ysize);
    std::vector< std::vector<float> >  dists(Ysize);

    index.knnSearch(mat_Y,
		    indices,
		    dists,
		    1, // k of knn
		    flann::SearchParams() );

    #pragma omp parallel for
    for(int i = 0; i < Ysize; i++){
      // put the closest point to Ycorr
      h_Xcorrx[i] = h_Xx[indices[i][0]];
      h_Xcorry[i] = h_Xy[indices[i][0]];
      h_Xcorrz[i] = h_Xz[indices[i][0]];
    }

    findCenter(h_Xcorr.get(), Ysize, h_Xc);

    #pragma omp parallel for
    for(int i = 0; i < Ysize; i++){
      // put the closest point to Ycorr
      h_Xcorrx[i] -= h_Xc[0];
      h_Xcorry[i] -= h_Xc[1];
      h_Xcorrz[i] -= h_Xc[2];
    }

    // compute S

    {
      // SGEMM(TRANSA,TRANSB,M,N,K,ALPHA,A,LDA,B,LDB,BETA,C,LDC)
      //  C := alpha*op( A )*op( B ) + beta*C
      //
      //  h_X^T * h_Y => S
      //  m*k     k*n    m*n
      int three = 3;
      float one = 1.0f, zero = 0.0f;
      sgemm_((char*)"t", (char*)"n",
	     &three, &three, &Ysize, // m,n,k
	     &one, h_Xcorr.get(), &Ysize, // alpha, op(A), lda
	     h_Ycentered.get(), &Ysize,  // op(B), ldb
	     &zero, h_S, &three);  // beta, C, ldc
    }

    // find RT from S

    findRTfromS(h_Xc, h_Yc, h_S, h_R, h_t);

#ifndef NOVIEWER
    if(!param.noviewer){
      Eigen::Matrix4f transformation;
      transformation <<
      			h_R[0], h_R[1], h_R[2], h_t[0],
			h_R[3], h_R[4], h_R[5], h_t[1],
			h_R[6], h_R[7], h_R[8], h_t[2],
			0, 0, 0, 1;
      pcl::transformPointCloud ( *param.cloud_source, *param.cloud_source_trans, transformation );
      param.viewer->updatePointCloud ( param.cloud_source_trans, *param.source_trans_color, "source trans" );
      param.viewer->spinOnce();
    }
#endif

  }

}


#include <boost/test/unit_test.hpp>

#include <boost/thread.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/ref.hpp>
#include <boost/filesystem.hpp>
#include <boost/filesystem/path.hpp>
#include <boost/random.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>

#include <iostream>
#include <fstream>
#include <map>

#include <ir/index_manager/utility/PriorityQueue.h>

using namespace std;
using namespace boost;

boost::mt19937 engine ;
boost::uniform_real<float> distribution(0.0, 1.0);
boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > gen(engine, distribution);

struct ScoreDoc
{
    unsigned int id;
    float score;
};

class ScoreDocQueue: public izenelib::ir::indexmanager::PriorityQueue<ScoreDoc*>{
public:
	ScoreDocQueue(int32_t count)
	{
		initialize(count,false);
	}
protected:
	bool lessThan(ScoreDoc* a, ScoreDoc* b) {
		return (a->score < b->score);
	}
};

BOOST_AUTO_TEST_SUITE( t_priorityqueue )

BOOST_AUTO_TEST_CASE(prorityqueue)
{
    size_t count = 1000;
    ScoreDocQueue queue(count);

    for (size_t i = 0; i < count; i++)
    {
        ScoreDoc* doc = new ScoreDoc;
        doc->id = i;
        doc->score = gen();
        queue.put( doc );
    }
    float last = -1.0F;
    for (size_t j = 0; j < count; j++) {
        ScoreDoc* doc = queue.pop();
        float next = doc->score;
        delete doc;
        BOOST_CHECK(next>=last);
        last = next;
    }

}

BOOST_AUTO_TEST_SUITE_END()


#ifndef DELAUNAY3D_HPP
#define DELAUNAY3D_HPP 1

#include "Vector3D.hpp"
#include "Tetrahedron.hpp"
#include <string>
#include <vector>
#include <set>
#include <array>
#include <boost/container/flat_set.hpp>

using std::vector;
using std::string;
//using std::stack;

class Delaunay3D
{
public:
	vector<Tetrahedron> tetras_;
	vector<Vector3D> points_;
	boost::container::flat_set<size_t> empty_tetras_;
	std::size_t Norg_;
	std::size_t outside_neighbor_;

	Delaunay3D();

	Delaunay3D(Delaunay3D const& other);

	Delaunay3D& operator=(Delaunay3D const& other);

	~Delaunay3D();

	void Build(vector<Vector3D> const& points,Vector3D const& maxv,Vector3D const& minv,std::vector<size_t> &order);

	void BuildExtra(vector<Vector3D> const& points);

	void output(string const& filename)const;

	bool CheckCorrect(void);

	void Clean(void);
private:
	void InsertPoint(std::size_t index);
	std::size_t Walk(std::size_t point, std::size_t first_guess);
	void flip14(std::size_t point,std::size_t tetra);
	void flip23(std::size_t tetra0, std::size_t tetra1,std::size_t location0,bool flat_check);
	void flip32(std::size_t tetra0, std::size_t tetra1, std::size_t location0, std::size_t shared_loction, bool flat_check);
	void flip44(std::size_t tetra0, std::size_t tetra1, std::size_t location0, std::size_t neigh0, std::size_t neigh1);
	void FindFlip(std::size_t tetra0, std::size_t tetra1, std::size_t p, size_t p_loc, size_t other_point_loc);
	void ExactFlip(std::size_t tetra0, std::size_t tetra1, std::size_t p);
	std::size_t FindThirdNeighbor(std::size_t tetra0, std::size_t tetra1);

	std::array<Vector3D, 3> b3_temp_,b3_temp2_;
	std::array<Vector3D, 4> b4_temp_;
	std::array<Vector3D, 5> b5_temp_;
	std::array<std::size_t, 4> b4s_temp_,b4s_temp2_;
	std::array<std::size_t, 8> b8s_temp_;
	vector<std::size_t> to_check_;
	std::size_t last_checked_;
	Tetrahedron tet_temp0_, tet_temp1_, newtet_;
};

#endif //DELAUNAY3D_HPP


// The MIT License (MIT)

// Copyright (c) 2014 Mohammad Dashti
// (www.mdashti.com - mohammad.dashti [at] epfl [dot] ch - mdashti [at] gmail [dot] com)

// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:

// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.

// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef KDOUBLE_H
#define KDOUBLE_H
#include <cmath>
#include <iostream>
// #include <boost/multiprecision/cpp_dec_float.hpp>

// typedef boost::multiprecision::number<boost::multiprecision::cpp_dec_float<1000, long long> > cpp_dec_float_1000;

#define DOUBLE_TYPE_STD_DOUBLE 1
#define DOUBLE_TYPE_KAHAN_DOUBLE 2
#define DOUBLE_TYPE_STD_LONG_DOUBLE 3
#define DOUBLE_TYPE_BOOST 4

// #ifndef DOUBLE_TYPE

//   //you can change the value of DOUBLE_TYPE_SYM
//   //to change the double type. Choices are:
//   // - DOUBLE_TYPE_STD_DOUBLE
//   // - DOUBLE_TYPE_KAHAN_DOUBLE
//   // - DOUBLE_TYPE_STD_LONG_DOUBLE
//   // - DOUBLE_TYPE_BOOST
//   #define DOUBLE_TYPE_SYM DOUBLE_TYPE_STD_DOUBLE

//   #if DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
//     #define DOUBLE_TYPE KDouble
//   #elif DOUBLE_TYPE_SYM == DOUBLE_TYPE_BOOST
//     #define DOUBLE_TYPE cpp_dec_float_1000
//   #elif DOUBLE_TYPE_SYM == DOUBLE_TYPE_STD_LONG_DOUBLE
//     #define DOUBLE_TYPE long double
//   #else
//     #define DOUBLE_TYPE double
//   #endif //DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
// #endif //DOUBLE_TYPE

namespace dbtoaster {

class KDouble
{
public:
  static double diff_p;
#if DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
private:
  double sum;
  double c;

protected:
  //friends
  friend bool operator==(const double, const KDouble &);
  friend bool operator!=(const double, const KDouble &);
  friend bool operator<(const double, const KDouble &);
  friend bool operator<=(const double, const KDouble &);
  friend bool operator>(const double, const KDouble &);
  friend bool operator>=(const double, const KDouble &);
  friend KDouble acos(const KDouble &);
  friend KDouble atan2(const KDouble &, const KDouble &);
  friend KDouble sqrt(const KDouble &);
  friend KDouble pow(const KDouble &, const KDouble &);
  // friend size_t hash_value(KDouble const &v);
  template <class T>
  friend void hash_combine(std::size_t& seed, const T& v);
  friend KDouble abs(const KDouble &dbl);

public:
  KDouble() : sum(0.0), c(0.0)
  {
  }

  KDouble(const KDouble &init) : sum(init.sum), c(init.c)
  {
  }

  KDouble(const double &init) : sum(init), c(0.0)
  {
  }

  ~KDouble()
  {
  }

  inline KDouble &operator=(const KDouble &other) { sum = other.sum; c = other.c; return *this; }
  inline KDouble &operator=(const double &other) { sum = other; c = 0.0; return *this; }
  inline KDouble &operator-=(const KDouble &other) {
    double y = -other.sum-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }
  inline KDouble &operator-=(const double &other) {
    double y = -other-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }
  inline KDouble &operator+=(const KDouble &other) {
    double y = other.sum-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }
  inline KDouble &operator+=(const double &other) {
    double y = other-c;
    double t = sum+y;
    c = static_cast<double>(t-sum)-y;
    sum = t;
    return *this;
  }

  //TODO Is this the right implementation?
  inline KDouble &operator*=(const KDouble &other) { sum *= other.sum; c *= other.sum; return *this; }
  inline KDouble &operator*=(const double &other) { sum *= other; c *= other; return *this; }
  inline KDouble &operator/=(const KDouble &other) { sum /= other.sum; c /= other.sum; return *this; }
  inline KDouble &operator/=(const double &other) { sum /= other; c /= other; return *this; }

  friend std::ostream &operator<<(std::ostream &out, const KDouble &kd) {
      return out << kd.sum;
  }

  //inline operator double() const { return sum; }
  // operator int() const { return static_cast<int>(sum); }
  // operator long() const { return static_cast<long>(sum); }

  inline bool operator==(const KDouble &other) const
  {
    return abs(sum-other.sum) < diff_p;
  }

  inline bool operator!=(const KDouble &other) const
  {
    return abs(sum-other.sum) >= diff_p;
  }

  // inline bool operator==(const double &other)
  // {
  //   return abs(sum-other) < diff_p;
  // }

  // inline bool operator!=(const double &other)
  // {
  //   return abs(sum-other) >= diff_p;
  // }

  inline bool operator<(const KDouble &other) const
  {
    return sum < other.sum;
  }

  inline bool operator<=(const KDouble &other) const
  {
    return sum <= other.sum;
  }

  inline bool operator>(const KDouble &other) const
  {
    return sum > other.sum;
  }

  inline bool operator>=(const KDouble &other) const
  {
    return sum >= other.sum;
  }
#endif //DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
};

#if DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE

  inline bool operator==(const double sum, const KDouble & other)
  {
    return abs(sum-other.sum) < KDouble::diff_p;
  }
  inline bool operator!=(const double sum, const KDouble & other)
  {
    return abs(sum-other.sum) >= KDouble::diff_p;
  }

  inline bool operator<(const double sum, const KDouble &other)
  {
    return sum < other.sum;
  }

  inline bool operator<=(const double sum, const KDouble &other)
  {
    return sum <= other.sum;
  }

  inline bool operator>(const double sum, const KDouble &other)
  {
    return sum > other.sum;
  }

  inline bool operator>=(const double sum, const KDouble &other)
  {
    return sum >= other.sum;
  }

  inline KDouble operator-(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const double &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const double &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator/(const double &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const long &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const long &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator/(const long &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const int &g1, const KDouble &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const double &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const long &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble operator-(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result -= g2;
      return result;
  }

  inline KDouble operator+(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result += g2;
      return result;
  }

  inline KDouble operator*(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result *= g2;
      return result;
  }

  inline KDouble operator/(const KDouble &g1, const int &g2)
  {
      KDouble result(g1);
      result /= g2;
      return result;
  }

  inline KDouble abs(const KDouble &dbl)
  {
      KDouble result(dbl);
      if(result.sum < 0.0) result.sum = -result.sum;
      return result;
  }

  inline KDouble acos(const KDouble &dbl)
  {
      return KDouble(acos(dbl.sum));
  }

  inline KDouble atan2(const KDouble &g1, const KDouble &g2)
  {
    return KDouble(atan2(g1.sum,g2.sum));
  }

  inline KDouble sqrt(const KDouble &dbl)
  {
    return KDouble(sqrt(dbl.sum));
  }

  inline KDouble pow(const KDouble &g1, const KDouble &g2)
  {
    return KDouble(pow(g1.sum, g2.sum));
  }
#endif //DOUBLE_TYPE_SYM == DOUBLE_TYPE_KAHAN_DOUBLE
}
#endif //KDOUBLE_H


//  numeric_cast.hpp
//
//  (C) Copyright 2011 Vicente J. Botet Escriba
//  Use, modification and distribution are subject to the Boost Software License,
//  Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt).
//

#ifndef BOOST_RATIO_MPL_NUMERIC_CAST_HPP
#define BOOST_RATIO_MPL_NUMERIC_CAST_HPP

#include <boost/ratio/ratio.hpp>
#include <boost/mpl/integral_c.hpp>
#include <boost/ratio/mpl/rational_c_tag.hpp>
#include <boost/mpl/numeric_cast.hpp>

namespace boost {
namespace mpl {

template<> struct numeric_cast< integral_c_tag,rational_c_tag >
{
    template< typename N > struct apply
        : ratio< N::value, 1 >
    {
    };
};

}
}

#endif  // BOOST_RATIO_MPL_NUMERIC_CAST_HPP


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018-2021 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

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

#include "open3d/Open3D.h"

using namespace open3d;

std::tuple<std::shared_ptr<geometry::PointCloud>,
           std::shared_ptr<geometry::PointCloud>,
           std::shared_ptr<pipelines::registration::Feature>>
PreprocessPointCloud(const char *file_name, const float voxel_size) {
    auto pcd = open3d::io::CreatePointCloudFromFile(file_name);
    auto pcd_down = pcd->VoxelDownSample(voxel_size);
    pcd_down->EstimateNormals(
            open3d::geometry::KDTreeSearchParamHybrid(2 * voxel_size, 30));
    auto pcd_fpfh = pipelines::registration::ComputeFPFHFeature(
            *pcd_down,
            open3d::geometry::KDTreeSearchParamHybrid(5 * voxel_size, 100));
    return std::make_tuple(pcd, pcd_down, pcd_fpfh);
}

void VisualizeRegistration(const open3d::geometry::PointCloud &source,
                           const open3d::geometry::PointCloud &target,
                           const Eigen::Matrix4d &Transformation) {
    std::shared_ptr<geometry::PointCloud> source_transformed_ptr(
            new geometry::PointCloud);
    std::shared_ptr<geometry::PointCloud> target_ptr(new geometry::PointCloud);
    *source_transformed_ptr = source;
    *target_ptr = target;
    source_transformed_ptr->Transform(Transformation);
    visualization::DrawGeometries({source_transformed_ptr, target_ptr},
                                  "Registration result");
}

void PrintHelp() {
    using namespace open3d;

    PrintOpen3DVersion();
    // clang-format off
    utility::LogInfo("Usage:");
    utility::LogInfo("    > RegistrationFGR source_pcd target_pcd"
                     "[--voxel_size=0.05] [--distance_multiplier=1.5]"
                     "[--max_iterations=64] [--max_tuples=1000]"
                     );
    // clang-format on
}

int main(int argc, char *argv[]) {
    using namespace open3d;

    utility::SetVerbosityLevel(utility::VerbosityLevel::Debug);

    if (argc < 3 ||
        utility::ProgramOptionExistsAny(argc, argv, {"-h", "--help"})) {
        PrintHelp();
        return 1;
    }

    float voxel_size =
            utility::GetProgramOptionAsDouble(argc, argv, "--voxel_size", 0.05);
    float distance_multiplier = utility::GetProgramOptionAsDouble(
            argc, argv, "--distance_multiplier", 1.5);
    float distance_threshold = voxel_size * distance_multiplier;
    int max_iterations =
            utility::GetProgramOptionAsInt(argc, argv, "--max_iterations", 64);
    int max_tuples =
            utility::GetProgramOptionAsInt(argc, argv, "--max_tuples", 1000);

    // Prepare input
    std::shared_ptr<geometry::PointCloud> source, source_down, target,
            target_down;
    std::shared_ptr<pipelines::registration::Feature> source_fpfh, target_fpfh;
    std::tie(source, source_down, source_fpfh) =
            PreprocessPointCloud(argv[1], voxel_size);
    std::tie(target, target_down, target_fpfh) =
            PreprocessPointCloud(argv[2], voxel_size);

    pipelines::registration::RegistrationResult registration_result =
            pipelines::registration::
                    FastGlobalRegistrationBasedOnFeatureMatching(
                            *source_down, *target_down, *source_fpfh,
                            *target_fpfh,
                            pipelines::registration::
                                    FastGlobalRegistrationOption(
                                            /* decrease_mu =  */ 1.4, true,
                                            true, distance_threshold,
                                            max_iterations,
                                            /* tuple_scale =  */ 0.95,
                                            max_tuples));

    VisualizeRegistration(*source, *target,
                          registration_result.transformation_);

    return 0;
}


#include <iostream>
#include <vector>
#include <cassert>
#include <cstring>
#include <limits>
#include <mpi.h>
#include <Eigen/Eigenvalues>

enum KernelType {CubicSpline = 0, WendlandC2 = 1, WendlandC4 = 2};

#include "particle_simulator.hpp"
#include "hdr_time.hpp"
#include "hdr_run.hpp"
#include "vector_x86.hpp"
#include "hdr_dimension.hpp"
#include "hdr_kernel.hpp"
#include "hdr_sph.hpp"
#include "hdr_hgas.hpp"
#include "hdr_bhns.hpp"

class SPHAnalysis : public HelmholtzGas {
public:
    SPHAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->uene  = 0.;
        this->alph  = 0.;
        this->alphu = 0.;
        this->ksr   = 0.;
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            this->cmps[k] = 0.;
        }
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf", &this->id, &this->istar, &this->mass);      //  3
        fscanf(fp, "%lf%lf%lf", &this->pos[0], &this->pos[1], &this->pos[2]); //  6
        fscanf(fp, "%lf%lf%lf", &this->vel[0], &this->vel[1], &this->vel[2]); //  9
        fscanf(fp, "%lf%lf%lf", &this->acc[0], &this->acc[1], &this->acc[2]); // 12
        fscanf(fp, "%lf%lf%lf", &this->uene, &this->alph, &this->alphu);      // 15
        fscanf(fp, "%lf%lf%6d", &this->dens, &this->ksr,  &this->np);         // 18
        fscanf(fp, "%lf%lf%lf", &this->vsnd, &this->pres, &this->temp);       // 21
        fscanf(fp, "%lf%lf%lf", &this->divv, &this->rotv, &this->bswt);       // 24
        fscanf(fp, "%lf%lf%lf", &this->pot,  &this->abar, &this->zbar);       // 27
        fscanf(fp, "%lf",       &this->enuc);                                 // 28
        fscanf(fp, "%lf%lf%lf", &this->vsmx, &this->udot, &this->dnuc);       // 31
        for(PS::S32 k = 0; k < NuclearReaction::NumberOfNucleon; k++) {       // 32 -- 44
            fscanf(fp, "%lf", &this->cmps[k]);
        }
        fscanf(fp, "%lf", &this->pot3);
        fscanf(fp, "%lf%lf%lf", &this->tempmax[0], &this->tempmax[1], &this->tempmax[2]);
        fscanf(fp, "%lf", &this->entr);
    }

    /*
    void write(FILE * fp = stdout) {
        fprintf(fp, "%8d %2d %+e",  this->id, this->istar, this->mass);
        fprintf(fp, " %+e %+e %+e", this->pos[0], this->pos[1], this->pos[2]);
        fprintf(fp, " %+e %+e %+e", this->vel[0], this->vel[1], this->vel[2]);
        fprintf(fp, " %+e %+e %+e", this->uene, this->alph, this->alphu);
        fprintf(fp, " %+e", this->ksr);
        for(PS::S32 k = 0; k < NR::NumberOfNucleon; k++) {
            fprintf(fp, " %+.3e", this->cmps[k]);
        }
        fprintf(fp, "\n");
    }
    */

    inline PS::F64mat calcMomentOfInertia(PS::F64vec xc) {
        PS::F64mat qq = 0.;
        PS::F64vec dx = this->pos - xc;
        PS::F64 r2 = dx * dx;
        qq.xx = this->mass * (r2 - dx[0] * dx[0]);
        qq.xy = this->mass * (   - dx[0] * dx[1]);
        qq.xz = this->mass * (   - dx[0] * dx[2]);
        qq.yy = this->mass * (r2 - dx[1] * dx[1]);
        qq.yz = this->mass * (   - dx[1] * dx[2]);
        qq.zz = this->mass * (r2 - dx[2] * dx[2]);
        return qq;
    }
};

class BlackHoleNeutronStarAnalysis : public BlackHoleNeutronStar {
public:
    BlackHoleNeutronStarAnalysis() {
        this->id    = 0;
        this->istar = 0;
        this->mass  = 0.;
        this->pos   = 0.;
        this->vel   = 0.;
        this->acc   = 0.;
        this->eps   = 0.;
        this->pot   = 0.;
    }

    void readAscii(FILE * fp) {
        fscanf(fp, "%lld%lld%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf",
               &this->id, &this->istar, &this->mass,
               &this->pos[0], &this->pos[1], &this->pos[2],
               &this->vel[0], &this->vel[1], &this->vel[2],
               &this->acc[0], &this->acc[1], &this->acc[2],
               &this->eps, &this->pot);
    }

    inline PS::F64mat calcMomentOfInertia(PS::F64vec xc) {
        PS::F64mat qq = 0.;
        PS::F64vec dx = this->pos - xc;
        PS::F64 r2 = dx * dx;
        qq.xx = this->mass * (r2 - dx[0] * dx[0]);
        qq.xy = this->mass * (   - dx[0] * dx[1]);
        qq.xz = this->mass * (   - dx[0] * dx[2]);
        qq.yy = this->mass * (r2 - dx[1] * dx[1]);
        qq.yz = this->mass * (   - dx[1] * dx[2]);
        qq.zz = this->mass * (r2 - dx[2] * dx[2]);
        return qq;
    }
};

int main(int argc, char ** argv) {
    MPI_Init(&argc, &argv);

    PS::ParticleSystem<SPHAnalysis> sph;
    sph.initialize();
    sph.createParticle(0);
    sph.setNumberOfParticleLocal(0);
    PS::ParticleSystem<BlackHoleNeutronStarAnalysis> bhns;
    bhns.initialize();
    bhns.createParticle(0);
    bhns.setNumberOfParticleLocal(0);

    char idir[1024], odir[1024];
    PS::F64 rcrit;
    PS::S64 ibgn, iend;
    FILE * fp = fopen(argv[1], "r");
    fscanf(fp, "%s", idir);
    fscanf(fp, "%s", odir);
    fscanf(fp, "%lf", &rcrit);
    fscanf(fp, "%lld%lld", &ibgn, &iend);
    fclose(fp);

    char ofile[1024];
    sprintf(ofile, "%s/axis.dat", odir);
    FILE * fout = fopen(ofile, "w");
    fprintf(fout, "# idir: %s\n", idir);
    fprintf(fout, "# crit: %+e\n", rcrit);
    fprintf(fout, "# time, Ixx, Iyy, Izz, Ixx, Iyy, Izz\n");
    for(PS::S64 itime = ibgn; itime <= iend; itime++) {
        char sfile[1024], bfile[1024];
        sprintf(sfile, "%s/sph_t%04d.dat",  idir, itime);
        sprintf(bfile, "%s/bhns_t%04d.dat", idir, itime);
        fp = fopen(sfile, "r");
        assert(fp);
        sph.readParticleAscii(sfile);
        fclose(fp);
        fp = fopen(bfile, "r");
        assert(fp);
        bhns.readParticleAscii(bfile);
        fclose(fp);

        PS::F64mat qq[2] = {0., 0.};
        for(PS::S64 i = 0; i < sph.getNumberOfParticleLocal(); i++) {
            PS::F64mat qtemp = sph[i].calcMomentOfInertia(bhns[0].pos);
            qq[0] = qq[0] + qtemp;
            PS::F64vec dx = sph[i].pos - bhns[0].pos;
            PS::F64    r2 = dx * dx;
            if(r2 < rcrit * rcrit) {
                qq[1] = qq[1] + qtemp;
            }
        }

        fprintf(fout, "%5d", itime);
        Eigen::Matrix3d MomentOfInertia[2];
        for(PS::S64 k = 0; k < 2; k++) {
            MomentOfInertia[k] <<
                qq[k].xx, qq[k].xy, qq[k].xz,
                qq[k].xy, qq[k].yy, qq[k].yz,
                qq[k].xz, qq[k].yz, qq[k].zz;
            Eigen::EigenSolver<Eigen::Matrix3d> es(MomentOfInertia[k], false);
            std::vector<PS::F64> ev;
            ev.push_back(es.eigenvalues()[0].real());
            ev.push_back(es.eigenvalues()[1].real());
            ev.push_back(es.eigenvalues()[2].real());
            std::sort(ev.begin(), ev.end());
            fprintf(fout, " %+e %+e %+e", ev[0], ev[1], ev[2]);
        }
        fprintf(fout, "\n");
        fflush(fout);

    }
    fclose(fout);

    MPI_Finalize();

    return 0;
}


// patroller node

// setup waypoints for patrolling robot
// for quaternion specification of rotation:
//[w, x, y, z] = [cos(a/2), sin(a/2) * nx, sin(a/2)* ny, sin(a/2) * nz]
// Where a is the angle of rotation and {nx,ny,nz} is the axis of rotation.
// we always want to rotate around the z axis, so x = y = 0
// for 0 rotation, just use w = 1, x=y=z=0

#include <ctime>
//#include <mutex>
//#include <thread>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Pose.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#define NUM_GOALS 6
#define HOME 10       // waypoint designated as home base
#define HOME_X 0.0
#define HOME_Y 0.0
#define BATTERY_RETURN_HOME 11.0 // main battery voltage that causes us to go home
#define BATT_DIGITAL_MAX 883
#define BATT_FULL_VOLTS 12.7
#define LAPTOP_BATT_RETURN_HOME 20.  // laptop battery percentage that causes us to go home
#define DISCHARGING 0
#define CHARGING 1

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

volatile bool patrolling_ = false, goHome_ = false, finished_ = false;
//double x_ = 0., y_ = 0., z_ = 0., w_ = 1.; // identity quaternion
double varX_ = 100., varY_ = 100., varYaw_ = 100.;
double batteryVolts_ = 10.5;
double laptopBattPercentage_ = 20.;
int chargingState_ = DISCHARGING;
bool goalReached_ = false;
move_base_msgs::MoveBaseGoal goal_;
MoveBaseClient *ac_;

ros::Publisher poseGoal_pub_, speech_pub_;
ros::Subscriber poseWithCovariance_, sensors_, keyboardCommandsSub_;

double Xwaypoint_[100], Ywaypoint_[100], Twaypoint_[100];
std::string Swaypoint_[100];
std::string sensorReadings_;
boost::mutex myLock;

void waypointsInit()
{
	// mid living room
	Xwaypoint_[0] = HOME_X + 2.0;
	Ywaypoint_[0] = HOME_Y + 0.5;
	Twaypoint_[0] = 90.0;	// positive is counterclockwise
   Swaypoint_[0] = "mid living room, 90 yaw";

	// front living room
	Xwaypoint_[1] = HOME_X + 1.6;
	Ywaypoint_[1] = HOME_Y + 3.3;
	Twaypoint_[1] = 135.0;
   Swaypoint_[1] = "front living room, 135 yaw";

	// front door
	Xwaypoint_[2] = HOME_X - 2.5;
	Ywaypoint_[2] = HOME_Y + 4.5;
	Twaypoint_[2] = -90.0;
   Swaypoint_[2] = "front door, -90 yaw";

   // hall
	Xwaypoint_[3] = HOME_X - 2.5;
	Ywaypoint_[3] = HOME_Y + 1.0;
	Twaypoint_[3] = -90.;
   Swaypoint_[3] = "hall, -90 yaw";

   // kitchen
	Xwaypoint_[4] = HOME_X - 2.0;
	Ywaypoint_[4] = HOME_Y - 1.4;
	Twaypoint_[4] = 0.;
   Swaypoint_[4] = "kitchen 0 yaw";

	// dining room
   Xwaypoint_[5] = HOME_X + 1.5;
	Ywaypoint_[5] = HOME_Y - 1.2;
	Twaypoint_[5] = 90.;
   Swaypoint_[5] = "dining room, 90 yaw";

	// kitchen table
   Xwaypoint_[6] = HOME_X - 6.0;
	Ywaypoint_[6] = HOME_Y - 1.2;
	Twaypoint_[6] = 180.;
   Swaypoint_[6] = "kitchen table, 180 yaw";

	// mudroom
   Xwaypoint_[7] = HOME_X - 10.0;
	Ywaypoint_[7] = HOME_Y + 0.5;
	Twaypoint_[7] = 180;
   Swaypoint_[7] = "mudroom, 180 yaw";

	// familyroom
   Xwaypoint_[8] = HOME_X - 14.0;
	Ywaypoint_[8] = HOME_Y + 0.5;
	Twaypoint_[8] = 0.0;
   Swaypoint_[8] = "familyroom, 0 yaw";

	// sue's office
   Xwaypoint_[9] = HOME_X - 6.0;
	Ywaypoint_[9] = HOME_Y + 3.5;
	Twaypoint_[9] = 90.;
   Swaypoint_[9] = "Sue's office, 90 yaw";

	// near home facing out
	Xwaypoint_[10] = 1.3;
	Ywaypoint_[10] = 0.1;
	Twaypoint_[10] = 0.;
   Swaypoint_[10] = "near home facing out, 0 yaw";

	// home facing in
	Xwaypoint_[11] = 0.0;
	Ywaypoint_[11] = 0.0;
	Twaypoint_[11] = 180.;
   Swaypoint_[11] = "home facing in, 180 yaw";

}

/*
// clock_t is a like typedef unsigned int clock_t. Use clock_t instead of integer in this context
void mySleep(clock_t millisec)
{
   clock_t start_time = clock();
   clock_t end_time = millisec * 1000 + start_time;
   while(clock() < end_time);
}
*/

void setVariances(double vX, double vY, double vYaw)
{
   varX_ = vX;
   varY_ = vY;
   varYaw_ = vYaw;
}

void getVariances(double *vX, double *vY, double *vYaw)
{
   *vX = varX_;
   *vY = varY_;
   *vYaw = varYaw_;
}

bool reduceVariance()
{
   int counter = 1;
   char buffer[2];
   // rotate back and forth in position up to four times to see walls and reduce variance
   double vX, vY, vYaw;
   ros::spinOnce();  // allow the callbacks a chance to see a new pose and update variance
   getVariances(&vX, &vY, &vYaw);
   ROS_INFO("Checking variances: vx = %f, vy = %f, vYaw = %f", vX, vY, vYaw);
   while ((vX > 0.05 || vY > 0.05 || vYaw > .01) && counter < 5)
   {
      if (counter % 2) buffer[0] = 'r';
      else buffer[0] = 'l';
      std_msgs::String rotate;
      rotate.data = buffer;
      poseGoal_pub_.publish(rotate);
      usleep(10000000);
      buffer[0] = 's';
      rotate.data = buffer;
      poseGoal_pub_.publish(rotate);
      usleep(1000000);
      counter++;
      ros::spinOnce();  // allow the callbacks a chance to see a new pose and update variance
      getVariances(&vX, &vY, &vYaw);
      ROS_INFO("After rotating to reduce variance: vx = %f, vy = %f, vYaw = %f", vX, vY, vYaw);
   }
   if (vX > 0.05 || vY > 0.05 || vYaw > .01)
   {
      ROS_INFO("Variance too high to continue: vx = %f, vy = %f, vYaw = %f", vX, vY, vYaw);
      return false;
   }
   ROS_INFO("Variance is within limits");
   return true;
}

bool sendWaypoint(int waypointNumber)
{
   static int firstPatrol = true;
   if (firstPatrol)  // after initial bootup, move out from home base, then reduce variance
   {
      char buffer[5] = "s";
      ROS_INFO("Enabling keyboard commands");
      std_msgs::String forward;
      forward.data = buffer;
      poseGoal_pub_.publish(forward);
      usleep(500000);
      ROS_INFO("Moving forward to start patrol");
      buffer[0] = 'f';
      forward.data = buffer;
      poseGoal_pub_.publish(forward);
      usleep(6000000);
      ROS_INFO("Stopped, checking location, starting patrol");
      buffer[0] = 's';
      forward.data = buffer;
      poseGoal_pub_.publish(forward);
      usleep(1000000);
      firstPatrol = false;
   }

   if (!reduceVariance()) return false; // if variance is too high, we don't know where we are well enough to find a goal

   goal_.target_pose.header.stamp = ros::Time::now();
   goal_.target_pose.pose.position.x = Xwaypoint_[waypointNumber];
   goal_.target_pose.pose.position.y = Ywaypoint_[waypointNumber];
   goal_.target_pose.pose.orientation.w = cos ((Twaypoint_[waypointNumber]/2)*(3.14/180));
   goal_.target_pose.pose.orientation.z = sin ((Twaypoint_[waypointNumber]/2)*(3.14/180));

   ROS_INFO("Sending goal of %s", Swaypoint_[waypointNumber].c_str());
   ac_->sendGoal(goal_);
   ac_->waitForResult();

   if(ac_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
   {
      ROS_INFO("goal reached ");
      goalReached_ = true;
   }
   else
   {
       ROS_INFO("failed to reach goal");
       goalReached_ = false;
       // provide some time and opportunity for messages
       for (int i=0; i < 5; i++)
       {
         usleep(500000);  // sleep for 0.5 sec
         ros::spinOnce();
       }
       double vX, vY, vYaw;
       getVariances(&vX, &vY, &vYaw);
       ROS_INFO("After failing goal and waiting a bit, variance: vx = %f, vy = %f, vYaw = %f", vX, vY, vYaw);
   }
   return true;  // return true even if we fail the goal, since we can reduce variance and try again for the next one
}

//void poseCommandCallback(const boost::shared_ptr<geometry_msgs::PoseWithCovarianceStamped>& msg) // this works
void poseCommandCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) // but this is cleaner
{
   geometry_msgs::PoseWithCovariance poseWithCovariance = msg->pose;
   geometry_msgs::Pose pose = poseWithCovariance.pose;
   geometry_msgs::Point position = pose.position;
   geometry_msgs::Quaternion orientation = pose.orientation;
   double x = position.x;
   double y = position.y;
   double z = orientation.z;
   double w = orientation.w;
   //setPositions(x,y,z,w);

   double varianceX = poseWithCovariance.covariance[0];      // x variance
   double varianceY = poseWithCovariance.covariance[7];      // y variance
   double varianceYaw = poseWithCovariance.covariance[35];   // Yaw variance
   setVariances(varianceX, varianceY, varianceYaw);

   //ROS_INFO("pose received by patrol_node: ");
   //ROS_INFO("pose position x,y = %f, %f", x, y);
   //ROS_INFO("pose orientation w,z = %f, %f", w, z);
   //ROS_INFO("variance x,y,Yaw = %f, %f, %f", varianceX, varianceY, varianceYaw);
}

void keyboardCommandCallback(const std_msgs::String::ConstPtr& msg)
{
   std::string keyboardCommand = msg->data.c_str();
   ROS_INFO("robotPose_node received keyboard command, %s", keyboardCommand.c_str());
   if (keyboardCommand[0] == 'p' && keyboardCommand.length() < 10) // toggle patrolling
   {
      if (patrolling_ || finished_)
      {
         patrolling_ = false;
         finished_ = true;
         char buffer[5] = "c";
         std_msgs::String cancelPatrol;
         cancelPatrol.data = buffer;
         poseGoal_pub_.publish(cancelPatrol);
         ROS_INFO("patrolling finished");
      }
      else
      {
         patrolling_ = true;
         goHome_ = false;
         ROS_INFO("starting to patrol");
      }
   }
   if (keyboardCommand[0] == 'h') // time to wrap it up and go home
   {
         patrolling_ = false;
         finished_ = true;
         goHome_ = true;
         char buffer[5] = "c";
         std_msgs::String cancelPatrol;
         cancelPatrol.data = buffer;
         poseGoal_pub_.publish(cancelPatrol);
         ROS_INFO("patrolling finished, heading home");
   }
}

void sensorReadingsCallback(const std_msgs::String::ConstPtr& msg)
{
   myLock.lock(); // for some reason, without the lock, it does not pick up the laptop message
   sensorReadings_ = msg->data.c_str();
   std::size_t found;

   found = sensorReadings_.find("charging");
   if (found != std::string::npos)
   {
      found = sensorReadings_.find("discharging");
      if (found != std::string::npos) chargingState_ = DISCHARGING;
      else chargingState_ = CHARGING;
   }

   found = sensorReadings_.find("%");
   if (found != std::string::npos && found > 7) // % character says this is a laptop battery reading
   {
      std::string percentageString = sensorReadings_.substr(found - 7, 7);
      laptopBattPercentage_ = atof(percentageString.c_str());
      //ROS_INFO("Laptop battery percentage = %f", laptopBattPercentage_);
   }

   else
   {
      found = sensorReadings_.find("=");  // = character says this is a robot main battery reading
      if (found != std::string::npos)
      {
         std::string batteryString = sensorReadings_.substr(found + 1, std::string::npos);
         double value = atof(batteryString.c_str());
         if (value > BATT_DIGITAL_MAX) value = BATT_DIGITAL_MAX;
         batteryVolts_ = BATT_FULL_VOLTS - (0.12*(BATT_DIGITAL_MAX - value));
         if (batteryVolts_ < 0.) batteryVolts_ = 0.;
         //ROS_INFO("Main robot battery voltage = %f", batteryVolts_);
      }
   }
   myLock.unlock();
}

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "patrol_node");
  ros::NodeHandle nh;
  poseGoal_pub_ = nh.advertise<std_msgs::String>("keyboard", 5);  // send out keyboard commands, these generate poses from amcl
  speech_pub_ = nh.advertise<std_msgs::String>("play_wavfile", 1);    // and the publisher for speech output
  poseWithCovariance_ = nh.subscribe("amcl_pose", 10, poseCommandCallback); // subscribe to amcl pose
  keyboardCommandsSub_ = nh.subscribe("keyboard", 1, keyboardCommandCallback);  // subscribe to keyboard commands
  sensors_ = nh.subscribe("sensor_readings",5, sensorReadingsCallback);
  waypointsInit(); // setup our patrol waypoints

  //tell the action client that we want to spin a thread by default
  ac_ = new MoveBaseClient("move_base",true);

  //wait for the action server to come up
  while(!ac_->waitForServer(ros::Duration(5.0)))
  {
    ROS_INFO("Waiting for the move_base action server to come up");
  }

  goal_.target_pose.header.frame_id = "map";  // we want coordinates on the map, not with respect to the robot
  int counter = 0, loopNumber = 1;
  double lastTime, totalTime;

  ROS_INFO("Patrol bot is ready for commands");

  while (nh.ok())
  {
      if (patrolling_)
      {
         if (counter == 0 && loopNumber == 1)
         {
            lastTime = ros::Time::now().toSec();
            totalTime = 0.;
         }
         if (sendWaypoint(counter))
         {
            counter++;
            if (counter > NUM_GOALS-1)
            {
               ros::spinOnce();  // make sure we have the latest messages, especially for battery
               double loopTime = ros::Time::now().toSec() - lastTime;
               lastTime = ros::Time::now().toSec();
               totalTime += loopTime;
               ROS_INFO("completed patrol loop %d, loop time = %f, total time = %f secs, batterystring is %s, battery volts = %f, laptop battery percentage = %f", loopNumber, loopTime, totalTime, sensorReadings_.c_str(), batteryVolts_, laptopBattPercentage_);

               if (batteryVolts_ <= BATTERY_RETURN_HOME  || laptopBattPercentage_ <= LAPTOP_BATT_RETURN_HOME)
               {
                  //patrolling_ = false;
                  goHome_ = true;
                  finished_ = true;
                  ROS_INFO("battery or laptop needs charging, battery volts = %f, laptop battery percentage = %f, heading home", batteryVolts_, laptopBattPercentage_);
               std_msgs::String msgSpeech;
               msgSpeech.data = "/MyBatterysLowPleasePlugMeIn.wav";
               speech_pub_.publish(msgSpeech);
               }
               loopNumber++;
               counter = 0;
             }
         }
         else patrolling_ = false;
         ros::spinOnce();  // update messages, especially want to know if new keyboard command sent
      }
      ros::spinOnce(); // update messages, especially want to know if new keyboard command sent
      //usleep(1000000);  //wait 1 sec at the waypoint before going to the next one
      if (finished_)
      {
         if (goHome_) // finish up by going home if we got the 'h' command or low battery
         {
            if (sendWaypoint(HOME))
            {
               if (goalReached_) // don't want to back up if we didn't make it to home pre-position.
               {
                  char buffer[5] = "b";
                  std_msgs::String backward;
                  backward.data = buffer;
                  ROS_INFO("Backing into home location");
                  poseGoal_pub_.publish(backward);
                  usleep(6000000);
                  buffer[0] = 's';
                  backward.data = buffer;
                  poseGoal_pub_.publish(backward);
               }
            }
         }

         // all done, just waiting for user to close the node, now hitting 'h' or 'p' will just cancel the home goal
         bool pluggedIn;
         if (chargingState_)
         {
            pluggedIn = true;
            ROS_INFO("Already plugged in");
         }
         else
         {
            pluggedIn = false;
            ROS_INFO("Not plugged in yet");
         }
         long bugCounter = 10;   // ask to be plugged in every 10th loop
         long waitMicrosec = 5000000; // loop every 5 seconds to see if we are charging
         if (chargingState_) waitMicrosec = 100000000;  // loop every 100 seconds if we are charging
         while (nh.ok())
         {
            ROS_INFO("Monitoring charge state");
            if (chargingState_ && (!pluggedIn)) // somebody plugged us in!
            {
               ROS_INFO("Got plugged in");
               pluggedIn = true;
               std_msgs::String msgSpeech;
               msgSpeech.data = "/ThanksForPluggingMeIn.wav";
               speech_pub_.publish(msgSpeech);
               ROS_INFO("Thanking for plugging in");
               waitMicrosec = 100000000; // now loop only once every 100 seconds
            }

            if ( (!chargingState_) && (!pluggedIn) && (!bugCounter % 10))
            {
               std_msgs::String msgSpeech;
               msgSpeech.data = "/MyBatterysLowPleasePlugMeIn.wav";
               speech_pub_.publish(msgSpeech);
               ROS_INFO("Asking to be plugged in");
            }
            bugCounter++;

            if (chargingState_ && laptopBattPercentage_ > 95 && batteryVolts_ > 12.6)
            {
               ROS_INFO("Fully charged");
               std_msgs::String msgSpeech;
               msgSpeech.data = "/AllChargedUpReadyToWork.wav";
               speech_pub_.publish(msgSpeech);
            }
            ros::spinOnce();
            usleep(waitMicrosec);
         }
      }
  }

  delete ac_;
  ROS_INFO("patrol_node exiting");
  ros::spinOnce(); // let that last ROS_INFO get out there.
}


#include <map>
#include <cmath>
#include <string>
#include <iostream>
#include <fstream>
#include <vector>
#include <stdio.h>
#include <glpk.h>
#include <algorithm>
#include <boost/algorithm/string.hpp>

#include "func.h"

using namespace std;
using namespace boost;

void readbounds(map<int,double> *lbs,map<int,double> *ubs,string boundsfile) {
    ifstream infile(boundsfile.c_str(),ios::in);
    char str[1024];
    string words = "";
	if (infile.is_open()) {
		while(!infile.eof()) {
			words = "";
			infile.getline(str,1024);
			if(strlen(str) < 1) {
				continue;
			}

			words += str;
			vector<string> x;
			split(x,words,is_any_of(" \t"));
			/*for(vector<string>::iterator viter=x.begin();viter!=x.end();viter++) {
			  cout<<(*viter)<<"\n";
			  }*/
			int ifir = atoi(x[0].c_str());
			double fsec = atof(x[1].c_str());
			double fthd = atof(x[2].c_str());
			//cout<<ifir<<"\t"<<fsec<<endl;
			if(ifir>0) {
				(*ubs)[ifir] = fsec;
				(*lbs)[ifir] = fthd;
			}
		}
	}
	else {
		cout<<"Can't open boundsfile from : "<<boundsfile<<endl;
		exit(-1);
	}
}
void read_mut(vector<int> *ind,vector<double> *mut,string mut_inputfile) {
    ifstream infile(mut_inputfile.c_str(),ios::in);
    char str[1024];
    string words = "";
	if (infile.is_open()) {
		while(!infile.eof()) {
			words = "";
			infile.getline(str,1024);
			if(strlen(str) < 1) {
				continue;
			}

			words += str;
			vector<string> x;
			split(x,words,is_any_of(" \t"));
			/*for(vector<string>::iterator viter=x.begin();viter!=x.end();viter++) {
			  cout<<(*viter)<<"\n";
			  }*/
			int ifir = atoi(x[0].c_str());
			double fsec = atof(x[1].c_str());
			//cout<<ifir<<"\t"<<fsec<<endl;
			if(ifir>0) {
				(*ind).push_back(ifir);
				(*mut).push_back(fsec);
			}
		}
	}
	else {
		cout<<"Can't open boundsfile from : "<<mut_inputfile<<endl;
		exit(-1);
	}
}

void get_constraints(glp_prob *lp,string confile,map<int,int> tp) {
	ifstream in(confile.c_str(),ios::in);
	//cout<<"Constraint file is "<<confile<<endl;
	char str[1024];
	string words = "";
	if (in.is_open()) {
		while(!in.eof()) {
			words = "";
			in.getline(str,1024);
			//cout<<strlen(str)<<endl;
			if(strlen(str) < 1) {
				continue;
			}
			words += str;
			vector<string> x;
			split(x,words,is_any_of(" \t"));
			for(vector<string>::iterator viter=x.begin();viter!=x.end();viter++) {
				cout<<(*viter)<<"\t";
			}
			cout<<"\n";
			int ind = atoi(x[0].c_str());
			int type = atoi(x[1].c_str());
			double lb = atof(x[2].c_str());
			double ub = atof(x[3].c_str());
			glp_set_col_bnds(lp,ind,tp[type],lb,ub);
		}
	} else {
		cout<<"Can't open constraint file from : "<<confile<<endl;
		exit(-1);
	}
}

void read_target(string confile,vector<double> *dt,int col) {
    ifstream in(confile.c_str(),ios::in);
    //cout<<"Constraint file is "<<confile<<endl;
    char str[10240];
    string words = "";
    int h_col = 0;
    cout<<"row is "<<col<<"\n";
	if (in.is_open()) {
		while(!in.eof()) {
			words = "";
			in.getline(str,10240);
			//cout<<strlen(str)<<endl;
			if(strlen(str) < 1) {
				continue;
			}
			h_col++;
			if(h_col>col) {
				cout<<"Reading target file error!!\n"<<endl;
			}
			(*dt)[h_col] = atof(str);
		}
		cout<<h_col<<" columns have been loaded for targets. "<<"\n";
	} else {
		cout<<"Can't open target file from : "<<confile<<endl;
		exit(-1);
	}
}


#include "Ssl.h"

#include <openssl/engine.h>
#include <openssl/hmac.h>
#include <openssl/evp.h>
#include <openssl/rsa.h>

#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#include <boost/archive/iterators/remove_whitespace.hpp>
#include <jde/Assert.h>
#define var const auto

namespace Jde
{
	string Ssl::Encode( sv url )noexcept
	{
		ostringstream os;
		std::ostream hexcout{ os.rdbuf() };
		hexcout << std::hex << std::uppercase << std::setfill('0');
		var end = url.data()+url.size();
		for( auto p=url.data(); p<end; ++p )
		{
			char ch = *p;
			if( ch>=0 )//&& ch<128
			{
				char ch = (char)*p;
				if( isalnum(*p) || ch == '-' || ch == '_' || ch == '.' || ch == '~' )
					os << ch;
				else
				{
					os << '%';
					int x = ch;
					hexcout << std::setw(2) << x;
				}
			}
			else
			{
				//%E2%89%88
				os << '%';
				unsigned char value = ch;
				hexcout << std::setw(2) << static_cast<uint>(value);
			}
		}
		return os.str();
	}

	//TODO: very bad!  https://stackoverflow.com/questions/9507184/can-openssl-on-windows-use-the-system-certificate-store
	bool Ssl::verify_certificate( bool preverified, boost::asio::ssl::verify_context& ctx )noexcept
	{
		char subject_name[256];
		X509* cert = X509_STORE_CTX_get_current_cert(ctx.native_handle());
		X509_NAME_oneline(X509_get_subject_name(cert), subject_name, 256);
		//std::cout << "Verifying:  " << subject_name << std::endl;
		preverified = true;
		return preverified;
	}

	RSA* CreatePrivateRsa( string key )
	{
		BIO* keybio = BIO_new_mem_buf( (void*)key.c_str(), -1 ); THROW_IF( !keybio, "!keybio" );
		RSA* pRsa = nullptr;
		pRsa = PEM_read_bio_RSAPrivateKey( keybio, &pRsa, nullptr, nullptr );
		if( !pRsa )
		{
			char buffer[120];
			ERR_error_string( ERR_get_error(), buffer );
			THROW( buffer );
		}
		return pRsa;
	}

	bool RSASign( RSA* rsa, const unsigned char* Msg, size_t MsgLen, unsigned char** EncMsg,  size_t* MsgLenEnc )
	{
		EVP_MD_CTX* m_RSASignCtx = EVP_MD_CTX_create();
		EVP_PKEY* priKey  = EVP_PKEY_new(); //free?
		EVP_PKEY_assign_RSA(priKey, rsa);
		if( EVP_DigestSignInit(m_RSASignCtx,nullptr, EVP_sha1(), nullptr,priKey)<=0 )
			return false;
		if( EVP_DigestSignUpdate(m_RSASignCtx, Msg, MsgLen) <= 0 )
			return false;
		if( EVP_DigestSignFinal(m_RSASignCtx, nullptr, MsgLenEnc) <=0 )
			return false;

		*EncMsg = (unsigned char*)malloc(*MsgLenEnc);
		if (EVP_DigestSignFinal(m_RSASignCtx, *EncMsg, MsgLenEnc) <= 0)
			return false;

		EVP_MD_CTX_reset( m_RSASignCtx );
		return true;
	}
	//https://stackoverflow.com/questions/7053538/how-do-i-encode-a-string-to-base64-using-only-boost
	std::string Ssl::Encode64( const std::string &val )
	{
		using namespace boost::archive::iterators;
		using It = base64_from_binary<transform_width<std::string::const_iterator, 6, 8>>;
		auto tmp = std::string( It(std::begin(val)), It(std::end(val)) );
		return tmp.append( (3 - val.size() % 3) % 3, '=' );
	}

	string Ssl::RsaSign( sv value, sv key )
	{
		auto p = HMAC_CTX_new();
		HMAC_Init_ex( p, (void*)key.data(), (int)key.size(), EVP_sha1(), nullptr );
		HMAC_Update( p, (const unsigned char*)value.data(), value.size() );
		unsigned length = 2048;
		unsigned char buffer[2048];
		HMAC_Final( p, buffer, &length );
		HMAC_CTX_free( p );
		return Encode64( string(buffer, buffer+length) );
	}

	std::string Ssl::Decode64( const std::string& s )noexcept(false) //https://stackoverflow.com/questions/10521581/base64-encode-using-boost-throw-exception
	{
		namespace bai = boost::archive::iterators; typedef bai::transform_width< bai::binary_from_base64<bai::remove_whitespace<std::string::const_iterator> >, 8, 6 > IT;
		return std::string{ IT(s.begin()), IT(s.end()) };
	}

	auto RsaDeleter=[](RSA* p){ RSA_free(p); };
	auto PKeyDeleter=[](EVP_PKEY* p){ EVP_PKEY_free(p); };

	//https://stackoverflow.com/questions/28770426/rsa-public-key-conversion-with-just-modulus
	unique_ptr<EVP_PKEY,decltype(PKeyDeleter)> RsaPemFromModExp( const string& modulus, const string& exponent )noexcept(false)
	{
		BIGNUM* pMod = BN_bin2bn( (const unsigned char *)modulus.c_str(), (int)modulus.size(), nullptr ); THROW_IF( !pMod, "BN_bin2bn"sv );
		BIGNUM *pExp = BN_bin2bn( (const unsigned char *)exponent.c_str(), (int)exponent.size(), nullptr ); THROW_IF( !pMod, "BN_bin2bn({})"sv, exponent );
		unique_ptr<RSA,decltype(RsaDeleter)> pRsa{ RSA_new(), RsaDeleter };
		RSA_set0_key( pRsa.get(), pMod, pExp, nullptr );

		unique_ptr<EVP_PKEY,decltype(PKeyDeleter)> pKey{ EVP_PKEY_new(), PKeyDeleter };
		int rc = EVP_PKEY_set1_RSA(pKey.get(), pRsa.get()); ASSERT(rc == 1);
		return pKey;
	}

	void rsatest();
	void Ssl::Verify( const string& modulus, const string& exponent, const string& decrypted, const string& signature )noexcept(false)
	{
		rsatest();
    	auto pCtx = EVP_MD_CTX_create();
    	var pMd = EVP_get_digestbyname( "SHA256" ); THROW_IF( !pMd, "EVP_get_digestbyname({})", "SHA256" ); //"SHA256"
		EVP_VerifyInit_ex( pCtx, pMd, nullptr );
		EVP_VerifyUpdate( pCtx, decrypted.c_str(), decrypted.size() );
		var pKey = RsaPemFromModExp( modulus, exponent );
		std::string decodedSignature = Encode64( signature );
		var result = EVP_VerifyFinal( pCtx, (const unsigned char *)decodedSignature.c_str(), (int)decodedSignature.size(), pKey.get() );
		THROW_IFX( result!=1, CodeException("Ssl::Verify - failed", {result, std::generic_category()} ) );
	}

	void rsatest()
	{
		const EVP_MD *sha256 = EVP_get_digestbyname("sha256");

		if(!sha256){
		fprintf(stderr,"SHA256 not available\n");
		return;
		}

		printf("Now try signing with X.509 certificates and EVP\n");

		char ptext[16];
		memset(ptext,0,sizeof(ptext));
		strcpy(ptext,"Simson");

		unsigned char sig[1024]={0};
		uint32_t  siglen = sizeof(sig);

		BIO *bp = BIO_new_file( "../signing_key.pem", "r" );

		auto pCtx = EVP_MD_CTX_create();
		//EVP_MD_CTX md;
		EVP_PKEY *pkey = PEM_read_bio_PrivateKey(bp,0,0,0);

		EVP_SignInit(pCtx,sha256);
		EVP_SignUpdate(pCtx,ptext,sizeof(ptext));
		EVP_SignFinal(pCtx,sig,&siglen,pkey);

		/* let's try to verify it */
		BIO* bp2 = BIO_new_file( "../signing_key.pub", "r" );
		X509 *x = 0;
		PEM_read_bio_X509( bp2,&x,0,0 );
		EVP_PKEY *pubkey = X509_get_pubkey(x);

		printf("pubkey=%p\n",pubkey);

		EVP_VerifyInit(pCtx,sha256);
		EVP_VerifyUpdate(pCtx,ptext,sizeof(ptext));
		int r = EVP_VerifyFinal(pCtx,sig,siglen,pubkey);
		printf("r=%d\n",r);

		printf("do it again...\n");
		EVP_VerifyInit(pCtx,sha256);
		EVP_VerifyUpdate(pCtx,ptext,sizeof(ptext));
		r = EVP_VerifyFinal(pCtx,sig,siglen,pubkey);
		printf("r=%d\n",r);

		printf("make a tiny change...\n");
		ptext[0]='f';
		EVP_VerifyInit(pCtx,sha256);
		EVP_VerifyUpdate(pCtx,ptext,sizeof(ptext));
		r = EVP_VerifyFinal(pCtx,sig,siglen,pubkey);
		printf("r=%d\n",r);
	}

	string Ssl::SendEmpty( sv host, sv target, sv authorization, http::verb verb )noexcept(false)
	{
		http::request<http::empty_body> req{ verb, string(target), 11 };
		SetRequest( req, host, "application/x-www-form-urlencoded"sv, authorization );
		return Send( req, host, target );
	}

}


#include <boost/ut.hpp>
#include <libembeddedhal/adc/mock.hpp>

namespace embed {
boost::ut::suite adc_mock_test = []() {
  using namespace boost::ut;

  // Setup
  auto expected1 = embed::percent::from_ratio(1, 4);
  auto expected2 = embed::percent::from_ratio(1, 2);
  auto expected3 = embed::percent::from_ratio(1, 1);
  embed::mock::adc mock(expected1);

  // Exercise + Verify
  expect(that % expected1.raw_value() == mock.read().value().raw_value());
  mock.set(expected2);
  expect(that % expected2.raw_value() == mock.read().value().raw_value());
  mock.set(expected3);
  expect(that % expected3.raw_value() == mock.read().value().raw_value());
};
}  // namespace embed


/*
@copyright Louis Dionne 2014
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/list/instance.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
using namespace boost::hana;

BOOST_HANA_CONSTEXPR_LAMBDA auto add = [](auto x, auto y) { return x + y; };
BOOST_HANA_CONSTEXPR_LAMBDA auto inc = [](auto x) { return x + 1; };

int main() {
    BOOST_HANA_CONSTANT_ASSERT(zip_with(inc, list()) == list());
    BOOST_HANA_CONSTEXPR_ASSERT(zip_with(inc, list(0)) == list(1));
    BOOST_HANA_CONSTEXPR_ASSERT(zip_with(inc, list(0, 1)) == list(1, 2));
    BOOST_HANA_CONSTEXPR_ASSERT(zip_with(inc, list(0, 1, 2)) == list(1, 2, 3));
    BOOST_HANA_CONSTANT_ASSERT(zip_with(inc, list(), list()) == list());

    BOOST_HANA_CONSTANT_ASSERT(zip_with(add, list()) == list());
    BOOST_HANA_CONSTANT_ASSERT(zip_with(add, list(), list()) == list());
    BOOST_HANA_CONSTANT_ASSERT(zip_with(add, list(1), list()) == list());
    BOOST_HANA_CONSTANT_ASSERT(zip_with(add, list(), list(3)) == list());
    BOOST_HANA_CONSTEXPR_ASSERT(zip_with(add, list(1), list(3)) == list(1 + 3));
    BOOST_HANA_CONSTEXPR_ASSERT(zip_with(add, list(1, 2), list(3, 4)) == list(1 + 3, 2 + 4));
    BOOST_HANA_CONSTEXPR_ASSERT(zip_with(add, list(1, 2, 3, 4), list(5, 6, 7)) == list(1 + 5, 2 + 6, 3 + 7));
    BOOST_HANA_CONSTANT_ASSERT(zip_with(add, list(), list(), list()) == list());
}


#include <boost/numeric/odeint/iterator/times_iterator.hpp>


#pragma once

#include <Eigen/Core>
#include <opencv2/core/core.hpp>

#include "md5.hpp"
#include "perceive/foundation.hpp"
#include "perceive/geometry/vector.hpp"

namespace perceive
{
namespace detail
{
   template<typename EigenType>
   inline cv::Mat to_cv_mat_32f(const EigenType& e) noexcept
   {
      int n_rows = int(e.rows());
      int n_cols = int(e.cols());
      cv::Mat m(n_rows, n_cols, CV_32F);
      for(int i = 0; i < n_rows; ++i)
         for(int j = 0; j < n_cols; ++j) m.at<float>(i, j) = float(e(i, j));
      return m;
   }
} // namespace detail

inline cv::Mat to_cv_mat_32f(const Eigen::Matrix<real, 3, 3>& e) noexcept
{
   return detail::to_cv_mat_32f(e);
}

template<typename T>
inline cv::Mat to_cv_mat_32f(
    const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& e) noexcept
{
   static_assert(std::is_floating_point<T>::value);
   return detail::to_cv_mat_32f(e);
}

template<typename T>
inline cv::Mat
to_cv_mat_32f(const Eigen::Matrix<T, Eigen::Dynamic, 1>& e) noexcept
{
   static_assert(std::is_floating_point<T>::value);
   return detail::to_cv_mat_32f(e);
}

inline cv::Mat unpack_vector(const real* d, uint32_t len) noexcept
{
   cv::Mat m(int(len), 1, CV_32F);
   for(int i = 0; i < int(len); ++i) m.at<float>(i, 0) = float(d[i]);
   return m;
}

MatrixXr cv_to_matXr(const cv::Mat& data) noexcept;

VectorXr row_to_vecXr(const cv::Mat& data, const int row) noexcept;

inline Vector3 to_vec3(const cv::Vec3d X) noexcept
{
   return Vector3(X(0), X(1), X(2));
}

inline cv::Rect to_cv_rect(const AABBi& aabb) noexcept
{
   return cv::Rect(aabb.left, aabb.top, aabb.width(), aabb.height());
}

inline AABBi image_bounds(const cv::Mat& im) noexcept
{
   return AABBi(0, 0, im.cols, im.rows);
}

string str(const cv::Rect roi) noexcept;

// The default background color is transparent
void render_string_cv(cv::Mat& im,
                      const string_view label,
                      const Point2 pos,
                      const uint32_t foreground_colour,
                      const uint32_t background_colour = uint32_t(-1)) noexcept;

string hexdigest(const vector<cv::Mat>& mats) noexcept;

} // namespace perceive


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

#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/dispatch/meta/factory_of.hpp>
#include <boost/mpl/apply.hpp>
#include <complex>

namespace nt2
{
  template<class T> struct dry;
}

namespace nt2 { namespace details
{
  template<class T, class F>
  struct as_complex
   : boost::mpl::apply1<F, std::complex<T> >
  {
  };

  template<class T, class F>
  struct as_complex< std::complex<T>, F >
   : boost::mpl::apply1<F, std::complex<T> >
  {
  };

  template<class T, class F>
  struct as_complex< dry<T>, F >
   : boost::mpl::apply1<F, std::complex<T> >
  {
  };
} }

namespace nt2 { namespace meta
{
  template<class T>
  struct as_complex
    : details::as_complex< typename boost::dispatch::meta::
                           scalar_of<T>::type
                         , typename boost::dispatch::meta::
                           factory_of<T, typename boost::dispatch::meta::
                                         scalar_of<T>::type
                                     >::type
                         >
  {
  };
} }

#endif


#define BOOST_TEST_MODULE pcraster dal dimension
#include <boost/test/unit_test.hpp>
#include "dal_Dimension.h"
#include "dal_RasterDimensions.h"

BOOST_AUTO_TEST_CASE(test_)
{
  using namespace dal;

  {
    std::set<std::string> scenarios;
    scenarios.insert("aap");
    Dimension dimension(Scenarios, scenarios);

    // Scenario names are exact.
    BOOST_CHECK_EQUAL(dimension.discretisation(), ExactDiscretisation);

    BOOST_CHECK(!dimension.isWide());
  }
}

BOOST_AUTO_TEST_CASE(contains_value_in_range)
{
  using namespace dal;

  {
    std::vector<size_t> values;
    values.push_back(2);
    values.push_back(5);
    values.push_back(1);
    Dimension dimension(Time, values);
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(0));
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(1));
    BOOST_CHECK( dimension.containsValueInRange<size_t>(2));
    BOOST_CHECK( dimension.containsValueInRange<size_t>(3));
    BOOST_CHECK( dimension.containsValueInRange<size_t>(4));
    BOOST_CHECK( dimension.containsValueInRange<size_t>(5));
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(6));
  }

  {
    std::vector<size_t> values;
    values.push_back(2);
    values.push_back(5);
    values.push_back(2);
    Dimension dimension(Time, values);
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(0));
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(1));
    BOOST_CHECK( dimension.containsValueInRange<size_t>(2));
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(3));
    BOOST_CHECK( dimension.containsValueInRange<size_t>(4));
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(5));
    BOOST_CHECK(!dimension.containsValueInRange<size_t>(6));
  }

  {
    std::vector<float> values;
    values.push_back(0.01f);
    values.push_back(0.99f);
    values.push_back(0.01f);
    Dimension dimension(CumulativeProbabilities, values);
    BOOST_CHECK(!dimension.containsValueInRange<float>(0.00f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.01f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.02f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.03f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.04f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.05f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.50f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.95f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.96f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.97f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.98f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.99f));
    BOOST_CHECK(!dimension.containsValueInRange<float>(1.00f));
  }

  {
    std::vector<float> values;
    values.push_back(0.01f);
    values.push_back(0.99f);
    values.push_back(0.02f);
    Dimension dimension(CumulativeProbabilities, values);
    BOOST_CHECK(!dimension.containsValueInRange<float>(0.00f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.01f));
    BOOST_CHECK(!dimension.containsValueInRange<float>(0.02f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.03f));
    BOOST_CHECK(!dimension.containsValueInRange<float>(0.04f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.05f));
    BOOST_CHECK(!dimension.containsValueInRange<float>(0.50f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.95f));
    BOOST_CHECK(!dimension.containsValueInRange<float>(0.96f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.97f));
    BOOST_CHECK(!dimension.containsValueInRange<float>(0.98f));
    BOOST_CHECK( dimension.containsValueInRange<float>(0.99f));
    BOOST_CHECK(!dimension.containsValueInRange<float>(1.00f));
  }
}

BOOST_AUTO_TEST_CASE(clamp)
{
  using namespace dal;

  std::vector<float> values;
  values.push_back(0.01f);
  values.push_back(0.99f);
  values.push_back(0.01f);
  Dimension dimension(CumulativeProbabilities, values);
  BOOST_CHECK(dal::comparable<float>(dimension.clamp<float>(0.5), 0.5f));
}

BOOST_AUTO_TEST_CASE(index_of_value_of)
{
  using namespace dal;

  float first = 0.01f;
  float last = 0.99f;
  float interval = 0.01f;

  std::vector<float> values;
  values.push_back(first);
  values.push_back(last);
  values.push_back(interval);
  Dimension dimension(CumulativeProbabilities, values);

  BOOST_REQUIRE_EQUAL(dimension.nrCoordinates(), size_t(99));

  for(size_t i = 0; i < dimension.nrCoordinates(); ++i) {
    BOOST_CHECK_EQUAL(dimension.indexOf<float>(first + i * interval), i);
    BOOST_CHECK(comparable<float>(dimension.coordinate<float>(i),
         first + i * interval));
  }
}

BOOST_AUTO_TEST_CASE(merge)
{
  using namespace dal;

  {
    std::vector<size_t> values1, values2;

    // 1, 3, 5, 7, 9
    values1.push_back(1);
    values1.push_back(10);
    values1.push_back(2);

    // 2, 4, 6, 8, 10
    values2.push_back(2);
    values2.push_back(10);
    values2.push_back(2);

    Dimension dimension(Time, values1);
    dimension |= Dimension(Time, values2);

    // 1, 2, 3, 4, 5, 6, 7, 8, 9, 10
    BOOST_CHECK_EQUAL(dimension.value<size_t>(0), size_t(1));
    BOOST_CHECK_EQUAL(dimension.value<size_t>(1), size_t(10));
    BOOST_CHECK_EQUAL(dimension.value<size_t>(2), size_t(1));
    BOOST_CHECK_EQUAL(dimension.nrCoordinates(), size_t(10));
  }

  {
    // Because of bug, only in release mode:
    // dimension1: /cumulative probabilities[0.01, 0.99, 0.01]
    // dimension2: /cumulative probabilities[0.01, 0.99, 0.01]
    // result:     /cumulative probabilities[0.01, 0.99, -2.14748e-07]   <------
    std::vector<float>quantiles;
    quantiles.push_back(0.01f);
    quantiles.push_back(0.99f);
    quantiles.push_back(0.01f);

    Dimension dimension1(CumulativeProbabilities, quantiles);
    Dimension dimension2(CumulativeProbabilities, quantiles);

    dimension1 |= dimension2;
    BOOST_CHECK_EQUAL(dimension1.nrValues(), size_t(3));
    BOOST_CHECK(comparable<float>(dimension1.value<float>(2), 0.01f));
  }

  {
    std::vector<double> values1, values2;

    // 1.1, 3.3
    values1.push_back(1.1);
    values1.push_back(3.3);

    // 2.2, 4.4
    values2.push_back(2.2);
    values2.push_back(4.4);

    //     3.3
    // 1.1 +----------+
    //     |          |
    //     |          |
    //     |          |
    //     +----------+ 2.2
    //              4.4
    SpaceDimensions dimensions1(1.1, 3.3, 4.4, 2.2);

    Dimension dimension(Space, BorderedDiscretisation, dimensions1);

    //     4.5
    // 1.3 +----------+
    //     |          |
    //     |          |
    //     |          |
    //     +----------+ 3.2
    //              3.4
    SpaceDimensions dimensions2(1.3, 4.5, 3.4, 3.2);

    dimension |= Dimension(Space, BorderedDiscretisation, dimensions2);

    SpaceDimensions const& result(dimension.value<SpaceDimensions>(0));
    BOOST_CHECK_CLOSE(result.west(), 1.1, 0.001);
    BOOST_CHECK_CLOSE(result.north(), 4.5, 0.001);
    BOOST_CHECK_CLOSE(result.east(), 4.4, 0.001);
    BOOST_CHECK_CLOSE(result.south(), 2.2, 0.001);
  }
}

BOOST_AUTO_TEST_CASE(intersect)
{
  using namespace dal;

  bool testIntersectImplemented = false;
  BOOST_WARN(testIntersectImplemented);
}

BOOST_AUTO_TEST_CASE(nr_coordinates)
{
  using namespace dal;

  {
    RasterDimensions dimensions(3, 4, 1.5);
    Dimension dimension(Space, RegularDiscretisation, dimensions);
    BOOST_CHECK_EQUAL(dimension.nrCoordinates(), size_t(12));
  }

  {
    float first = 0.01f;
    float last = 0.99f;
    float interval = 0.01f;

    std::vector<float> values;
    values.push_back(first);
    values.push_back(last);
    values.push_back(interval);
    Dimension dimension(CumulativeProbabilities, values);
    BOOST_CHECK_EQUAL(dimension.nrCoordinates(), size_t(99));
  }
}

BOOST_AUTO_TEST_CASE(is_wide)
{
  using namespace dal;

  {
    Dimension dimension(Time, size_t(5), size_t(5), size_t(1));
    BOOST_CHECK(!dimension.isWide());
  }

  {
    Dimension dimension(Time, size_t(5), size_t(5), size_t(2));
    BOOST_CHECK(!dimension.isWide());
  }

  {
    Dimension dimension(Time, size_t(5), size_t(6), size_t(2));
    BOOST_CHECK(!dimension.isWide());
  }

  {
    Dimension dimension(Time, size_t(5), size_t(7), size_t(2));
    BOOST_CHECK(dimension.isWide());
  }
}


/*
 * AllDiff.cpp
 * @brief General "all-different" constraint
 * @date Feb 6, 2012
 * @author Frank Dellaert
 */

#include <gtsam_unstable/discrete/Domain.h>
#include <gtsam_unstable/discrete/AllDiff.h>
#include <gtsam/base/Testable.h>
#include <boost/make_shared.hpp>

namespace gtsam {

  /* ************************************************************************* */
  AllDiff::AllDiff(const DiscreteKeys& dkeys) :
    Constraint(dkeys.indices()) {
    for(const DiscreteKey& dkey: dkeys)
        cardinalities_.insert(dkey);
  }

  /* ************************************************************************* */
  void AllDiff::print(const std::string& s,
      const KeyFormatter& formatter) const {
    std::cout << s << "AllDiff on ";
    for (Key dkey: keys_)
      std::cout << formatter(dkey) << " ";
    std::cout << std::endl;
  }

  /* ************************************************************************* */
  double AllDiff::operator()(const Values& values) const {
    std::set < size_t > taken; // record values taken by keys
    for(Key dkey: keys_) {
      size_t value = values.at(dkey); // get the value for that key
      if (taken.count(value)) return 0.0;// check if value alreday taken
      taken.insert(value);// if not, record it as taken and keep checking
    }
    return 1.0;
  }

  /* ************************************************************************* */
  DecisionTreeFactor AllDiff::toDecisionTreeFactor() const {
    // We will do this by converting the allDif into many BinaryAllDiff constraints
    DecisionTreeFactor converted;
    size_t nrKeys = keys_.size();
    for (size_t i1 = 0; i1 < nrKeys; i1++)
      for (size_t i2 = i1 + 1; i2 < nrKeys; i2++) {
        BinaryAllDiff binary12(discreteKey(i1),discreteKey(i2));
        converted = converted * binary12.toDecisionTreeFactor();
      }
    return converted;
  }

  /* ************************************************************************* */
  DecisionTreeFactor AllDiff::operator*(const DecisionTreeFactor& f) const {
    // TODO: can we do this more efficiently?
    return toDecisionTreeFactor() * f;
  }

  /* ************************************************************************* */
  bool AllDiff::ensureArcConsistency(size_t j, std::vector<Domain>& domains) const {
    // Though strictly not part of allDiff, we check for
    // a value in domains[j] that does not occur in any other connected domain.
    // If found, we make this a singleton...
    // TODO: make a new constraint where this really is true
    Domain& Dj = domains[j];
    if (Dj.checkAllDiff(keys_, domains)) return true;

    // Check all other domains for singletons and erase corresponding values
    // This is the same as arc-consistency on the equivalent binary constraints
    bool changed = false;
    for(Key k: keys_)
      if (k != j) {
        const Domain& Dk = domains[k];
        if (Dk.isSingleton()) { // check if singleton
          size_t value = Dk.firstValue();
          if (Dj.contains(value)) {
            Dj.erase(value); // erase value if true
            changed = true;
          }
        }
      }
    return changed;
  }

  /* ************************************************************************* */
  Constraint::shared_ptr AllDiff::partiallyApply(const Values& values) const {
    DiscreteKeys newKeys;
    // loop over keys and add them only if they do not appear in values
    for(Key k: keys_)
      if (values.find(k) == values.end()) {
        newKeys.push_back(DiscreteKey(k,cardinalities_.at(k)));
      }
    return std::make_shared<AllDiff>(newKeys);
  }

  /* ************************************************************************* */
  Constraint::shared_ptr AllDiff::partiallyApply(
      const std::vector<Domain>& domains) const {
    DiscreteFactor::Values known;
    for(Key k: keys_) {
        const Domain& Dk = domains[k];
        if (Dk.isSingleton())
          known[k] = Dk.firstValue();
      }
    return partiallyApply(known);
  }

  /* ************************************************************************* */
} // namespace gtsam


#if !defined(BALSA_SCENE_GRAPH_FEATURES_BOUNDING_BOX_HPP)
#define BALSA_SCENE_GRAPH_FEATURES_BOUNDING_BOX_HPP

#include <Eigen/Geometry>
#include "../abstract_feature.hpp"
#include <iostream>
#include "../embedding_traits.hpp"

namespace balsa::scene_graph::features {

template<concepts::embedding_traits ETraits>
class BoundingBoxNode : public AbstractFeature<ETraits> {
  public:
    using embedding_traits = ETraits;
    using bounding_box_type = Eigen::AlignedBox<typename ETraits::scalar_type, ETraits::embedding_dimension>;

    virtual bounding_box_type bounding_box() const;
    virtual ~BoundingBoxNode() = default;
    void add_child(const BoundingBoxNode *n);
    const std::vector<const BoundingBoxNode *> &children() const { return _children; }

  private:
    std::vector<const BoundingBoxNode *> _children;
};
template<concepts::embedding_traits ETraits>
auto BoundingBoxNode<ETraits>::bounding_box() const -> bounding_box_type {
    bounding_box_type bbox;
    for (auto &&child : _children) {
        auto bb = child->bounding_box();
        std::cout << bb.min().transpose() << " => " << bb.max().transpose() << std::endl;
        bbox.extend(bb);
    }
    return bbox;
}

template<concepts::embedding_traits ETraits>
void BoundingBoxNode<ETraits>::add_child(const BoundingBoxNode *n) {
    std::cout << "Adding bounding box child!" << std::endl;
    _children.emplace_back(n);
}

// caches the local bounding box value
template<concepts::embedding_traits ETraits>
class CachedBoundingBoxNode : public BoundingBoxNode<ETraits> {
  public:
    using bounding_box_type = typename BoundingBoxNode<ETraits>::bounding_box_type;

    bounding_box_type bounding_box() const override final;

    void set_bounding_box(const bounding_box_type &bb) { _bbox = bb; }
    void update_from_children();

  private:
    bounding_box_type _bbox;
};
template<concepts::embedding_traits ETraits>
auto CachedBoundingBoxNode<ETraits>::bounding_box() const -> bounding_box_type {
    return _bbox;
}

template<concepts::embedding_traits ETraits>
void CachedBoundingBoxNode<ETraits>::update_from_children() {
    _bbox = BoundingBoxNode<ETraits>::bounding_box();
}
}// namespace balsa::scene_graph::features
#endif


// ---------------------------------------------------------------------------
//
//  This file is part of PermLib.
//
// Copyright (c) 2009-2011 Thomas Rehn <thomas@carmen76.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. The name of the author may not be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
// IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
// OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
// IN NO EVENT SHALL THE AUTHOR 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.
//
// ---------------------------------------------------------------------------

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

#include <iostream>
#include <fstream>

#include <permlib/permutation.h>
#include <permlib/permutationword.h>
#include <permlib/bsgs.h>

#include <permlib/transversal/shallow_schreier_tree_transversal.h>
#include <permlib/transversal/schreier_tree_transversal.h>
#include <permlib/transversal/explicit_transversal.h>
//#include <permlib/construct/schreier_sims_construction.h>

#include "test-common.h"

using namespace permlib;
using namespace permlib::test;

template<class PERM, class TRANS>
void checkTransversal(const GroupInformation &info) {
	std::list<typename PERM::ptr> generators;
	const unsigned int n = readGroup<PERM>(info.filename, generators);

	for (unsigned long alpha = 0; alpha < n; ++alpha) {
		//std::cout << "alpha = " << alpha << std::endl;
		//std::cout << "elements: " << PERM::elementsNumber() << std::endl;
		boost::scoped_ptr<TRANS> U(new TRANS(n));
		U->orbit(alpha, generators);
		BOOST_CHECK_LE(U->size(), n);

		std::list<unsigned long>::const_iterator it;
		for (it = U->begin(); it != U->end(); ++it) {
			const unsigned long &beta = *it;
			boost::scoped_ptr<PERM> u_beta(U->at(beta));
			BOOST_REQUIRE(u_beta);

			BOOST_CHECK_EQUAL(*u_beta / alpha, beta);

			BOOST_CHECK_LE(U->m_statMaxDepth, n);
		}
		BOOST_CHECK_GE(U->m_statMaxDepth, 1);
		//std::cout << "depth " << U->m_statMaxDepth << " for " << U->size() << " orbit length" << std::endl;
		//std::cout << "STAT " << U->m_statAlphaCheck << std::endl;
	}
}

template<class PERM, class TRANS1, class TRANS2>
void compareTransversal(const GroupInformation &info) {
	std::list<typename PERM::ptr> generators;
	const unsigned int n = readGroup<PERM>(info.filename, generators);

	for (unsigned long alpha = 0; alpha < n; ++alpha) {
		boost::scoped_ptr<TRANS1> U1(new TRANS1(n));
		U1->orbit(alpha, generators);

		boost::scoped_ptr<TRANS2> U2(new TRANS2(n));
		U2->orbit(alpha, generators);

		BOOST_CHECK_EQUAL(U1->size(), U2->size());

		std::list<unsigned long>::const_iterator it;
		for (it = U1->begin(); it != U1->end(); ++it) {
			const unsigned long &beta = *it;
			boost::scoped_ptr<PERM> u_beta(U2->at(beta));
			BOOST_CHECK(u_beta);
		}
	}
}

BOOST_AUTO_TEST_CASE( transversal_schreier )
{
	typedef PermutationWord PERM;
    typedef SchreierTreeTransversal<PERM> TRANS;

	checkTransversal<PERM,TRANS>(info_cyclic500);
	checkTransversal<PERM,TRANS>(info_1997);
}

BOOST_AUTO_TEST_CASE( transversal_shallow_schreier )
{
	typedef PermutationWord PERM;
    typedef ShallowSchreierTreeTransversal<PERM> TRANS;

	checkTransversal<PERM,TRANS>(info_cyclic500);
	checkTransversal<PERM,TRANS>(info_1997);
}

BOOST_AUTO_TEST_CASE( transversal_compare_exact_shallow_schreier )
{
	typedef PermutationWord PERM;
	typedef ExplicitTransversal<PERM> TRANS1;
    typedef ShallowSchreierTreeTransversal<PERM> TRANS2;

	compareTransversal<PERM,TRANS1,TRANS2>(info_1997);
	compareTransversal<PERM,TRANS1,TRANS2>(info_cyclic500);
}

template<class PERM, class TRANS>
void conjugateGroup(const GroupInformation &info) {
	std::list<typename PERM::ptr> generators;
	BSGS<PERM,TRANS> bsgs = ConstructDeterministic1<PERM, TRANS>()(info);
	BSGS<PERM,TRANS> bsgsCopy(bsgs);

	const unsigned int n = bsgs.n;
	Permutation::perm permC(n);
	for (unsigned int i = 0; i < n; ++i)
		permC[i] = i;

	// introduce some random transpositions
	const unsigned int numberOfRandomTranspositions = 5;
	for (unsigned int i = 0; i < numberOfRandomTranspositions; ++i) {
		const dom_int a = randomInt(bsgs.n);
		const dom_int b = randomInt(bsgs.n);
		std::swap(permC[a], permC[b]);
	}
	const PERM c(permC);
	bsgsCopy.conjugate(c);

	for (unsigned int i = 0; i < bsgs.U.size(); ++i) {
		BOOST_REQUIRE_EQUAL(bsgsCopy.U[i].size(), bsgs.U[i].size());

		// test that all transversal values are correct
		for (unsigned int beta = 0; beta < bsgsCopy.n; ++beta) {
			boost::scoped_ptr<PERM> u_beta(bsgsCopy.U[i].at(beta));
			if (u_beta)
				BOOST_CHECK_EQUAL(*u_beta / bsgsCopy.B[i], beta);
		}

		// test that the orbit stored in the transversal is correct
		BOOST_FOREACH(unsigned long beta, bsgsCopy.U[i].pairIt()) {
			BOOST_REQUIRE_LT(beta, bsgsCopy.n);
			boost::scoped_ptr<PERM> u_beta(bsgsCopy.U[i].at(beta));
			BOOST_CHECK(u_beta);
		}
	}

	const PERM cInv(~c);
	for (unsigned int i = 0; i < 5; ++i) {
		const PERM bsgsCopyElement(cInv * bsgs.random() * c);
		BOOST_CHECK(bsgsCopy.sifts(bsgsCopyElement));
	}
}

template<class PERM, class TRANS>
void testConjugateGroup() {
	conjugateGroup<PERM,TRANS>(info_1997);
	conjugateGroup<PERM,TRANS>(info_cyclic500);
	conjugateGroup<PERM,TRANS>(info_test33);
}

BOOST_AUTO_TEST_CASE( transversal_conjugate_schreier )
{
	typedef Permutation PERM;
	typedef SchreierTreeTransversal<PERM> TRANS;
	// 50 runs with random conjugation
	for (unsigned int i = 0; i < 30; ++i)
		testConjugateGroup<PERM,TRANS>();
}

BOOST_AUTO_TEST_CASE( transversal_conjugate_explicit )
{
	typedef Permutation PERM;
	typedef ExplicitTransversal<PERM> TRANS;
	// 50 runs with random conjugation
	for (unsigned int i = 0; i < 100; ++i)
		testConjugateGroup<PERM,TRANS>();
}

template<class PERM, class TRANS>
void generateAllGroupElements(const BSGS<PERM,TRANS>& bsgs, std::list<typename PERM::ptr>& all, int k) {
	typedef typename PERM::ptr PERMptr;
	const std::list<PERMptr> tempList(all.begin(), all.end());
	all.clear();
	BOOST_FOREACH(const PERMptr& p, tempList) {
		BOOST_FOREACH(unsigned long beta, bsgs.U[k].pairIt()) {
			PERMptr g(new PERM(*p));
			boost::scoped_ptr<PERM> u_beta(bsgs.U[k].at(beta));
			BOOST_ASSERT(u_beta);
			*g *= *u_beta;
			all.push_back(g);
		}
	}
	if (k > 0)
		generateAllGroupElements(bsgs, all, k-1);
}

template<class PERM, class TRANS>
void checkGroupElements(const GroupInformation& info) {
	typedef typename PERM::ptr PERMptr;
	BSGS<PERM,TRANS> bsgs = ConstructDeterministic1<PERM, TRANS>()(info);
	std::list<PERMptr> allPerms;
	PERMptr identity(new PERM(bsgs.n));
	allPerms.push_back(identity);
	generateAllGroupElements(bsgs, allPerms, bsgs.U.size()-1);

	BOOST_REQUIRE_EQUAL(allPerms.size(), bsgs.order());

	boost::uint64_t fixSum = 0;
	BOOST_FOREACH(const PERMptr& p, allPerms) {
		for (dom_int i = 0; i < bsgs.n; ++i)
			if (p->at(i) == i)
				++fixSum;
	}

	BOOST_CHECK_EQUAL(fixSum, bsgs.order());
}

void checkTransitiveGroupElements(const GroupInformation& info) {
	checkGroupElements<Permutation, ExplicitTransversal<Permutation> >(info);
	checkGroupElements<Permutation, SchreierTreeTransversal<Permutation> >(info);
}

BOOST_AUTO_TEST_CASE( group_elements )
{
	checkTransitiveGroupElements(info_1997);
	checkTransitiveGroupElements(info_test33);
	checkTransitiveGroupElements(info_cyclic500);
}


#include <spatialops/structured/FVStaggeredFieldTypes.h>
#include <spatialops/Nebo.h>
#include <test/TestHelper.h>
#include <spatialops/structured/FieldHelper.h>
#include <spatialops/structured/ExternalAllocators.h>

#ifdef ENABLE_THREADS
#include <spatialops/ThreadPool.h>
#include <boost/thread/barrier.hpp>
#endif

#include <limits>
#include <cmath>
#include <list>

#include <sstream>
#include <fstream>

#include <spatialops/structured/SpatialMask.h>
#include <spatialops/NeboMask.h>

#include <spatialops/util/TimeLogger.h>

using namespace SpatialOps;
using std::cout;
using std::endl;

int main(int argc, const char *argv[])
{
  const bool print = false;
  TestHelper status(print);

  TimeLogger timer( "testMask.log" );

  typedef SVolField FieldT;

  const int nghost = 1;
  const GhostData ghost(nghost);
  const BoundaryCellInfo bcinfo = BoundaryCellInfo::build<FieldT>(false,false,false,false,false,false);
  MemoryWindow w(IntVec(30, 30, 30));
  FieldT f(w, bcinfo, ghost, NULL, InternalStorage);
  FieldT result(w, bcinfo, ghost, NULL, InternalStorage);
  FieldT::iterator ir = result.begin();

  result <<= 4;

  //result field + nghost for proper indexing
  *(ir + w.flat_index(IntVec(3, 3, 0) + nghost)) = 3;
  *(ir + w.flat_index(IntVec(4, 3, 0) + nghost)) = 3;
  *(ir + w.flat_index(IntVec(3, 4, 0) + nghost)) = 3;
  *(ir + w.flat_index(IntVec(4, 4, 0) + nghost)) = 3;

  *(ir + w.flat_index(IntVec(1, 1, 0) + nghost)) = 7;
  *(ir + w.flat_index(IntVec(6, 1, 0) + nghost)) = 7;
  *(ir + w.flat_index(IntVec(1, 6, 0) + nghost)) = 7;
  *(ir + w.flat_index(IntVec(6, 6, 0) + nghost)) = 7;

  std::vector<IntVec> maskSet;
  std::vector<IntVec> maskSet2;

  timer.start("mask-creation");

  //mask of center
  maskSet.push_back(IntVec(3, 3, 0));
  maskSet.push_back(IntVec(4, 3, 0));
  maskSet.push_back(IntVec(3, 4, 0));
  maskSet.push_back(IntVec(4, 4, 0));

  //mask of square corners
  maskSet2.push_back(IntVec(1, 1, 0));
  maskSet2.push_back(IntVec(6, 1, 0));
  maskSet2.push_back(IntVec(1, 6, 0));
  maskSet2.push_back(IntVec(6, 6, 0));

  //SpatialMasks
  SpatialMask<FieldT> mask(f, maskSet);
  SpatialMask<FieldT> mask2(f, maskSet2);

  timer.stop( "mask-creation" );

  timer.start( "cond-mask" );
  SpatialMask<FieldT> typedef SpatialMaskT;
  NeboMask<Initial, FieldT> typedef NeboMaskT;
  f <<= cond(mask, 3)
            (mask2, 7)
            (4);
  timer.stop( "cond-mask" );

  status( (display_fields_compare(result, f, print, print)), "Cond version");

  timer.start( "masked-assign" );
  f <<= 4;
  masked_assign(mask, f, 3);
  masked_assign(mask2, f, 7);
  timer.stop( "masked-assign" );

  status( (display_fields_compare(result, f, print, print)), "masked_assign version");

  std::cout << "Time: " << timer.total_time() << std::endl;

  if( status.ok() ) {
    std::cout << "ALL TESTS PASSED :)" << std::endl;
    return 0;
  } else {
    std::cout << "******************************" << std::endl
              << " At least one test FAILED! :(" << std::endl
              << "******************************" << std::endl;
    return -1;
  }
}


#ifndef BOOST_METAPARSE_GETTING_STARTED_11_3_1_HPP
#define BOOST_METAPARSE_GETTING_STARTED_11_3_1_HPP

// Automatically generated header file

// Definitions before section 11.3
#include "11_3.hpp"

// Definitions of section 11.3
// query:
//    exp_parser20::apply<BOOST_METAPARSE_STRING("(1+2")>::type

// query:
//    exp_parser20::apply<BOOST_METAPARSE_STRING("0+(1+2")>::type

#include <boost/metaparse/fail_at_first_char_expected.hpp>

#include <boost/metaparse/first_of.hpp>

struct plus_exp4 :
 first_of<
   foldl_start_with_parser<
     sequence<one_of<plus_token, minus_token>, mult_exp6>,
     mult_exp6,
     boost::mpl::quote2<binary_op>
   >,
   fail_at_first_char_expected<
     sequence<one_of<plus_token, minus_token>, mult_exp6>
   >
 > {};

using exp_parser21 = build_parser<plus_exp4>;

// query:
//    exp_parser21::apply<BOOST_METAPARSE_STRING("0+(1+2")>::type

#endif


// Copyright Louis Dionne 2013-2017
// 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/equal.hpp>
#include <boost/hana/front.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/type.hpp>

#include <boost/mpl/size.hpp>
#include <boost/mpl/vector.hpp>
namespace hana = boost::hana;
namespace mpl = boost::mpl;

//! [front]
#include <boost/hana/ext/boost/mpl/vector.hpp> // bridge header

using Vector = mpl::vector<int, char, float>;
static_assert(hana::front(Vector{}) == hana::type_c<int>, "");
//! [front]

namespace _ns0 {
//! [size]
using Size = mpl::size<Vector>::type;
static_assert(hana::equal(Size{}, hana::int_c<3>), ""); // breaks!
//! [size]
}

//! [size-fixed]
#include <boost/hana/ext/boost/mpl/integral_c.hpp>

using Size = mpl::size<Vector>::type;
static_assert(hana::equal(Size{}, hana::int_c<3>), "");
//! [size-fixed]

int main() { }


/* Copyright John Reid 2007
*/

#include "bio-pch.h"

#include "bio/application.h"
#include "bio/remo.h"
#include "bio/markov_model.h"
USING_BIO_NS

#include <boost/filesystem/path.hpp>
using namespace boost;
namespace po = boost::program_options;
namespace fs = boost::filesystem;

using namespace std;

struct MarkovModelsApp : Application
{
	std::string remo_extraction_filename;
	bool masked;
	bool sorted;

	MarkovModelsApp()
	{
		get_options().add_options()
			("remo,r", po::value(&remo_extraction_filename)->default_value("remo_space.bin"), "remo extraction file")
			("masked,m", po::value(&masked)->default_value(true), "use masked remos")
			("sorted,s", po::value(&sorted)->default_value(true), "sort markov model counts")
			;
	}

	int task()
	{
		cout << "Reading remos from " << remo_extraction_filename << "\n";
		cout << (masked ? "Using" : "Not using") << " masked remos\n";
		cout << (sorted ? "Sorting" : "Not sorting") << " markov model counts\n";

		//deserialise the binary remo archive
		ReMoExtraction::ptr_t remo;
		{
			fs::path
				remo_extraction_archive(
					remo_extraction_filename
				);
			cout << "Deserialising remo extraction from \"" << remo_extraction_archive._BOOST_FS_NATIVE() << "\"\n";
			remo = ReMoExtraction::deserialise(remo_extraction_archive);
		}

		DnaSymbolAlphabet alphabet;
		MarkovModel< 0 > zeroth_order_mm(4);
		MarkovModel< 1 > first_order_mm(4);
		MarkovModel< 2 > second_order_mm(4);
		MarkovModel< 3 > third_order_mm(4);

		//do the analysis on each remo
		{
			const unsigned num_groups = remo->sequence_groups.size();

			for (ReMoSequenceGroup::list_t::const_iterator sg = remo->sequence_groups.begin();
				remo->sequence_groups.end() != sg;
				++sg)
			{
				for (ReMoBundle::map_t::const_iterator rb = sg->get()->remo_bundles.begin();
					sg->get()->remo_bundles.end() != rb;
					++rb)
				{
					try
					{
						//get the centre sequence
						const seq_t centre_sequence = rb->second->get_sequence(rb->second->centre_sequence, masked);

						zeroth_order_mm.add_to_counts(
							centre_sequence.begin(),
							centre_sequence.end(),
							alphabet);
						first_order_mm.add_to_counts(
							centre_sequence.begin(),
							centre_sequence.end(),
							alphabet);
						second_order_mm.add_to_counts(
							centre_sequence.begin(),
							centre_sequence.end(),
							alphabet);
						third_order_mm.add_to_counts(
							centre_sequence.begin(),
							centre_sequence.end(),
							alphabet);
					}
					catch (const std::exception & ex)
					{
						cerr << "Error: " << ex.what() << endl;
					}
					catch (const string & msg)
					{
						cerr << "Error: " << msg << endl;
					}
					catch (const char * msg)
					{
						cerr << "Error: " << msg << endl;
					}
					catch (...)
					{
						cerr << "Undefined error" << endl;
					}
				}
			}
		}

		std::cout << "\n";
		zeroth_order_mm.print(alphabet, sorted); std::cout << "\n";
		first_order_mm.print(alphabet, sorted); std::cout << "\n";
		//second_order_mm.print(alphabet, sorted); std::cout << "\n";
		//third_order_mm.print(alphabet, sorted); std::cout << "\n";

		return 0;
	}

};

int
main(int argc, char * argv[])
{
	return MarkovModelsApp().main(argc, argv);
}