//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <utility>
#include <string>
#include <Eigen/Core>
#include "genfile/VariantIdentifyingData.hpp"
#include "genfile/VariantEntry.hpp"
#include "genfile/VariantDataReader.hpp"
#include "genfile/vcf/get_set.hpp"
#include "components/SNPSummaryComponent/IntensitySummaryComputation.hpp"
#include "metro/mean_and_covariance.hpp"

namespace stats {
	IntensitySummaryComputation::IntensitySummaryComputation( double call_threshhold ):
		m_call_threshhold( call_threshhold )
	{}

	void IntensitySummaryComputation::operator()(
		VariantIdentifyingData const&,
		Genotypes const& genotypes,
		Ploidy const&,
		genfile::VariantDataReader& data_reader,
		ResultCallback callback
	) {
		if( !data_reader.supports( "XY" )) {
			return ;
		}

		int const N = genotypes.rows() ;

		{
			genfile::vcf::MatrixSetter< IntensityMatrix > intensity_setter( m_intensities, m_nonmissingness ) ;
			data_reader.get( "XY", intensity_setter ) ;
			assert( m_intensities.rows() == N ) ;
			assert( m_intensities.cols() == 2 ) ;
			assert( m_nonmissingness.rows() == N ) ;
			assert( m_nonmissingness.cols() == 2 ) ;
		}

#if 0
		Eigen::RowVectorXd mean ;
		Eigen::MatrixXd covariance ;

		metro::compute_mean_and_covariance(
			m_intensities,
			m_nonmissingness,
			mean,
			covariance
		) ;
#endif
		double total_nonmissing = ( m_nonmissingness.rowwise().sum().array() > 0 ).cast< double >().sum() ;
		callback( "mean_X", m_intensities.col(0).sum() / m_nonmissingness.col(0).sum() ) ;
		callback( "mean_Y", m_intensities.col(1).sum() / m_nonmissingness.col(1).sum() ) ;
		callback( "mean_X+Y", (m_intensities.sum() / ( 0.5 * m_nonmissingness.sum() ))) ;
#if 0
		callback( "var_X", covariance(0,0) ) ;
		callback( "var_Y", covariance(1,1) ) ;
		callback( "cov_XY",covariance(0,1) ) ;
		for( int g = 0; g < 3; ++g ) {
			m_nonmissingness_by_genotype = m_nonmissingness ;

			for( int i = 0; i < N; ++i ) {
				if( g == 3 ) { // representing missing genotype
					if( genotypes.row( i ).maxCoeff() >= m_call_threshhold ) {
						m_nonmissingness_by_genotype.row( i ).setZero() ;
					}
				} else {
					if( genotypes( i, g ) < m_call_threshhold ) {
						m_nonmissingness_by_genotype.row( i ).setZero() ;
					}
				}
			}
			metro::compute_mean_and_covariance(
				m_intensities,
				m_nonmissingness_by_genotype,
				mean,
				covariance
			) ;
			std::string const stub = "g=" + ( g == 3 ? std::string( "NA" ) : genfile::string_utils::to_string( g ) ) ;
			callback( stub + ":mean_X", mean(0) ) ;
			callback( stub + ":mean_Y", mean(1) ) ;
			callback( stub + ":var_X", covariance(0,0) ) ;
			callback( stub + ":var_Y", covariance(1,1) ) ;
			callback( stub + ":cov_XY",covariance(0,1) ) ;
		}
#endif
	}

	std::string IntensitySummaryComputation::get_summary( std::string const& prefix, std::size_t column_width ) const {
		return prefix + "IntensitySummaryComputation" ;
	}
}


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

#ifndef UNIPD_CALIBRATION_IMPL_CALIBRATION_PCL_UTILITIES_POINT_PLANE_EXTRACTION_HPP_
#define UNIPD_CALIBRATION_IMPL_CALIBRATION_PCL_UTILITIES_POINT_PLANE_EXTRACTION_HPP_

#include <Eigen/Geometry>
#include <Eigen/StdVector>

#include <pcl/impl/point_types.hpp>
#include <pcl/PCLHeader.h>
#include <pcl/PointIndices.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/keyboard_event.h>

#include <vector>

#include <calibration_pcl/utilities/point_plane_extraction.h>

namespace unipd
{
namespace calib
{

template <typename PCLPointT_>
  void PointPlaneExtraction<PCLPointT_>::setInputCloud(const PointCloudConstPtr & cloud)
  {
    cloud_ = cloud;
    cloud_tree_->setInputCloud(cloud_);

    NormalEstimator normal_estimator;
    normal_estimator.setInputCloud(cloud_);
    normal_estimator.setSearchMethod(cloud_tree_);
    normal_estimator.setKSearch(100); // TODO setRadiusSearch
    //normal_estimator.setRadiusSearch(0.05f);
    normal_estimator.compute(*cloud_normals_);
  }

template <typename PCLPointT_>
  bool PointPlaneExtraction<PCLPointT_>::extract(PlaneInfo & plane_info) const
  {
    if (not plane_info.indices)
      plane_info.indices = boost::make_shared<Indices1>();
    else
      plane_info.indices->clear();

    pcl::PointIndices init_indices;
    std::vector<float> sqr_distances;
    cloud_tree_->radiusSearch(point_, radius_, init_indices.indices, sqr_distances);

    if (init_indices.indices.size() < 50)
    {
      //ROS_ERROR_STREAM("Not enough points found near (" << point_.x << ", " << point_.y << ", " << point_.z << ")");
      return false;
    }

    Eigen::VectorXf coefficients(Eigen::VectorXf::Zero(4));
    ModelNormalPlane model(cloud_);
    model.setNormalDistanceWeight(0.2);
    model.setInputNormals(cloud_normals_);
    Eigen::VectorXf new_coefficients(4);
    model.optimizeModelCoefficients(init_indices.indices, coefficients, new_coefficients);
    if (coefficients == new_coefficients)
      return false;
    coefficients = new_coefficients;

    boost::shared_ptr<std::vector<int> > all_cloud_indices = model.getIndices();

    model.setIndices(init_indices.indices);
    std::vector<Scalar> distances;
    model.getDistancesToModel(coefficients, distances);
    Eigen::VectorXd v = Eigen::VectorXd::Map(&distances[0], distances.size());
    plane_info.std_dev = 0.0;

    if (v.size() > 0)
      plane_info.std_dev = std::sqrt(v.cwiseAbs2().mean() - std::pow(v.mean(), 2));

    model.setIndices(all_cloud_indices);
    model.selectWithinDistance(coefficients, 5 * plane_info.std_dev, *plane_info.indices);

    model.optimizeModelCoefficients(*plane_info.indices, coefficients, new_coefficients);
    if (coefficients == new_coefficients)
      return false;
    coefficients = new_coefficients;

    model.setNormalDistanceWeight(0.05);
    model.selectWithinDistance(coefficients, 5 * plane_info.std_dev, *plane_info.indices);

    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PCLPointT_> ec;
    ec.setClusterTolerance(radius_ / 8);
    ec.setMinClusterSize(plane_info.indices->size() / 8);
    ec.setMaxClusterSize(plane_info.indices->size());
    //ec.setSearchMethod(cloud_tree_);
    ec.setInputCloud(cloud_);
    ec.setIndices(plane_info.indices);
    ec.extract(cluster_indices);

    if (cluster_indices.empty())
      return false;

    Size1 max_index = 0;
    Size1 max_size = cluster_indices[0].indices.size();
    for (Size1 i = 1; i < cluster_indices.size(); ++i)
    {
      if (cluster_indices[i].indices.size() > max_size)
      {
        max_size = cluster_indices[i].indices.size();
        max_index = i;
      }
    }

    *plane_info.indices = cluster_indices[max_index].indices;

    plane_info.plane.normal()[0] = coefficients[0];
    plane_info.plane.normal()[1] = coefficients[1];
    plane_info.plane.normal()[2] = coefficients[2];
    plane_info.plane.offset() = coefficients[3];

    return true;
  }

} // namespace calib
} // namespace unipd
#endif // UNIPD_CALIBRATION_sIMPL_CALIBRATION_PCL_UTILITIES_POINT_PLANE_EXTRACTION_HPP_


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

#include <GraphMol/RDKitBase.h>

#include "AUTOCORR3D.h"
#include "MolData3Ddescriptors.h"

#include <math.h>
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <iostream>
#include <Eigen/Core>
#include <Eigen/QR>
#include <boost/math/special_functions/fpclassify.hpp>

using namespace Eigen;
namespace RDKit {
namespace Descriptors {

namespace {

MolData3Ddescriptors moldata3D;

VectorXd getEigenVect(std::vector<double> v) {
  double* varray_ptr = &v[0];
  Map<VectorXd> V(varray_ptr, v.size());
  return V;
}

double* GetGeodesicMatrix(double* dist, int lag, int numAtoms) {
  int sizeArray = numAtoms * numAtoms;
  double* Geodesic = new double[sizeArray];
  for (int i = 0; i < sizeArray; i++) {
    if (dist[i] == lag)
      Geodesic[i] = 1.0;
    else
      Geodesic[i] = 0.0;
  }
  return Geodesic;
}

// matrix prod that mimic the v2 version
// the code is in respect to Dragon 6 descriptors.
// replace the number of "Bicount" vertex per lag by a (numAtoms * (numAtoms -
// 1))!
// provided by Kobe team!
void get3DautocorrelationDesc(double* dist3D, double* topologicaldistance,
                              int numAtoms, const ROMol& mol,
                              std::vector<double>& res) {
  Map<MatrixXd> dm(dist3D, numAtoms, numAtoms);
  Map<MatrixXd> di(topologicaldistance, numAtoms, numAtoms);

  std::vector<double> wp = moldata3D.GetRelativePol(mol);
  VectorXd Wp = getEigenVect(wp);

  std::vector<double> wm = moldata3D.GetRelativeMW(mol);
  VectorXd Wm = getEigenVect(wm);

  std::vector<double> wi = moldata3D.GetRelativeIonPol(mol);
  VectorXd Wi = getEigenVect(wi);

  std::vector<double> wv = moldata3D.GetRelativeVdW(mol);
  VectorXd Wv = getEigenVect(wv);

  std::vector<double> we = moldata3D.GetRelativeENeg(mol);
  VectorXd We = getEigenVect(we);

  std::vector<double> wu = moldata3D.GetUn(numAtoms);
  VectorXd Wu = getEigenVect(wu);

  std::vector<double> ws = moldata3D.GetIState(mol);
  VectorXd Ws = getEigenVect(ws);

  std::vector<double> wr = moldata3D.GetRelativeRcov(mol);
  VectorXd Wr = getEigenVect(wr);

  MatrixXd Bi;
  MatrixXd tmp;
  double TDBmat[8][10];
  double dtmp;

  for (int i = 0; i < 10; i++) {
    double* Bimat = GetGeodesicMatrix(topologicaldistance, i + 1, numAtoms);
    Map<MatrixXd> Bi(Bimat, numAtoms, numAtoms);
    MatrixXd RBi = Bi.cwiseProduct(dm);
    // double Bicount = (double)Bi.sum();

    tmp = Wu.transpose() * RBi * Wu;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[0][i] = dtmp;

    tmp = Wm.transpose() * RBi * Wm;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[1][i] = dtmp;

    tmp = Wv.transpose() * RBi * Wv;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[2][i] = dtmp;

    tmp = We.transpose() * RBi * We;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[3][i] = dtmp;

    tmp = Wp.transpose() * RBi * Wp;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[4][i] = dtmp;

    tmp = Wi.transpose() * RBi * Wi;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[5][i] = dtmp;

    tmp = Ws.transpose() * RBi * Ws;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[6][i] = dtmp;

    tmp = Wr.transpose() * RBi * Wr;
    dtmp = (double)tmp(0);
    if (boost::math::isnan(dtmp)) dtmp = 0.0;
    TDBmat[7][i] = dtmp;
    delete[] Bimat;
  }

  // update the Output vector!
  for (unsigned int j = 0; j < 8; ++j) {
    for (unsigned int i = 0; i < 10; ++i) {
      res[j * 10 + i] =
          round(1000 * TDBmat[j][i] / (numAtoms * (numAtoms - 1))) / 1000;
    }
  }
}

void Get3Dauto(double* dist3D, double* topologicaldistance, int numAtoms,
               const ROMol& mol, std::vector<double>& res) {
  // AUTOCORRNAMES={"TDB01u","TDB02u","TDB03u","TDB04u","TDB05u","TDB06u","TDB07u","TDB08u","TDB09u","TDB10u","TDB01m","TDB02m","TDB03m","TDB04m","TDB05m","TDB06m","TDB07m","TDB08m","TDB09m","TDB10m","TDB01v","TDB02v","TDB03v","TDB04v","TDB05v","TDB06v","TDB07v","TDB08v","TDB09v","TDB10v","TDB01e","TDB02e","TDB03e","TDB04e","TDB05e","TDB06e","TDB07e","TDB08e","TDB09e","TDB10e","TDB01p","TDB02p","TDB03p","TDB04p","TDB05p","TDB06p","TDB07p","TDB08p","TDB09p","TDB10p","TDB01i","TDB02i","TDB03i","TDB04i","TDB05i","TDB06i","TDB07i","TDB08i","TDB09i","TDB10i","TDB01s","TDB02s","TDB03s","TDB04s","TDB05s","TDB06s","TDB07s","TDB08s","TDB09s","TDB10s","TDB01r","TDB02r","TDB03r","TDB04r","TDB05r","TDB06r","TDB07r","TDB08r","TDB09r","TDB10r"};
  get3DautocorrelationDesc(dist3D, topologicaldistance, numAtoms, mol, res);
}

}  // end of anonymous namespace

void AUTOCORR3D(const ROMol& mol, std::vector<double>& res, int confId) {
  PRECONDITION(mol.getNumConformers() >= 1, "molecule has no conformers")
  int numAtoms = mol.getNumAtoms();

  const Conformer& conf = mol.getConformer(confId);

  double* topologicaldistance =
      MolOps::getDistanceMat(mol, false);  // topological matrix
  double* dist3D =
      MolOps::get3DDistanceMat(mol, confId, false, true);  // 3D distance matrix

  res.clear();
  res.resize(80);
  Get3Dauto(dist3D, topologicaldistance, numAtoms, mol, res);
}
}  // end of Descriptors namespace
}  // end of RDKit namespace


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

#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Stirlingsplitlim, double
                                , 0, 0x41d628f6         // 26.77f
                                , 0x4061e083ba3443d4ll  // 143.01608
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Stirlingsplitlim, Stirlingsplitlim);
}

#endif


/*
 * File:   q_func_calc.h
 * Author: Dunarel Badescu
 *
 */

#ifndef _Q_FUNC_HGT_H
#define	_Q_FUNC_HGT_H

#include <string>
#include <map>
//#include <ext/hash_map>
#include <vector>
#include <set>

//#include "distance_measures.h"
//#include "win_q.h"
#include <boost/interprocess/containers/flat_map.hpp>
#include <boost/multi_array.hpp>
#include "boost/random.hpp"

#include "application_data.hpp"
#include "aligned_storage.hpp"

using namespace std;
using namespace uqam_doc;
using namespace boost;

// see http://www.cplusplus.com/forum/general/69135/
//using namespace boost::numeric::ublas;
//typedef vector<double>* p_vec;
//typedef AlignedStorage<float>* p_work_gr;

//typedef multi_array<p_float, 2> array_type2d;
//typedef multi_array<AlignedStorage<float>*, 2> array_type2d;
//typedef array_type2d::index index2;

// this is a functor
struct rand_float {
  rand_float(float x) : x(x) {}
  float operator()(float y) { return x + y; }

private:
  float x;
};

struct rand_int {
  //initializer
  rand_int(int init, int max_val) : x(max_val),
                    rng(boost::mt19937(init)),
				    uniform_normal(boost::uniform_int<int>(0,max_val-1)),
                    rand_normal(boost::variate_generator<boost::mt19937, boost::uniform_int<int> >(rng,uniform_normal)) { }

  //int operator()(int y) {
	  //return rand_normal();
  //	  return 1;
  //}

  int operator()() {
	  return rand_normal();
    }

  int next() {
	  return rand_normal();
  }

private:
  //max length
  int x;
  boost::mt19937 rng;
  boost::uniform_int<int> uniform_normal;
  boost::variate_generator<boost::mt19937, boost::uniform_int<int> > rand_normal;

};

namespace uqam_doc
{

//typedef map< string, float > Dist1D;

//typedef map< string, map< string, string > >  data;
//data[ "hey" ][ "yo" ] = "whassup";

//typedef vector<int> Int1D;
//typedef std::vector<Int1D> Int2D;

class QFuncHgt
{
public:

	//aplication data
	ApplicationData* appl_data_;

	//configurations should be made elswere
	//int MEM_ALIGN = 16;
	//int MEM_PAD = 16;
	//int CACHE_LINE = 64;

	//members
	size_t align_len;
	float align_len_reciprocal;

	int nb_mutations;
	int nb_repl_;

	int nb_seqs;
	int nb_groups;

	//column number generator
	// Initialise Boost random numbers, uniform integers from min to max

	//groups to sequences in global space
	//internal vector order by locseqidx
	vector< vector<int> > grpidx_seqidx_;
	//groups and info
	vector<int> grpidx_grpid_;

	//like AssocVector
	container::flat_map<string,int> seqid_seqidx_;

	//ordered by seqidx
	//search it for msa layout
	vector<string> seqidx_seqid_;

	////read only - no false sharing
	AlignedStorage<char>* msa_st_;

	//replicate MSA
    AlignedStorage<char>* msa_rp_;

    //Grouped Work Dist Matrix
	//array_type2d* pcell_2d;
	multi_array<AlignedStorage<float>*, 2>* work_groups_;
    //linear distance matrix
	multi_array<float, 2>* dist_mtx_;

	//Group (REST) values
	//array_type2d* rest_groups_;
	multi_array<AlignedStorage<float>*, 2>* rest_groups_;

	//inter/intra group average distance
	//simple implementation
	multi_array<float, 2>* ig_dist_;

	//TRSF flat aligned array of nb_seqs x nb_seqs
	AlignedStorage<float>* trsfs_;

	//Replicate transfers
	AlignedStorage<float>* repls_;

	//PVALS
	AlignedStorage<float>* pvals_;

	//
	rand_int* align_rand_int_;

	//default destructor

	virtual ~QFuncHgt();

	//custom constructors
	QFuncHgt(ApplicationData* appl_data);

	//methods

	void allocBuffers();

	void initBuffers();

	void deallocBuffers();

	void readGrSeqsCsv();

	void readMsaFasta();

	void calcWorkDistMatrix(AlignedStorage<char>* msa_st,
             multi_array<AlignedStorage<float>*, 2>* work_groups,
             multi_array<AlignedStorage<float>*, 2>* rest_groups);

	void calcTrsfs(multi_array<AlignedStorage<float>*, 2>* work_groups,
	               multi_array<AlignedStorage<float>*, 2>* rest_groups,
						AlignedStorage<float>* trsfs);

	void calcScore();

	void calcRepl();

	void mutateMsa();

	void calcVals();

	void calcPvals();

	void calcHgts();

	void calculate();

	void displayData();

	void displayMsaData(AlignedStorage<char>* msa);

	void displayWorkDistMatrix(multi_array<AlignedStorage<float>*, 2>* work_groups, int rowGroup, int colGroup);

	void displayRestVector(multi_array<AlignedStorage<float>*, 2>* rest_groups,int rowGroup, int colGroup);

    void displayTrsfs(AlignedStorage<float>* trsfs);

	void fileTrsfs();

	void fileDistMatrix();

	//members

private:
};

}

#endif	/* _Q_FUNC_HGT_H */


// TODO: create info to this file

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Header.h"
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h>
#include <Eigen/StdVector>
#include <Eigen/StdDeque>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "tf_writer/AprilTagDetection.h"
#include <iostream>
#include <fstream>

//void tagCallback(const tf_writer::AprilTagDetectionArray& msg) {
////    if(msg.detections[0==19){
//        ROS_INFO("Yes cam to 19");
////    }
//}

int main(int argc, char **argv)
{
  //Eigen Variable
  Eigen::Translation<double,3> pos_cam_tag;
  Eigen::Quaterniond rot_cam_tag;
  Eigen::Transform<double,3,Eigen::Affine> pose_cam_tag;

//  tf_writer::AprilTagDetectionArray msg_tagpose;

  ros::init(argc, argv, "tfwriter_node");
  ros::NodeHandle nh;

//  tf2_ros::Buffer tfBuffer;
//  tf2_ros::TransformListener tfListener(tfBuffer);
//  ros::Rate rate(10.0);
  std::ofstream myfile;

  myfile.open ("groundtruth.txt");
  myfile << "#  Timestamps tx, ty, tz, qx, qy, qz, qw\n";
//  ros::Subscriber posesubs = nh.subscribe("tag_detections",10,tagCallback);

//  while (nh.ok()) {

////      geometry_msgs::TransformStamped tfstamped;
////      try {
////          //get tf camera to tag 15
////          tfstamped = tfBuffer.lookupTransform("camera",
////          "tag_19", ros::Time(0));

//          pos_cam_tag.x()=tfstamped.transform.translation.x;
//          pos_cam_tag.y()=tfstamped.transform.translation.y;
//          pos_cam_tag.z()=tfstamped.transform.translation.z;
//          rot_cam_tag.w() = tfstamped.transform.rotation.w;
//          rot_cam_tag.vec() << tfstamped.transform.rotation.x,
//                  tfstamped.transform.rotation.y,
//                  tfstamped.transform.rotation.z;
//          long int timestamp_sec = tfstamped.header.stamp.sec;
//          long int timestamp_nsec = tfstamped.header.stamp.nsec;

////          ROS_INFO_STREAM(tfstamped.transform.translation.x<<" "<<tfstamped.transform.translation.y<<" "<<tfstamped.transform.translation.z);

//          //Get 4x4 transform
//          pose_cam_tag = pos_cam_tag * rot_cam_tag;
//          //Inverse to get tag to camera
//          pose_cam_tag = pose_cam_tag.inverse();
//          Eigen::Quaterniond rot_tag_cam(pose_cam_tag.rotation());

//          myfile << timestamp_sec <<"" << timestamp_nsec << " "
//                 << pose_cam_tag.translation()[0] << " " << pose_cam_tag.translation()[1] << " " << pose_cam_tag.translation()[2] << " "
//                 << rot_tag_cam.x() << " " << rot_tag_cam.y() << " " << rot_tag_cam.z() << " " << rot_tag_cam.w()
//                 << "\n";

//          timecounter++;

//          }
//      catch (tf2::TransformException &exception) {
//          ROS_WARN("%s", exception.what());
//          ros::Duration(1.0).sleep();
//          continue;
//      }
//      rate.sleep();
//  }
  ros::spin();
  myfile.close();
  return 0;
}


///
/// \file matrix_free_gemv.hpp
///
#ifndef MXPFIT_FITTING_MATRIX_FREE_GEMV_HPP
#define MXPFIT_FITTING_MATRIX_FREE_GEMV_HPP

#include <Eigen/Core>
#include <Eigen/SparseCore> // for sparse product interface

namespace mxpfit
{
///
/// ### OperationType
///
/// Flags describing the form of matrix operator
///
///  | Variable     | Form of operator                       |
///  |:------------ |:-------------------------------------- |
///  | `NoTranspose`| \f$ A \f$                              |
///  | `Transposed` | transpose \f$ A^{T} \f$                |
///  | `Conjugate`  | complex conjugate \f$ \overline{A} \f$ |
///  | `Adjoint`    | Hermitian conjugate \f$ A^{\ast} \f$   |
///
enum OperationType : std::uint32_t
{
    NoTranspose = 0,
    Transposed  = 1,
    Conjugate   = 1 << 1u,
    Adjoint     = Transposed | Conjugate
};

template <typename MatrixType, std::uint32_t Trans = NoTranspose>
class MatrixFreeGEMV;

} // namespace mxpfit

namespace Eigen
{
namespace internal
{

///
/// Specialization of `Eigen::internal::traits` for `mxpfit::MatrixFreeGEMV`
///
template <typename MatrixType, std::uint32_t Trans>
struct traits<mxpfit::MatrixFreeGEMV<MatrixType, Trans>>
{
    using Scalar       = typename MatrixType::Scalar;
    using StorageIndex = Index;
    using StorageKind  = Sparse;
    using XprKind      = MatrixXpr;
    enum
    {
        RowsAtCompileTime    = Dynamic,
        ColsAtCompileTime    = Dynamic,
        MaxRowsAtCompileTime = Dynamic,
        MaxColsAtCompileTime = Dynamic,
        Flags                = 0u // unused(?)
    };
};

} // namespace internal
} // namespace Eigen

namespace mxpfit
{
///
/// Generic wrapper of matrix-vector product for Einge matrix-free solver
///
/// \tparam MatrixType  A type of matrix operator to be wrapped
/// \tparam Trans  A parameter of type `mxpfit::OperationType` specifying
///     the operator form
///
/// The template parameter `MatrixType` must have `Scalar` and `RealScalar`
/// typedefs, and special member functions with the following signature,
///
/// ``` c++
///   /* Compute `dst += alpha * A * rhs` */
///   template <typename Dest, typename RHS>
///   void apply(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
///              Scalar alpha) const;
///
///   /* Compute `dst += alpha * A.conjugate() * rhs */
///   template <typename Dest, typename RHS>
///   void applyConjugate(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
///                       Scalar alpha) const;
///
///   /* Compute `dst += alpha * A.transpose() * rhs` */
///   template <typename Dest, typename RHS>
///   void applyTranspose(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
///                       Scalar alpha) const;
///
///   /* Compute `dst += alpha * A.transpose() * rhs` */
///   template <typename Dest, typename RHS>
///   void applyAdjoint(Dest& dst, const Eigen::MatrixBase<RHS>& rhs,
///                     Scalar alpha) const;
/// ```
///
/// where `Dest` and `RHS` are the expressions for target and destination
/// vectors.
///
template <typename MatrixType, std::uint32_t Trans>
class MatrixFreeGEMV
    : public Eigen::EigenBase<MatrixFreeGEMV<MatrixType, Trans>>
{
public:
    using Scalar       = typename MatrixType::Scalar;
    using RealScalar   = typename MatrixType::RealScalar;
    using StorageIndex = Eigen::Index;
    using Index        = StorageIndex;

    using TransposeReturnType =
        MatrixFreeGEMV<MatrixType, (Trans ^ Transposed)>;

    using ConjugateReturnType = MatrixFreeGEMV<MatrixType, (Trans ^ Conjugate)>;

    using AdjointReturnType = MatrixFreeGEMV<MatrixType, (Trans ^ Adjoint)>;

    enum
    {
        ColsAtCompileTime    = Eigen::Dynamic,
        MaxColsAtCompileTime = Eigen::Dynamic,
        IsRowMajor           = false
    };

    MatrixFreeGEMV() : m_matrix(nullptr)
    {
    }

    MatrixFreeGEMV(const MatrixType& mat) : m_matrix(&mat)
    {
    }

    MatrixFreeGEMV(const MatrixFreeGEMV& other) : m_matrix(other.m_matrix)
    {
    }

    ~MatrixFreeGEMV()
    {
    }

    /// \return  The number of rows of matrix `A`
    Index rows() const
    {
        return (Trans & Transposed) ? m_matrix->cols() : m_matrix->rows();
    }

    /// \return  The number of columns of matrix `A`
    Index cols() const
    {
        return (Trans & Transposed) ? m_matrix->rows() : m_matrix->cols();
    }

    ///
    /// Overlaoded `operator*` with Eigen dense column vector
    ///
    template <typename Rhs>
    Eigen::Product<MatrixFreeGEMV, Rhs, Eigen::AliasFreeProduct>
    operator*(const Eigen::MatrixBase<Rhs>& x) const
    {
        using ResultType =
            Eigen::Product<MatrixFreeGEMV, Rhs, Eigen::AliasFreeProduct>;
        return ResultType(*this, x.derived());
    }
    ///
    /// Set new matrix-like operator
    ///
    void attachMatrix(MatrixType& mat)
    {
        m_matrix = &mat;
    }
    ///
    /// Get the reference to the attached matrix.
    ///
    const MatrixType& nestedExpression() const
    {
        return *m_matrix;
    }
    ///
    /// Retrun expression to the transposed matrix operator.
    ///
    TransposeReturnType transpose() const
    {
        return TransposeReturnType(*m_matrix);
    }
    ///
    /// Retrun expression to the complex conjugate of matrix operator.
    ///
    ConjugateReturnType conjugate() const
    {
        return ConjugateReturnType(*m_matrix);
    }
    ///
    /// Retrun expression to the adjoint of matrix operator.
    ///
    AdjointReturnType adjoint() const
    {
        return AdjointReturnType(*m_matrix);
    }

private:
    const MatrixType* m_matrix;
};

} // namespace mxpfit

namespace Eigen
{
namespace internal
{

template <typename MatrixType, typename Rhs>
struct generic_product_impl<
    mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::NoTranspose>, Rhs, SparseShape,
    DenseShape, GemvProduct>
    : generic_product_impl_base<
          mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::NoTranspose>, Rhs,
          generic_product_impl<
              mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::NoTranspose>, Rhs>>
{
    using Lhs    = mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::NoTranspose>;
    using Scalar = typename Product<Lhs, Rhs>::Scalar;
    template <typename Dest>
    static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs,
                              const Scalar& alpha)
    {
        lhs.nestedExpression().apply(dst, rhs, alpha);
    }
};

template <typename MatrixType, typename Rhs>
struct generic_product_impl<
    mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Transposed>, Rhs, SparseShape,
    DenseShape, GemvProduct>
    : generic_product_impl_base<
          mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Transposed>, Rhs,
          generic_product_impl<
              mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Transposed>, Rhs>>
{
    using Lhs    = mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Transposed>;
    using Scalar = typename Product<Lhs, Rhs>::Scalar;
    template <typename Dest>
    static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs,
                              const Scalar& alpha)
    {
        lhs.nestedExpression().applyTranspose(dst, rhs, alpha);
    }
};

template <typename MatrixType, typename Rhs>
struct generic_product_impl<
    mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Conjugate>, Rhs, SparseShape,
    DenseShape, GemvProduct>
    : generic_product_impl_base<
          mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Conjugate>, Rhs,
          generic_product_impl<
              mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Conjugate>, Rhs>>
{
    using Lhs    = mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Conjugate>;
    using Scalar = typename Product<Lhs, Rhs>::Scalar;
    template <typename Dest>
    static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs,
                              const Scalar& alpha)
    {
        lhs.nestedExpression().applyConjugate(dst, rhs, alpha);
    }
};

template <typename MatrixType, typename Rhs>
struct generic_product_impl<mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Adjoint>,
                            Rhs, SparseShape, DenseShape, GemvProduct>
    : generic_product_impl_base<
          mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Adjoint>, Rhs,
          generic_product_impl<
              mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Adjoint>, Rhs>>
{
    using Lhs    = mxpfit::MatrixFreeGEMV<MatrixType, mxpfit::Adjoint>;
    using Scalar = typename Product<Lhs, Rhs>::Scalar;
    template <typename Dest>
    static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs,
                              const Scalar& alpha)
    {
        lhs.nestedExpression().applyAdjoint(dst, rhs, alpha);
    }
};

} // namespace internal
} // namespace Eigen

#endif /* MXPFIT_FITTING_MATRIX_FREE_GEMV_HPP */


#ifndef __terark_bits_rotate_hpp__
#define __terark_bits_rotate_hpp__

#if defined(_MSC_VER)
// Seems Visual C++ didn't optimize rotate shift, so use intrinsics
#include <stdlib.h> // for rol/ror intrinsics
#include <intrin.h>
#endif

#include <cassert>

#include <boost/static_assert.hpp>
#include <boost/type_traits/is_unsigned.hpp>

namespace terark {

#if defined(_MSC_VER)
inline unsigned __int64
msc_rotate_left(unsigned __int64 val, int c) { return _rotl64(val, c); }
inline unsigned int
msc_rotate_left(unsigned   int   val, int c) { return _rotl  (val, c); }
inline unsigned short
msc_rotate_left(unsigned short   val, int c) { return _rotl16(val, c); }
inline unsigned char
msc_rotate_left(unsigned  char   val, int c) { return _rotl8 (val, c); }

inline unsigned __int64
msc_rotate_right(unsigned __int64 val, int c) { return _rotr64(val, c); }
inline unsigned int
msc_rotate_right(unsigned   int   val, int c) { return _rotr  (val, c); }
inline unsigned short
msc_rotate_right(unsigned short   val, int c) { return _rotr16(val, c); }
inline unsigned char
msc_rotate_right(unsigned  char   val, int c) { return _rotr8 (val, c); }

template<class Uint>
inline Uint BitsRotateLeft(Uint x, int c) {
	assert(c >= 0);
	assert(c < (int)sizeof(Uint)*8);
	BOOST_STATIC_ASSERT(boost::is_unsigned<Uint>::value);
	return msc_rotate_left(x, c);
}
template<class Uint>
inline Uint BitsRotateRight(Uint x, int c) {
	assert(c >= 0);
	assert(c < (int)sizeof(Uint)*8);
	BOOST_STATIC_ASSERT(boost::is_unsigned<Uint>::value);
	return msc_rotate_right(x, c);
}
#else
template<class Uint>
inline Uint BitsRotateLeft(Uint x, int c) {
	assert(c >= 0);
	assert(c < (int)sizeof(Uint)*8);
	BOOST_STATIC_ASSERT(boost::is_unsigned<Uint>::value);
	return x << c | x >> (sizeof(Uint)*8 - c);
}
template<class Uint>
inline Uint BitsRotateRight(Uint x, int c) {
	assert(c >= 0);
	assert(c < (int)sizeof(Uint)*8);
	BOOST_STATIC_ASSERT(boost::is_unsigned<Uint>::value);
	return x >> c | x << (sizeof(Uint)*8 - c);
}
#endif

} // namespace terark

#endif // __terark_bits_rotate_hpp__


//
// Created by xetql on 9/4/20.
//

#ifndef ORB_ORB_HPP
#define ORB_ORB_HPP

#include <algorithm>
#include <array>
#include <vector>
#include <cmath>
#include <mpi.h>
#include <boost/geometry.hpp>
#include <median.hpp>
#include <yalbb/parallel_utils.hpp>

namespace orb {
    using Real = float;
    namespace {
        template<int N, class T, class GetPosF>
        std::pair<std::array<Real, N>, std::array<Real, N>> get_dimension_size(std::vector<T> &p, GetPosF getPosition) {
            const size_t size = p.size();
            const size_t nparts = size / N;
            std::array<Real, N> max_by_dim;
            std::array<Real, N> min_by_dim;
            for (auto i = 0; i < N; ++i) {
                max_by_dim.at(i) = std::numeric_limits<Real>::lowest();
                min_by_dim.at(i) = std::numeric_limits<Real>::max();
            }
            for (size_t i = 0; i < nparts; ++i) {
                for (auto dim = 0; dim < N; ++dim) {
                    max_by_dim.at(dim) = std::max(max_by_dim.at(dim), getPosition(&p.at(i * N + dim))->at(dim));
                    min_by_dim.at(dim) = std::min(min_by_dim.at(dim), getPosition(&p.at(i * N + dim))->at(dim));
                }
            }
            return {max_by_dim, min_by_dim};
        }
    }

    template<int N> using Subdomain = std::array<Real, 2 * N>;

    template<int N>
    struct ORBBalancer {
        typedef boost::geometry::model::point<Real, N, boost::geometry::cs::cartesian> point_t;
        typedef boost::geometry::model::box<point_t> box_t;

        MPI_Datatype datatype;
        MPI_Comm comm;
        int nbProcessors;
        int myRank;
        std::vector<Subdomain<N>> partitions;

        explicit ORBBalancer(MPI_Datatype datatype, MPI_Comm comm = MPI_COMM_WORLD) : datatype(datatype), comm(comm) {
            MPI_Comm_size(comm, &nbProcessors);
            MPI_Comm_rank(comm, &myRank);
        }

        inline point_t array_to_point(const std::array<Real, N> &pos) {
            if constexpr(N == 3)
                return point_t(pos.at(0), pos.at(1), pos.at(2));
            else
                return point_t(pos.at(0), pos.at(1));
        }

        inline box_t domain_to_box(const Subdomain<N> &domain) {
            if constexpr(N == 3)
                return box_t(point_t(domain.at(0), domain.at(2), domain.at(3)),
                             point_t(domain.at(1), domain.at(3), domain.at(5)));
            else
                return box_t(point_t(domain.at(0), domain.at(2)), point_t(domain.at(1), domain.at(3)));
        }

        void lookup_domain(const point_t &p, int *PE) {
            for ((*PE) = partitions.size() - 1; (*PE) > -1; (*PE)--) {
                if (boost::geometry::covered_by(p, domain_to_box(partitions.at((*PE))))) return;
            }
        }

        void lookup_domain(std::array<Real, N> *pos, int *PE) {
            auto point = array_to_point(*pos);
            lookup_domain(point, PE);
        }

        void lookup_domain(const std::array<Real, N> &pos, int *PE) {
            auto point = array_to_point(pos);
            lookup_domain(point, PE);
        }

        void lookup_domain(const std::array<Real, N> &&pos, int *PE) {
            auto point = array_to_point(pos);
            lookup_domain(point, PE);
        }

        void lookup_domain(Real x, Real y, Real z, int *PE) {
            if constexpr(N == 3)
                lookup_domain(point_t(x, y, z), PE);
            else
                lookup_domain(point_t(x, y), PE);
        }

        template<class T, class GetPosF>
        void lookup_domain(T *p, int *PE, GetPosF getPosition) {
            lookup_domain(getPosition(p), PE);
        }

        std::vector<int> get_neighbors(int PE, double min_distance) {
            int worldsize;
            std::vector<int> neighbors;
            MPI_Comm_size(comm, &worldsize);
            box_t targeted_box = domain_to_box(partitions.at(PE));
            for (int i = 0; i < worldsize; ++i) {
                box_t current_box = domain_to_box(partitions.at(i));
                if (PE != i && std::abs(boost::geometry::distance(targeted_box, current_box)) < min_distance) {
                    neighbors.push_back(i);
                }
            }
            return neighbors;
        }
    };

    template<int N>
    ORBBalancer<N>* ORBBalancer_create_ptr_from(ORBBalancer<N>* orb){
        auto *cpy_orb = new ORBBalancer<N>(orb->datatype, orb->comm);
        cpy_orb->nbProcessors = orb->nbProcessors;
        cpy_orb->myRank = orb->myRank;
        cpy_orb->partitions = orb->partitions;
        return cpy_orb;
    }

    template<int N>
    void ORBBalancer_destroy(ORBBalancer<N>* orb) {
        delete orb;
    }

    template<int N, class T, class GetPosF, class MigrationFunction>
    void parallel_orb(ORBBalancer<N> &lb, std::vector <T> &elements, GetPosF getPosition, MigrationFunction do_migration) {
        auto datatype = lb.datatype;
        auto comm = lb.comm;
        const size_t size = elements.size();
        const size_t nparts = size / N;
        auto ncuts = std::log(lb.nbProcessors) / std::log(2);
        Subdomain<N> box;
        for (int dim = 0; dim < N; ++dim) {
            box.at(2 * dim) = std::numeric_limits<Real>::lowest();
            box.at(2 * dim + 1) = std::numeric_limits<Real>::max();
        }
        std::vector<Subdomain<N> > domains;
        std::vector<std::vector<T> > domains_data{std::move(elements)};
        domains.emplace_back(box);
        while (ncuts--) {
            std::vector<Subdomain<N> > subdomains;
            subdomains.reserve(2 * domains.size());
            std::vector<std::vector<T> > subdomains_data;
            subdomains_data.reserve(2 * domains_data.size());
            //for(const auto& d : domains) {
            const size_t nb_domains = domains.size();
            for (auto id = 0; id < nb_domains; ++id) {
                auto &d = domains.at(id);
                const auto &parent_boundaries = domains.at(id);
                std::vector<T> &elements = domains_data.at(id);
                // Get the largest dimension for current domain
                auto[maxdim, mindim] = get_dimension_size<N, T, GetPosF>(elements, getPosition);
                MPI_Allreduce(MPI_IN_PLACE, maxdim.data(), N, par::get_mpi_type<Real>(), MPI_MAX, MPI_COMM_WORLD);
                MPI_Allreduce(MPI_IN_PLACE, mindim.data(), N, par::get_mpi_type<Real>(), MPI_MIN, MPI_COMM_WORLD);
                for (auto i = 0; i < N; ++i) maxdim.at(i) -= mindim.at(i);
                auto largest_dim = std::distance(begin(maxdim), std::max_element(begin(maxdim), end(maxdim)));
                std::vector<Real> dim_position(elements.size());
                std::transform(elements.begin(), elements.end(), dim_position.begin(),
                               [getPosition, largest_dim](auto &e) { return getPosition(&e)->at(largest_dim); });
                // get the median
                auto median = par::median(dim_position);
                // divide the dataset in two parts: above and below median
                std::array<Real, 2 * N> above_boundaries = parent_boundaries, below_boundaries = parent_boundaries;
                above_boundaries.at(2 * largest_dim) = median;
                below_boundaries.at(2 * largest_dim + 1) = median;
                subdomains.emplace_back(below_boundaries);
                subdomains.emplace_back(above_boundaries);
                auto median_it = std::partition(elements.begin(), elements.end(),
                                                [getPosition, largest_dim, median](auto &e) {
                                                    return getPosition(&e)->at(largest_dim) <= median;
                                                });
                subdomains_data.emplace_back(std::begin(elements), median_it);
                subdomains_data.emplace_back(median_it, std::end(elements));
            }
            domains = std::move(subdomains);
            domains_data = std::move(subdomains_data);
        }
        do_migration(0, elements, domains_data, datatype, comm);
        lb.partitions = domains;
    }

}
#endif //ORB_ORB_HPP


/*
    Copyright (C) 2009 Wei Dong <wdong@princeton.edu>. All Rights Reserved.

    This file is part of LSHKIT.

    LSHKIT is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    LSHKIT is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with LSHKIT.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <cassert>
#include <queue>
#include <boost/progress.hpp>
#include <boost/format.hpp>
#include <idmlib/lshkit/apost.h>

#include "gsl_util.h"

namespace lshkit
{

// Gaussian model of nearest neighbor hash components
// this is the gaussian model for one training query point
struct ExampleModel {
    std::vector<float> H;
    std::vector<float> mean;
    std::vector<float> var;

    void estimate (const APostLsh &lsh, const APostExample &example)
    {
#if 0
        // calculate H values
        const float *query = example.query;
        H.resize(lsh.M);
        lsh.apply1(query, &H);

        // mean & cov matrix of examples
        const std::vector<const float *> &results = example.results;
        std::vector<float> M;
        std::vector<std::vector<float> > cov; // lower matrix
        unsigned dim = lsh.dim;

        BOOST_VERIFY(results.size() > 1);
        // calculate mean of NNs
        M.resize(dim);
        std::fill(M.begin(), M.end(), 0.0);
        BOOST_FOREACH(const float *v, results) {
            for (unsigned i = 0; i < dim; ++i) {
                M[i] += v[i];
            }
        }
        for (unsigned i = 0; i < dim; ++i) {
            M[i] /= results.size();
        }

        // calculate cov of NNs
        cov.resize(dim);
        for (unsigned i = 0; i < dim; i++) {
            cov[i].resize(i+1);
            std::fill(cov[i].begin(), cov[i].end(), 0.0);
        }

        BOOST_FOREACH(const float *v, results) {
            for (unsigned i = 0; i < dim; i++) {
                for (unsigned j = 0; j <= i; j++) {
                    cov[i][j] += (v[i] - M[i]) * (v[j] - M[j]);
                }
            }
        }

        for (unsigned i = 0; i < dim; i++) {
            for (unsigned j = 0; j <= i; j++) {
                cov[i][j] /= (results.size() - 1);
            }
        }

        // mean & std of hash values
        mean.resize(lsh.M);
        lsh.apply1(&M[0], &mean);

        var.resize(lsh.M);
        for (unsigned i = 0; i < lsh.M; i++) { // for each hash component
            var[i] = 0;
            for (unsigned ii = 0; ii < lsh.dim; ii++) {
                unsigned jj;
                for (jj = 0; jj <= ii; jj++) {
                    var[i] += lsh.a[i][ii] * cov[ii][jj] * lsh.a[i][jj];
                }
                for (; jj < lsh.dim; jj++) {
                    var[i] += lsh.a[i][ii] * cov[jj][ii] * lsh.a[i][jj];
                }
            }
            var[i] /= sqr(lsh.W);
        }
#else   // equivalent but simpler method: directly calculate the mean & var of h values

        // calculate H values
        const float *query = example.query;
        H.resize(lsh.M);
        lsh.apply1(query, &H);

        // mean & cov matrix of examples
        mean.resize(lsh.M);
        var.resize(lsh.M);
        const std::vector<const float *> &results = example.results;

        for (unsigned i = 0; i < lsh.M; ++i) {
            float sum = 0;
            float sum2 = 0;
            BOOST_FOREACH(const float *v, results) {
                float h = lsh.apply1(v, i);
                sum += h;
                sum2 += sqr(h);
            }
            mean[i] = sum / results.size();
            var[i] = (sum2 - mean[i] * sum)/(results.size() - 1);
        }
#endif
#ifdef DEBUGGING
        for (unsigned i = 0; i < lsh.M; ++i) {
            std::cout << H[i] << ':' << mean[i] << ':' << var[i] << std::endl;
        }
#endif
    }
};

class GaussianHashModel
{
    std::vector<ExampleModel> M;
    float sigma;
public:
    GaussianHashModel (const APostLsh &lsh, const std::vector<APostExample> &examples, float s = 0.0) {
        if (s == 0.0) {
            s = 1.0 / 5;
            // I believe there's a bug in paper.
            // Right below equation 16, the paper says the default
            // sigma is W/5.  However,
            // the h values are already divided by W (h = (a v + b) / W)
            // and it makes no sense to divide it by W again.
        }
        sigma = s;

        M.resize(examples.size());
        for (unsigned i = 0; i < examples.size(); ++i) {
            M[i].estimate(lsh, examples[i]);
        }
    }

    void estimate (unsigned m, float h, float *mean, float *std) { // mth hash component
        float mm = 0, vv = 0, ss = 0;
        for (unsigned i = 0; i < M.size(); i++) { // for each example
            float k = gsl_ran_gaussian_pdf(M[i].H[m] - h, sigma);
            mm += k * M[i].mean[m];
            vv += k * M[i].var[m];
            ss += k;
        }
        mm /= ss;
        vv /= ss;
        *mean = mm;
        *std = std::sqrt(vv);
    }
};

static inline float GaussianInterval (float mean, float std, float l, float u) {
    return gsl_cdf_gaussian_P(u - mean, std)
        - gsl_cdf_gaussian_P(l - mean, std);
}

void APostModel::train (const APostLsh &lsh,
                        const std::vector<APostExample> &examples,
                        unsigned N, float k_sigma, float expand) {
    Nz = N;
    ex = expand;
    GaussianHashModel parzen(lsh, examples, k_sigma);

    umin.resize(lsh.M);
    umax.resize(lsh.M);
    lookup.resize(lsh.M);
    means.resize(lsh.M);
    stds.resize(lsh.M);
    // for each component h
    std::cout << "Training LSH table." << std::endl;
    boost::progress_display progress(lsh.M * Nz);
    for (unsigned m = 0; m < lsh.M; ++m) {
        {
            float ex = expand * (lsh.umax[m] - lsh.umin[m]);
            umin[m] = lsh.umin[m] - ex;
            umax[m] = lsh.umax[m] + ex;
        }

        float delta = (umax[m] - umin[m]) / Nz;

        lookup[m].resize(Nz);
        means[m].resize(Nz);
        stds[m].resize(Nz);
        // for each quantum of h(q)
        for (unsigned n = 0; n < Nz; ++n) {
            float mean, std;
            parzen.estimate(m, umin[m] + (n + 0.5) * delta, &mean, &std);
            means[m][n] = mean;
            stds[m][n] = std;
            std::vector<PrH> &lmn = lookup[m][n];
#if 0    // this is the original method
            // a losts of h values will have very small probability and
            // those are essentially not useful
            int minh(std::floor(umin[m]));
            int maxh(std::floor(umax[m])); // min & max inclusive
            unsigned size = maxh - minh + 1;
            BOOST_VERIFY(size >= 1);
            lmn.resize(size);
            for (int h = minh; h <= maxh; ++h) {
                lmn[h].h = h;
                lmn[h].pr = GaussianInterval(mean, std, float(h), float(h + 1));
            }
#else       // here we only generate those h values with reasonably large probability
            {
                const static float THRESHOLD = 1e-7;
                int h0(std::floor(mean));
                for (int h = h0; ; h++) {
                    PrH prh;
                    prh.h = h;
                    prh.pr = GaussianInterval(mean, std, float(h), float(h+1));
                    if (prh.pr < THRESHOLD) break;
                    lmn.push_back(prh);

                }
                for (int h = h0 - 1; ; h--) {
                    PrH prh;
                    prh.h = h;
                    prh.pr = GaussianInterval(mean, std, float(h), float(h+1));
                    if (prh.pr < THRESHOLD) break;
                    lmn.push_back(prh);

                }
                BOOST_VERIFY(lmn.size() > 0);
            }

#endif
            std::sort(lmn.begin(), lmn.end());
            ++progress;
        }
    }
}

struct PrC {
    unsigned m;
    const std::vector<PrH> *prh;
    friend bool operator < (const PrC &c1, const PrC &c2) {
        if (c1.prh->size() <= 1) return false;
        if (c2.prh->size() <= 1) return true;
        return c1.prh->at(1).pr > c2.prh->at(1).pr;
    }
};

struct Probe {
    std::vector<unsigned> off;
    unsigned last;
    float pr;
    const std::vector<unsigned> *range;

    Probe () {
        BOOST_VERIFY(0);
    }

    Probe (const std::vector<unsigned> *range_):
        off(range_->size()),
        last(0),
        range(range_)
    {
        std::fill(off.begin(), off.end(), 0);
    }

    bool canShift () const {
        if (off[last] != 1) return false;
        if (last + 1 >= off.size()) return false;
        if (range->at(last + 1) <= 1) return false;
        return true;
    }

    void shift () {
        off[last] = 0;
        last++;
        off[last] = 1;
    }

    bool canExpand () const {
        if (last + 1 >= off.size()) return false;
        if (range->at(last + 1) <= 1) return false;
        return true;
    }

    void expand () {
        last++;
        off[last] = 1;
    }

    bool canExtend () const {
        if (off[last] + 1 >= range->at(last)) return false;
        return true;
    }

    void extend () {
        off[last]++;
    }

    friend bool operator < (const Probe &p1, const Probe &p2) {
        return p1.pr < p2.pr;
    }

    void setPr (const std::vector<PrC> &pl) {
        pr = 1.0;
        for (unsigned i = 0; i < off.size(); ++i) {
            pr *= pl[i].prh->at(off[i]).pr;
        }
//        std::cerr << "PR = " << pr << std::endl;
    }

    unsigned hash (const APostLsh &lsh,
                    const std::vector<PrC> &pl) {
        unsigned r = 0;
        for (unsigned i = 0; i < lsh.M; ++i) {
            r += lsh.c[pl[i].m] * unsigned(pl[i].prh->at(off[i]).h);
        }
        return r % lsh.H;
    }

#ifdef DEBUGGING
    unsigned print (const std::vector<PrC> &pl) {
        BOOST_FOREACH(unsigned v, off) {
            std::cout << ' ' << v;
        }
        std::cout << std::endl;
    }
#endif
};

void APostModel::genProbeSequence (const APostLsh &lsh,
                                APostLsh::Domain query,
                                float recall, unsigned T,
                                std::vector<unsigned> *seq) const
{
#ifdef DEBUGGING
    std::cout << "Range of each hash component:" << std::endl;
    for (unsigned i = 0; i < lsh.M; ++i) {
        std::cout << boost::format("m = %1%: %2%, %3%") % i % umin[i] % umax[i] << std::endl;
    }
    std::cout << "Data distribution on each hash component:" << std::endl;
#endif
    std::vector<PrC> pl(lsh.M);
    for (unsigned i = 0; i < lsh.M; ++i) {
        pl[i].m = i;

        float h = lsh.apply1(query, i);

        if (h < umin[i]) {
            std::cerr << "hash[" << i << "] out of range " << h << " < umin = " << umin[i] << std::endl;
            h = umin[i];
        }
        else if (h > umax[i]) {
            std::cerr << "hash[" << i << "] out of range " << h << " > umax = " << umax[i] << std::endl;
            h = umax[i];
        }

        unsigned qh = (h - umin[i]) * Nz / ( umax[i] - umin[i]);
        BOOST_VERIFY(qh < lookup[i].size());

        pl[i].prh = &lookup[i][qh];

#ifdef DEBUGGING

        std::cout << boost::format("m = %1%: h = %2% mean = %3% std= %4%")
                                    % i % h % means[i][qh] % stds[i][qh] << std::endl;
#endif
    }

    std::sort(pl.begin(), pl.end());
#ifdef DEBUGGING
    std::cout << "Sorted probability of windows on each component:" << std::endl;

    BOOST_FOREACH(const PrC &prc, pl) {
        std::cout << boost::format("m = %1%:") % prc.m;
        BOOST_FOREACH(PrH prh, *prc.prh) {
            if (prh.pr == 0) break;
            std::cout << boost::format(" %1%@h=%2%") % prh.pr % prh.h;
        }
        std::cout << std::endl;
    }

    std::cout << "Probe sequence" << std::endl;
#endif
    // generate probe sequence
    seq->clear();

    std::vector<unsigned> range(pl.size());
    for (unsigned i = 0; i < range.size(); ++i) {
        range[i] = pl[i].prh->size();
    }

    std::vector<Probe> heap;
    heap.push_back(Probe(&range));

    heap.back().setPr(pl);
    float pr = heap.back().pr;
    seq->push_back(heap.back().hash(lsh, pl));

    if (pr >= recall) return;
    if (seq->size() >= T) return;

    heap.back().off[0] = 1;
    heap.back().setPr(pl);

    for (;;) {
        if (pr >= recall) break;
        if (seq->size() >= T) break;
        if (heap.empty()) break;

        pop_heap(heap.begin(), heap.end());
        Probe p = heap.back();
        heap.pop_back();

        seq->push_back(p.hash(lsh, pl));
        pr += p.pr;
#ifdef DEBUGGING
        std::cout << pr << ", " << p.pr << ":";
        p.print(pl);
#endif

        if (p.canShift()) {
            heap.push_back(p);
            heap.back().shift();
            heap.back().setPr(pl);
            push_heap(heap.begin(), heap.end());
        }

        if (p.canExpand()) {
            heap.push_back(p);
            heap.back().expand();
            heap.back().setPr(pl);
            push_heap(heap.begin(), heap.end());
        }

        if (p.canExtend()) {
            heap.push_back(p);
            heap.back().extend();
            heap.back().setPr(pl);
            push_heap(heap.begin(), heap.end());
        }
    }
#ifdef DEBUGGING
        std::cout << "DONE" << std::endl;
#endif
}

}


#pragma once

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationAlgorithm/populationBasedOptimisationAlgorithm.hpp"
namespace mant {
  class OptimisationProblem;
}

namespace mant {
  class ParticleSwarmOptimisation : public PopulationBasedOptimisationAlgorithm {
   public:
    explicit ParticleSwarmOptimisation();

    using PopulationBasedOptimisationAlgorithm::optimise;

    void optimise(
        OptimisationProblem& optimisationProblem);

    arma::uword getNumberOfParticles() const;

    void setMaximalAcceleration(
        const double maximalAcceleration);
    double getMaximalAcceleration() const;

    void setMaximalLocalAttraction(
        const double maximalLocalAttraction);
    double getMaximalLocalAttraction() const;

    void setMaximalGlobalAttraction(
        const double maximalGlobalAttraction);
    double getMaximalGlobalAttraction() const;

    // Public accessible to be used in lambdas
    arma::mat velocities_;

    arma::mat localBestSolutions_;
    arma::vec localBestObjectiveValues_;

   protected:
    arma::uword numberOfParticles_;

    double maximalAcceleration_;
    double maximalLocalAttraction_;
    double maximalGlobalAttraction_;
  };
}


#include <Eigen/Core>

#include <Pita/Program.hpp>
#include <Pita/Parser.hpp>
#include <Pita/Vectorizer.hpp>
#include <Pita/Printer.hpp>

std::ofstream ofs;
bool calculating;
int constraintViolationCount;

namespace cgl
{
	boost::optional<Expr> Program::parse(const std::string& program)
	{
		using namespace cgl;

		boost::u8_to_u32_iterator<std::string::const_iterator> tbegin(program.begin()), tend(program.end());

		Lines lines;

		SpaceSkipper<IteratorT> skipper;
		Parser<IteratorT, SpaceSkipperT> grammer;

		auto it = tbegin;
		if (!boost::spirit::qi::phrase_parse(it, tend, grammer, skipper, lines))
		{
			//std::cerr << "Syntax Error: parse failed\n";
			std::cout << "Syntax Error: parse failed\n";
			return boost::none;
		}

		if (it != tend)
		{
			//std::cout << "Syntax Error: ramains input\n" << std::string(it, program.end());
			std::cout << "Syntax Error: ramains input\n";
			return boost::none;
		}

		Expr result = lines;
		return result;
	}

	boost::optional<Val> Program::execute(const std::string& program)
	{
		if (auto exprOpt = parse(program))
		{
			try
			{
				return pEnv->expand(boost::apply_visitor(evaluator, exprOpt.value()));
			}
			catch (const cgl::Exception& e)
			{
				std::cerr << "Exception: " << e.what() << std::endl;
			}
		}

		return boost::none;
	}

	bool Program::draw(const std::string& program, bool logOutput)
	{
		if (logOutput) std::cout << "parse..." << std::endl;

		if (auto exprOpt = parse(program))
		{
			try
			{
				if (logOutput)
				{
					std::cout << "parse succeeded" << std::endl;
					printExpr(exprOpt.value());
				}

				if (logOutput) std::cout << "execute..." << std::endl;
				const LRValue lrvalue = boost::apply_visitor(evaluator, exprOpt.value());
				const Val result = pEnv->expand(lrvalue);
				if (logOutput) std::cout << "execute succeeded" << std::endl;

				if (logOutput) std::cout << "output SVG..." << std::endl;
				OutputSVG(std::cout, result, pEnv);
				if (logOutput) std::cout << "output succeeded" << std::endl;
			}
			catch (const cgl::Exception& e)
			{
				std::cerr << "Exception: " << e.what() << std::endl;
			}
		}

		return false;
	}

	void Program::execute1(const std::string& program, const std::string& output_filename, bool logOutput)
	{
		clear();

		if (logOutput)
		{
			std::cerr << "parse..." << std::endl;
			std::cerr << program << std::endl;
		}

		if (auto exprOpt = parse(program))
		{
			try
			{
				if (logOutput)
				{
					std::cerr << "parse succeeded" << std::endl;
					printExpr(exprOpt.value(), pEnv, std::cerr);
				}

				if (logOutput) std::cerr << "execute..." << std::endl;
				const LRValue lrvalue = boost::apply_visitor(evaluator, exprOpt.value());
				evaluated = pEnv->expand(lrvalue);
				if (logOutput)
				{
					std::cerr << "execute succeeded" << std::endl;
					//printVal(evaluated.value(), pEnv, std::cout, 0);
				}

				if (logOutput)
					std::cerr << "output SVG..." << std::endl;

				if (output_filename.empty())
				{
					OutputSVG2(std::cout, evaluated.value(), pEnv, "shape");
				}
				else
				{
					std::ofstream file(output_filename);
					OutputSVG2(file, evaluated.value(), pEnv, "shape");
					file.close();
				}

				if (logOutput)
				std::cerr << "completed" << std::endl;

				succeeded = true;
			}
			catch (const cgl::Exception& e)
			{
				//std::cerr << "Exception: " << e.what() << std::endl;
				std::cerr << "Exception: " << e.what() << std::endl;

				succeeded = false;
			}
		}
		else
		{
			succeeded = false;
		}

		calculating = false;
	}

	void Program::run(const std::string& program, bool logOutput)
	{
		clear();

		if (logOutput)
		{
			std::cout << "parse..." << std::endl;
			std::cout << program << std::endl;
		}

		if (auto exprOpt = parse(program))
		{
			try
			{
				if (logOutput)
				{
					std::cout << "parse succeeded" << std::endl;
					printExpr(exprOpt.value(), pEnv, std::cout);
				}

				if (logOutput) std::cout << "execute..." << std::endl;
				const LRValue lrvalue = boost::apply_visitor(evaluator, exprOpt.value());
				evaluated = pEnv->expand(lrvalue);
				if (logOutput) std::cout << "completed" << std::endl;

				succeeded = true;
			}
			catch (const cgl::Exception& e)
			{
				//std::cerr << "Exception: " << e.what() << std::endl;
				std::cout << "Exception: " << e.what() << std::endl;

				succeeded = false;
			}
		}
		else
		{
			succeeded = false;
		}

		calculating = false;
	}

	void Program::clear()
	{
		pEnv = Context::Make();
		evaluated = boost::none;
		evaluator = Eval(pEnv);
		succeeded = false;
	}

	bool Program::test(const std::string& program, const Expr& expr)
	{
		clear();

		if (auto result = execute(program))
		{
			std::shared_ptr<Context> pEnv2 = Context::Make();
			Eval evaluator2(pEnv2);

			const Val answer = pEnv->expand(boost::apply_visitor(evaluator2, expr));

			return IsEqualVal(result.value(), answer);
		}

		return false;
	}

	boost::optional<int> Program::asIntOpt()
	{
		if (evaluated)
		{
			if (auto opt = AsOpt<int>(evaluated.value()))
			{
				return opt.value();
			}
		}

		return boost::none;
	}

	boost::optional<double> Program::asDoubleOpt()
	{
		if (evaluated)
		{
			if (auto opt = AsOpt<double>(evaluated.value()))
			{
				return opt.value();
			}
		}

		return boost::none;
	}
}


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN

#include <boost/bind/bind.hpp>
using namespace boost::placeholders;

#include <boost/make_shared.hpp>
#include <boost/test/unit_test.hpp>

#include "tudat/astro/observation_models/lightTimeSolution.h"
#include "tudat/astro/observation_models/testLightTimeCorrections.h"

#include <limits>
#include <string>

#include <Eigen/Core>

#include "tudat/astro/basic_astro/physicalConstants.h"
#include "tudat/basics/testMacros.h"

#include "tudat/interface/spice/spiceEphemeris.h"
#include "tudat/interface/spice/spiceInterface.h"
#include "tudat/io/basicInputOutput.h"

namespace tudat
{
namespace unit_tests
{

using namespace ephemerides;
using namespace observation_models;
using namespace spice_interface;

BOOST_AUTO_TEST_SUITE( test_light_time )

//! Test light-time calculator.
BOOST_AUTO_TEST_CASE( testLightWithSpice )
{
    // Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Define names of bodies and frames.
    const std::string earth = "Earth";
    const std::string moon = "Moon";
    const std::string frame = "ECLIPJ2000";

    // Create ephemerides of Earth and Moon, with data from Spice.
    std::shared_ptr< SpiceEphemeris > earthEphemeris = std::make_shared< SpiceEphemeris >(
                earth, "SSB", false, false, false, frame );
    std::shared_ptr< SpiceEphemeris > moonEphemeris = std::make_shared< SpiceEphemeris >(
                moon, "SSB", false, false, false, frame );

    // Create light-time calculator, Earth center transmitter, Moon center receiver.
    std::shared_ptr< LightTimeCalculator< > > lightTimeEarthToMoon =
            std::make_shared< LightTimeCalculator< > >
            ( std::bind( &Ephemeris::getCartesianState, earthEphemeris, std::placeholders::_1 ),
              std::bind( &Ephemeris::getCartesianState, moonEphemeris, std::placeholders::_1 ) );

    // Define input time for tests.
    const double testTime = 1.0E6;

    // Define Spice output variables.
    double spiceOutputState[ 6 ] = { };
    double spiceMoonLightTime = 0.0;

    // Calculate observed (i.e. relative) position of Earth, and 'light time' at 'testTime' on
    // Moon, using spice. (Reception case with converged Newtonian light-time correction.)
    spkezr_c( earth.c_str( ), testTime, frame.c_str( ), std::string( "CN" ).c_str( ),
              moon.c_str( ), spiceOutputState, &spiceMoonLightTime );
    Eigen::Vector3d spiceMoonToEarthVector = Eigen::Vector3d::Zero( );
    for( int i = 0; i < 3; i++ )
    {
        // Convert from kilometers to meters.
        spiceMoonToEarthVector( i ) = -1000.0 * spiceOutputState[ i ];
    }

    // Calculate light time, with as input time the reception time, using light-time calculator.
    double testMoonLightTime = lightTimeEarthToMoon->calculateLightTime( testTime, true );
    BOOST_CHECK_CLOSE_FRACTION( testMoonLightTime, spiceMoonLightTime, 1.0E-9 );

    // Calculate relativeRange vector, with as input time the reception time, using light-time
    // calculator.
    const Eigen::Vector3d testMoonToEarthVector =
            lightTimeEarthToMoon->calculateRelativeRangeVector( testTime, true );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testMoonToEarthVector, spiceMoonToEarthVector, 1.0E-12 );

    // Calculate observed (i.e. relative) position of Earth, and 'light time' at
    // 'testTime+light time' on Moon, using spice. (Transmission case with converged Newtonian
    // light-time correction.)
    spkezr_c( moon.c_str( ), testTime, frame.c_str( ), std::string( "XCN" ).c_str( ),
              earth.c_str( ), spiceOutputState, &spiceMoonLightTime );
    Eigen::Vector3d spiceEarthToMoonVector = Eigen::Vector3d::Zero( );
    for( int i = 0; i < 3; i++ )
    {
        // Convert from kilometers to meters.
        spiceEarthToMoonVector( i ) = 1000.0 * spiceOutputState[ i ];
    }

    // Calculate light time, with as input time the transmission time, using light-time calculator.
    testMoonLightTime = lightTimeEarthToMoon->calculateLightTime( testTime, false );
    BOOST_CHECK_CLOSE_FRACTION( testMoonLightTime, spiceMoonLightTime, 1.0E-9 );

    // Calculate relativeRange vector, with as input time the transmission time, using light-time
    // calculator.
    const Eigen::Vector3d testEarthToMoonVector =
            lightTimeEarthToMoon->calculateRelativeRangeVector( testTime, false );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( testEarthToMoonVector, spiceEarthToMoonVector, 1.0E-10 );

    // Test light time and link end state functions.
    double testOutputTime = 0.0;
    Eigen::Vector6d testEarthState = Eigen::Vector6d::Zero( );
    Eigen::Vector6d testMoonState = Eigen::Vector6d::Zero( );
    Eigen::Vector6d spiceEarthState = Eigen::Vector6d::Zero( );
    Eigen::Vector6d spiceMoonState = Eigen::Vector6d::Zero( );

    // Get link end states, assuming input time is transmission time.
    // SSB = Solar system barycenter.
    testOutputTime = lightTimeEarthToMoon->calculateLightTimeWithLinkEndsStates(
                testMoonState, testEarthState, testTime, false );
    spiceEarthState = getBodyCartesianStateAtEpoch( earth, "SSB", "ECLIPJ2000", "NONE", testTime );
    spiceMoonState = getBodyCartesianStateAtEpoch(
                moon, "SSB", "ECLIPJ2000", "NONE", testTime + testOutputTime );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                spiceEarthState, testEarthState, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                spiceMoonState, testMoonState, std::numeric_limits< double >::epsilon( ) );

    // Get link end states, assuming input time is reception time.
    testOutputTime = lightTimeEarthToMoon->calculateLightTimeWithLinkEndsStates(
                testMoonState, testEarthState, testTime, true );
    spiceEarthState = getBodyCartesianStateAtEpoch( earth, "SSB", "ECLIPJ2000", "NONE",
                                                    testTime - testOutputTime );
    spiceMoonState = getBodyCartesianStateAtEpoch( moon, "SSB", "ECLIPJ2000", "NONE", testTime );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                spiceEarthState, testEarthState, std::numeric_limits< double >::epsilon( ) );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION(
                spiceMoonState, testMoonState, std::numeric_limits< double >::epsilon( ) );

    // Test light time with corrections.

    // Set single light-time correction function.
    std::vector< LightTimeCorrectionFunction > lightTimeCorrections;
    lightTimeCorrections.push_back( &getTimeDifferenceLightTimeCorrection );

    // Create light-time object with correction.
    std::shared_ptr< LightTimeCalculator< > > lightTimeEarthToMoonWithCorrection =
            std::make_shared< LightTimeCalculator< > >
            ( std::bind( &Ephemeris::getCartesianState, earthEphemeris, std::placeholders::_1 ),
              std::bind( &Ephemeris::getCartesianState, moonEphemeris, std::placeholders::_1 ),
              lightTimeCorrections, true );

    // Calculate newtonian light time.
    double newtonianLightTime = lightTimeEarthToMoonWithCorrection->calculateRelativeRangeVector(
                testTime, true ).norm( ) / physical_constants::SPEED_OF_LIGHT;

    // Calculate light time (including correction), at reception.
    testMoonLightTime = lightTimeEarthToMoonWithCorrection->calculateLightTime( testTime, true );

    // Calculate expected correction.
    double expectedCorrection = getTimeDifferenceLightTimeCorrection(
                Eigen::Vector6d::Zero( ), Eigen::Vector6d::Zero( ),
                testTime - testMoonLightTime, testTime );

    // Test whether results are approximately equal.
    BOOST_CHECK_CLOSE_FRACTION( newtonianLightTime + expectedCorrection,
                                testMoonLightTime,
                                1E-14 );

    // Create light-time object with correction, without iterating light-time corrections.
    lightTimeEarthToMoonWithCorrection =
            std::make_shared< LightTimeCalculator< > >
            ( std::bind( &Ephemeris::getCartesianState, earthEphemeris, std::placeholders::_1 ),
              std::bind( &Ephemeris::getCartesianState, moonEphemeris, std::placeholders::_1 ),
              lightTimeCorrections, false );

    // Calculate newtonian light time.
    newtonianLightTime = lightTimeEarthToMoonWithCorrection->calculateRelativeRangeVector(
                testTime, true ).norm( ) / physical_constants::SPEED_OF_LIGHT;

    // Calculate light time (including correction), at reception.
    testMoonLightTime = lightTimeEarthToMoonWithCorrection->calculateLightTime( testTime, true );

    // Calculate expected correction.
    expectedCorrection = getTimeDifferenceLightTimeCorrection(
                Eigen::Vector6d::Zero( ), Eigen::Vector6d::Zero( ),
                testTime - testMoonLightTime, testTime );

    // Test whether results are approximately equal.
    BOOST_CHECK_CLOSE_FRACTION( newtonianLightTime + expectedCorrection,
                                testMoonLightTime,
                                1E-14 );

    // Add two more light-time corrections.
    lightTimeCorrections.push_back( &getVelocityDifferenceLightTimeCorrection );
    lightTimeCorrections.push_back( &getPositionDifferenceLightTimeCorrection );

    // Create light-time object with multiple corrections.
    lightTimeEarthToMoonWithCorrection =
            std::make_shared< LightTimeCalculator< > >
            ( std::bind( &Ephemeris::getCartesianState, earthEphemeris, std::placeholders::_1 ),
              std::bind( &Ephemeris::getCartesianState, moonEphemeris, std::placeholders::_1 ),
              lightTimeCorrections, true );

    // Calculate newtonian light time.
    newtonianLightTime = lightTimeEarthToMoonWithCorrection->calculateRelativeRangeVector(
                testTime, true ).norm( ) / physical_constants::SPEED_OF_LIGHT;

    // Calculate light time (including corrections), at reception.
    testMoonLightTime = lightTimeEarthToMoonWithCorrection->calculateLightTimeWithLinkEndsStates(
                testMoonState, testEarthState, testTime, true );

    // Calculate and sum expected correction.
    expectedCorrection = getTimeDifferenceLightTimeCorrection(
                testEarthState, testMoonState, testTime - testMoonLightTime, testTime );
    expectedCorrection += getPositionDifferenceLightTimeCorrection(
                testEarthState, testMoonState, testTime - testMoonLightTime, testTime );
    expectedCorrection += getVelocityDifferenceLightTimeCorrection(
                testEarthState, testMoonState, testTime - testMoonLightTime, testTime );

    // Test whether results are approximately equal.
    BOOST_CHECK_CLOSE_FRACTION( newtonianLightTime + expectedCorrection,
                                testMoonLightTime,
                                1E-14 );
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests
} // namespace tudat


/*
 [auto_generated]
 boost/numeric/odeint/stepper/runge_kutta_cash_karp54_classic.hpp

 [begin_description]
 Classical implementation of the Runge-Kutta Cash-Karp 5(4) method.
 [end_description]

 Copyright 2010-2013 Mario Mulansky
 Copyright 2010-2013 Karsten Ahnert
 Copyright 2012 Christoph Koke

 Distributed under the Boost Software License, Version 1.0.
 (See accompanying file LICENSE_1_0.txt or
 copy at http://www.boost.org/LICENSE_1_0.txt)
 */

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED

#include <boost/numeric/odeint/util/bind.hpp>

#include <boost/numeric/odeint/stepper/base/explicit_error_stepper_base.hpp>
#include <boost/numeric/odeint/algebra/range_algebra.hpp>
#include <boost/numeric/odeint/algebra/default_operations.hpp>
#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>
#include <boost/numeric/odeint/stepper/stepper_categories.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
class State ,
class Value = double ,
class Deriv = State ,
class Time = Value ,
class Algebra = typename algebra_dispatcher< State >::algebra_type ,
class Operations = typename operations_dispatcher< State >::operations_type ,
class Resizer = initially_resizer
>
#ifndef DOXYGEN_SKIP
class runge_kutta_cash_karp54_classic
: public explicit_error_stepper_base<
  runge_kutta_cash_karp54_classic< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
  5 , 5 , 4 , State , Value , Deriv , Time , Algebra , Operations , Resizer >
#else
class runge_kutta_cash_karp54_classic : public explicit_error_stepper_base
#endif
{

public :

    #ifndef DOXYGEN_SKIP
    typedef explicit_error_stepper_base<
    runge_kutta_cash_karp54_classic< State , Value , Deriv , Time , Algebra , Operations , Resizer > ,
    5 , 5 , 4 , State , Value , Deriv , Time , Algebra , Operations , Resizer > stepper_base_type;
    #else
    typedef explicit_error_stepper_base< runge_kutta_cash_karp54_classic< ... > , ... > stepper_base_type;
    #endif

    typedef typename stepper_base_type::state_type state_type;
    typedef typename stepper_base_type::value_type value_type;
    typedef typename stepper_base_type::deriv_type deriv_type;
    typedef typename stepper_base_type::time_type time_type;
    typedef typename stepper_base_type::algebra_type algebra_type;
    typedef typename stepper_base_type::operations_type operations_type;
    typedef typename stepper_base_type::resizer_type resizer_type;

    #ifndef DOXYGEN_SKIP
    typedef typename stepper_base_type::wrapped_state_type wrapped_state_type;
    typedef typename stepper_base_type::wrapped_deriv_type wrapped_deriv_type;
    typedef typename stepper_base_type::stepper_type stepper_type;
    #endif

    runge_kutta_cash_karp54_classic( const algebra_type &algebra = algebra_type() ) : stepper_base_type( algebra )
    { }

    template< class System , class StateIn , class DerivIn , class StateOut , class Err >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt , Err &xerr )
    {
        const value_type c1 = static_cast<value_type> ( 37 ) / static_cast<value_type>( 378 );
        const value_type c3 = static_cast<value_type> ( 250 ) / static_cast<value_type>( 621 );
        const value_type c4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 594 );
        const value_type c6 = static_cast<value_type> ( 512 ) / static_cast<value_type>( 1771 );

        const value_type dc1 = c1 - static_cast<value_type> ( 2825 ) / static_cast<value_type>( 27648 );
        const value_type dc3 = c3 - static_cast<value_type> ( 18575 ) / static_cast<value_type>( 48384 );
        const value_type dc4 = c4 - static_cast<value_type> ( 13525 ) / static_cast<value_type>( 55296 );
        const value_type dc5 = static_cast<value_type> ( -277 ) / static_cast<value_type>( 14336 );
        const value_type dc6 = c6 - static_cast<value_type> ( 1 ) / static_cast<value_type> ( 4 );

        do_step_impl( system , in , dxdt , t , out , dt );

        //error estimate
        stepper_base_type::m_algebra.for_each6( xerr , dxdt , m_k3.m_v , m_k4.m_v , m_k5.m_v , m_k6.m_v ,
                typename operations_type::template scale_sum5< time_type , time_type , time_type , time_type , time_type >( dt*dc1 , dt*dc3 , dt*dc4 , dt*dc5 , dt*dc6 ));

    }

    template< class System , class StateIn , class DerivIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
    {
        const value_type a2 = static_cast<value_type> ( 1 ) / static_cast<value_type> ( 5 );
        const value_type a3 = static_cast<value_type> ( 3 ) / static_cast<value_type> ( 10 );
        const value_type a4 = static_cast<value_type> ( 3 ) / static_cast<value_type> ( 5 );
        const value_type a5 = static_cast<value_type> ( 1 );
        const value_type a6 = static_cast<value_type> ( 7 ) / static_cast<value_type> ( 8 );

        const value_type b21 = static_cast<value_type> ( 1 ) / static_cast<value_type> ( 5 );
        const value_type b31 = static_cast<value_type> ( 3 ) / static_cast<value_type>( 40 );
        const value_type b32 = static_cast<value_type> ( 9 ) / static_cast<value_type>( 40 );
        const value_type b41 = static_cast<value_type> ( 3 ) / static_cast<value_type> ( 10 );
        const value_type b42 = static_cast<value_type> ( -9 ) / static_cast<value_type> ( 10 );
        const value_type b43 = static_cast<value_type> ( 6 ) / static_cast<value_type> ( 5 );
        const value_type b51 = static_cast<value_type> ( -11 ) / static_cast<value_type>( 54 );
        const value_type b52 = static_cast<value_type> ( 5 ) / static_cast<value_type> ( 2 );
        const value_type b53 = static_cast<value_type> ( -70 ) / static_cast<value_type>( 27 );
        const value_type b54 = static_cast<value_type> ( 35 ) / static_cast<value_type>( 27 );
        const value_type b61 = static_cast<value_type> ( 1631 ) / static_cast<value_type>( 55296 );
        const value_type b62 = static_cast<value_type> ( 175 ) / static_cast<value_type>( 512 );
        const value_type b63 = static_cast<value_type> ( 575 ) / static_cast<value_type>( 13824 );
        const value_type b64 = static_cast<value_type> ( 44275 ) / static_cast<value_type>( 110592 );
        const value_type b65 = static_cast<value_type> ( 253 ) / static_cast<value_type>( 4096 );

        const value_type c1 = static_cast<value_type> ( 37 ) / static_cast<value_type>( 378 );
        const value_type c3 = static_cast<value_type> ( 250 ) / static_cast<value_type>( 621 );
        const value_type c4 = static_cast<value_type> ( 125 ) / static_cast<value_type>( 594 );
        const value_type c6 = static_cast<value_type> ( 512 ) / static_cast<value_type>( 1771 );

        typename odeint::unwrap_reference< System >::type &sys = system;

        m_resizer.adjust_size( in , detail::bind( &stepper_type::template resize_impl<StateIn> , detail::ref( *this ) , detail::_1 ) );

        //m_x1 = x + dt*b21*dxdt
        stepper_base_type::m_algebra.for_each3( m_x_tmp.m_v , in , dxdt ,
                typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , dt*b21 ) );

        sys( m_x_tmp.m_v , m_k2.m_v , t + dt*a2 );
        // m_x_tmp = x + dt*b31*dxdt + dt*b32*m_x2
        stepper_base_type::m_algebra.for_each4( m_x_tmp.m_v , in , dxdt , m_k2.m_v ,
                typename operations_type::template scale_sum3< value_type , time_type , time_type >( 1.0 , dt*b31 , dt*b32 ));

        sys( m_x_tmp.m_v , m_k3.m_v , t + dt*a3 );
        // m_x_tmp = x + dt * (b41*dxdt + b42*m_x2 + b43*m_x3)
        stepper_base_type::m_algebra.for_each5( m_x_tmp.m_v , in , dxdt , m_k2.m_v , m_k3.m_v ,
                typename operations_type::template scale_sum4< value_type , time_type , time_type , time_type >( 1.0 , dt*b41 , dt*b42 , dt*b43 ));

        sys( m_x_tmp.m_v, m_k4.m_v , t + dt*a4 );
        stepper_base_type::m_algebra.for_each6( m_x_tmp.m_v , in , dxdt , m_k2.m_v , m_k3.m_v , m_k4.m_v ,
                typename operations_type::template scale_sum5< value_type , time_type , time_type , time_type , time_type >( 1.0 , dt*b51 , dt*b52 , dt*b53 , dt*b54 ));

        sys( m_x_tmp.m_v , m_k5.m_v , t + dt*a5 );
        stepper_base_type::m_algebra.for_each7( m_x_tmp.m_v , in , dxdt , m_k2.m_v , m_k3.m_v , m_k4.m_v , m_k5.m_v ,
                typename operations_type::template scale_sum6< value_type , time_type , time_type , time_type , time_type , time_type >( 1.0 , dt*b61 , dt*b62 , dt*b63 , dt*b64 , dt*b65 ));

        sys( m_x_tmp.m_v , m_k6.m_v , t + dt*a6 );
        stepper_base_type::m_algebra.for_each6( out , in , dxdt , m_k3.m_v , m_k4.m_v , m_k6.m_v ,
                typename operations_type::template scale_sum5< value_type , time_type , time_type , time_type , time_type >( 1.0 , dt*c1 , dt*c3 , dt*c4 , dt*c6 ));

    }

    /**
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */
    template< class StateIn >
    void adjust_size( const StateIn &x )
    {
        resize_impl( x );
        stepper_base_type::adjust_size( x );
    }

private:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_x_tmp , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_k2 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k3 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k4 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k5 , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_k6 , x , typename is_resizeable<deriv_type>::type() );
        return resized;
    }

    wrapped_state_type m_x_tmp;
    wrapped_deriv_type m_k2, m_k3, m_k4, m_k5, m_k6;
    resizer_type m_resizer;

};

/************ DOXYGEN *************/

/**
 * \class runge_kutta_cash_karp54_classic
 * \brief The Runge-Kutta Cash-Karp method implemented without the generic Runge-Kutta algorithm.
 *
 * The Runge-Kutta Cash-Karp method is one of the standard methods for
 * solving ordinary differential equations, see
 * en.wikipedia.org/wiki/Cash-Karp_method.
 * The method is explicit and fulfills the Error Stepper concept. Step size control
 * is provided but continuous output is not available for this method.
 *
 * This class derives from explicit_error_stepper_base and inherits its interface via CRTP (current recurring
 * template pattern). This class implements the method directly, hence the generic Runge-Kutta algorithm is not used.
 *
 * \tparam State The state type.
 * \tparam Value The value type.
 * \tparam Deriv The type representing the time derivative of the state.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam Algebra The algebra type.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn runge_kutta_cash_karp54_classic::runge_kutta_cash_karp54_classic( const algebra_type &algebra )
     * \brief Constructs the runge_kutta_cash_karp54_classic class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn runge_kutta_cash_karp54_classic::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt , Err &xerr )
     * \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
     *
     * The result is updated out-of-place, hence the input is in `in` and the output in `out`. Futhermore, an
     * estimation of the error is stored in `xerr`.
     * Access to this step functionality is provided by explicit_error_stepper_base and
     * `do_step_impl` should not be called directly.

     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     * \param xerr The result of the error estimation is written in xerr.
     */

    /**
     * \fn runge_kutta_cash_karp54_classic::do_step_impl( System system , const StateIn &in , const DerivIn &dxdt , time_type t , StateOut &out , time_type dt )
     * \brief This method performs one step. The derivative `dxdt` of `in` at the time `t` is passed to the method.
     * The result is updated out-of-place, hence the input is in `in` and the output in `out`.
     * Access to this step functionality is provided by explicit_error_stepper_base and
     * `do_step_impl` should not be called directly.
     *
     * \param system The system function to solve, hence the r.h.s. of the ODE. It must fulfill the
     *               Simple System concept.
     * \param in The state of the ODE which should be solved. in is not modified in this method
     * \param dxdt The derivative of x at t.
     * \param t The value of the time, at which the step should be performed.
     * \param out The result of the step is written in out.
     * \param dt The step size.
     */

} // odeint
} // numeric
} // boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_RUNGE_KUTTA_CASH_KARP54_CLASSIC_HPP_INCLUDED


#ifndef BNI_SERIALIZER_BIF_HPP
#define BNI_SERIALIZER_BIF_HPP

#include <string>
#include <vector>
#include <unordered_map>
#include <boost/spirit/home/qi.hpp>
#include <boost/spirit/include/phoenix.hpp>
#include <boost/fusion/include/std_pair.hpp>
#include <boost/fusion/include/adapt_struct.hpp>
#include <bayesian/graph.hpp>

namespace bn {
namespace serializer {

// 辛い
namespace qi  = boost::spirit::qi;
namespace phx = boost::phoenix;

class bif {
public:
    // パース直後の木を形成する
    struct variable_holder {
        std::string variable_name;
        unsigned int possible_value;
        std::vector<std::string> possible_value_name;
    };

    struct network_holder {
        std::string network_name;
    };

    struct probability_holder {
        typedef std::pair<std::string, std::vector<std::string>> rv_condition_type;
        typedef std::vector<std::pair<std::vector<std::string>, std::vector<double>>> probability_type;
        rv_condition_type rv_condition;
        probability_type probability;
    };

    template<class Iterator>
    std::tuple<bn::graph_t, bn::database_t> parse(Iterator const& begin, Iterator const& end)
    {
        parse_grammar<Iterator> grammar;

        // Parse
        auto it = begin;
        bool success = qi::phrase_parse(it, end, grammar, qi::ascii::space);
        if(!success || it != end)
        {
            // 木構造作成が正常でない(パース失敗)
            throw std::runtime_error("cannot parse bif");
        }

        //
        // 木構造をグラフとして作成していく
        //
        bn::graph_t graph;
        bn::database_t database;
        std::unordered_map<std::string, std::pair<bn::vertex_type, variable_holder>> dictionary;

        // グラフ全体についての設定
        if(grammar.networks.size() > 1)
        {
            // networkセクションは1つまで
            throw std::runtime_error("too many network section in bif");
        }
        else if(grammar.networks.size() == 1)
        {
            database.graph_name = grammar.networks[0].network_name;
        }

        // ノードについての作成・辞書作成
        for(auto const& node : grammar.variables)
        {
            auto vertex = graph.add_vertex();
            auto const id = database.node_name.size();

            // IDの割当と，ノードネームの保存
            vertex->id = id;
            database.node_name[id] = node.variable_name;

            // 選択可能(パラメータ)数を保存
            vertex->selectable_num = node.possible_value;
            database.options_name[id] = node.possible_value_name;

            dictionary[node.variable_name] = std::pair<bn::vertex_type, variable_holder>(vertex, node);

        }

        // CPTの読み取り
        for(auto const& probability : grammar.probabilities)
        {
            // 注目しているノードについて
            auto const& random_variables = probability.rv_condition;
            auto const& target_vertex = dictionary[random_variables.first].first;

            // ノードの親についてまとめ
            // CPTの雛形まで作成する
            std::vector<bn::vertex_type> conditional_vertexes;
            for(auto const& conditional_node : random_variables.second)
            {
                auto const& conditional_vertex = dictionary[conditional_node].first;
                graph.add_edge(conditional_vertex, target_vertex);  // 辺を張る
                conditional_vertexes.push_back(conditional_vertex); // target_vertexの条件として保存
            }
            target_vertex->cpt.assign(conditional_vertexes, target_vertex); // 雛形

            // CPTが指定されていれば，CPTを確定
            // されていなければループに入らない
            for(auto const& one_condition : probability.probability)
            {
                // 条件として与えられた文字列を便宜的に数字(index)に直し，条件とする．
                bn::condition_t cond;
                std::transform(
                    one_condition.first.begin(), one_condition.first.end(),
                    random_variables.second.begin(),
                    std::inserter(cond, cond.begin()),
                    [&dictionary](std::string const& value, std::string const& label) -> bn::condition_t::value_type
                    {
                        auto const& cond_node = dictionary[label];
                        auto const index = std::distance(
                            cond_node.second.possible_value_name.begin(),
                            std::find(cond_node.second.possible_value_name.begin(), cond_node.second.possible_value_name.end(), value)
                            );

                        return std::make_pair(cond_node.first, static_cast<int>(index));
                    });

                target_vertex->cpt[cond].second = one_condition.second;
            }
        }

        return std::forward_as_tuple(std::move(graph), std::move(database));
    }

private:
    template<class Iterator>
    struct parse_grammar : qi::grammar<Iterator, void(), qi::ascii::space_type> {
        //
        // Functions
        //
        std::function<std::pair<std::vector<std::string>, std::vector<double>>(std::vector<double>)> nonconditional;

        //
        // Characters
        //
        qi::rule<Iterator, char(), qi::ascii::space_type> usable_char;
        qi::rule<Iterator, std::string(), qi::ascii::space_type> usable_string;

        //
        // Rule of "variable" Section
        //
        qi::rule<Iterator, std::vector<std::string>(), qi::ascii::space_type> enumeration_rule;
        qi::rule<Iterator, unsigned int(), qi::ascii::space_type> array_size_rule;
        qi::rule<Iterator, variable_holder(), qi::ascii::space_type> variable_rule;

        //
        // Rule of "network" Section
        //
        qi::rule<Iterator, network_holder(), qi::ascii::space_type> network_rule;

        //
        // Rule of "probability" Section
        //
        qi::rule<Iterator, std::pair<std::string, std::vector<std::string>>(), qi::ascii::space_type> rv_enum_rule;
        qi::rule<Iterator, std::vector<std::string>(), qi::ascii::space_type> condition_enum_rule;
        qi::rule<Iterator, std::vector<double>(), qi::ascii::space_type> probability_enum_rule;
        qi::rule<Iterator, std::pair<std::vector<std::string>, std::vector<double>>(), qi::ascii::space_type> probability_line_rule;
        qi::rule<Iterator, std::pair<std::vector<std::string>, std::vector<double>>(), qi::ascii::space_type> independent_line_rule;
        qi::rule<Iterator, probability_holder(), qi::ascii::space_type> rv_rule;
        qi::rule<Iterator, probability_holder(), qi::ascii::space_type> probability_rule;

        //
        // Rule of ALL
        //
        qi::rule<Iterator, void(), qi::ascii::space_type> global_rule;

        //
        // Parsed Holder
        //
        std::vector<network_holder> networks;
        std::vector<variable_holder> variables;
        std::vector<probability_holder> probabilities;

        parse_grammar() : parse_grammar::base_type(global_rule)
        {
            //
            // Functions
            //
            nonconditional =
                [](std::vector<double> target)
                {
                    return std::make_pair(std::vector<std::string>(), std::move(target));
                };

            //
            // Characters
            //
            usable_char = qi::alnum | qi::char_("-_");
            usable_string = +usable_char;

            //
            // Rule of "variable" Section
            //
            enumeration_rule =
                qi::lit("{")
                >> usable_string % qi::lit(",")
                >> qi::lit("}");
            array_size_rule =
                qi::lit("[") >> qi::uint_ >> qi::lit("]");
            variable_rule =
                qi::lit("variable")
                >> usable_string
                >> qi::lit("{")
                >> qi::lit("type") >> qi::lit("discrete") >> array_size_rule >> enumeration_rule >> qi::lit(";")
                >> qi::lit("}");

            //
            // Rule of "network" Section
            //
            network_rule =
                qi::lit("network")
                >> usable_string
                >> qi::lit("{") >> qi::lit("}");

            //
            // Rule of "probability" Section
            //
            rv_enum_rule =
                qi::lit("(")
                >> usable_string
                >>-( +qi::lit("|")
                >> usable_string % qi::lit(",") )
                >> qi::lit(")");
            condition_enum_rule =
                qi::lit("(")
                >> usable_string % qi::lit(",")
                >> qi::lit(")");
            probability_enum_rule =
                qi::double_ % qi::lit(",");
            probability_line_rule =
                condition_enum_rule >> probability_enum_rule >> qi::lit(";");
            independent_line_rule =
                qi::lit("table") >> probability_enum_rule[qi::_val = phx::bind(nonconditional, qi::_1)] >> qi::lit(";");
            rv_rule =
                rv_enum_rule >> qi::lit("{")
                >> -(+independent_line_rule | +probability_line_rule)
                >> qi::lit("}");
            probability_rule =
                qi::lit("probability") >> rv_rule;

            //
            // Rule of ALL
            //
            global_rule =
                *(
                    network_rule[phx::push_back(phx::ref(networks), qi::_1)]
                    | variable_rule[phx::push_back(phx::ref(variables), qi::_1)]
                    | probability_rule[phx::push_back(phx::ref(probabilities), qi::_1)]
                );

        }
    };

};

} // namespace serializer
} // namespace bn

BOOST_FUSION_ADAPT_STRUCT(
    bn::serializer::bif::variable_holder,
    (std::string, variable_name)
    (unsigned int, possible_value)
    (std::vector<std::string>, possible_value_name)
)

BOOST_FUSION_ADAPT_STRUCT(
    bn::serializer::bif::network_holder,
    (std::string, network_name)
)

BOOST_FUSION_ADAPT_STRUCT(
    bn::serializer::bif::probability_holder,
    (bn::serializer::bif::probability_holder::rv_condition_type, rv_condition)
    (bn::serializer::bif::probability_holder::probability_type, probability)
)

#endif // #ifndef BNI_SERIALIZER_BIF_HPP