#include "OccTest.hpp"

#include "MechanicsFwd.hpp"
#include "OccContactShape.hpp"
#include "OccContactFace.hpp"
#include "OccContactEdge.hpp"
#include "ContactShapeDistance.hpp"
#include "ContactPoint.hpp"
#include "WhichGeometer.hpp"
#include "WhichGeometer.hpp"

#include <TopoDS_Shape.hxx>
#include <BRepPrimAPI_MakeSphere.hxx>
#include <BRepTools.hxx>
#include <TopExp_Explorer.hxx>
#include <TopoDS.hxx>
#include <gp_XYZ.hxx>
#include <gp_Quaternion.hxx>

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

#include <iostream>

CPPUNIT_TEST_SUITE_REGISTRATION(OccTest);

void OccTest::setUp()
{
}

void OccTest::tearDown()
{
}

void OccTest::exportBRepAsString()
{

  BRepPrimAPI_MakeSphere mksphere(1.0);

  OccContactShape sphere(mksphere.Shape());

  std::string s1 = sphere.exportBRepToString();

  std::stringstream out;

  BRepTools::Write(mksphere.Shape(), out);

  CPPUNIT_ASSERT(out.str() == s1);

}

void OccTest::computeUVBounds()
{

  TopExp_Explorer exp;
  BRepPrimAPI_MakeSphere mksphere(1.0);

  OccContactShape sphere_shape = OccContactShape(mksphere.Shape());

  OccContactFace sphere_face(sphere_shape, 0);

  sphere_face.computeUVBounds();

  CPPUNIT_ASSERT(std::abs(sphere_face.bsup1[0] - 6.28319) < 1e-4);
  CPPUNIT_ASSERT(std::abs(sphere_face.bsup1[1] - 1.5708) < 1e-4);

  CPPUNIT_ASSERT(std::abs(sphere_face.binf1[0] - 0.) < 1e-4);
  CPPUNIT_ASSERT(std::abs(sphere_face.binf1[1] + 1.5708) < 1e-4);

  std::cout << sphere_face.bsup1[0] << "," << sphere_face.bsup1[1] << std::endl;
  std::cout << sphere_face.binf1[0] << "," << sphere_face.binf1[1] << std::endl;

}

#include <SiconosVector.hpp>
#include <SimpleMatrix.hpp>
#include <OccBody.hpp>
void OccTest::move()
{
  BRepPrimAPI_MakeSphere mksphere(1.0);

  TopExp_Explorer exp;

  exp.Init(mksphere.Shape(), TopAbs_SHELL);

  TopoDS_Shell shell = TopoDS::Shell(exp.Current().Composed(mksphere.Shape().Orientation()));

  exp.Init(shell, TopAbs_FACE);

  TopoDS_Face face = TopoDS::Face(exp.Current().Composed(shell.Orientation()));

  OccContactShape sphere(mksphere.Shape());
  OccContactFace sphere_contact(sphere, 0);

  SP::SiconosVector position(new SiconosVector(7));
  SP::SiconosVector velocity(new SiconosVector(6));
  SP::SimpleMatrix inertia(new SimpleMatrix(3,3));
  position->zero();
  (*position)(0) = 1.;
  (*position)(1) = 2.;
  (*position)(2) = 3.;

  /* unit quaternion from 4,5,6,7 */
  (*position)(3) = 0.35634832254989918;
  (*position)(4) = 0.44543540318737401;
  (*position)(5) = 0.53452248382484879;
  (*position)(6) = 0.62360956446232352;

  velocity->zero();
  inertia->eye();

  SP::OccBody body(new OccBody(position, velocity, 1, inertia));

  body->addContactShape(createSPtrOccContactShape(sphere_contact));

  gp_XYZ translat = body->contactShape(0).data().Location().Transformation().
                    TranslationPart();

  std::cout << translat.X() << "," << translat.Y() << "," << translat.Z()
            << std::endl;

  CPPUNIT_ASSERT(translat.X() == 1.);
  CPPUNIT_ASSERT(translat.Y() == 2.);
  CPPUNIT_ASSERT(translat.Z() == 3.);

  gp_Quaternion rotat = body->contactShape(0).data().Location().Transformation().
                        GetRotation();

  CPPUNIT_ASSERT(std::abs(rotat.X() - 0.44543540318737401) < 1e-9);
  CPPUNIT_ASSERT(std::abs(rotat.Y() - 0.53452248382484879) < 1e-9);
  CPPUNIT_ASSERT(std::abs(rotat.Z() - 0.62360956446232352) < 1e-9);
  CPPUNIT_ASSERT(std::abs(rotat.W() - 0.35634832254989918) < 1e-9);

}
#ifdef HAS_FORTRAN
void OccTest::distance()
{
  const double pi = boost::math::constants::pi<double>();

  BRepPrimAPI_MakeSphere mksphere1(1, pi);
  BRepPrimAPI_MakeSphere mksphere2(1, pi);

  OccContactShape sphere1(mksphere1.Shape());
  OccContactShape sphere2(mksphere2.Shape());

  OccContactFace sphere1_contact(sphere1, 0);
  OccContactFace sphere2_contact(sphere2, 0);

  SP::SiconosVector position1(new SiconosVector(7));
  SP::SiconosVector position2(new SiconosVector(7));
  SP::SiconosVector velocity(new SiconosVector(6));
  SP::SimpleMatrix inertia(new SimpleMatrix(3,3));
  position1->zero();
  (*position1)(0) = 0.;
  (*position1)(1) = 0.;
  (*position1)(2) = 0.;

  (*position1)(3) = 1;

  position2->zero();
  (*position2)(0) = 3.;
  (*position2)(1) = 0.;
  (*position2)(2) = 0.;

  (*position2)(3) = cos(pi/2.);
  (*position2)(5) = sin(pi/2.);

  velocity->zero();
  inertia->eye();

  SP::OccBody body1(new OccBody(position1, velocity, 1, inertia));
  SP::OccBody body2(new OccBody(position2, velocity, 1, inertia));

  body1->addContactShape(createSPtrOccContactShape(sphere1_contact));
  body2->addContactShape(createSPtrOccContactShape(sphere2_contact));

  std::cout << "umin1:" << body1->contactShape(0).binf1[0] << std::endl;
  std::cout << "umax1:" << body1->contactShape(0).bsup1[0] << std::endl;
  std::cout << "vmin1:" << body1->contactShape(0).binf1[1] << std::endl;
  std::cout << "vmax1:" << body1->contactShape(0).bsup1[1] << std::endl;

  std::cout << "umin2:" << body2->contactShape(0).binf1[0] << std::endl;
  std::cout << "umax2:" << body2->contactShape(0).bsup1[0] << std::endl;
  std::cout << "vmin2:" << body2->contactShape(0).binf1[1] << std::endl;
  std::cout << "vmax2:" << body2->contactShape(0).bsup1[1] << std::endl;

  gp_XYZ translat1 = body1->contactShape(0).data().Location().Transformation().
                     TranslationPart();

  std::cout << translat1.X() << "," << translat1.Y() << "," << translat1.Z()
            << std::endl;

  gp_XYZ translat2 = body2->contactShape(0).data().Location().Transformation().
                     TranslationPart();

  std::cout << translat2.X() << "," << translat2.Y() << "," << translat2.Z()
            << std::endl;

  SP::Geometer geometer = ask<WhichGeometer<CadmbtbDistanceType> >(body1->contactShape(0));

  body2->contactShape(0).accept(*geometer);

  ContactShapeDistance& dist = geometer->answer;

  std::cout << dist.value << std::endl;

  std::cout << dist.x1 << "," << dist.y1 << "," << dist.z1 << std::endl;

  std::cout << dist.x2 << "," << dist.y2 << "," << dist.z2 << std::endl;

  std::cout << dist.nx << "," << dist.ny << "," << dist.nz << std::endl;

  CPPUNIT_ASSERT(std::abs(dist.value - 1.0) < 1e-9);

}
#endif


#pragma once

#include <set>
#include <vector>
#include <iostream>

#include <boost/unordered_map.hpp>
#include <boost/foreach.hpp>

namespace Clustering{

  typedef float Coord;            // a coordinate
  typedef float Distance;         // distance
  typedef unsigned int Dimensions; // how many dimensions
  typedef unsigned int PointId;    // the id of this point
  typedef unsigned int ClusterId;  // the id of this cluster

  typedef std::vector<Coord> PointND;    // a point (a centroid)
  typedef std::vector<PointND> Points;   // collection of points

  typedef std::set<PointId> SetPoints; // set of points

  // ClusterId -> (PointId, PointId, PointId, .... )
  typedef std::vector<SetPoints> ClustersToPoints;
  // PointId -> ClusterId
  typedef std::vector<ClusterId> PointsToClusters;
  // coll of centroids
  typedef std::vector<PointND> Centroids;

  //
  // Dump a point
  //
  std::ostream& operator << (std::ostream& os, PointND& p);

  //
  // distance between two points
  //
  Distance distance(const PointND & x, const PointND & y);

  //
  // Dump collection of Points
  //
  std::ostream& operator << (std::ostream& os, Points& cps);

  //
  // Dump a Set of points
  //
  std::ostream& operator << (std::ostream& os, SetPoints & sp);

  //
  // Dump centroids
  //
  std::ostream& operator << (std::ostream& os, Centroids & cp);

  //
  // Dump ClustersToPoints
  //
  std::ostream& operator << (std::ostream& os, ClustersToPoints & cp);

  //
  // Dump ClustersToPoints
  //
  std::ostream& operator << (std::ostream& os, PointsToClusters & pc);

  //
  // This class stores all the points available in the model
  //
  class PointsSpace{

    //
    // Dump collection of points
    //
    friend std::ostream& operator << (std::ostream& os, PointsSpace & ps){
      PointId i = 0;
      BOOST_FOREACH(Points::value_type p, ps.points__){
	      os << "point["<<i++<<"]=" << p << std::endl;
      }
      return os;
    };

  public:

    PointsSpace(PointId num_points, Dimensions num_dimensions)
      : num_points__(num_points), num_dimensions__(num_dimensions)
    {init_points();};

    PointsSpace(Points points){
      num_points__ = points.size();
      if(num_points__ > 0) num_dimensions__ = points[0].size();
      points__ = points;
    }

    inline const PointId getNumPoints() const {return num_points__;}
    inline const PointId getNumDimensions() const {return num_dimensions__;}
    inline const PointND& getPoint(PointId pid) const { return points__[pid];}

  private:
    //
    // Init collection of points
    //
    void init_points();

    PointId num_points__;
    Dimensions num_dimensions__;
    Points points__;

  };

  //
  //  This class represents a cluster
  //
  class Clusters {

  private:

    ClusterId num_clusters__;    // number of clusters
    PointsSpace& ps__;           // the point space
    Dimensions num_dimensions__; // the dimensions of vectors
    PointId num_points__;        // total number of points
    ClustersToPoints clusters_to_points__;
    PointsToClusters points_to_clusters__;
    Centroids centroids__;

    //
    // Zero centroids
    //
    void zero_centroids();

    //
    // Compute centroids, returns true if successful, false if one or more clusters has no points in it
    // (and therefore the centroid is invalid)
    //
    bool compute_centroids();
    //
    // Initial partition points randomly among available clusters
    //
    void initial_partition_points();

    //
    // Initial partition points among available clusters using Subset Furthest First
    //
    void SFF_partition_points();

  public:

    Centroids getCentroids(){
      return centroids__;
    }

    //
    // Dump ClustersToPoints
    //
    friend std::ostream& operator << (std::ostream& os, Clusters & cl){
      /*
      ClusterId cid = 0;
      BOOST_FOREACH(ClustersToPoints::value_type set, cl.clusters_to_points__){
	      os << "Cluster["<<cid<<"]=(";
	      BOOST_FOREACH(SetPoints::value_type pid, set){
	        Point p = cl.ps__.getPoint(pid);
	        os << "(" << p << ")";
	      }
	      os << ")" ;
	      cid++;
      }
      */
      os << cl.centroids__ << std::endl;
      return os;
    }

    Clusters(ClusterId num_clusters, PointsSpace & ps) :
      num_clusters__(num_clusters), ps__(ps),
	    num_dimensions__(ps.getNumDimensions()),
	    num_points__(ps.getNumPoints()),
	    points_to_clusters__(num_points__, 0) {

      ClusterId i = 0;
      Dimensions dim;
      for ( ; i < num_clusters; i++){
	      PointND point;   // each centroid is a point
	      for (dim=0; dim<num_dimensions__; dim++) point.push_back(0.0);
	      SetPoints set_of_points;

	      // init centroids
	      centroids__.push_back(point);

	      // init clusterId -> set of points
	      clusters_to_points__.push_back(set_of_points);
	      // init point <- cluster
      }
    };

    //
    // k-means
    //
    void k_means (void);
  };

};


/*  LICENSE

 _This file is Copyright 2018 by the Image Processing and Analysis Group (BioImage Suite Team). Dept. of Radiology & Biomedical Imaging, Yale School of Medicine._

 BioImage Suite Web is licensed under the Apache License, Version 2.0 (the "License");

 - you may not use this software except in compliance with the License.
 - You may obtain a copy of the License at [http://www.apache.org/licenses/LICENSE-2.0](http://www.apache.org/licenses/LICENSE-2.0)

 __Unless required by applicable law or agreed to in writing, software
 distributed under the License is distributed on an "AS IS" BASIS,
 WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 See the License for the specific language governing permissions and
 limitations under the License.__

 ENDLICENSE */

#include "bisfMRIAlgorithms.h"
#include "bisEigenUtil.h"
#include <Eigen/Dense>
#include <vector>

namespace bisfMRIAlgorithms {

  std::unique_ptr<bisSimpleImage<float> > computeGLM(bisSimpleImage<float>* input,bisSimpleImage<unsigned char>* mask,bisSimpleMatrix<float>* regressorMatrix,int num_tasks)
  {
    int dim[5]; input->getDimensions(dim);

    int nc=dim[3]*dim[4];
    int nt=dim[0]*dim[1]*dim[2];

    if (num_tasks<0 || num_tasks>nc)
      num_tasks=nc;

    int numrows=regressorMatrix->getNumRows();
    int numcols=regressorMatrix->getNumCols();
    if (numrows!=nc || numcols<num_tasks)
      {
	std::cerr << "Bad Regressor Matrix " << numrows << "*" << numcols << " Need " << nc << "rows and at least " << num_tasks << " columns" << std::endl;
	std::unique_ptr<bisSimpleImage<float> > tmp;
	return std::move(tmp);
      }

    int usemask=0;
    unsigned char* maskdata=0;
    if (mask!=0)
      {
	int m_dim[3]; mask->getImageDimensions(m_dim);
	int sum=0;
	for (int ia=0;ia<=2;ia++)
	  sum+=abs(m_dim[ia]-dim[ia]);

	if (sum>0)
	  {
	    std::cerr << "Bad Mask for compute GLM ... ignoring " << std::endl;
	  }
	else
	  {
	    usemask=1;
	    maskdata=mask->getImageData();
	  }
      }

    std::unique_ptr<bisSimpleImage<float> >output(new bisSimpleImage<float>("beta_image"));
    int outdim[5] = { dim[0],dim[1],dim[2],num_tasks,1};
    float spa[5]; input->getSpacing(spa);
    output->allocate(outdim,spa);
    output->fill(0.0f);

    float* outdata=output->getImageData();
    Eigen::MatrixXf A=bisEigenUtil::mapToEigenMatrix(regressorMatrix);
    Eigen::MatrixXf LSQ=bisEigenUtil::createLSQMatrix(A);
    Eigen::VectorXf b=Eigen::VectorXf::Zero(numcols);
    Eigen::MatrixXf inputdata=bisEigenUtil::mapImageToEigenMatrix(input);

    int task_offset=numcols-num_tasks;
    int volsize=nt;
    for (int voxel=0;voxel<nt;voxel++)
      {
	int compute=0;
	if (usemask)
	  {
	    if (double(maskdata[voxel])>0)
	      compute=1;
	  }
	else
	  compute=1;

	if (compute)
	  {
            Eigen::VectorXf x=inputdata.col(voxel);
    	    bisEigenUtil::inPlaceMultiplyMV(LSQ,x,b);
            for (int task=0;task<num_tasks;task++)
              outdata[task*volsize+voxel]=b[task+task_offset];
	  }
      }
    return std::move(output);
  }

  // ---------------------------------------------------------------------------------------
  // Legendre Polynomial
  // ---------------------------------------------------------------------------------------
  float legendre(float t,int order)
  {
    order=bisUtil::irange(order,0,6);

    if (order==0)
      return 1;

    if (order ==1) // P_1(x)=x;
      return t;

    if (order ==2) // P_2(x)=0.5*(3x^2-1)
      return 1.5f*t*t-0.5f;

    if (order ==3) // P_3(x) =0.5*(5x^3-3x)
      return 2.5f*t*t*t-1.5f*t;

    if (order ==4) // P_4(x) = 1/8*(35x^4-30x^2+3)
      return 0.125f*35.0f*t*t*t*t-0.125f*30.0f*t*t+0.375f;

    if (order ==5) // P_5(x) = 1/8*(63*x^5-70*x^3+15x)
      return 0.125f*63.0f*t*t*t*t*t-0.125f*70.0f*t*t*t+0.125f*15.0f*t;

    // order == 6
    return (231.0f*t*t*t*t*t*t-315.0f*t*t*t*t+105.0f*t*t-5.0f)/16.0f;
  }

  Eigen::MatrixXf createDriftRegressor(int numframes,int order) {

    order = bisUtil::irange(order,0,6);
    if (numframes<1)
      numframes=1;

    float shift=(numframes-1)*0.5f;
    float bot=0.5f*(numframes-1);

    Eigen::MatrixXf m=Eigen::MatrixXf(numframes,order+1);
    for (int i=0;i<numframes;i++) {
      float t=(i-shift)/bot;
      for (int j=0;j<=order;j++)
	m(i,j)=legendre(t,j);
    }
    return m;
  }

  // ---------------------------------------------------------------------------
  // Regress out "regressors"
  // ---------------------------------------------------------------------------

  int regressOut(Eigen::MatrixXf& input,Eigen::MatrixXf& regressors,Eigen::MatrixXf& LSQ,Eigen::MatrixXf& output) {

    int ok=bisEigenUtil::inPlaceMultiply3(regressors,LSQ,input,output);
    if (ok==0)
      return 0;
    int sz[2]; bisEigenUtil::getMatrixDimensions(input,sz);
    for (int row=0;row<sz[0];row++)
      for (int col=0;col<sz[1];col++)
	output(row,col)=input(row,col)-output(row,col);
    return 1;
  }

  // ---------------------------------------------------------------------------------------------------
  // Regress out "regressors" using weight vector `weights' which signifies quality of each frame (row)
  // ---------------------------------------------------------------------------------------------------
  int weightedRegressOut(Eigen::MatrixXf& input,Eigen::MatrixXf& weightedRegressors,Eigen::VectorXf& weights,Eigen::MatrixXf& LSQ,
						Eigen::MatrixXf& wI,
						Eigen::MatrixXf& output)
  {

    int sz_inp[2];
    bisEigenUtil::getMatrixDimensions(input,sz_inp);

    bisEigenUtil::resizeZeroMatrix(wI,sz_inp);

    for (int i=0;i<sz_inp[0];i++)
      {
	float w=weights(i);
	for(int j=0;j<sz_inp[1];j++)
	  wI(i,j)=w*input(i,j);
      }

    int ok=regressOut(wI,weightedRegressors,LSQ,output);
    if (ok==0)
      return 0;

    for (int i=0;i<sz_inp[0];i++)
      {
	float w=weights(i);
	for(int j=0;j<sz_inp[1];j++)
	  if (fabs(w)>0.001)
	    output(i,j)=output(i,j)/w;
      }

    return 1;
  }

  Eigen::MatrixXf createWeightedLSQ(Eigen::MatrixXf& regressors,Eigen::VectorXf& weights,Eigen::MatrixXf& wR)
  {
    int sz_reg[2];
    bisEigenUtil::getMatrixDimensions(regressors,sz_reg);
    bisEigenUtil::resizeZeroMatrix(wR,sz_reg);

    // Basically transform input data and regressors, do ordinary LSQ and scale back
    // Do regressors here and inputs in precomputed_weightedregressors
    // Multiply by weights
    for (int i=0;i<weights.rows();i++)
      {
	float w=weights(i);
	for(int j=0;j<sz_reg[1];j++)
	  wR(i,j)=w*regressors(i,j);
      }

    Eigen::MatrixXf LSQ=bisEigenUtil::createLSQMatrix(wR);
    return LSQ;
  }

  int butterworthFilter(Eigen::MatrixXf& input,Eigen::MatrixXf& output,Eigen::VectorXf& w,Eigen::MatrixXf& temp,
			std::string passType,float frequency,float sampleRate,int )
  {

    class internal
    {
   protected:
      double c,a1,a2,a3,b1,b2;
      double inputHistory[2];
      double outputHistory[3];
      int filter_element_count;

    public:
      internal(float frequency,std::string passType,float sampleRate) {

	this->inputHistory[0]=0.0;
	this->inputHistory[1]=0.0;
	this->outputHistory[0]=0.0;
	this->outputHistory[1]=0.0;
	this->outputHistory[2]=0.0;

	filter_element_count=0;
	if (passType !="high")
	  passType = "low";

	if (sampleRate<0.0)
	  sampleRate=0.6452f; // (1.0/1.55s);

	if (passType=="low" && frequency<0.0)
	  frequency=0.02f;
	if (passType=="high" && frequency<0.0)
	  frequency=0.1f;

	float resonance=float(sqrt(2.0));

	if (passType=="low")
	  {
	    c = 1.0 / tan(bisUtil::PI * frequency / sampleRate);
	    a1 = 1.0 / (1.0 + resonance * c + c * c);
	    a2 = 2.0 * a1;
	    a3 = a1;
	    b1 = 2.0 * (1.0 - c * c) * a1;
	    b2 = (1.0 - resonance * c + c * c) * a1;
	  }
	else
	  {
	    c =  tan(bisUtil::PI * frequency / sampleRate);
	    a1 = 1.0 / (1.0 + resonance * c + c * c);
	    a2 = -2 * a1;
	    a3 = a1;
	    b1 = 2.0 * (c * c - 1.0) * a1;
	    b2 = (1.0 - resonance * c + c * c) * a1;
	  }
      }

    protected:
      // Compute filter
      // While zero fill is good we need to think harder about this
      double update(double updated_Input) {

	double updated_Output=0.0;

	if (filter_element_count>1)
	  {
	    updated_Output= this->a1 * updated_Input + this->a2 * inputHistory[0] + this->a3 * inputHistory[1] -
	      this->b1 * outputHistory[0] - this->b2 * outputHistory[1];
	  }
	else if (filter_element_count==1)
	  {
	    updated_Output= this->a1 * updated_Input + this->a2 * inputHistory[0]  -
	      this->b1 * outputHistory[0];
	  }
	else if (filter_element_count==0)
	  {
	    updated_Output= this->a1 * updated_Input;
	  }

	filter_element_count=filter_element_count+1;
	inputHistory[1] = inputHistory[0];
	inputHistory[0] = updated_Input;

	outputHistory[2] = outputHistory[1];
	outputHistory[1] = outputHistory[0];
	outputHistory[0] = updated_Output;
	return updated_Output;
      };

      int backfill(Eigen::MatrixXf& input,Eigen::VectorXf& w,Eigen::MatrixXf& output) {

	int sz[2]; bisEigenUtil::getMatrixDimensions(input,sz);

	int sw=w.rows();
	if (sz[0]!=sw)
	  {
	    std::cerr << "Bad array sizes for backfill." << std::endl;
	    return 0;
	  }

	bisEigenUtil::resizeZeroMatrix(output,sz);

	int row=0,nextgoodrow=0;
	while (row< sz[0])
	  {
	    for (int i=0;i<sz[1];i++)
	      output(row,i)=input(row,i);

	    if (w(row)<0.5)
	      {
		nextgoodrow=row+1;
		while (w(nextgoodrow)<0.5 && nextgoodrow < sz[0])
		  {
		    nextgoodrow++;
		  }

		if (nextgoodrow!=sz[0])
		  {
		    // We found a good one, now back fill
		    for (long c=0;c<sz[1];c++)
		      {
			float v=output(nextgoodrow,c);
			for (long r=row;r<nextgoodrow;r++) {
			  output(r,c)=v;
			}
		      }
		  }
	      }
	    ++row;
	  }
	return 1;
      };

    public:

      int filter(Eigen::MatrixXf& input,Eigen::VectorXf& w,Eigen::MatrixXf& output,Eigen::MatrixXf& temp)
      {
	int sz[2]; bisEigenUtil::getMatrixDimensions(input,sz);
	bisEigenUtil::resizeZeroMatrix(output,sz);

	int doweight=0;
	if (w.rows()>2)
	  {
	    int ok=backfill(input,w,temp);
	    if (ok==0)
	      return 0;
	    doweight=1;
	  }

	for (int col=0;col<output.cols();col++)
	  {
	    filter_element_count=0; // Reset  filter
	    for (int row=0;row<output.rows();row++)
	      {
		if (doweight)
		  output(row,col)=(float)update(temp(row,col));
		else
		  output(row,col)=(float)update(input(row,col));
	      }
	  }
	return 1;
      }

    };

    internal filter_obj(frequency,passType,sampleRate);

    return filter_obj.filter(input,w,output,temp);

  }

  // ------------------------------------------------------------------------------------------
  // Compute correlation matrix stuff
  // ------------------------------------------------------------------------------------------------

  int computeGlobalSignal(Eigen::MatrixXf& input,Eigen::VectorXf& weights,Eigen::VectorXf& mean)
  {
    int dm[2]; bisEigenUtil::getMatrixDimensions(input,dm);

    int sw=weights.rows();
    if (sw<=2)
      {
	weights=Eigen::VectorXf::Zero(dm[0]);
	for (int ia=0;ia<dm[0];ia++)
	  weights(ia)=1.0;
      }
    else if (sw!=dm[0])
      {
	std::cerr << "Bad weight size for global Signal Regression. Mush be a vector of size " << dm[0] << std::endl;
	return 0;
      }

    bisEigenUtil::resizeZeroVector(mean,dm[0]);

    double sumv=0.0;
    for (int row=0;row<dm[0];row++) {
      float sum=0.0;
      if (weights(row)>0.5)
	for (int col=0;col<dm[1];col++) {
	  sum=sum+input(row,col);
	}
      mean(row)=sum/dm[1];
      sumv+=pow(mean(row),2.0);
    }

    double magn=sqrt(sumv);
    for (int row=0;row<dm[0];row++) {
      mean(row)=(float)(mean(row)/magn);
    }

    return 1;
  }

  int regressGlobalSignal(Eigen::MatrixXf& input,Eigen::VectorXf& weights,Eigen::VectorXf& mean,Eigen::MatrixXf& output)
  {
    int sz[2]; bisEigenUtil::getMatrixDimensions(input,sz);
    int sw=weights.rows();
    int sm=mean.rows();

    if (sm!=sz[0])
      {
	std::cerr << "Bad mean vector size for global Signal Regression. Mush be a vector of size " << sz[0] << std::endl;
	return 0;
      }

    if (sw<=2)
      {
	weights=Eigen::VectorXf::Zero(sz[0]);
	for (int ia=0;ia<sz[0];ia++)
	  weights(ia)=1.0;
      }
    else if (sw!=sz[0])
      {
	std::cerr << "Bad weight size for global Signal Regression. Mush be a vector of size " << sz[0] << std::endl;
	return 0;
      }

    bisEigenUtil::resizeZeroMatrix(output,sz);

    for (int col=0;col<sz[1];col++)
      {
	float sum=0.0;
	for (int row=0;row<sz[0];row++)
	  {
	    if (weights(row)>0.5)
	      sum=sum+input(row,col)*mean(row);
	  }
	for (int row=0;row<sz[0];row++)
	  {
	    if (weights(row)>0.5)
	      output(row,col)=input(row,col)-sum*mean(row);
	  }
      }
    return 1;
  }

  /** This function computes a correlation matrix from a set of timeseries. Weights are binary either use or do not use frame (>0.01 = use)
   * @alias BisfMRIMatrixConnectivity.computeCorrelationMatrix
   * @param {Matrix} input - the input timeseries vectors (row=frames)
   * @param {boolean} toz - if true compute r->z transform and return z-values else r's (default = false)
   * @param {array} weights - the input regressors vectors (weights for each row)
   * @returns {Matrix} correlation matrix
   */
  int computeCorrelationMatrix(Eigen::MatrixXf& input,int toz,Eigen::VectorXf& weights,Eigen::MatrixXf& output)
  {
    int sz[2]; bisEigenUtil::getMatrixDimensions(input,sz);
    int sw=weights.rows();
    if (sw<=2)
      {
	weights=Eigen::VectorXf::Zero(sz[0]);
	for (int ia=0;ia<sz[0];ia++)
	  weights(ia)=1.0;
      }
    else if (sw!=sz[0])
      {
	std::cerr << "Bad weight size. Must be a vector of size " << sz[1] << std::endl;
	return 0;
      }

    Eigen::MatrixXf norm=Eigen::MatrixXf::Zero(sz[0],sz[1]);

    // First normalize
    double sumw=0.0;

    for (int row=0;row<sz[0];row++) {
      if (weights(row)>0.0)
        weights(row)=1.0;
      else
        weights(row)=0.0;
      sumw+=weights(row);
    }

    if (sumw<0.00001)
      {
	std::cerr << "bad weights, must have a positive sum!" <<std::endl;
	return 0;
      }

    for (int row=0;row<sz[0];row++) {
      weights(row)=(float)(weights(row)/sumw);
    }

    for (int col=0;col<sz[1];col++)
      {
	double sum=0.0;
	double sum2=0.0;

	for (int row=0;row<sz[0];row++)
	  {
	    float v=input(row,col);
	    sum=sum+v*weights(row);
	    sum2=sum2+v*v*weights(row);
	  }
	double mean=sum;
	double sigma=sqrt(sum2-mean*mean);
	if (sigma>0.0)
	  {
	    for (int row=0;row<sz[0];row++)
	      norm(row,col)=(float)((input(row,col)-mean)/sigma);
	  }
      }

    // Now compute matrix
    int odim[2] = { sz[1],sz[1] };
    bisEigenUtil::resizeZeroMatrix(output,odim);

    for (int outrow=0;outrow<sz[1];outrow++)
      {
	for (int outcol=outrow;outcol<sz[1];outcol++)
	  {
	    double sum=0.0;
	    for (int row=0;row<sz[0];row++) {
	      sum=sum+norm(row,outrow)*norm(row,outcol)*weights(row);
	    }

	    if (toz)
	      {
		std::cout << " toz " << sum << " --> ";
		sum=bisUtil::rhoToZConversion(sum);
		std::cout  << sum << std::endl;
	      }

	    output(outrow,outcol)=(float)sum;
	    output(outcol,outrow)=(float)sum; // symmetric;
	  }
      }
    return 1;
  }

  /** This function computes a correlation matrix from a set of timeseries. Weights are binary either use or do not use frame (>0.01 = use)
   * @alias BisfMRIMatrixConnectivity.computeSeedMapImage
   * @param {Image} input - the input timeseries vectors as image
   * @param {Matrix} seedtime series -- seed timeseries vectors as matrix (rows = frames);
   * @param {boolean} toz - if true compute r->z transform and return z-values else r's (default = false)
   * @param {array} weights - the input regressors vectors (weights for each row)
   * @returns {Matrix} seed map image
   */
  int computeSeedMapImage(bisSimpleImage<float>* input,Eigen::MatrixXf& roi,int toz,Eigen::VectorXf& weights,bisSimpleImage<float>* output)
  {

    int dim[5]; input->getDimensions(dim);
    int sz[2]; bisEigenUtil::getMatrixDimensions(roi,sz);

    if (dim[3]!=sz[0]) {
      std::cerr << "Bad roi numframes size. Image frames=" << dim[3] << " roimatrix frames=" << sz[0] << ". Not Equal!" << std::endl;
      return 0;
    }

    // ---------------------------------------------------
    // Weights stuff
    // ---------------------------------------------------
    int sw=weights.rows();
    if (sw<=2)
      {
	weights=Eigen::VectorXf::Zero(sz[0]);
	for (int ia=0;ia<sz[0];ia++)
	  weights(ia)=1.0;
      }
    else if (sw!=sz[0])
      {
	std::cerr << "Bad weight size. Must be a vector of size " << sz[1] << std::endl;
	return 0;
      }

    for (int ia=0;ia<sz[0];ia++) {
      if (weights(ia)>0.0)
        weights(ia)=1.0;
      else
        weights(ia)=0.0;
    }

    double sumw=0.0;
    for (int row=0;row<sz[0];row++) {
      sumw+=weights(row);
    }

    std::cout << "Sumw=" << sumw << std::endl;

    if (sumw<0.00001)
      {
	std::cerr << "bad weights, must have a positive sum!" <<std::endl;
	return 0;
      }

    for (int row=0;row<sz[0];row++)
      weights(row)=weights(row)/sumw;

    // ---------------------------------------------------
    // Normalize ROI Time cources
    // ---------------------------------------------------
    Eigen::MatrixXf norm=Eigen::MatrixXf::Zero(sz[0],sz[1]);

    for (int col=0;col<sz[1];col++)
      {
	double sum=0.0;
	double sum2=0.0;

	for (int row=0;row<sz[0];row++)
	  {
	    float v=roi(row,col);
	    sum=sum+v*weights(row);
	    sum2=sum2+v*v*weights(row);
	  }
	double mean=sum;
	double sigma=sqrt(sum2-mean*mean);
	if (sigma>0.0)
	  {
	    for (int row=0;row<sz[0];row++)
	      norm(row,col)=(float)((roi(row,col)-mean)/sigma);
	  }

      }

    // Now compute for each pixel
    // --------------------------
    float* imagedata=input->getImageData();
    Eigen::VectorXf img_matrix=Eigen::VectorXf::Zero(sz[0]);
    int numvoxels=dim[0]*dim[1]*dim[2];
    int numframes=sz[0];

    int outdim[5] = { dim[0],dim[1],dim[2],sz[1],1};
    float spa[5]; input->getSpacing(spa);
    output->allocate(outdim,spa);
    output->fill(0.0f);
    float* outimagedata=output->getImageData();

    for (int voxel=0;voxel<numvoxels;voxel++) {
      double sum=0.0;
      double sum2=0.0;

      for (int frame=0;frame<numframes;frame++)
        {
          float v=imagedata[voxel+frame*numvoxels];
          sum=sum+v*weights(frame);
          sum2=sum2+v*v*weights(frame);
        }

      double mean=sum;
      double sigma=sqrt(sum2-mean*mean);
      if (sigma>0.0) {
        for (int frame=0;frame<numframes;frame++) {
          img_matrix(frame)=(float)((imagedata[voxel+frame*numvoxels]-mean)/sigma);
        }
      } else {
        for (int frame=0;frame<numframes;frame++) {
          img_matrix(frame)=0.0;
        }
      }

      for (int seed=0;seed<sz[1];seed++) {
        float sum=0.0;
        for (int frame=0;frame<numframes;frame++) {
          sum=sum+norm(frame,seed)*img_matrix(frame)*weights(frame);
        }

        if (toz)
          sum=bisUtil::rhoToZConversion(sum);

        outimagedata[voxel+seed*numvoxels]=(float)sum;
      }
    }
    return 1;

  }

  // End of namespace
  }


#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/median.hpp>

#include <iostream>
#include <vector>
#include <chrono>

#include "tsc_clock.h"

int main(int, char** argv)
{
	TSCClock::Initialise();
	std::vector<uint64_t> samples(std::atoi(argv[1]));

	for (auto& s : samples)
	{
		s = TSCClock::now();
	}

	namespace acc = boost::accumulators;

	using features = acc::stats<
		acc::tag::mean, acc::tag::variance,
		acc::tag::min, acc::tag::max,
		acc::tag::median>;

	acc::accumulator_set<double, features> c;

	for (std::size_t i = 1; i < samples.size(); ++i)
	{
		c(TSCClock::FromCycles(samples[i] - samples[i - 1]).count());
	}

	std::cout << "mean " << acc::mean(c) << "\n"
			  << "stddev " << std::sqrt(acc::variance(c) * samples.size() / double(samples.size() - 1)) << "\n"
			  << "min " << acc::min(c) << "\n"
			  << "median " << acc::median(c) << "\n"
			  << "max " << acc::max(c) << std::endl;

	return 0;
}


#include "PolygonOffset2_EIK.h"
#include "PolygonOffset2.h"

#include <vector>
#include <CGAL/Polygon_2.h>
#include <CGAL/Polygon_with_holes_2.h>
#include <CGAL/create_offset_polygons_2.h>
#include <CGAL/create_straight_skeleton_from_polygon_with_holes_2.h>
#include <boost/shared_ptr.hpp>

void* PolygonOffset2_EIK_Create()
{
	return PolygonOffset2<EIK>::NewPolygonOffset2();
}

void PolygonOffset2_EIK_Release(void* ptr)
{
	PolygonOffset2<EIK>::DeletePolygonOffset2(ptr);
}

int PolygonOffset2_EIK_PolygonBufferSize(void* ptr)
{
	return PolygonOffset2<EIK>::PolygonBufferSize(ptr);
}

int PolygonOffset2_EIK_SegmentBufferSize(void* ptr)
{
	return PolygonOffset2<EIK>::SegmentBufferSize(ptr);
}

void PolygonOffset2_EIK_ClearPolygonBuffer(void* ptr)
{
	PolygonOffset2<EIK>::ClearPolygonBuffer(ptr);
}

void PolygonOffset2_EIK_ClearSegmentBuffer(void* ptr)
{
	PolygonOffset2<EIK>::ClearSegmentBuffer(ptr);
}

void* PolygonOffset2_EIK_GetBufferedPolygon(void* ptr, int index)
{
	return PolygonOffset2<EIK>::GetBufferedPolygon(ptr, index);
}

Segment2d PolygonOffset2_EIK_GetSegment(void* ptr, int index)
{
	return PolygonOffset2<EIK>::GetSegment(ptr, index);
}

void PolygonOffset2_EIK_GetSegments(void* ptr, Segment2d* segments, int count)
{
	PolygonOffset2<EIK>::GetSegments(ptr, segments, count);
}

void PolygonOffset2_EIK_CreateInteriorOffset(void* ptr, void* polyPtr, double offset)
{
	PolygonOffset2<EIK>::CreateInteriorOffset(ptr, polyPtr, offset);
}

void PolygonOffset2_EIK_CreateInteriorOffsetPWH(void* ptr, void* pwhPtr, double offset, BOOL boundaryOnly)
{
	PolygonOffset2<EIK>::CreateInteriorOffsetPWH(ptr, pwhPtr, offset, boundaryOnly);
}

void PolygonOffset2_EIK_CreateExteriorOffset(void* ptr, void* polyPtr, double offset)
{
	PolygonOffset2<EIK>::CreateExteriorOffset(ptr, polyPtr, offset);
}

void PolygonOffset2_EIK_CreateExteriorOffsetPWH(void* ptr, void* pwhPtr, double offset, BOOL boundaryOnly)
{
	PolygonOffset2<EIK>::CreateExteriorOffsetPWH(ptr, pwhPtr, offset, boundaryOnly);
}

void PolygonOffset2_EIK_CreateInteriorSkeleton(void* ptr, void* polyPtr, BOOL includeBorder)
{
	PolygonOffset2<EIK>::CreateInteriorSkeleton(ptr, polyPtr, includeBorder);
}

void PolygonOffset2_EIK_CreateInteriorSkeletonPWH(void* ptr, void* pwhPtr, BOOL includeBorder)
{
	PolygonOffset2<EIK>::CreateInteriorSkeletonPWH(ptr, pwhPtr, includeBorder);
}

void PolygonOffset2_EIK_CreateExteriorSkeleton(void* ptr, void* polyPtr, double maxOffset, BOOL includeBorder)
{
	PolygonOffset2<EIK>::CreateExteriorSkeleton(ptr, polyPtr, maxOffset, includeBorder);
}

void PolygonOffset2_EIK_CreateExteriorSkeletonPWH(void* ptr, void* pwhPtr, double maxOffset, BOOL includeBorder)
{
	PolygonOffset2<EIK>::CreateExteriorSkeletonPWH(ptr, pwhPtr, maxOffset, includeBorder);
}


////////////////////////////////////////////////////////////////////////////////
// IsotropicValidation.cc
////////////////////////////////////////////////////////////////////////////////
/*! @file
//      Determines whether a pattern is isotropic under a given base material by
//      homogenizing rotated versions of the pattern.
//
//      Rotations are chosen randomly by picking a random axis and angle.
*/
//  Author:  Julian Panetta (jpanetta), julian.panetta@gmail.com
//  Company:  New York University
//  Created:  01/02/2015 06:17:34
////////////////////////////////////////////////////////////////////////////////
#include <MeshFEM/MeshIO.hh>
#include <MeshFEM/MSHFieldWriter.hh>
#include <MeshFEM/LinearElasticity.hh>
#include <MeshFEM/Materials.hh>
#include <MeshFEM/PeriodicHomogenization.hh>
#include <MeshFEM/GlobalBenchmark.hh>
#include <vector>
#include <queue>
#include <iostream>
#include <iomanip>
#include <memory>
#include <cmath>
#include <random>

#include <boost/program_options.hpp>
#include <boost/filesystem.hpp>

namespace po = boost::program_options;
using namespace std;
using namespace PeriodicHomogenization;

void usage(int exitVal, const po::options_description &visible_opts) {
    cout << "Usage: PeriodicHomogenization_cli [options] mesh" << endl;
    cout << visible_opts << endl;
    exit(exitVal);
}

po::variables_map parseCmdLine(int argc, const char *argv[])
{
    po::options_description hidden_opts("Hidden Arguments");
    hidden_opts.add_options()
        ("mesh",       po::value<string>(),                     "input mesh")
        ;
    po::positional_options_description p;
    p.add("mesh",                1);

    po::options_description visible_opts;
    visible_opts.add_options()("help", "Produce this help message")
        ("material,m", po::value<string>(), "base material")
        ("degree,d",   po::value<int>()->default_value(2), "degree of finite elements")
        ("nsamples,n", po::value<int>()->default_value(100), "Number of samples to test")
        ("transformOnly,t", "Only use the tensor transformation rule (much faster)")
        ;

    po::options_description cli_opts;
    cli_opts.add(visible_opts).add(hidden_opts);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).
                  options(cli_opts).positional(p).run(), vm);
        po::notify(vm);
    }
    catch (exception &e) {
        cout << "Error: " << e.what() << endl << endl;
        usage(1, visible_opts);
    }

    bool fail = false;
    if (vm.count("mesh") == 0) {
        cout << "Error: must specify input mesh" << endl;
        fail = true;
    }

    int d = vm["degree"].as<int>();
    if (d < 1 || d > 2) {
        cout << "Error: FEM Degree must be 1 or 2" << endl;
        fail = true;
    }

    if (fail || vm.count("help"))
        usage(fail, visible_opts);

    return vm;
}

template<size_t _N>
using HMG = LinearElasticity::HomogenousMaterialGetter<Materials::Constant>::template Getter<_N>;

template<size_t _N>
Eigen::Matrix<Real, _N, _N> randRotation();

template<>
Eigen::Matrix<Real, 3, 3> randRotation<3>() {
    random_device rd;
    mt19937 gen(rd());
    uniform_real_distribution<> angle(0, 2 * M_PI);

    Real theta = angle(gen), phi = angle(gen) / 2.0;
    Real rotationAngle = angle(gen) / 2.0;

    Vector3D rotationAxis(cos(theta) * sin(phi), sin(theta) * sin(phi), cos(phi));
    return Eigen::AngleAxisd(rotationAngle, rotationAxis).matrix();
}

template<>
Eigen::Matrix<Real, 2, 2> randRotation<2>() {
    random_device rd;
    mt19937 gen(rd());
    uniform_real_distribution<> angle(0, 2 * M_PI);
    return Eigen::Rotation2D<Real>(angle(gen)).matrix();
}

struct ErrorRecord {
    ErrorRecord(const string &n) : name(n) { }

    void insertError(Real error) {
        cout << name << ":\t" << error << std::endl;
        errors.push_back(error);
    }

    Real average() const {
        Real avg = 0;
        for (Real err : errors) avg += err;
        return avg / errors.size();
    }

    Real percentile(Real pct) {
        assert(pct <= 1.0 && pct >= 0.0);
        std::sort(errors.begin(), errors.end());
        return errors.at(pct * errors.size());
    }

    void report() {
        cout << name << " average:\t" << average() << endl;
        cout << name << " 98th percentile:\t" << percentile(0.98) << endl;
    }

    vector<Real> errors;
    string name;
};

template<size_t _N, size_t _FEMDegree>
void execute(const po::variables_map &args,
             const vector<MeshIO::IOVertex> &inVertices,
             const vector<MeshIO::IOElement> &inElements) {
    auto &mat = HMG<_N>::material;
    if (args.count("material")) mat.setFromFile(args["material"].as<string>());

    typedef LinearElasticity::Mesh<_N, _FEMDegree, HMG> Mesh;
    typedef LinearElasticity::Simulator<Mesh> Simulator;
    Simulator sim(inElements, inVertices);
    typedef typename Simulator::ETensor ETensor;
    typedef typename Simulator::VField  VField;

    // The axis-aligned bounding box of the unrotated mesh is the base cell.
    // After rotating, homogenizedElasticityTensor is unable to compute the base
    // cell volume accurately since it uses the axis-aligned bounding box of the
    // rotated mesh. So we must store it here.
    Real baseCellVolume = sim.mesh().boundingBox().volume();

    BENCHMARK_START_TIMER_SECTION("Cell Problems");
    vector<VField> w_ij;
    solveCellProblems(w_ij, sim);
    BENCHMARK_STOP_TIMER_SECTION("Cell Problems");

    BENCHMARK_START_TIMER_SECTION("Compute Tensor");
    ETensor Eh = homogenizedElasticityTensor(w_ij, sim);
    ETensor Sh = Eh.inverse();
    BENCHMARK_STOP_TIMER_SECTION("Compute Tensor");

    cout << setprecision(16) << endl;
    cout << "Homogenized compliance tensor:" << endl;
    cout << Sh << endl << endl;
    cout << "Testing rotations..." << endl;

    Real Ex, Ey, Ez, nuYX, nuZX, nuZY, muYZ, muZX, muXY;
    Eh.getOrthotropic3D(Ex, Ey, Ez, nuYX, nuZX, nuZY, muYZ, muZX, muXY);
    Real E_avg = (Ex + Ey + Ez) / 3.0;
    Real nu_avg = (nuYX + nuZX + nuZY) / 3.0;
    Real mu_avg = (muYZ + muZX + muXY) / 3.0;
    std::cout << "Anisotropy: " << mu_avg / (E_avg / (2 * (1 + nu_avg))) << std::endl;
    std::cout << "Anisotropy2: " << (Sh.D(1, 1) - Sh.D(0, 1)) / (2 * Sh.D(5, 5)) << std::endl;
    std::cout << "Poisson: " << nu_avg << std::endl;
    Real Ex_true = Ex;

    ErrorRecord relError("Rel error compliance");
    ErrorRecord transformRelError("Transformed rel error compliance");
    ErrorRecord ExRelError("Rel error Ex");
    ErrorRecord transformExRelError("Transformed rel error Ex");
    bool runRotatedHomogenization = args.count("transformOnly") == 0;

    int nSamples = args["nsamples"].as<int>();
    for (int s = 0; s < nSamples; ++s) {
        auto rot = randRotation<_N>();
        ETensor diff;
        if (runRotatedHomogenization) {
            vector<MeshIO::IOVertex> rotVertices;
            for (const auto &v : inVertices)
                rotVertices.emplace_back(PointND<_N>(rot * PointND<_N>(v)));
            sim.updateMeshNodePositions(rotVertices);

            w_ij.clear();
            typedef typename Simulator::SMatrix SMatrix;
            constexpr size_t numStrains = SMatrix::flatSize();
            for (size_t i = 0; i < numStrains; ++i) {
                VField rhs(sim.constantStrainLoad(-SMatrix::CanonicalBasis(i)));
                w_ij.push_back(sim.solve(rhs));
            }
            ETensor EhRot = homogenizedElasticityTensor(w_ij, sim, baseCellVolume);
            ETensor ShRot = EhRot.inverse();
            diff = ShRot - Sh;
            cout << "Compliance tensor:" << endl << ShRot << endl;
            relError.insertError(sqrt(diff.quadrupleContract(diff) / Sh.quadrupleContract(Sh)));
            EhRot.getOrthotropic3D(Ex, Ey, Ez, nuYX, nuZX, nuZY, muYZ, muZX, muXY);
            ExRelError.insertError(std::abs((Ex_true - Ex) / Ex_true));
        }

        ETensor EhTrans = Eh.transform(rot);
        ETensor ShTrans = EhTrans.inverse();
        diff = ShTrans - Sh;
        cout << "Transformed original compliance tensor:" << endl << ShTrans << endl;
        transformRelError.insertError(sqrt(diff.quadrupleContract(diff) / Sh.quadrupleContract(Sh)));
        EhTrans.getOrthotropic3D(Ex, Ey, Ez, nuYX, nuZX, nuZY, muYZ, muZX, muXY);
        transformExRelError.insertError(std::abs((Ex_true - Ex) / Ex_true));
    }

    if (runRotatedHomogenization) {
        relError.report();
        ExRelError.report();
    }
    transformRelError.report();
    transformExRelError.report();

    BENCHMARK_REPORT();
}

////////////////////////////////////////////////////////////////////////////////
/*! Program entry point
//  @param[in]  argc    Number of arguments
//  @param[in]  argv    Argument strings
//  @return     status  (0 on success)
*///////////////////////////////////////////////////////////////////////////////
int main(int argc, const char *argv[])
{
    po::variables_map args = parseCmdLine(argc, argv);

    vector<MeshIO::IOVertex>  inVertices;
    vector<MeshIO::IOElement> inElements;
    string meshPath = args["mesh"].as<string>();
    auto type = load(meshPath, inVertices, inElements, MeshIO::FMT_GUESS,
                     MeshIO::MESH_GUESS);

    // Infer dimension from mesh type.
    size_t dim;
    if      (type == MeshIO::MESH_TET) dim = 3;
    else if (type == MeshIO::MESH_TRI) dim = 2;
    else    throw runtime_error("Mesh must be triangle or tet.");

    // Look up and run appropriate homogenizer instantiation.
    int deg = args["degree"].as<int>();
    auto exec = (dim == 3) ? ((deg == 2) ? execute<3, 2> : execute<3, 1>)
                           : ((deg == 2) ? execute<2, 2> : execute<2, 1>);

    exec(args, inVertices, inElements);

    return 0;
}


/*
 * optimization_iteration_data.hpp
 *
 *  Created on: Mar 19, 2019
 *      Author: Gregory Kramida
 *   Copyright: 2019 Gregory Kramida
 *
 *   Licensed under the Apache License, Version 2.0 (the "License");
 *   you may not use this file except in compliance with the License.
 *   You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0
 *
 *   Unless required by applicable law or agreed to in writing, software
 *   distributed under the License is distributed on an "AS IS" BASIS,
 *   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *   See the License for the specific language governing permissions and
 *   limitations under the License.
 */

//libraries
#include <Eigen/Dense>

//local
#include "../math/typedefs.hpp"
#include "../error_handling/throw_assert.hpp"

#pragma once
namespace telemetry {

template<typename ScalarContainer, typename VectorContainer>
class OptimizationIterationData {
public:
	OptimizationIterationData() = default;

	void add_iteration_result(ScalarContainer live_field = ScalarContainer(),
			VectorContainer warp_field = VectorContainer(),
			VectorContainer data_term_gradients = VectorContainer(),
			VectorContainer tikhonov_term_gradients = VectorContainer());

	const std::vector<ScalarContainer>  get_live_fields() const;
	const std::vector<VectorContainer> get_warp_fields() const;
	const std::vector<VectorContainer> get_data_term_gradients() const;
	const std::vector<VectorContainer> get_tikhonov_term_gradients() const;

	int get_frame_count() const;

	bool operator==(const OptimizationIterationData& rhs){
		throw_assert(false, "not implemented (properly, because of bug in Eigen Tensor module)");
		//TODO: remote the "&" and somehow fix the bug with the Tensor comparison operator
		return &this->live_fields == &rhs.live_fields &&
			   &this->warp_fields == &rhs.warp_fields &&
			   &this->data_term_gradients == &rhs.data_term_gradients &&
			   &this->tikhonov_term_gradients == &rhs.tikhonov_term_gradients
				;
	}

private:
	std::vector<ScalarContainer> live_fields;
	std::vector<VectorContainer> warp_fields;
	std::vector<VectorContainer> data_term_gradients;
	std::vector<VectorContainer> tikhonov_term_gradients;
};

typedef OptimizationIterationData<eig::MatrixXf, math::MatrixXv2f> OptimizationIterationData2d;
typedef OptimizationIterationData<math::Tensor3f, math::Tensor3v3f> OptimizationIterationData3d;

} //namespace telemetry


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

  @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-constant

    Generates 6! that is 720

    @par Header <boost/simd/constant/fact_6.hpp>

    @par Semantic:

    @code
    T r = Fact_6<T>();
    @endcode

    is similar to:

    @code
    T r = T(720);
    @endcode

    @return The Fact_6 constant for the proper type
  **/
  template<typename T> T Fact_6();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant
      Generate the  constant fact_6.

      @return The Fact_6 constant for the proper type
    **/
    const boost::dispatch::functor<tag::fact_6_> fact_6 = {};
  }
} }
#endif

#include <boost/simd/constant/scalar/fact_6.hpp>
#include <boost/simd/constant/simd/fact_6.hpp>

#endif


#include <boost/mpl/int.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/simd/sdk/memory/align_on.hpp>

using boost::mpl::int_;
using boost::mpl::equal_to;
using boost::simd::meta::align_on;

int main()
{
  BOOST_MPL_ASSERT(( equal_to<align_on<int_<0>,int_<4> >::type,int_<0> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on<int_<1>,int_<4> >::type,int_<4> >::type ));
  BOOST_MPL_ASSERT(( equal_to<align_on<int_<1055>,int_<4> >::type,int_<1056> >::type ));
}


#include <ros/ros.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <iostream>
#include <vector>
#include <string.h>
#include <fstream>
#include <sstream>
#include <iomanip>
#include <cmath>
#include <thread>
#include <fstream>
#include <Eigen/SVD>
#include <Eigen/QR>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
#include <boost/filesystem.hpp>
namespace fs = boost::filesystem;
using namespace std;

// get_filenames
int get_filenames(const std::string& dir, std::vector<std::string>& filenames)
{
	fs::path path(dir);
	if (!fs::exists(path))
	{
		return -1;
	}

	fs::directory_iterator end_iter;
	for (fs::directory_iterator iter(path); iter!=end_iter; ++iter)
	{
		if (fs::is_regular_file(iter->status()))
		{
			filenames.push_back(iter->path().string());
		}

		if (fs::is_directory(iter->status()))
		{
			get_filenames(iter->path().string(), filenames);
		}
	}

	return filenames.size();
}

// del_filenames
int del_filenames(const std::string& dir)
{
	fs::path path(dir);
	if (!fs::exists(path))
	{
		return -1;
	}

	fs::directory_iterator end_iter;
	for (fs::directory_iterator iter(path); iter!=end_iter; ++iter)
	{
		if (fs::is_regular_file(iter->status()))
		{
			fs::remove(iter->path());
		}

		if (fs::is_directory(iter->status()))
		{
			del_filenames(iter->path().string());
		}
	}

  return 0;
}

std::string output_path;
std::string input_odom_path;
std::ofstream pose_odom_file;
std::ofstream pose_gt_file;
std::ofstream odom_file;
bool is_average_output_odom = false;

// CSVDATA_ODOM
struct CSVDATA_ODOM {
  double time;
  double Dtime;
  Eigen::Vector3d pB;
  Eigen::Vector4d qB;
  Eigen::Vector4d qB_G;
  Eigen::Vector3d vB;
  Eigen::Vector3d wB;

  Eigen::Vector3d gt_pB;
  Eigen::Vector4d gt_qB;
  Eigen::Vector4d gt_qB_G;
  Eigen::Vector3d gt_vB;
  Eigen::Vector3d gt_wB;

  double Sr;
  double Sr_avg;
  double Sr_rms;

  long long int numlines;

  Eigen::Vector3d pIBs;
  Eigen::Vector4d qIBs;

  Eigen::Matrix<double, 5, 1> k_b;

};

// align_odom
double freq_time_odom = 0;
void align_odom(std::vector<struct CSVDATA_ODOM> & csvData1, std::vector<struct CSVDATA_ODOM> &csvData2){
  std::vector<struct CSVDATA_ODOM>  csvData1_tmp;
  std::vector<struct CSVDATA_ODOM>  csvData2_tmp;
  for(auto csvdata1 : csvData1){

    double min_dtime = csvdata1.time;
    bool flag_has_min = false;
    struct CSVDATA_ODOM csvdata2_min;

    for(auto csvdata2 : csvData2){

      double dtime = abs(csvdata1.time - csvdata2.time);

      if( dtime < (freq_time_odom  - 0.005) ){

        if( dtime < min_dtime){
          min_dtime = dtime;

          csvdata2_min = csvdata2;
          flag_has_min = true;
        }
      }

    }

    if(flag_has_min)
    {
      csvData1_tmp.push_back(csvdata1);
      csvData2_tmp.push_back(csvdata2_min);
    }

  }
  csvData1 = csvData1_tmp;
  csvData2 = csvData2_tmp;
}

// do_average_output_odom
void do_average_output_odom(){

      std::vector<std::string> filenames;
      int num_files = get_filenames(input_odom_path, filenames);
      if(num_files <= 0){
        ROS_INFO_STREAM("do_average_output_odom: filenames.size() == 0");
        return;
      }
      cout << "do_average_output_odom num_files: " << num_files << endl;

      string file_time;
      [&]() {
          time_t timep;
            time (&timep);
            char tmp[64];
            strftime(tmp, sizeof(tmp), "%Y-%m-%d-%H-%M-%S",localtime(&timep) );
            file_time = tmp;
          }();

      cout.precision(16);
      std::vector<std::vector<struct CSVDATA_ODOM>> csvDataBus;
      int lineArray_length = 0;
      for(auto & filename : filenames){
        ifstream inFile(filename.c_str(), std::ios::in);
        if (!inFile)
        {
            cout << "read csv file error：can not open" << endl;
            return;
        }

        bool flag_first = true;
        double start_time = 0;
        double end_time = 0;
        double avg_time = 0;
        long long int count  = 0;
        double Sr_sum_square = 0;

        std::string lineStr;
        std::vector<struct CSVDATA_ODOM> csvData;
        std::getline(inFile, lineStr);
        while (std::getline(inFile, lineStr)) {
            std::stringstream ss(lineStr);
            std::string str;
            vector<string> lineArray;
            while (getline(ss, str, ',')) {
                lineArray.push_back(str);
            }
            lineArray_length = lineArray.size();
            struct CSVDATA_ODOM csvdata;
            csvdata.time = stod(lineArray[0]);
            csvdata.Dtime = stod(lineArray[1]);
            csvdata.pB = {stod(lineArray[3]),stod(lineArray[4]),stod(lineArray[5])};
            csvdata.qB = {stod(lineArray[6]),stod(lineArray[7]),stod(lineArray[8]),stod(lineArray[9])};

            if(flag_first){
              start_time = csvdata.time;
              flag_first = false;
            }
            end_time = csvdata.time;
            count++;

            if(lineArray_length > 10){

              geometry_msgs::Quaternion q_rpy_G = tf::createQuaternionMsgFromRollPitchYaw(stod(lineArray[10]) * M_PI / 180,
                stod(lineArray[11]) * M_PI / 180, stod(lineArray[12]) * M_PI / 180);
              csvdata.qB_G = {q_rpy_G.x, q_rpy_G.y, q_rpy_G.z, q_rpy_G.w};

              csvdata.vB = {stod(lineArray[13]),stod(lineArray[14]),stod(lineArray[15])};
              csvdata.wB = {stod(lineArray[16]),stod(lineArray[17]),stod(lineArray[18])};

              csvdata.gt_pB = {stod(lineArray[20]),stod(lineArray[21]),stod(lineArray[22])};
              csvdata.gt_qB = {stod(lineArray[23]),stod(lineArray[24]),stod(lineArray[25]),stod(lineArray[26])};

              geometry_msgs::Quaternion gt_q_rpy_G = tf::createQuaternionMsgFromRollPitchYaw(stod(lineArray[27]) * M_PI / 180,
                stod(lineArray[28]) * M_PI / 180, stod(lineArray[29]) * M_PI / 180);
              csvdata.gt_qB_G = {gt_q_rpy_G.x, gt_q_rpy_G.y, gt_q_rpy_G.z, gt_q_rpy_G.w};

              csvdata.gt_vB = {stod(lineArray[30]),stod(lineArray[31]),stod(lineArray[32])};
              csvdata.gt_wB = {stod(lineArray[33]),stod(lineArray[34]),stod(lineArray[35])};

              csvdata.Sr = stod(lineArray[37]);
              csvdata.Sr_avg = stod(lineArray[38]);

              Sr_sum_square += csvdata.Sr * csvdata.Sr;
              double Sr_sum_square_avg = Sr_sum_square / count;
              csvdata.Sr_rms = sqrt(Sr_sum_square_avg);

            }

            csvData.push_back(csvdata);
        }
        csvDataBus.push_back(csvData);

        if(count > 1){
          avg_time = (end_time - start_time) / (count - 1);
        }
        freq_time_odom += avg_time;

      }
      freq_time_odom /= num_files;
      cout << "freq_time_odom(ms): " << freq_time_odom * 1000 << endl;

      for(int j = 1; j < csvDataBus.size(); j++){
        // align_odom
        align_odom(csvDataBus.at(0),csvDataBus.at(j));
      }
      for(int j = 1; j < csvDataBus.size(); j++){
        // align_odom again
        align_odom(csvDataBus.at(0),csvDataBus.at(j));
      }
      for(int j = 1; j < csvDataBus.size(); j++){
        if(csvDataBus.at(0).size() != csvDataBus.at(j).size()){
          ROS_INFO_STREAM("do_average_output_odom: csvDataBus.at(0).size() != csvDataBus.at(j).size()");
          return;
        }
      }

      std::string delim = ",";
      // pose odom
      pose_odom_file.open((output_path + "pose_odom_avg" + "_" + file_time + ".txt"), std::ios::out);
      if(!pose_odom_file.is_open())
      {
          cerr << "pose_odom_avg is not open" << endl;
      }

      // pose gt
      pose_gt_file.open((output_path + "pose_gt_avg" + "_" + file_time + ".txt"), std::ios::out);
      if(!pose_gt_file.is_open())
      {
          cerr << "pose_gt_avg is not open" << endl;
      }

      // odom
      odom_file.open((output_path + "odom_avg" + "_" + file_time + ".csv"), std::ios::out);
      if(!odom_file.is_open())
      {
          cerr << "odom_avg is not open" << endl;
      }
      if(lineArray_length > 10){
          odom_file << "#Time(sec),";
          odom_file << "Dtime(sec),";
          odom_file  << delim;
          odom_file << "x(m),y(m),z(m),";
          odom_file << "qx,qy,qz,qw,";
          odom_file << "roll_x_G(deg),pitch_y_G(deg),yaw_z_G(deg),";
          odom_file << "vx(m/s),vy(m/s),vz(m/s),";
          odom_file << "wx(rad/s),wy(rad/s),wz(rad/s),";
          odom_file  << delim;
          odom_file << "gt_x(m),gt_y(m),gt_z(m),";
          odom_file << "gt_qx,gt_qy,gt_qz,gt_qw,";
          odom_file << "gt_roll_x_G(deg),gt_pitch_y_G(deg),gt_yaw_z_G(deg),";
          odom_file << "gt_vx(m/s),gt_vy(m/s),gt_vz(m/s),";
          odom_file << "gt_wx(rad/s),gt_wy(rad/s),gt_wz(rad/s),";
          odom_file  << delim;
          odom_file << "Sr,Sr_avg,Sr_rms_avg,";
          odom_file << std::endl;

      }else{
        odom_file << "#Time(sec),";
        odom_file << "Dtime(sec),";
        odom_file  << delim;
        odom_file << "x(m),y(m),z(m),";
        odom_file << "qx,qy,qz,qw,";
        odom_file << std::endl;
      }

      std::vector<std::vector<Eigen::Vector3d>> pB_bus;
      std::vector<std::vector<Eigen::Vector4d>> qB_bus;
      std::vector<std::vector<Eigen::Vector4d>> qB_bus_G;
      std::vector<std::vector<Eigen::Vector3d>> vB_bus;
      std::vector<std::vector<Eigen::Vector3d>> wB_bus;
      std::vector<std::vector<Eigen::Vector3d>> gt_pB_bus;
      std::vector<std::vector<Eigen::Vector4d>> gt_qB_bus;
      std::vector<std::vector<Eigen::Vector4d>> gt_qB_bus_G;
      std::vector<std::vector<Eigen::Vector3d>> gt_vB_bus;
      std::vector<std::vector<Eigen::Vector3d>> gt_wB_bus;
      std::vector<std::vector<double>> Sr_bus;
      std::vector<std::vector<double>> Sr_avg_bus;
      std::vector<std::vector<double>> Sr_rms_bus;

      std::vector<std::vector<Eigen::Vector3d>> pIBs_bus;
      std::vector<std::vector<Eigen::Vector4d>> qIBs_bus;

      std::vector<std::vector<Eigen::Matrix<double, 5, 1>>> k_b_bus;

      std::vector<struct CSVDATA_ODOM> first_csv_bus = csvDataBus.at(0);
      int num_lines = first_csv_bus.size();
      for(int i = 0; i < num_lines; i++){
        std::vector<Eigen::Vector3d> pB_bus_data;
        std::vector<Eigen::Vector4d> qB_bus_data;
        std::vector<Eigen::Vector4d> qB_bus_data_G;
        std::vector<Eigen::Vector3d> vB_bus_data;
        std::vector<Eigen::Vector3d> wB_bus_data;
        std::vector<Eigen::Vector3d> gt_pB_bus_data;
        std::vector<Eigen::Vector4d> gt_qB_bus_data;
        std::vector<Eigen::Vector4d> gt_qB_bus_data_G;
        std::vector<Eigen::Vector3d> gt_vB_bus_data;
        std::vector<Eigen::Vector3d> gt_wB_bus_data;
        std::vector<double> Sr_bus_data;
        std::vector<double> Sr_avg_bus_data;
        std::vector<double> Sr_rms_bus_data;

        std::vector<Eigen::Vector3d> pIBs_bus_data;
        std::vector<Eigen::Vector4d> qIBs_bus_data;

        std::vector<Eigen::Matrix<double, 5, 1>> k_b_bus_data;

        for(int j = 0; j < num_files; j++){
          std::vector<struct CSVDATA_ODOM> csvdata = csvDataBus.at(j);
          pB_bus_data.push_back(csvdata.at(i).pB);
          qB_bus_data.push_back(csvdata.at(i).qB);
          vB_bus_data.push_back(csvdata.at(i).vB);
          wB_bus_data.push_back(csvdata.at(i).wB);
          gt_pB_bus_data.push_back(csvdata.at(i).gt_pB);
          gt_qB_bus_data.push_back(csvdata.at(i).gt_qB);
          gt_vB_bus_data.push_back(csvdata.at(i).gt_vB);
          gt_wB_bus_data.push_back(csvdata.at(i).gt_wB);
          Sr_bus_data.push_back(csvdata.at(i).Sr);
          Sr_avg_bus_data.push_back(csvdata.at(i).Sr_avg);
          Sr_rms_bus_data.push_back(csvdata.at(i).Sr_rms);

          pIBs_bus_data.push_back(csvdata.at(i).pIBs);
          qIBs_bus_data.push_back(csvdata.at(i).qIBs);

          k_b_bus_data.push_back(csvdata.at(i).k_b);

          qB_bus_data_G.push_back(csvdata.at(i).qB_G);
          gt_qB_bus_data_G.push_back(csvdata.at(i).gt_qB_G);

        }
        pB_bus.push_back(pB_bus_data);
        qB_bus.push_back(qB_bus_data);
        vB_bus.push_back(vB_bus_data);
        wB_bus.push_back(wB_bus_data);
        gt_pB_bus.push_back(gt_pB_bus_data);
        gt_qB_bus.push_back(gt_qB_bus_data);
        gt_vB_bus.push_back(gt_vB_bus_data);
        gt_wB_bus.push_back(gt_wB_bus_data);
        Sr_bus.push_back(Sr_bus_data);
        Sr_avg_bus.push_back(Sr_avg_bus_data);
        Sr_rms_bus.push_back(Sr_rms_bus_data);

        pIBs_bus.push_back(pIBs_bus_data);
        qIBs_bus.push_back(qIBs_bus_data);

        k_b_bus.push_back(k_b_bus_data);

        qB_bus_G.push_back(qB_bus_data_G);
        gt_qB_bus_G.push_back(gt_qB_bus_data_G);

      }

      // qB_G
      std::vector<double> weightVector;
      for(int i = 0; i < num_files; i++){
        weightVector.push_back(1.0);
      }
      std::vector<Eigen::Quaterniond> avg_qB_G;
      for(auto qB_bus_data_G : qB_bus_G){

        int idex = avg_qB_G.size();
        double first_csv_w = (first_csv_bus.at(idex).qB_G).w();
        double sign = first_csv_w / abs(first_csv_w);

        std::vector<tf::Quaternion> tf_quaternions;
        tf::Quaternion tf_Quaternion;
        for(auto Qua_data : qB_bus_data_G){
          tf_Quaternion = tf::Quaternion(Qua_data.x(),Qua_data.y(),Qua_data.z(),Qua_data.w());
          tf_quaternions.push_back(tf_Quaternion);
        }

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, tf_quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < tf_quaternions.size(); ++i)
        {
          tf::Quaternion quat = tf_quaternions[i] * 1.0;
          Q(0,i) = quat.x();
          Q(1,i) = quat.y();
          Q(2,i) = quat.z();
          Q(3,i) = quat.w();
        }
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i)
        {
          double real = eigenvalues[i].real();
          if (real > max_value)
          {
            max_value = real;
            max_idx = i;
          }
        }
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();
        tf::Quaternion mean_orientation(
          eigenvector[0].real(),
          eigenvector[1].real(),
          eigenvector[2].real(),
          eigenvector[3].real()
        );
        // Eigen::Quaterniond w x y z
        Eigen::Quaterniond q_B(mean_orientation.w(),mean_orientation.x(),mean_orientation.y(),mean_orientation.z());

        double sign_tmp = mean_orientation.w() / abs(mean_orientation.w());
        if(sign != sign_tmp){
          q_B = Eigen::Quaterniond(-mean_orientation.w(),-mean_orientation.x(),-mean_orientation.y(),-mean_orientation.z());
        }

        q_B.normalize();
        avg_qB_G.push_back(q_B);
      }

      // gt_qB_G
      std::vector<Eigen::Quaterniond> gt_avg_qB_G;
      for(auto gt_qB_bus_data_G : gt_qB_bus_G){

        int idex = gt_avg_qB_G.size();
        double first_csv_w = (first_csv_bus.at(idex).gt_qB_G).w();
        double sign = first_csv_w / abs(first_csv_w);

        std::vector<tf::Quaternion> tf_quaternions;
        tf::Quaternion tf_Quaternion;
        for(auto Qua_data : gt_qB_bus_data_G){
          tf_Quaternion = tf::Quaternion(Qua_data.x(),Qua_data.y(),Qua_data.z(),Qua_data.w());
          tf_quaternions.push_back(tf_Quaternion);
        }

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, tf_quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < tf_quaternions.size(); ++i)
        {
          tf::Quaternion quat = tf_quaternions[i] * 1.0;
          Q(0,i) = quat.x();
          Q(1,i) = quat.y();
          Q(2,i) = quat.z();
          Q(3,i) = quat.w();
        }
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i)
        {
          double real = eigenvalues[i].real();
          if (real > max_value)
          {
            max_value = real;
            max_idx = i;
          }
        }
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();
        tf::Quaternion mean_orientation(
          eigenvector[0].real(),
          eigenvector[1].real(),
          eigenvector[2].real(),
          eigenvector[3].real()
        );
        // Eigen::Quaterniond w x y z
        Eigen::Quaterniond q_B(mean_orientation.w(),mean_orientation.x(),mean_orientation.y(),mean_orientation.z());

        double sign_tmp = mean_orientation.w() / abs(mean_orientation.w());
        if(sign != sign_tmp){
          q_B = Eigen::Quaterniond(-mean_orientation.w(),-mean_orientation.x(),-mean_orientation.y(),-mean_orientation.z());
        }

        q_B.normalize();
        gt_avg_qB_G.push_back(q_B);
      }

      // avg_Sr
      std::vector<double> avg_Sr;
      double Sr_square_sum = 0;
      long long int numlines = 0;
      for(auto Sr_bus_data : Sr_bus){
        double sum_tmp = 0;
        for(auto data : Sr_bus_data){
          sum_tmp = sum_tmp + data;
        }
        double avg_tmp = sum_tmp / num_files;
        avg_Sr.push_back(avg_tmp);
      }

      // avg_Sr_rms
      std::vector<double> avg_Sr_rms;
      for(auto Sr_rms_bus_data : Sr_rms_bus){
        double sum_tmp = 0;
        for(auto data : Sr_rms_bus_data){
          sum_tmp = sum_tmp + data;
        }
        double avg_tmp = sum_tmp / num_files;
        avg_Sr_rms.push_back(avg_tmp);
      }

      std::vector<double> avg_Sr_avg;
      for(auto Sr_avg_bus_data : Sr_avg_bus){
        double sum_tmp = 0;
        for(auto data : Sr_avg_bus_data){
          sum_tmp = sum_tmp + data;
        }
        double avg_tmp = sum_tmp / num_files;
        avg_Sr_avg.push_back(avg_tmp);
      }

      std::vector<Eigen::Vector3d> avg_pB;
      for(auto pB_bus_data : pB_bus){
        Eigen::Vector3d sum_tmp = Eigen::Vector3d::Zero();
        for(auto data : pB_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
        Eigen::Vector3d avg_tmp = sum_tmp / num_files;
        avg_pB.push_back(avg_tmp);
      }

      std::vector<Eigen::Vector3d> avg_vB;
      for(auto vB_bus_data : vB_bus){
        Eigen::Vector3d sum_tmp = Eigen::Vector3d::Zero();
        for(auto data : vB_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
        Eigen::Vector3d avg_tmp = sum_tmp / num_files;
        avg_vB.push_back(avg_tmp);
      }

      std::vector<Eigen::Vector3d> avg_wB;
      for(auto wB_bus_data : wB_bus){
        Eigen::Vector3d sum_tmp = Eigen::Vector3d::Zero();
        for(auto data : wB_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
        Eigen::Vector3d avg_tmp = sum_tmp / num_files;
        avg_wB.push_back(avg_tmp);
      }

      // avg_qB
      std::vector<Eigen::Quaterniond> avg_qB;
      for(auto qB_bus_data : qB_bus){

        int idex = avg_qB.size();
        double first_csv_w = (first_csv_bus.at(idex).qB).w();
        double sign = first_csv_w / abs(first_csv_w);

        std::vector<tf::Quaternion> tf_quaternions;
        tf::Quaternion tf_Quaternion;
        for(auto Qua_data : qB_bus_data){
          tf_Quaternion = tf::Quaternion(Qua_data.x(),Qua_data.y(),Qua_data.z(),Qua_data.w());
          tf_quaternions.push_back(tf_Quaternion);
        }

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, tf_quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < tf_quaternions.size(); ++i)
        {
          tf::Quaternion quat = tf_quaternions[i] * 1.0;
          Q(0,i) = quat.x();
          Q(1,i) = quat.y();
          Q(2,i) = quat.z();
          Q(3,i) = quat.w();
        }
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i)
        {
          double real = eigenvalues[i].real();
          if (real > max_value)
          {
            max_value = real;
            max_idx = i;
          }
        }
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();
        tf::Quaternion mean_orientation(
          eigenvector[0].real(),
          eigenvector[1].real(),
          eigenvector[2].real(),
          eigenvector[3].real()
        );
        // Eigen::Quaterniond w x y z
        Eigen::Quaterniond q_B(mean_orientation.w(),mean_orientation.x(),mean_orientation.y(),mean_orientation.z());

        double sign_tmp = mean_orientation.w() / abs(mean_orientation.w());
        if(sign != sign_tmp){
          q_B = Eigen::Quaterniond(-mean_orientation.w(),-mean_orientation.x(),-mean_orientation.y(),-mean_orientation.z());
        }

        q_B.normalize();
        avg_qB.push_back(q_B);
      }

      // gt_avg_pB
      std::vector<Eigen::Vector3d> gt_avg_pB;
      for(auto gt_pB_bus_data : gt_pB_bus){
        Eigen::Vector3d sum_tmp = Eigen::Vector3d::Zero();
        for(auto data : gt_pB_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
        Eigen::Vector3d avg_tmp = sum_tmp / num_files;
        gt_avg_pB.push_back(avg_tmp);
      }

      std::vector<Eigen::Vector3d> gt_avg_vB;
      for(auto gt_vB_bus_data : gt_vB_bus){
        Eigen::Vector3d sum_tmp = Eigen::Vector3d::Zero();
        for(auto data : gt_vB_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
        Eigen::Vector3d avg_tmp = sum_tmp / num_files;
        gt_avg_vB.push_back(avg_tmp);
      }

      std::vector<Eigen::Vector3d> gt_avg_wB;
      for(auto gt_wB_bus_data : gt_wB_bus){
        Eigen::Vector3d sum_tmp = Eigen::Vector3d::Zero();
        for(auto data : gt_wB_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
        Eigen::Vector3d avg_tmp = sum_tmp / num_files;
        gt_avg_wB.push_back(avg_tmp);
      }

      // gt_avg_qB
      std::vector<Eigen::Quaterniond> gt_avg_qB;
      for(auto gt_qB_bus_data : gt_qB_bus){

        int idex = gt_avg_qB.size();
        double first_csv_w = (first_csv_bus.at(idex).gt_qB).w();
        double sign = first_csv_w / abs(first_csv_w);

        std::vector<tf::Quaternion> tf_quaternions;
        tf::Quaternion tf_Quaternion;
        for(auto Qua_data : gt_qB_bus_data){
          tf_Quaternion = tf::Quaternion(Qua_data.x(),Qua_data.y(),Qua_data.z(),Qua_data.w());
          tf_quaternions.push_back(tf_Quaternion);
        }

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, tf_quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < tf_quaternions.size(); ++i)
        {
          tf::Quaternion quat = tf_quaternions[i] * 1.0;
          Q(0,i) = quat.x();
          Q(1,i) = quat.y();
          Q(2,i) = quat.z();
          Q(3,i) = quat.w();
        }
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i)
        {
          double real = eigenvalues[i].real();
          if (real > max_value)
          {
            max_value = real;
            max_idx = i;
          }
        }
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();
        tf::Quaternion mean_orientation(
          eigenvector[0].real(),
          eigenvector[1].real(),
          eigenvector[2].real(),
          eigenvector[3].real()
        );
        // Eigen::Quaterniond w x y z
        Eigen::Quaterniond q_B(mean_orientation.w(),mean_orientation.x(),mean_orientation.y(),mean_orientation.z());

        double sign_tmp = mean_orientation.w() / abs(mean_orientation.w());
        if(sign != sign_tmp){
          q_B = Eigen::Quaterniond(-mean_orientation.w(),-mean_orientation.x(),-mean_orientation.y(),-mean_orientation.z());
        }

        q_B.normalize();
        gt_avg_qB.push_back(q_B);
      }

      // avg_pIBs avg_qIBs
      std::vector<Eigen::Vector3d> avg_pIBs;
      for(auto pIBs_bus_data : pIBs_bus){
        Eigen::Vector3d sum_tmp = Eigen::Vector3d::Zero();
        for(auto data : pIBs_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
        Eigen::Vector3d avg_tmp = sum_tmp / num_files;
        avg_pIBs.push_back(avg_tmp);
      }

      std::vector<Eigen::Quaterniond> avg_qIBs;
      for(auto qIBs_bus_data : qIBs_bus){

        int idex = avg_qIBs.size();
        double first_csv_w = (first_csv_bus.at(idex).qIBs).w();
        double sign = first_csv_w / abs(first_csv_w);

        std::vector<tf::Quaternion> tf_quaternions;
        tf::Quaternion tf_Quaternion;
        for(auto Qua_data : qIBs_bus_data){
          tf_Quaternion = tf::Quaternion(Qua_data.x(),Qua_data.y(),Qua_data.z(),Qua_data.w());
          tf_quaternions.push_back(tf_Quaternion);
        }

        Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4, tf_quaternions.size());
        Eigen::Vector3d vec;
        for (size_t i = 0; i < tf_quaternions.size(); ++i)
        {
          tf::Quaternion quat = tf_quaternions[i] * 1.0;
          Q(0,i) = quat.x();
          Q(1,i) = quat.y();
          Q(2,i) = quat.z();
          Q(3,i) = quat.w();
        }
        Eigen::EigenSolver<Eigen::MatrixXd> es(Q * Q.transpose());
        auto eigenvalues = es.eigenvalues();
        size_t max_idx = 0;
        double max_value = eigenvalues[max_idx].real();
        for (size_t i = 1; i < 4; ++i)
        {
          double real = eigenvalues[i].real();
          if (real > max_value)
          {
            max_value = real;
            max_idx = i;
          }
        }
        auto eigenvector = es.eigenvectors().col(max_idx).normalized();
        tf::Quaternion mean_orientation(
          eigenvector[0].real(),
          eigenvector[1].real(),
          eigenvector[2].real(),
          eigenvector[3].real()
        );
        // Eigen::Quaterniond w x y z
        Eigen::Quaterniond q_B(mean_orientation.w(),mean_orientation.x(),mean_orientation.y(),mean_orientation.z());

        double sign_tmp = mean_orientation.w() / abs(mean_orientation.w());
        if(sign != sign_tmp){
          q_B = Eigen::Quaterniond(-mean_orientation.w(),-mean_orientation.x(),-mean_orientation.y(),-mean_orientation.z());
        }

        q_B.normalize();
        avg_qIBs.push_back(q_B);
      }

      // avg k_b
      std::vector< Eigen::Matrix<double, 5, 1> > avg_k_b;
      for(auto k_b_bus_data : k_b_bus){
        Eigen::Matrix<double, 5, 1> sum_tmp = Eigen::MatrixXd::Zero(5,1);

        for(auto data : k_b_bus_data){
          sum_tmp.noalias() = sum_tmp + data;
        }
       Eigen::Matrix<double, 5, 1> avg_tmp =  (1.0 / num_files) * sum_tmp;
        avg_k_b.push_back(avg_tmp);
      }

      assert(avg_pB.size() == avg_qB.size() == avg_vB.size() == avg_wB.size() ==
        gt_avg_pB.size() == gt_avg_qB.size() == gt_avg_vB.size() == gt_avg_wB.size() ==
        avg_Sr.size() == avg_Sr_avg.size() == avg_Sr_rms.size() == avg_pIBS.size() == avg_qIBs.size() == avg_k_b.size() ==
        avg_qB_G.size() == gt_avg_qB_G.size());
      assert(avg_pB.size() == first_csv_bus.size());

      ROS_INFO_STREAM("***average_output_odom Saving***");

      for(int i = 0; i < avg_pB.size(); i++){

        double dStamp = first_csv_bus.at(i).time;
        double Dtime = first_csv_bus.at(i).Dtime;
        Eigen::Vector3d avg_pB_data = avg_pB.at(i);
        Eigen::Quaterniond avg_qB_data = avg_qB.at(i);
        Eigen::Quaterniond avg_qB_data_G = avg_qB_G.at(i);
        Eigen::Vector3d avg_vB_data = avg_vB.at(i);
        Eigen::Vector3d avg_wB_data = avg_wB.at(i);
        Eigen::Vector3d gt_avg_pB_data = gt_avg_pB.at(i);
        Eigen::Quaterniond gt_avg_qB_data = gt_avg_qB.at(i);
        Eigen::Quaterniond gt_avg_qB_data_G = gt_avg_qB_G.at(i);
        Eigen::Vector3d gt_avg_vB_data = gt_avg_vB.at(i);
        Eigen::Vector3d gt_avg_wB_data = gt_avg_wB.at(i);
        double avg_Sr_data = avg_Sr.at(i);
        double avg_Sr_avg_data = avg_Sr_avg.at(i);
        double avg_Sr_rms_data = avg_Sr_rms.at(i);

        Eigen::Vector3d avg_pIBs_data = avg_pIBs.at(i);
        Eigen::Quaterniond avg_qIBs_data = avg_qIBs.at(i);

        Eigen::Matrix<double, 5, 1> avg_k_b_data = avg_k_b.at(i);

        // save pose {GB}
        // TUM #timestamp(sec) x y z q_x q_y q_z q_w
        pose_odom_file.precision(16);
        pose_odom_file << fixed << dStamp << " " << avg_pB_data(0) << " " << avg_pB_data(1) << " " << avg_pB_data(2) << " "
                << avg_qB_data.x() << " " << avg_qB_data.y() << " " << avg_qB_data.z() << " " << avg_qB_data.w() << endl;

        pose_gt_file.precision(16);
        pose_gt_file << fixed << dStamp << " " << gt_avg_pB_data(0) << " " << gt_avg_pB_data(1) << " " << gt_avg_pB_data(2) << " "
                << gt_avg_qB_data.x() << " " << gt_avg_qB_data.y() << " " << gt_avg_qB_data.z() << " " << gt_avg_qB_data.w() << endl;

        // save odom
        if(lineArray_length > 10){

              std::string delim = ",";
              odom_file.precision(16);

              double roll_x_G, pitch_y_G, yaw_z_G;
              tf::Matrix3x3(tf::Quaternion(avg_qB_data_G.x(),avg_qB_data_G.y(),avg_qB_data_G.z(),avg_qB_data_G.w())).getRPY(roll_x_G, pitch_y_G, yaw_z_G, 1);
              roll_x_G = roll_x_G * 180 / M_PI;
              pitch_y_G = pitch_y_G * 180 / M_PI;
              yaw_z_G = yaw_z_G * 180 / M_PI;
              double gt_roll_x_G, gt_pitch_y_G, gt_yaw_z_G;
              tf::Matrix3x3(tf::Quaternion(gt_avg_qB_data_G.x(),gt_avg_qB_data_G.y(),gt_avg_qB_data_G.z(),gt_avg_qB_data_G.w())).getRPY(gt_roll_x_G, gt_pitch_y_G, gt_yaw_z_G, 1);
              gt_roll_x_G = gt_roll_x_G * 180 / M_PI;
              gt_pitch_y_G = gt_pitch_y_G * 180 / M_PI;
              gt_yaw_z_G = gt_yaw_z_G * 180 / M_PI;

              odom_file << fixed << dStamp << delim << Dtime << delim;
              odom_file  << delim;
              odom_file << avg_pB_data(0) << delim << avg_pB_data(1) << delim << avg_pB_data(2) << delim;
              odom_file << avg_qB_data.x() << delim <<  avg_qB_data.y() << delim << avg_qB_data.z() << delim << avg_qB_data.w() << delim;
              odom_file << roll_x_G << delim << pitch_y_G << delim << yaw_z_G << delim;
              odom_file << avg_vB_data(0) << delim << avg_vB_data(1) << delim << avg_vB_data(2) << delim;
              odom_file << avg_wB_data(0) << delim << avg_wB_data(1) << delim << avg_wB_data(2) << delim;
              odom_file  << delim;
              odom_file << gt_avg_pB_data(0) << delim << gt_avg_pB_data(1) << delim << gt_avg_pB_data(2) << delim;
              odom_file << gt_avg_qB_data.x() << delim <<  gt_avg_qB_data.y() << delim << gt_avg_qB_data.z() << delim << gt_avg_qB_data.w() << delim;
              odom_file << gt_roll_x_G << delim << gt_pitch_y_G << delim << gt_yaw_z_G << delim;
              odom_file << gt_avg_vB_data(0) << delim << gt_avg_vB_data(1) << delim << gt_avg_vB_data(2) << delim;
              odom_file << gt_avg_wB_data(0) << delim << gt_avg_wB_data(1) << delim << gt_avg_wB_data(2) << delim;
              odom_file  << delim;
              odom_file << avg_Sr_data << delim << avg_Sr_avg_data << delim << avg_Sr_rms_data << delim;
              odom_file << std::endl;

        }else{
          std::string delim = ",";
          odom_file.precision(16);
          odom_file << fixed << dStamp << delim << Dtime << delim;
          odom_file  << delim;
          odom_file << avg_pB_data(0) << delim << avg_pB_data(1) << delim << avg_pB_data(2) << delim;
          odom_file << avg_qB_data.x() << delim <<  avg_qB_data.y() << delim << avg_qB_data.z() << delim << avg_qB_data.w() << delim;
          odom_file << std::endl;
        }

      }
      pose_odom_file.close();
      pose_gt_file.close();
      odom_file.close();
      ROS_INFO_STREAM("***average_output_odom Done***");

      // // del input_odom_path
      // if(del_filenames(input_odom_path) == 0){
      //   ROS_INFO_STREAM("***del input_odom_path Done***");
      // }

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "average_output");
  ros::NodeHandle pnh("~");

  pnh.param<string>("output_path",output_path,"");
  ROS_INFO_STREAM("Loaded output_path: " << output_path);

  pnh.param<string>("input_odom_path",input_odom_path,"");
  ROS_INFO_STREAM("Loaded input_odom_path: " << input_odom_path);

  pnh.param<bool>("is_average_output_odom", is_average_output_odom, false);

  // thread
  std::thread thread_do_average_output_odom;

  if(is_average_output_odom){
     thread_do_average_output_odom = std::thread(do_average_output_odom);
     thread_do_average_output_odom.join();
  }

  pnh.shutdown();
  return 0;
}


#include <knights_tour_problem/knights_tour_problem.hpp>

#include <boost/range/combine.hpp>

#include <atomic>
#include <functional>
#include <future>

namespace {

int constexpr g_init = -1;
int constexpr g_directions[][2] = {{2, 1}, {1, 2}, {-1, 2}, {-2, 1}, {-2, -1}, {-1, -2}, {1, -2}, {2, -1}};

int valid(int n, int x, int y)
{
    return x >= 0 && x < n && y >= 0 && y < n;
}

int valid(board_t const & board, int n, int x, int y)
{
    return valid(n, x, y) && board[x][y] == g_init;
}

bool solve_kt_problem(board_t & board, int n, int x, int y, int move, std::atomic_bool & stop_thread)
{
    if (move == n * n) {
        stop_thread = true;
        return true;
    }

    if (stop_thread) {
        return false;
    }

    for (auto const [dx, dy] : g_directions) {
        int const next_x = x + dx;
        int const next_y = y + dy;
        if (valid(board, n, next_x, next_y)) {
            board[next_x][next_y] = move;
            if (solve_kt_problem(board, n, next_x, next_y, move + 1, stop_thread)) {
                return true;
            }

            board[next_x][next_y] = -1;
        }
    }

    return false;
}

} // namespace

board_t solve_knights_tour_problem(int n, int x, int y)
{
    if (!valid(n, x, y)) {
        return {};
    }

    std::atomic_bool stop_thread = false;
    board_t init_board(n, std::vector<int>(n, g_init));
    int move = ++init_board[x][y];

    if (n >= 5) {
        size_t constexpr num = std::size(g_directions);
        std::vector<board_t> boards(num, init_board);
        std::vector<std::future<bool>> results(num);

        for (auto const & [direction, board, result] : boost::combine(g_directions, boards, results)) {
            auto const [dx, dy] = direction;
            int const next_x = x + dx;
            int const next_y = y + dy;
            if (valid(n, next_x, next_y)) {
                int const next_move = move + 1;
                board[next_x][next_y] = next_move;

                // clang-format off
                result = std::async(std::launch::async,
                    solve_kt_problem, std::ref(board), n, next_x, next_y, next_move + 1, std::ref(stop_thread));
                // clang-format on
            }
        }

        board_t * result = nullptr;
        for (auto const & [board, rc] : boost::combine(boards, results)) {
            if (rc.valid()) {
                if (auto const ok = rc.get(); ok && !result) {
                    result = &board;
                }
            }
        }

        if (result) {
            return *result;
        }
    }

    return init_board;
}


/* Copyright (C) 2012-2019 IBM Corp.
 * This program is Licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance
 * with the License. You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License. See accompanying LICENSE file.
 */
#include <cassert>
#include <string>
#include <sstream>
#include <NTL/ZZ.h>
NTL_CLIENT
#include <helib/NumbTh.h>
#include <helib/Context.h>
#include <helib/ArgMap.h>

using namespace helib;

int main(int argc, char *argv[])
{
  ArgMap amap;

  long m=17;
  amap.arg("m", m, "cyclotomic index");
  amap.note("e.g., m=1024, m=2047");

  long p=2;
  amap.arg("p", p, "plaintext base");
  amap.note("use p=-1 for the complex field");

  long r=1;
  amap.arg("r", r,  "lifting");

  Vec<long> gens0;
  amap.arg("gens", gens0, "use specified vector of generators", nullptr);
  amap.note("e.g., gens='[562 1871 751]'");

  Vec<long> ords0;
  amap.arg("ords", ords0, "use specified vector of orders", nullptr);
  amap.note("e.g., ords='[4 2 -4]', negative means 'bad'");

  bool noPrint = true;
  amap.arg("noPrint", noPrint, "suppress printouts");

  amap.parse(argc, argv);

  vector<long> gens1, ords1;
  convert(gens1, gens0);
  convert(ords1, ords0);

  Context context(m, p, r, gens1, ords1);
  buildModChain(context, 5, 2);
  if (!noPrint) {
    vector<long> f;
    factorize(f,m);
    cout << "factoring "<<m<<" gives [";
    for (unsigned long i=0; i<f.size(); i++)
      cout << f[i] << " ";
    cout << "]\n";
    context.zMStar.printout();
    cout << endl;
  }

  stringstream s1;
  writeContextBase(s1, context);
  s1 << context;
  string s2 = s1.str();

  if (!noPrint)
    cout << s2 << endl;

  stringstream s3(s2);

  unsigned long m1, p1, r1;
  vector<long> gens, ords;
  readContextBase(s3, m1, p1, r1, gens, ords);

  Context c1(m1, p1, r1, gens, ords);
  s3 >> c1;

  if (context == c1)
    cout << "GOOD\n";
  else
    cout << "BAD\n";

  return 0;
}


/*=============================================================================

  MYPROJECT: A software package for whatever.

  Copyright (c) University College London (UCL). All rights reserved.

  This software is distributed WITHOUT ANY WARRANTY; without even
  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
  PURPOSE.

  See LICENSE.txt in the top level directory for details.

=============================================================================*/

#include "catch.hpp"
#include "mpCatchMain.h"
#include "mpMyFunctions.h"
#include <iostream>
#include <vector>

#ifdef BUILD_Eigen
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#endif

#ifdef BUILD_Boost
#include <boost/math/special_functions/round.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/filesystem/path.hpp>
#endif

#ifdef BUILD_OpenCV
#include <opencv2/core/core_c.h>
#ifdef MYPROJECT_USE_CUDA
#include <opencv2/core/cuda.hpp>
#endif
#endif

#ifdef BUILD_PCL
#include <pcl/point_types.h>
#include <pcl/common/projection_matrix.h>
#endif

#ifdef BUILD_ArrayFire
#include <arrayfire.h>
#endif

TEST_CASE("My first test", "[some group identifier]")
{
  int a = 5;
  REQUIRE(a < 6);
}

TEST_CASE("My second test", "[some group identifier]")
{
  std::vector<int> a;
  REQUIRE(a.size() == 0);
}

TEST_CASE("Simple add", "[MyFirstAddFunction]")
{
  REQUIRE(mp::MyFirstAddFunction(1, 2) == 3);
}

TEST_CASE("Simple multiply", "[MyFirstMultiplyFunction]")
{
  REQUIRE(mp::MyFirstMultiplicationFunction(3, 4) == 12);
}

TEST_CASE("Boost test", "[Boost]")
{
#ifdef BUILD_Boost
  REQUIRE(boost::math::round(0.1) == 0);
#endif
}

TEST_CASE("Eigen test", "[Eigen]")
{
#ifdef BUILD_Eigen
  Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();
  REQUIRE(mat.determinant() == 1);
#endif
}

TEST_CASE("OpenCV test", "[OpenCV]")
{
#ifdef BUILD_OpenCV
  cv::Matx44d mat = cv::Matx44d::eye();
  REQUIRE(cv::determinant(mat) == 1);
#endif
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// This file is manually converted from PROJ4

// Copyright (c) 2008-2012 Barend Gehrels, Amsterdam, the Netherlands.

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

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Geometry Library by Barend Gehrels (Geodan, Amsterdam)

// Original copyright notice:

// 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 BOOST_GEOMETRY_PROJECTIONS_PJ_INV_HPP
#define BOOST_GEOMETRY_PROJECTIONS_PJ_INV_HPP

#include <boost/geometry/extensions/gis/projections/impl/adjlon.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/util/math.hpp>

/* general inverse projection */

namespace boost { namespace geometry { namespace projections
{

namespace detail
{

namespace inv
{
    static const double EPS = 1.0e-12;
}

 /* inverse projection entry */
template <typename PRJ, typename LL, typename XY, typename PAR>
inline void pj_inv(PRJ const& prj, PAR const& par, XY const& xy, LL& ll)
{
    /* can't do as much preliminary checking as with forward */
    /* descale and de-offset */
    double xy_x = (geometry::get<0>(xy) * par.to_meter - par.x0) * par.ra;
    double xy_y = (geometry::get<1>(xy) * par.to_meter - par.y0) * par.ra;
    double lon = 0, lat = 0;
    prj.inv(xy_x, xy_y, lon, lat); /* inverse project */
    lon += par.lam0; /* reduce from del lp.lam */
    if (!par.over)
        lon = adjlon(lon); /* adjust longitude to CM */
    if (par.geoc && geometry::math::abs(fabs(lat)-geometry::math::half_pi<double>()) > inv::EPS)
        lat = atan(par.one_es * tan(lat));

    geometry::set_from_radian<0>(ll, lon);
    geometry::set_from_radian<1>(ll, lat);
}

} // namespace detail
}}} // namespace boost::geometry::projections

#endif


#include <boost/graph/two_graphs_common_spanning_trees.hpp>


#define BOOST_TEST_MODULE ideas

#include "graph.hpp"
#include "utils.hpp"

#include <boost/test/unit_test.hpp>

#include <optional>
#include <queue>
#include <utility>

using namespace std;

// Tree node type declared.
struct tnt;

// Tree node type defined.
struct tnt: std::tuple<COST, std::pair<vertex, vertex>,
                       std::pair<COST, COST>, edge,
                       std::shared_ptr<tnt> >
{
  using base = std::tuple<COST, std::pair<vertex, vertex>,
                          std::pair<COST, COST>, edge,
                          std::shared_ptr<tnt> >;

  using base::base;

  bool
  operator <(const tnt &a) const
  {
    return std::get<0>(*this) < std::get<0>(a);
  }
};

// A priority queue element type.
using pqet = std::shared_ptr<tnt>;

// The priority queue type.
using pqt = std::priority_queue<pqet, std::vector<pqet>,
                                std::function<bool(const pqet &,
                                                   const pqet &)> >;
void
relax(const graph &g,
      std::vector<std::vector<std::pair<COST, COST> > > &known,
      const edge &e, vertex nv1, COST nc1, COST c2,
      const pqet &pqe, pqt &pq)
{
  // The target of edge e.
  vertex nv2 = target(e, g);
  // The edge cost.
  auto ec = boost::get(boost::edge_weight, g, e);
  // The candidate cost.
  auto nc2 = c2 + ec;

  if (nv1 > nv2)
    {
      std::swap(nv1, nv2);
      std::swap(nc1, nc2);
    }
  else if (nv1 == nv2 && nc1 > nc2)
    std::swap(nc1, nc2);

  if (known[nv1][nv2].first < nc1 || known[nv1][nv2].second < nc2)
    return;

  // Make sure that e has not been already used.
  const tnt *ptr = pqe.get();

  for(; ptr != nullptr; ptr = std::get<4>(*ptr).get())
    if (std::get<3>(*ptr) == e)
      break;

  // Edge e is not used when we reached the tree root.
  if (ptr == nullptr)
    {
      known[nv1][nv2] = {nc1, nc2};
      pq.push(std::make_shared<tnt>(nc1 + nc2,
                                    std::pair(nv1, nv2),
                                    std::pair(nc1, nc2),
                                    e, pqe));
    }
}

auto
gd_search(vertex src, vertex dst, const graph &g)
{
  std::optional<std::pair<path, path> > result;

  // That's the maximal pair.
  std::pair<COST, COST> mp(std::numeric_limits<int>::max(),
                           std::numeric_limits<int>::max());

  std::vector<std::vector<std::pair<COST, COST> > >
    known(num_vertices(g),
          std::vector<std::pair<COST, COST> >(num_vertices(g), mp));

  // That's our priority queue that does the sorting based on the cost
  // of the tree node.  We use operators <, and ! to sort in the
  // ascending order.
  pqt pq([](const pqet &a, const pqet &b){return !(*a < *b);});

  // Boot the search with the root of the search tree.
  pq.push(std::make_shared<tnt>(0,
                                std::pair<vertex, vertex>(src, src),
                                std::pair<COST, COST>(0, 0),
                                edge(), nullptr));

  while(!pq.empty())
    {
      // Here we get a copy of a shared_ptr.
      auto pqe = pq.top();
      pq.pop();
      const tnt &tn = *pqe;
      // Vertexes.
      const auto [v1, v2] = std::get<1>(tn);
      // Costs.
      const auto [c1, c2] = std::get<2>(tn);

      if (v1 == dst && v2 == dst)
        {
          path p1, p2;

          // Trace back the paths.  We start at the current tree node,
          // i.e., tnp, and end at the root (which has nullptr as the
          // parent node).
          for(const tnt *ptr = pqe.get();
              std::get<4>(*ptr).get() != nullptr;
              ptr = std::get<4>(*ptr).get())
            {
              // This is the edge that we add.
              const edge &e = std::get<3>(*ptr);
              vertex t = target(e, g);

              if (!p1.empty() && source(p1.front(), g) == t)
                p1.push_front(e);
              else if (!p2.empty() && source(p2.front(), g) == t)
                p2.push_front(e);
              else
                {
                  assert(p1.empty() || p2.empty());

                  if (p1.empty())
                    p1.push_front(e);
                  else if (p2.empty())
                    p2.push_front(e);
                }
            }

          if (get_path_length(g, p1) <= get_path_length(g, p2))
            result = pair(p1, p2);
          else
            result = pair(p2, p1);
        }

      // We are leaving vertex v1.
      for(const auto &e: make_iterator_range(out_edges(v1, g)))
        relax(g, known, e, v2, c2, c1, pqe, pq);

      // We are leaving vertex v2.
      for(const auto &e: make_iterator_range(out_edges(v2, g)))
        relax(g, known, e, v1, c1, c2, pqe, pq);
    }

  return result;
}

// -------------------------------------------------------------------
//
//     (1)
//     /|\
// e1 4 | 1 e2
//   /e3|  \
// (0)  1  (3)
//   \  |  /
// e4 1 | 5 e5
//     \|/
//     (2)
//
BOOST_AUTO_TEST_CASE(ideas_1)
{
  graph g(4);
  auto p1 = boost::add_edge(0, 1, g);
  auto p2 = boost::add_edge(1, 3, g);
  auto p3 = boost::add_edge(1, 2, g);
  auto p4 = boost::add_edge(0, 2, g);
  auto p5 = boost::add_edge(2, 3, g);

  assert(p1.second);
  assert(p2.second);
  assert(p3.second);
  assert(p4.second);
  assert(p5.second);

  const edge &e1 = p1.first;
  const edge &e2 = p2.first;
  const edge &e3 = p3.first;
  const edge &e4 = p4.first;
  const edge &e5 = p5.first;

  boost::get(boost::edge_weight, g, e1) = 4;
  boost::get(boost::edge_weight, g, e2) = 1;
  boost::get(boost::edge_weight, g, e3) = 1;
  boost::get(boost::edge_weight, g, e4) = 1;
  boost::get(boost::edge_weight, g, e5) = 5;

  // Search for edge-disjoint paths of minimal weight.
  auto opaths = gd_search(0, 3, g);
  assert(opaths);
  auto paths = opaths.value();

  // The size of the first path should be two edges.
  BOOST_CHECK(paths.first.size() == 2);
  // The size of the second path should be two edges.
  BOOST_CHECK(paths.second.size() == 2);

  // The first path should have edges e1, and e2.
  {
    auto i = paths.first.begin();
    BOOST_CHECK(*i == e1);
    ++i;
    BOOST_CHECK(*i == e2);
    ++i;
    BOOST_CHECK(i == paths.first.end());
  }

  // The second path should have edges e4, and e5.
  {
    auto i = paths.second.begin();
    BOOST_CHECK(*i == e4);
    ++i;
    BOOST_CHECK(*i == e5);
    ++i;
    BOOST_CHECK(i == paths.second.end());
  }
}

// -------------------------------------------------------------------
//
//     e1      e2
//    --1--   --2--
//   /     \ /     \
// (0)     (1)     (2)
//   \     / \     /
//    --3--   --4--
//     e3      e4
//
BOOST_AUTO_TEST_CASE(ideas_2)
{
  graph g(3);
  auto p1 = boost::add_edge(0, 1, g);
  auto p2 = boost::add_edge(1, 2, g);
  auto p3 = boost::add_edge(0, 1, g);
  auto p4 = boost::add_edge(1, 2, g);

  assert(p1.second);
  assert(p2.second);
  assert(p3.second);
  assert(p4.second);

  const edge &e1 = p1.first;
  const edge &e2 = p2.first;
  const edge &e3 = p3.first;
  const edge &e4 = p4.first;

  boost::get(boost::edge_weight, g, e1) = 1;
  boost::get(boost::edge_weight, g, e2) = 2;
  boost::get(boost::edge_weight, g, e3) = 3;
  boost::get(boost::edge_weight, g, e4) = 4;

  // Search for edge-disjoint paths of minimal weight.
  auto opaths = gd_search(0, 2, g);
  assert(opaths);
  auto paths = opaths.value();

  // The size of the first path should be two edges.
  BOOST_CHECK(paths.first.size() == 2);
  // The size of the second path should be two edges.
  BOOST_CHECK(paths.second.size() == 2);

  // The first path should have edges e1, and e2.
  {
    auto i = paths.first.begin();
    BOOST_CHECK(*i == e1);
    ++i;
    BOOST_CHECK(*i == e2);
    ++i;
    BOOST_CHECK(i == paths.first.end());
  }

  // The second path should have edges e4, and e5.
  {
    auto i = paths.second.begin();
    BOOST_CHECK(*i == e3);
    ++i;
    BOOST_CHECK(*i == e4);
    ++i;
    BOOST_CHECK(i == paths.second.end());
  }
}