////
////  ExpressionFit.cpp
////  Face AR
////
////  Created by shiwei zhou on 2018/1/31.
////  Copyright © 2018年 shiwei zhou. All rights reserved.
////
//
//#include <stdio.h>
//#include <Eigen/Eigen>
//#ifndef __ANDROID__
//#include "nnls.h"
//#endif
//#include <opencv2/core/core.hpp>
//
//
//using namespace std;
//using namespace cv;
//auto FitExpressionBlendshape(cv::Mat &currentShape,cv::Mat &blenshape,std::vector<cv::Point2f> &landmarks,std::vector<int> &mapping_3d,cv::Mat &affine_matrix)
//{
//
//    int num=(int)landmarks.size();
//    int num_coeffs_to_fit=(int)blenshape.cols;
//    cv::Mat A(3*num,num_coeffs_to_fit,CV_32FC1,Scalar(0));
//    //    cv::Mat current_hat_h = cv::Mat::zeros(4, num_coeffs_to_fit, CV_32FC1);
//    cv::Mat affine_matrix_ROI=affine_matrix(cv::Rect(0,0,3,3));
//    for (int i=0; i<mapping_3d.size(); i++) {
//
//        int index=(int)mapping_3d.at(i)*3;
//        //        cv::Mat vertex_basis=blenshape.rowRange(index, index+3);
//        cv::Mat vertex_basis=blenshape(cv::Rect(0,index,num_coeffs_to_fit,3));
//        //        vertex_basis.copyTo(current_hat_h.rowRange(0,3));
//        Mat B=affine_matrix_ROI*vertex_basis;
//        Mat roi=A(cv::Rect(0,i*3,num_coeffs_to_fit,3));
//        B.copyTo(roi);
//    }
//
//    //    cv::Mat V_hat_h = cv::Mat::zeros(4 *num, num_coeffs_to_fit, CV_32FC1);
//    //    int rowIndex=0;
//    //    for (int i=0; i<mapping_3d.size(); i++) {
//    //        int index=(int)mapping_3d.at(i)*3;
//    //        cv::Mat vertex_basis=blenshape.rowRange(index, index+3);
//    //        vertex_basis.colRange(0,num_coeffs_to_fit).copyTo(V_hat_h.rowRange(rowIndex, rowIndex+3));
//    //        rowIndex +=4;
//    //    }
//
//    //    cv::Mat camera_matrix=affine_matrix;
//    ////  affine_matrix.convertTo(camera_matrix,CV_32F);
//    //
//    //    cv::Mat Project_matrix(3*num,4*num,CV_32FC1,Scalar(0));
//    //    for (int i = 0; i < num; ++i) {
//    //        Mat submatrix_to_replace = Project_matrix.colRange(4 * i, (4 * i) + 4).rowRange(3 * i, (3 * i) + 3);
//    //        camera_matrix.copyTo(submatrix_to_replace);
//    //    }
//
//    cv::Mat y = Mat::ones(3 * num, 1, CV_32FC1);
//    for (int i = 0; i < num; ++i) {
//        y.at<float>(3 * i, 0) = landmarks.at(i).x;
//        y.at<float>((3 * i) + 1, 0) = landmarks.at(i).y;
//    }
//    cv::Mat AffineMat= Mat::ones(3*num,1,CV_32FC1);
//    cv::Mat cur_v_bar = Mat::ones(4, 1, CV_32FC1);
//    for (int i = 0; i < num; ++i) {
//
//        int index=(int)mapping_3d.at(i)*3;
//        cv::Mat cur_num_roi=currentShape(cv::Rect(0,index,1,3));
//        cur_num_roi.copyTo(cur_v_bar(cv::Rect(0,0,1,3)));
//        Mat tranform=affine_matrix*cur_v_bar;
//        tranform.copyTo(AffineMat(cv::Rect(0,i*3,1,3)));
//
//
//    }
//
//    //    cv::Mat v_bar = Mat::ones(4 * num, 1, CV_32FC1);
//    //    for (int i = 0; i < num; ++i) {
//    //        int index=(int)mapping_3d.at(i)*3;
//    //        cv::Vec4f model_mean(currentShape.at<float>(index+0,0), currentShape.at<float>(index + 1,0), currentShape.at<float>(index + 2,0), 1.0f);
//    //        v_bar.at<float>(4 * i, 0) = model_mean[0];
//    //        v_bar.at<float>((4 * i) + 1, 0) = model_mean[1];
//    //        v_bar.at<float>((4 * i) + 2, 0) = model_mean[2];
//    //        //  v_bar.at<float>((4 * i) + 3, 0) = 1;
//    //        //    cout<<mean.at<float>(index+0,0)<<"  "<< mean.at<float>(index + 1,0)<<"   "<<mean.at<float>(index + 2,0)<<endl;
//    //    }
//
//    //  cv::Mat A = Project_matrix * V_hat_h;
//    //    cv::Mat b = Project_matrix * v_bar - y;
//    cv::Mat b = AffineMat - y;
//
//
//
//    //    // Solve using NNLS:
//    using RowMajorMatrixXf = Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
//    Eigen::Map<RowMajorMatrixXf> A_Eigen(A.ptr<float>(), A.rows, A.cols);
//    Eigen::Map<RowMajorMatrixXf> b_Eigen(b.ptr<float>(), b.rows, b.cols);
//
//    // Meathod one
//    //#ifndef __ANDROID__
//    //    Eigen::VectorXf x;
//    //    Eigen::NNLS<Eigen::MatrixXf>::solve(A_Eigen, -b_Eigen, x);
//    //    cv::Mat c_s((int)x.rows(), (int)x.cols(), CV_32FC1, x.data());
//    //    return std::vector<float>(c_s);
//    //#else
//
//    // Meathod two
//    int num_blendshapes=num_coeffs_to_fit;
//    float lambda =200.0f;
//    const Eigen::MatrixXf AtAReg =
//    A_Eigen.transpose() * A_Eigen + lambda * Eigen::MatrixXf::Identity(num_blendshapes, num_blendshapes);
//    const Eigen::MatrixXf rhs = -A_Eigen.transpose() * b_Eigen;
//    Eigen::VectorXf coefficients = AtAReg.colPivHouseholderQr().solve(rhs);
//    return std::vector<float>(coefficients.data(), coefficients.data() + coefficients.size());
//
//    // Meathod three
//    //    const int num_shape_pc = num_coeffs_to_fit;
//    //    float lambda=15.0f;//50
//    //    Mat AtOmegaA = A.t() * Omega * A;
//    //    Mat AtOmegaAReg = AtOmegaA + lambda * Mat::eye(num_shape_pc, num_shape_pc, CV_32FC1);
//    //    Mat c_s;
//    //    bool non_singular = cv::solve(AtOmegaAReg, -A.t() * Omega.t() * b, c_s, cv::DECOMP_SVD);
//    //    return std::vector<float>(c_s);
//    // #endif
//
//}
//


//--------------------------- Compose all the input displacement fields -----------//
/* Options include :
--numOfFields       number of fields to compose.
--inPrefix          inputFileBase    : Filenames to be composed should be in xxNyy format. Provide xx to this argument.
--inPostfix         inputExt         : yy (including the extension)
--outputImage       outImage         : output filename.
--interpolator      l/n            : linear/nearestNeighbor. Currently no implemented, so uses linear by default.

Author:      Bishesh Khanal
Asclepios, INRIA Sophia Antipolis

FIXME: Experiment different interpolators. Currently default vector interpolator is used.
*/

#include <iostream>
#include <sstream>
#include <string>

#include <itkImage.h>
#include <itkImageAdaptor.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>

#include "InverseDisplacementImageFilter.h"
#include <itkComposeDisplacementFieldsImageFilter.h>

//#include <itkVectorInterpolateImageFunction.h>
//#include <itkVectorNearestNeighborInterpolateImageFunction.h>
//#include <itkVectorLinearInterpolateNearestNeighborExtrapolateImageFunction.h>

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

#undef __FUNCT__
#define __FUNCT__ "main"
int main(int argc, char **argv)
{
    std::string inPrefix;
    std::string inPostfix;
    std::string outFile;
    std::string interpolator;

    int     numOfFields;

    //------------------- Set up the command line options database and parse them -----------------------------//
    boost::program_options::options_description optionsDescription("Possible options");
    optionsDescription.add_options()
            ("help,h",
             "Note: All the displacement fields to be composed should be named in the format xxNyy.\n"
             "N starts from 1 up to the final field to be composed.")
             ("inPrefix", boost::program_options::value< std::string >(&inPrefix),
              "Input filename prefix. E.g. step for the series step1vel.mha, step2vel.mha ...")
             ("inPostfix", boost::program_options::value< std::string >(&inPostfix),
              "Input filename postfix. vel.mha for the series  step1vel.mha, step2vel.mha ...")
             ("outFile", boost::program_options::value< std::string >(&outFile),
              "output displacement field filename.")
             ("interpolator", boost::program_options::value< std::string >(&interpolator)->default_value("linear"),
              "linear/bspline/nearestneighbor interpolator.")
             ("numOfFields",boost::program_options::value< int >(&numOfFields),
              "number of displacement fields to be composed.")
             ;

    boost::program_options::variables_map options;
    boost::program_options::store(boost::program_options::parse_command_line(argc,argv,optionsDescription),options);
    boost::program_options::notify(options);

    //If help is asked!
    if(options.count("help")) {
        std::cout<<optionsDescription<<std::endl;
        return EXIT_SUCCESS;
    }

    //Confirm all the required options are given.
    if(!options.count("inPrefix") || !options.count("inPostfix") || !options.count("numOfFields") || !options.count("outFile")
            ) {
        std::cerr<<"invalid options! run with --help or -h to see the proper options."<<std::endl;
        return EXIT_FAILURE;
    }

    //

    //---------------------  displacement field types ----------------------//
    typedef itk::Vector<double, 3>                      DisplacementPixelType;
    typedef itk::Image<DisplacementPixelType, 3>        DisplacementImageType;
    typedef itk::ImageFileReader<DisplacementImageType> DisplacementReaderType;

    //------- read the first disp. field and set it as a warper field -----------//
    DisplacementImageType::Pointer warperField;
    DisplacementReaderType::Pointer warperReader = DisplacementReaderType::New();
    warperReader->SetFileName(inPrefix + boost::lexical_cast<std::string>(1) + inPostfix);
    warperReader->Update();
    warperField = warperReader->GetOutput();

    //----------- read disp. field starting from the second one, compose it and set the composed one as a new warper. Iterate until the end.-----//
    DisplacementImageType::Pointer displacementField;
    typedef itk::ComposeDisplacementFieldsImageFilter<DisplacementImageType, DisplacementImageType> ComposerType;
    for (int i = 2; i<=numOfFields; ++i) {
        std::string displacementFile = inPrefix + boost::lexical_cast<std::string>(i) + inPostfix;
        DisplacementReaderType::Pointer displacementReader = DisplacementReaderType::New();
        displacementReader->SetFileName(displacementFile);
        displacementReader->Update();
        displacementField = displacementReader->GetOutput();

        ComposerType::Pointer composer = ComposerType::New();
        composer->SetDisplacementField(displacementField);
        composer->SetWarpingField(warperField);

        /*FIXME: experiment with other interpolators. if (interpolator.compare("nearestneighbor")==0) {
            typedef itk::VectorNearestNeighborInterpolateImageFunction<DisplacementImageType, ..> InterpolatorFilterType;
            InterpolatorFilterType::Pointer interpolatorFilter = InterpolatorFilterType::New();
            composer->SetInterpolator(interpolatorFilter);
        }*/

        composer->Update();
        warperField = composer->GetOutput();
    }

    //----------- Write the output -------------//
    typedef itk::ImageFileWriter<DisplacementImageType> WriterType;
    WriterType::Pointer writer = WriterType::New();
    writer->SetFileName(outFile);
    writer->SetInput(warperField);
    writer->Update();

    return(EXIT_SUCCESS);

}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "ParserHelper.hpp"

#include <armnn/Descriptors.hpp>
#include <armnnUtils/Permute.hpp>

#include <boost/format.hpp>

namespace armnnUtils
{

const armnn::PermutationVector NHWCToArmNN = { 0, 2, 3, 1 };
const armnn::PermutationVector ArmNNToNHWC = { 0, 3, 1, 2 };

void ProcessConcatInputTensorInfo(armnn::TensorInfo& inputTensorInfo,
                                  armnn::OriginsDescriptor& concatDescriptor,
                                  const unsigned int& concatAxis,
                                  unsigned int inputIndex,
                                  unsigned int& mergeDimOrigin)
{
    const uint32_t inputRank = concatDescriptor.GetNumDimensions();

    // double check dimensions of the tensors
    if (inputTensorInfo.GetNumDimensions() != inputRank)
    {
        throw armnn::ParseException(
            boost::str(
                boost::format(
                    "The number of dimensions: %1% for input tensors of the "
                    "concatenation op should be %2% %3%")
                % inputTensorInfo.GetNumDimensions()
                % inputRank
                % CHECK_LOCATION().AsString()));
    }

    for (unsigned int j = 0; j < concatAxis; ++j)
    {
        concatDescriptor.SetViewOriginCoord(inputIndex, j, 0);
    }

    concatDescriptor.SetViewOriginCoord(inputIndex, concatAxis, mergeDimOrigin);
    mergeDimOrigin += inputTensorInfo.GetShape()[concatAxis];

    for (unsigned int j = concatAxis + 1; j < inputRank; ++j)
    {
        concatDescriptor.SetViewOriginCoord(inputIndex, j, 0);
    }
}

void CalculateReducedOutputTensoInfo(const armnn::TensorInfo& inputTensorInfo,
                                     const std::set<unsigned int>& axisSet,
                                     bool keepDims,
                                     armnn::TensorInfo& outputTensorInfo)
{
    std::vector<unsigned int> outputShapeVector;
    bool dimensionFound = false;
    unsigned int size = 1;

    for (unsigned int i = 0; i < inputTensorInfo.GetNumDimensions(); ++i)
    {
        dimensionFound = false;
        for (unsigned int axis: axisSet)
        {
            if (axis == i)
            {
                dimensionFound = true;
                break;
            }
        }

        if (!dimensionFound)
        {
            size *= inputTensorInfo.GetShape()[i];

            if (keepDims)
            {
                outputShapeVector.push_back(inputTensorInfo.GetShape()[i]);
            }
        }
        else
        {
            if (keepDims)
            {
                outputShapeVector.push_back(1);
            }
        }
    }

    if (keepDims)
    {
        armnn::TensorShape outputTensorShape(inputTensorInfo.GetNumDimensions(), &outputShapeVector[0]);
        outputTensorInfo = armnn::TensorInfo(outputTensorShape, inputTensorInfo.GetDataType());
    }
    else
    {
        outputTensorInfo = armnn::TensorInfo({size}, inputTensorInfo.GetDataType());
    }
}

void CalculateStridedSliceOutputTensorInfo(const armnn::TensorInfo& inputTensorInfo,
                                           const armnn::StridedSliceDescriptor& desc,
                                           armnn::TensorInfo& outputTensorInfo)
{
    const armnn::TensorShape& inputShape = inputTensorInfo.GetShape();

    std::vector<unsigned int> outputShapeVector;
    for (unsigned int i = 0; i < inputTensorInfo.GetNumDimensions(); i++)
    {
        if (desc.m_ShrinkAxisMask & (1 << i))
        {
            continue;
        }

        int stride = desc.m_Stride[i];
        int start = desc.GetStartForAxis(inputShape, i);
        int stop = desc.GetStopForAxis(inputShape, i, start);

        int newSize = stride > 0 ? ((stop - start) + stride - 1) / stride :
                      ((start - stop) - stride - 1) / -stride;

        newSize = std::max(0, newSize);

        outputShapeVector.push_back(static_cast<unsigned int>(newSize));
    }

    armnn::TensorShape outputTensorShape(inputTensorInfo.GetNumDimensions(), &outputShapeVector[0]);
    outputTensorInfo = armnn::TensorInfo(armnn::TensorShape(outputTensorShape), inputTensorInfo.GetDataType());
}
} // namespace armnnUtils


#include <boost/python.hpp>

#include "../../../def_submodule.hpp"

#include "IceRay/geometry/flat/plane.hpp"

#include "./simple.hpp"

typedef GS_DDMRM::S_IceRay::S_geometry::GC_plane            GTs_plane;

void expose_IceRay_geometry_plane()
 {
  //MAKE_SUBMODULE( IceRay );
  MAKE_SUBMODULE( core );
  MAKE_SUBMODULE( geometry );

  typedef  GTs_coord3D const& (GTs_plane::*Tf_getPivot  )(void) const;
  Tf_getPivot I_getPivot = &GTs_plane::F_pivot;

  typedef  bool (GTs_plane::*Tf_setPivot  )(GTs_coord3D const&);
  Tf_setPivot I_setPivot = &GTs_plane::F_pivot;

  typedef  GTs_coord3D const& (GTs_plane::*Tf_getNormal)(void) const;
  Tf_getNormal I_getNormal = &GTs_plane::F_normal;

  typedef  bool (GTs_plane::*Tf_setNormal  )(GTs_coord3D const&);
  Tf_setNormal I_setNormal = &GTs_plane::F_normal;

  boost::python::class_< GTs_plane, boost::python::bases<GTs__base> >( "GeometrySimplePlane" )
    .def( boost::python::init<>() )
    .def( boost::python::init<GTs_coord3D, GTs_coord3D>() )
    .def("pivot",  I_getPivot, boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    .def("pivot",  I_setPivot )
    .def("normal",  I_getNormal, boost::python::return_value_policy<boost::python::copy_const_reference>()  )
    .def("normal",  I_setNormal )
  ;

 }


//
//  Raycaster.hpp
//  KinFu
//
//  Created by Dave on 2/06/2016.
//  Copyright © 2016 Sindesso. All rights reserved.
//

#ifndef GPURaycaster_hpp
#define GPURaycaster_hpp

#include <Eigen/Core>

#include "TSDFVolume.hpp"
#include "DepthImage.hpp"

class GPURaycaster {
public:
    explicit GPURaycaster( int width=640, int height=480) {
        m_width = width;
        m_height = height;
    }

    /**
     * Raycast the TSDF and store discovered vertices and normals in the ubput arrays
     * @param volume The volume to cast
     * @param camera The camera
     * @param vertices The vertices discovered
     * @param normals The normals
     */
    void raycast( const TSDFVolume & volume, const Camera & camera,
                          Eigen::Matrix<float, 3, Eigen::Dynamic> & vertices,
                          Eigen::Matrix<float, 3, Eigen::Dynamic> & normals ) const;

    /**
     * Render a depth image from a TSDF
     * @param volume The volume to cast
     * @param camera The camera
     * @return The DepthImage
     */
    DepthImage * render_to_depth_image( const TSDFVolume & volume, const Camera & camera ) const;

    void render_with_shading(const TSDFVolume & volume, const Camera & camera,
                             Eigen::Matrix<float, 3, Eigen::Dynamic> & vertices,
                             Eigen::Matrix<float, 3, Eigen::Dynamic> & normals,
                             const Eigen::Vector3f &light_source, int n_samples, uint8_t *image) const;

protected:

    uint16_t    m_width;
    uint16_t    m_height;
};
#endif /* GPURaycaster_hpp */


#ifndef RPVG_SRC_PATHESTIMATOR_HPP
#define RPVG_SRC_PATHESTIMATOR_HPP

#include <vector>

#include <Eigen/Dense>

#include "path_cluster_estimates.hpp"
#include "read_path_probabilities.hpp"
#include "utils.hpp"

using namespace std;

class PathEstimator {

    public:

        PathEstimator(const double prob_precision_in);
        virtual ~PathEstimator() {};

        virtual void estimate(PathClusterEstimates * path_cluster_estimates, const vector<ReadPathProbabilities> & cluster_probs, mt19937 * mt_rng) = 0;

    protected:

        const double prob_precision;

        void constructProbabilityMatrix(Utils::ColMatrixXd * read_path_probs, Utils::ColVectorXd * noise_probs, Utils::RowVectorXd * read_counts, const vector<ReadPathProbabilities> & cluster_probs, const uint32_t num_paths) const;
        void constructPartialProbabilityMatrix(Utils::ColMatrixXd * read_path_probs, Utils::ColVectorXd * noise_probs, Utils::RowVectorXd * read_counts, const vector<ReadPathProbabilities> & cluster_probs, const vector<uint32_t> & path_ids, const uint32_t num_paths, const bool remove_zero_row) const;
        void constructGroupedProbabilityMatrix(Utils::ColMatrixXd * read_path_probs, Utils::ColVectorXd * noise_probs, Utils::RowVectorXd * read_counts, const vector<ReadPathProbabilities> & cluster_probs, const vector<vector<uint32_t> > & path_groups, const uint32_t num_paths) const;

        void addNoiseAndNormalizeProbabilityMatrix(Utils::ColMatrixXd * read_path_probs, const Utils::ColVectorXd & noise_probs) const;
        void detractNoiseAndNormalizeProbabilityMatrix(Utils::ColMatrixXd * read_path_probs, Utils::ColVectorXd * noise_probs, Utils::RowVectorXd * read_counts) const;

        void readCollapseProbabilityMatrix(Utils::ColMatrixXd * read_path_probs, Utils::RowVectorXd * read_counts) const;
        void pathCollapseProbabilityMatrix(Utils::ColMatrixXd * read_path_probs) const;

        void calculatePathGroupPosteriorsFull(PathClusterEstimates * path_cluster_estimates, const Utils::ColMatrixXd & read_path_probs, const Utils::ColVectorXd & noise_probs, const Utils::RowVectorXd & read_counts, const vector<uint32_t> & path_counts, const uint32_t group_size) const;
        void calculatePathGroupPosteriorsBounded(PathClusterEstimates * path_cluster_estimates, const Utils::ColMatrixXd & read_path_probs, const Utils::ColVectorXd & noise_probs, const Utils::RowVectorXd & read_counts, const vector<uint32_t> & path_counts, const uint32_t group_size, const double min_rel_likelihood) const;
        void estimatePathGroupPosteriorsGibbs(PathClusterEstimates * path_cluster_estimates, const Utils::ColMatrixXd & read_path_probs, const Utils::ColVectorXd & noise_probs, const Utils::RowVectorXd & read_counts, const vector<uint32_t> & path_counts, const uint32_t group_size, mt19937 * mt_rng) const;

    private:

        void rowSortProbabilityMatrix(Utils::ColMatrixXd * read_path_probs, Utils::RowVectorXd * read_counts) const;
        void colSortProbabilityMatrix(Utils::ColMatrixXd * read_path_probs) const;

        vector<double> calcPathLogFrequences(const vector<uint32_t> & path_counts) const;
};

namespace std {

    template<>
    struct hash<vector<uint32_t> >
    {
        size_t operator()(vector<uint32_t> const & vec) const
        {
            size_t seed = 0;

            for (auto & val: vec) {

                spp::hash_combine(seed, val);
            }

            return seed;
        }
    };
}

#endif


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

#include <boost/cstdfloat.hpp>
#include <boost/static_assert.hpp>

#ifndef BOOST_FLOAT128_C
#error "There is no 128 bit floating point type"
#endif

BOOST_STATIC_ASSERT(sizeof(boost::floatmax_t) * CHAR_BIT == 128);

int main()
{
   return 0;
}


#include <iostream>
#include <string>
#include <algorithm>
#include <vector>
#include <map>
#include <boost/python.hpp>
#include <boost/python/numpy.hpp>
#include "kernel.hpp"
#include "vector.hpp"
#include "io.hpp"
#include "cosmology.hpp"
#include "spectra.hpp"

namespace p = boost::python;
namespace np = boost::python::numpy;

/* This function coverts vector to ndarray */
template<class T>
np::ndarray vec_to_ndarray(vector<T> &vec){
    size_t size;

    size = vec.size();
    np::dtype dt = np::dtype::get_builtin<double>();
    p::tuple shape = p::make_tuple(size);
    np::ndarray a = np::zeros(shape, dt);

    for(int i=0;i<size;++i) a[i] = vec[i];

    return a;
}

/* This function checks the given array is compatible */
void check_array(np::ndarray a){
    if(a.get_nd() != 1){
        cerr << "[ERROR] array must be 1-dimensional" << endl;
        exit(1);
    }

    if(a.get_dtype() != np::dtype::get_builtin<double>()){
        cerr << "[ERROR] array must be float64 array" << endl;
        exit(1);
    }

    return;
}

class pyspectrum
{
private:
  cosmology *Cosmo;
  spectra *Spectra;
  params Params;
public:
  void set_cosmology(p::dict &py_dict, np::ndarray k, np::ndarray Tk);
  np::ndarray calc_linear(np::ndarray k);
  np::ndarray calc_no_wiggle(np::ndarray k);
  np::ndarray calc_RegPT_2loop(np::ndarray k);
  np::ndarray calc_SPT_2loop(np::ndarray k);
  np::ndarray calc_RegPT_1loop(np::ndarray k);
  np::ndarray calc_SPT_1loop(np::ndarray k);
};

void pyspectrum::set_cosmology(p::dict &py_dict, np::ndarray k, np::ndarray Tk){
  int N;
  bool flag_smoothing = false;
  vector<double> k_set, Tk_set;

  check_array(k);
  check_array(Tk);

  if(k.shape(0) != Tk.shape(0)){
      cerr << "[ERROR] The length of k and Tk should be the same." << endl;
      exit(1);
  }

  N = k.shape(0);

  for(int i=0;i<N;++i){
    k_set.push_back(p::extract<double>(k[i]));
    Tk_set.push_back(p::extract<double>(Tk[i]));
  }

  p::list keys = py_dict.keys();
  for(int i=0;i<len(keys);++i){
    p::extract<string> extracted_key(keys[i]);
    if(!extracted_key.check()){
      cout << "[NOTE] Key invalid, map might be incomplete" << endl;
      continue;
    }

    string key = extracted_key;
    if(key == "smoothing"){
      p::extract<bool> extracted_val(py_dict[key]);
      if(extracted_val) flag_smoothing = true;
    }
    else{
      p::extract<double> extracted_val(py_dict[key]);
      if(!extracted_val.check()){
        cout << "[NOTE] Value invalid, map might be incomplete" << endl;
        continue;
      }
      double value = extracted_val;
      Params.dparams[key] = value;
    }
  }

  Params.bparams["output"] = false; // no output

  Cosmo = new cosmology(Params);
  Cosmo->set_transfer(k_set, Tk_set);
  Cosmo->set_spectra();
  if(flag_smoothing){
    cout << "[NOTE] Smoothed linear power spectrum is set." << endl;
    Cosmo->set_smoothed_spectra();
  }

  Spectra = new spectra(*Cosmo);

  return;
}

np::ndarray pyspectrum::calc_linear(np::ndarray k){
  double ki, Pi;
  size_t N;
  vector<double> res;

  check_array(k);
  N = k.shape(0);

  for(int i=0;i<N;++i){
    ki = p::extract<double>(k[i]);
    Pi = Spectra->Plin(ki);
    res.push_back(Pi);
  }

  return vec_to_ndarray(res);
}

np::ndarray pyspectrum::calc_no_wiggle(np::ndarray k){
  double ki, Pi;
  size_t N;
  vector<double> res;

  check_array(k);
  N = k.shape(0);

  for(int i=0;i<N;++i){
    ki = p::extract<double>(k[i]);
    Pi = Spectra->Pno_wiggle(ki);
    res.push_back(Pi);
  }

  return vec_to_ndarray(res);
}

np::ndarray pyspectrum::calc_RegPT_2loop(np::ndarray k){
  double ki, Pi;
  size_t N;
  vector<double> res;

  check_array(k);
  N = k.shape(0);

  for(int i=0;i<N;++i){
    ki = p::extract<double>(k[i]);
    Pi = Spectra->Preg_2loop(DENS, DENS, ki);
    res.push_back(Pi);
  }

  return vec_to_ndarray(res);
}

np::ndarray pyspectrum::calc_SPT_2loop(np::ndarray k){
  double ki, Pi;
  size_t N;
  vector<double> res;

  check_array(k);
  N = k.shape(0);

  for(int i=0;i<N;++i){
    ki = p::extract<double>(k[i]);
    Pi = Spectra->Pspt_2loop(DENS, DENS, ki);
    res.push_back(Pi);
  }

  return vec_to_ndarray(res);
}

np::ndarray pyspectrum::calc_RegPT_1loop(np::ndarray k){
  double ki, Pi;
  size_t N;
  vector<double> res;

  check_array(k);
  N = k.shape(0);

  for(int i=0;i<N;++i){
    ki = p::extract<double>(k[i]);
    Pi = Spectra->Preg_1loop(DENS, DENS, ki);
    res.push_back(Pi);
  }

  return vec_to_ndarray(res);
}

np::ndarray pyspectrum::calc_SPT_1loop(np::ndarray k){
  double ki, Pi;
  size_t N;
  vector<double> res;

  check_array(k);
  N = k.shape(0);

  for(int i=0;i<N;++i){
    ki = p::extract<double>(k[i]);
    Pi = Spectra->Pspt_1loop(DENS, DENS, ki);
    res.push_back(Pi);
  }

  return vec_to_ndarray(res);
}

BOOST_PYTHON_MODULE(pyeclairs){
  np::initialize();

  p::class_<pyspectrum>("pyspectrum")
      .def("set_cosmology", &pyspectrum::set_cosmology)
      .def("calc_linear", &pyspectrum::calc_linear)
      .def("calc_no_wiggle", &pyspectrum::calc_no_wiggle)
      .def("calc_RegPT_2loop", &pyspectrum::calc_RegPT_2loop)
      .def("calc_SPT_2loop", &pyspectrum::calc_SPT_2loop)
      .def("calc_RegPT_1loop", &pyspectrum::calc_RegPT_1loop)
      .def("calc_SPT_1loop", &pyspectrum::calc_SPT_1loop)
  ;

}


#pragma once

#include <Eigen/Dense>

#include "configuration.hpp"

class FixedDracoStateProvider;
class RobotSystem;
class FixedDracoSensorData;

class FixedDracoStateEstimator {
public:
  FixedDracoStateEstimator(RobotSystem *robot);
  ~FixedDracoStateEstimator();

  void initialize(FixedDracoSensorData *);
  void update(FixedDracoSensorData *);

protected:
  FixedDracoStateProvider *sp_;
  RobotSystem *robot_;

  Eigen::Isometry3d iso_base_joint_to_imu_;
  Eigen::Isometry3d iso_base_com_to_imu_;
};


/*    Copyright (c) 2010-2018, 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_MAIN

#include <limits>
#include <string>
#include <vector>

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

#include "Tudat/Basics/testMacros.h"

#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"

#include "Tudat/SimulationSetup/EstimationSetup/createObservationModel.h"
#include "Tudat/Astrodynamics/ObservationModels/oneWayRangeObservationModel.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationRate.h"
#include "Tudat/SimulationSetup/EstimationSetup/createObservationPartials.h"
#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/UnitTests/numericalObservationPartial.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createGroundStations.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"

#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/UnitTests/observationPartialTestFunctions.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::gravitation;
using namespace tudat::ephemerides;
using namespace tudat::observation_models;
using namespace tudat::simulation_setup;
using namespace tudat::spice_interface;
using namespace tudat::observation_partials;
using namespace tudat::estimatable_parameters;

BOOST_AUTO_TEST_SUITE( test_euler_angle_partials)

//! Test partial derivatives of Euler angle observable, using general test suite of observation partials.
BOOST_AUTO_TEST_CASE( testEulerAnglePartials )
{

    std::cout<<" **************************************************************************************** "<<std::endl;

    // Test partials with constant rotational ephemerides (with test of rotation state partial)
    {

        // Create environment
        NamedBodyMap bodyMap = setupEnvironment( std::vector< std::pair< std::string, std::string > >( ),
                                                 1.0E7, 1.2E7, 1.1E7, false, 1.0, true );

        // Set link ends for observation model
        LinkEnds linkEnds;
        linkEnds[ observed_body ] = std::make_pair( "Mars", "" );

        // Generate one-way range model
        std::shared_ptr< ObservationModel< 3 > > eulerAngleModel =
                observation_models::ObservationModelCreator< 3, double, double >::createObservationModel(
                    linkEnds, std::make_shared< observation_models::ObservationSettings >(
                        euler_angle_313_observable ), bodyMap  );

        // Create parameter objects.
        std::shared_ptr< EstimatableParameterSet< double > > fullEstimatableParameterSet =
                createEstimatableParameters( bodyMap, 1.1E7, false, true );

        testObservationPartials< 3 >(
                    eulerAngleModel, bodyMap, fullEstimatableParameterSet, linkEnds, euler_angle_313_observable, 1.0E-6, false, false );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include <ndt_map/ndt_map_hmt.h>
#include <ndt_map/lazy_grid.h>
#include <ndt_map/cell_vector.h>

#include <pcl_conversions/pcl_conversions.h>
#include "pcl/point_cloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/io/pcd_io.h"

#include "ros/ros.h"
#include "pcl/point_cloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/io/pcd_io.h"
#include "pcl/features/feature.h"
#include <cstdio>

#include <boost/shared_ptr.hpp>
#include <boost/thread/mutex.hpp>

#include <Eigen/Eigen>

static int ctr = 0, ctr2 = 0;
static boost::mutex mutex;
static bool fresh = false;

void ndtCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
{
    pcl::PointCloud<pcl::PointXYZ> cloud;
    pcl::fromROSMsg(*msg,cloud);

    ROS_INFO ("Received %d data points with the following fields: %s", (int)(msg->width * msg->height),
              pcl::getFieldsList (*msg).c_str ());

    perception_oru::NDTMap nd(new perception_oru::LazyGrid(2));

    Eigen::Affine3d T;
    T.setIdentity();
    nd.addPointCloud(T.translation(),cloud);
    nd.computeNDTCells();

    ROS_INFO("Loaded point cloud");
    if(ctr%10 == 0) {
	nd.writeToJFF("maps02.jff");
    }
    ctr++;

}

int
main (int argc, char** argv)
{

    ros::init(argc, argv, "ndt_builder");
    ros::NodeHandle n;
    ros::Subscriber sub1 = n.subscribe("points2_in", 10, ndtCallback);
    ros::spin();

    return (0);
}


// Copyright (c) 2015 The Bitcoin Core developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "chain.h"
#include "chainparams.h"
#include "config.h"
#include "pow.h"
#include "random.h"
#include "test/test_bitcoin.h"
#include "util.h"
#include "block_index_store.h"

#include <boost/test/unit_test.hpp>

#include <map>

namespace{ class pow_tests_uid; } // only used as unique identifier

template <>
struct CBlockIndex::UnitTestAccess<pow_tests_uid>
{
    UnitTestAccess() = delete;

    static void SetHeight( CBlockIndex& index, int32_t height)
    {
        index.nHeight = height;
    }
};
using TestAccessCBlockIndex = CBlockIndex::UnitTestAccess<pow_tests_uid>;

BOOST_FIXTURE_TEST_SUITE(pow_tests, BasicTestingSetup)

/* Test calculation of next difficulty target with no constraints applying */
BOOST_AUTO_TEST_CASE(get_next_work) {
    CBlockHeader header;
    header.nTime = 1262152739; // Block #32255
    header.nBits = 0x1d00ffff;

    CBlockIndex::TemporaryBlockIndex pindexLast{ header };
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1261130161; // Block #30240
    TestAccessCBlockIndex::SetHeight( pindexLast, 32255 );
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(pindexLast, nLastRetargetTime, config),
        0x1d00d86a);
}

/* Test the constraint on the upper bound for next work */
BOOST_AUTO_TEST_CASE(get_next_work_pow_limit) {
    CBlockHeader header;
    header.nTime = 1233061996; // Block #2015
    header.nBits = 0x1d00ffff;

    CBlockIndex::TemporaryBlockIndex pindexLast{ header };
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1231006505; // Block #0
    TestAccessCBlockIndex::SetHeight( pindexLast, 2015 );
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(pindexLast, nLastRetargetTime, config),
        0x1d00ffff);
}

/* Test the constraint on the lower bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_lower_limit_actual) {
    CBlockHeader header;
    header.nTime = 1279297671; // Block #68543
    header.nBits = 0x1c05a3f4;

    CBlockIndex::TemporaryBlockIndex pindexLast{ header };
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1279008237; // Block #66528
    TestAccessCBlockIndex::SetHeight( pindexLast, 68543 );
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(pindexLast, nLastRetargetTime, config),
        0x1c0168fd);
}

/* Test the constraint on the upper bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_upper_limit_actual) {
    CBlockHeader header;
    header.nTime = 1269211443; // Block #46367
    header.nBits = 0x1c387f6f;

    CBlockIndex::TemporaryBlockIndex pindexLast{ header };
    DummyConfig config(CBaseChainParams::MAIN);

    int64_t nLastRetargetTime = 1263163443; // NOTE: Not an actual block time
    TestAccessCBlockIndex::SetHeight( pindexLast, 46367 );
    BOOST_CHECK_EQUAL(
        CalculateNextWorkRequired(pindexLast, nLastRetargetTime, config),
        0x1d00e1fd);
}

BOOST_AUTO_TEST_CASE(GetBlockProofEquivalentTime_test) {
    DummyConfig config(CBaseChainParams::MAIN);
    BlockIndexStore blockIndexStore;
    CChain blocks;

    uint256 prev;

    for (int i = 0; i < 10000; i++) {
        CBlockHeader header;
        header.nTime =
            1269211443 +
            i * config.GetChainParams().GetConsensus().nPowTargetSpacing;
        header.nBits = 0x207fffff; /* target 0x7fffff000... */
        header.hashPrevBlock = prev;

        blocks.SetTip( blockIndexStore.Insert( header ) );

        prev = blocks.Tip()->GetBlockHash();
    }

    for (int j = 0; j < 1000; j++) {
        CBlockIndex *p1 = blocks[InsecureRandRange(10000)];
        CBlockIndex *p2 = blocks[InsecureRandRange(10000)];
        CBlockIndex *p3 = blocks[InsecureRandRange(10000)];

        int64_t tdiff = GetBlockProofEquivalentTime(
            *p1, *p2, *p3, config.GetChainParams().GetConsensus());
        BOOST_CHECK_EQUAL(tdiff, p1->GetBlockTime() - p2->GetBlockTime());
    }
}

static CBlockIndex* GetBlockIndex(
    CBlockIndex* pindexPrev,
    int64_t nTimeInterval,
    uint32_t nBits,
    BlockIndexStore& blockIndexStore)
{
    CBlockHeader header;
    header.nTime = pindexPrev->GetBlockTime() + nTimeInterval;
    header.nBits = nBits;
    header.nNonce = blockIndexStore.Count();
    header.hashPrevBlock = pindexPrev->GetBlockHash();

    return blockIndexStore.Insert( header );
}

BOOST_AUTO_TEST_CASE(retargeting_test) {
    DummyConfig config(CBaseChainParams::MAIN);
    BlockIndexStore blockIndexStore;
    CChain blocks;

    const Consensus::Params &params = config.GetChainParams().GetConsensus();
    const arith_uint256 powLimit = UintToArith256(params.powLimit);
    arith_uint256 currentPow = powLimit >> 1;
    uint32_t initialBits = currentPow.GetCompact();

    // Genesis block.
    {
        CBlockHeader header;
        header.nTime = 1269211443;
        header.nBits = initialBits;
        blocks.SetTip( blockIndexStore.Insert( header ) );
    }

    // Pile up some blocks.
    for (size_t i = 1; i < 100; i++) {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                params.nPowTargetSpacing,
                initialBits,
                blockIndexStore) );
    }

    CBlockHeader blkHeaderDummy;

    // We start getting 2h blocks time. For the first 5 blocks, it doesn't
    // matter as the MTP is not affected. For the next 5 block, MTP difference
    // increases but stays below 12h.
    for (size_t i = 100; i < 110; i++) {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                2 * 3600,
                initialBits,
                blockIndexStore) );
        BOOST_CHECK_EQUAL(
            GetNextWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
            initialBits);
    }

    // Now we expect the difficulty to decrease.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            2 * 3600,
            initialBits,
            blockIndexStore) );
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
        currentPow.GetCompact());

    // As we continue with 2h blocks, difficulty continue to decrease.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            2 * 3600,
            currentPow.GetCompact(),
            blockIndexStore) );
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
        currentPow.GetCompact());

    // We decrease again.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            2 * 3600,
            currentPow.GetCompact(),
            blockIndexStore) );
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
        currentPow.GetCompact());

    // We check that we do not go below the minimal difficulty.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            2 * 3600,
            currentPow.GetCompact(),
            blockIndexStore) );
    currentPow.SetCompact(currentPow.GetCompact());
    currentPow += (currentPow >> 2);
    BOOST_CHECK(powLimit.GetCompact() != currentPow.GetCompact());
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
        powLimit.GetCompact());

    // Once we reached the minimal difficulty, we stick with it.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            2 * 3600,
            currentPow.GetCompact(),
            blockIndexStore) );
    BOOST_CHECK(powLimit.GetCompact() != currentPow.GetCompact());
    BOOST_CHECK_EQUAL(
        GetNextWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
        powLimit.GetCompact());
}

BOOST_AUTO_TEST_CASE(cash_difficulty_test) {
    DummyConfig config(CBaseChainParams::MAIN);
    BlockIndexStore blockIndexStore;
    CChain blocks;

    const Consensus::Params &params = config.GetChainParams().GetConsensus();
    const arith_uint256 powLimit = UintToArith256(params.powLimit);
    uint32_t powLimitBits = powLimit.GetCompact();
    arith_uint256 currentPow = powLimit >> 4;
    uint32_t initialBits = currentPow.GetCompact();

    // Genesis block.
    {
        CBlockHeader header;
        header.nTime = 1269211443;
        header.nBits = initialBits;
        blocks.SetTip( blockIndexStore.Insert( header ) );
    }

    // Pile up some blocks every 10 mins to establish some history.
    for (size_t i = 1; i < 2050; ++i)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                600,
                initialBits,
                blockIndexStore) );
    }

    CBlockHeader blkHeaderDummy;
    uint32_t nBits =
        GetNextCashWorkRequired(blocks[2049], &blkHeaderDummy, config);

    // Difficulty stays the same as long as we produce a block every 10 mins.
    for (size_t j = 0; j < 10; ++j)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                600,
                nBits,
                blockIndexStore) );
        BOOST_CHECK_EQUAL(
            GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
            nBits);
    }

    // Make sure we skip over blocks that are out of wack. To do so, we produce
    // a block that is far in the future, and then produce a block with the
    // expected timestamp.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            6000,
            nBits,
            blockIndexStore) );
    BOOST_CHECK_EQUAL(
        GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config), nBits);
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            2 * 600 - 6000,
            nBits,
            blockIndexStore) );
    BOOST_CHECK_EQUAL(
        GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config), nBits);

    // The system should continue unaffected by the block with a bogous
    // timestamps.
    for (size_t j = 0; j < 20; ++j)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                600,
                nBits,
                blockIndexStore) );
        BOOST_CHECK_EQUAL(
            GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config),
            nBits);
    }

    // We start emitting blocks slightly faster. The first block has no impact.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            550,
            nBits,
            blockIndexStore) );
    BOOST_CHECK_EQUAL(
        GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config), nBits);

    // Now we should see difficulty increase slowly.
    for (size_t j = 0; j < 10; ++j)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                550,
                nBits,
                blockIndexStore) );
        const uint32_t nextBits =
            GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Make sure that difficulty increases very slowly.
        BOOST_CHECK(nextTarget < currentTarget);
        BOOST_CHECK((currentTarget - nextTarget) < (currentTarget >> 10));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c0fe7b1);

    // If we dramatically shorten block production, difficulty increases faster.
    for (size_t j = 0; j < 20; ++j)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                10,
                nBits,
                blockIndexStore) );
        const uint32_t nextBits =
            GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Make sure that difficulty increases faster.
        BOOST_CHECK(nextTarget < currentTarget);
        BOOST_CHECK((currentTarget - nextTarget) < (currentTarget >> 4));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c0db19f);

    // We start to emit blocks significantly slower. The first block has no
    // impact.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            6000,
            nBits,
            blockIndexStore) );
    nBits = GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config);

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c0d9222);

    // If we dramatically slow down block production, difficulty decreases.
    for (size_t j = 0; j < 93; ++j)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                6000,
                nBits,
                blockIndexStore) );
        const uint32_t nextBits =
            GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Check the difficulty decreases.
        BOOST_CHECK(nextTarget <= powLimit);
        BOOST_CHECK(nextTarget > currentTarget);
        BOOST_CHECK((nextTarget - currentTarget) < (currentTarget >> 3));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1c2f13b9);

    // Due to the window of time being bounded, next block's difficulty actually
    // gets harder.
    blocks.SetTip(
        GetBlockIndex(
            blocks.Tip(),
            6000,
            nBits,
            blockIndexStore) );
    nBits = GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config);
    BOOST_CHECK_EQUAL(nBits, 0x1c2ee9bf);

    // And goes down again. It takes a while due to the window being bounded and
    // the skewed block causes 2 blocks to get out of the window.
    for (size_t j = 0; j < 192; ++j)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                6000,
                nBits,
                blockIndexStore) );
        const uint32_t nextBits =
            GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config);

        arith_uint256 currentTarget;
        currentTarget.SetCompact(nBits);
        arith_uint256 nextTarget;
        nextTarget.SetCompact(nextBits);

        // Check the difficulty decreases.
        BOOST_CHECK(nextTarget <= powLimit);
        BOOST_CHECK(nextTarget > currentTarget);
        BOOST_CHECK((nextTarget - currentTarget) < (currentTarget >> 3));

        nBits = nextBits;
    }

    // Check the actual value.
    BOOST_CHECK_EQUAL(nBits, 0x1d00ffff);

    // Once the difficulty reached the minimum allowed level, it doesn't get any
    // easier.
    for (size_t j = 0; j < 5; ++j)
    {
        blocks.SetTip(
            GetBlockIndex(
                blocks.Tip(),
                6000,
                nBits,
                blockIndexStore) );
        const uint32_t nextBits =
            GetNextCashWorkRequired(blocks.Tip(), &blkHeaderDummy, config);

        // Check the difficulty stays constant.
        BOOST_CHECK_EQUAL(nextBits, powLimitBits);
        nBits = nextBits;
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef TEMPL_PLANNING_SOLVERS_TEMPORAL_INTERVAL_HPP
#define TEMPL_PLANNING_SOLVERS_TEMPORAL_INTERVAL_HPP

#include "point_algebra/TimePoint.hpp"
#include "point_algebra/TimePointComparator.hpp"
#include <set>
#include <vector>

#include <boost/serialization/set.hpp>
#include <boost/serialization/vector.hpp>

namespace templ {
namespace solvers {
namespace temporal {

/**
 * \class Interval
 * \brief An interval between two TimePoints
 * \details
 */
class Interval
{
    point_algebra::TimePoint::Ptr mpFrom;
    point_algebra::TimePoint::Ptr mpTo;

    point_algebra::TimePointComparator mTimePointComparator;

public:
    typedef std::vector<Interval> List;
    typedef std::set<Interval> Set;

    Interval();

    Interval(const point_algebra::TimePoint::Ptr& from,
            const point_algebra::TimePoint::Ptr& to,
            const point_algebra::TimePointComparator& comparator);

    /**
     * Check the valid initialization of this interval
     * \throw std::invalid_argument Throw if interval is not properly initialized
     */
    void validate() const;

    void setFrom(const point_algebra::TimePoint::Ptr& from) { mpFrom = from; }
    const point_algebra::TimePoint::Ptr& getFrom() const { return mpFrom; }

    void setTo(const point_algebra::TimePoint::Ptr& to) { mpTo = to; }
    const point_algebra::TimePoint::Ptr& getTo() const { return mpTo; }

    void setTimePointComparator(const point_algebra::TimePointComparator& comparator) { mTimePointComparator = comparator; }
    const point_algebra::TimePointComparator& getTimePointComparator() const { return mTimePointComparator; }

    bool operator==(const Interval& other) const { return equals(other); }
    /**
     * Implement less operator for general use in set etc.
     */
    bool operator<(const Interval& other) const;

    /**
     * Check if interval [s,t) contains a particular timepoint t, i.e
     * t in [s,t]
     */
    bool contains(const point_algebra::TimePoint::Ptr& t) const;

    /**
     * Check if interval [s,t) contains a particular timepoint t, i.e
     * t in [s,t]
     * \param t TimePoint
     * \param tpc TimePointComparator which is associated with a particular
     * temporal constraint network
     */
    bool contains(const point_algebra::TimePoint::Ptr& t,
                const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Check before:
     * Symbol: b , inverse: bi
     \verbatim
     [ ---- this ---- ]
                             [ ---- other ---- ]
     \endverbatim
     */
    bool before(const Interval& other) const;

    /**
     * Check before:
     * Symbol: b , inverse: bi
     \verbatim
     [ ---- this ---- ]
                             [ ---- other ---- ]
     \endverbatim
     * \param t TimePoint
     * \param tpc TimePointComparator which is associated with a
     temporal constraint network
     */
    bool before(const Interval& other,
                const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Check equals
     * Symbol: =, inverse: =
     \verbatim
     [ ---- this ---- ]
     [ --- other ---- ]
     \endverbatim
     */
    bool equals(const Interval& other) const;

    /**
     * Check equals
     * Symbol: =, inverse: =
     \verbatim
     [ ---- this ---- ]
     [ --- other ---- ]
     \endverbatim
     */
    bool equals(const Interval& other,
                const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Check meets
     * Symbol: m, inverse: m
     \verbatim
     [ ---- this ---- ]
                      [ --- other ---- ]
     \endverbatim
     */
    bool meets(const Interval& other) const;

    /**
     * Check meets
     * Symbol: m, inverse: m
     \verbatim
     [ ---- this ---- ]
                      [ --- other ---- ]
     \endverbatim
     */
    bool meets(const Interval& other,
            const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Check overlaps
     * Symbol: o, inverse: oi
     \verbatim
     [ ---- this ---- ]
                  [ --- other ---- ]
     \endverbatim
     */
    bool overlaps(const Interval& other) const;

    /**
     * Check overlaps
     * Symbol: o, inverse: oi
     \verbatim
     [ ---- this ---- ]
                  [ --- other ---- ]
     \endverbatim
     \param other Other interval
     \param tpc TimePointComparator which is related to a temporal constraint network
     */
    bool overlaps(const Interval& other,
            const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Check during
     * Symbol: d, inverse: di
     \verbatim
     [ ----           this          ---- ]
                  [ --- other ---- ]
     \endverbatim
     */
    bool during(const Interval& other) const;

    /**
     * Check during
     * Symbol: d, inverse: di
     \verbatim
     [ ----           this          ---- ]
                  [ --- other ---- ]
     \endverbatim
     */
    bool during(const Interval& other,
                const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Check starts
     * Symbol: s, inverse: si
     \verbatim
     [ ----           this          ---- ]
     [ --- other ---- ]
     \endverbatim
     */
    bool starts(const Interval& other) const;

    /**
     * Check starts
     * Symbol: s, inverse: si
     \verbatim
     [ ----           this          ---- ]
     [ --- other ---- ]
     \endverbatim
     */
    bool starts(const Interval& other,
                const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Check finishes
     * Symbol: f, inverse: fi
     \verbatim
     [ ----           this          ---- ]
                        [ --- other ---- ]
     \endverbatim
     */
    bool finishes(const Interval& other) const;

    /**
     * Check finishes
     * Symbol: f, inverse: fi
     \verbatim
     [ ----           this          ---- ]
                        [ --- other ---- ]
     \endverbatim
     */
    bool finishes(const Interval& other,
                const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Negation of equals
     */
    bool distinctFrom(const Interval& other) const;

    /**
     * Negation of equals
     */
    bool distinctFrom(const Interval& other,
                const temporal::point_algebra::TimePointComparator& tpc) const;

    /**
     * Stringify interval object
     */
    std::string toString(uint32_t indent = 0, bool compact = false) const;

    /**
     * Stringify list
     */
    static std::string toString(const List& list, size_t indent = 0, bool compact = false);

    /**
     * Check if all given intervals overlap with each other
     * \return true if all intervals in the given list overlap with each other
     *      false otherwise
     */
    static bool areOverlapping(const std::vector<uint32_t>& indices, const std::vector<Interval>& intervals);

    /**
     * Get all combinations of intervals that overlap with each other
     * \return set of overlapping interval combinations using the index of given
     * set of intervals
     */
    static std::set< std::vector<uint32_t> > overlappingIntervals(const std::vector<Interval>& intervals);

};

} // end namespace temporal
} // end namespace solvers
} // end namespace templ

namespace std
{
/// Define hash function so that class can be used in std::ordered_set
/// \see http://en.cppreference.com/w/cpp/utility/hash
template<>
struct hash<templ::solvers::temporal::Interval>
{
    size_t operator()(const templ::solvers::temporal::Interval &i) const
    {
        using namespace templ::solvers::temporal::point_algebra;

        // std::hash<T*> is defined as well as for std::shared_ptr
        size_t h1 = std::hash<TimePoint*>()(i.getFrom().get());
        size_t h2 = std::hash<TimePoint*>()(i.getTo().get());
        return h1 ^ ( h2 << 1 );
    }
};

}
#endif // TEMPL_SOLVERS_TEMPORAL_INTERVAL_HPP


#include <fstream>
#include <iostream>

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>

#include <yaml-cpp/yaml.h>

#include <ecbs.hpp>
#include "timer.hpp"

using libMultiRobotPlanning::ECBS;
using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;

struct State {
  State(int time, int x, int y, int z) : time(time), x(x), y(y), z(z) {}

  bool operator==(const State& s) const {
    return time == s.time && x == s.x && y == s.y && z == s.z;
  }

  bool equalExceptTime(const State& s) const { return x == s.x && y == s.y && z == s.z; }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << s.time << ": (" << s.x << "," << s.y << s.z << ")";
    // return os << "(" << s.x << "," << s.y << s.z <<")";
  }

  int time;
  int x;
  int y;
  int z;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    boost::hash_combine(seed, s.z);
    return seed;
  }
};
}  // namespace std

///
enum class Action {
  Up,
  Down,
  Left,
  Right,
  Top,
  Bottom,
  Wait,
};

std::ostream& operator<<(std::ostream& os, const Action& a) {
  switch (a) {
    case Action::Up:
      os << "Up";
      break;
    case Action::Down:
      os << "Down";
      break;
    case Action::Left:
      os << "Left";
      break;
    case Action::Right:
      os << "Right";
      break;
    case Action::Top:
        os << "Top";
        break;
    case Action::Bottom:
        os << "Bottom";
        break;
    case Action::Wait:
      os << "Wait";
      break;
  }
  return os;
}

///

struct Conflict {
  enum Type {
    Vertex,
    Edge,
  };

  int time;
  size_t agent1;
  size_t agent2;
  Type type;

  int x1;
  int y1;
  int x2;
  int y2;
  int z1;
  int z2;

  friend std::ostream& operator<<(std::ostream& os, const Conflict& c) {
    switch (c.type) {
      case Vertex:
        return os << c.time << ": Vertex(" << c.x1 << "," << c.y1 << c.z1 << ")";
      case Edge:
        return os << c.time << ": Edge(" << c.x1 << "," << c.y1 << c.z1 << "," << c.x2
                  << "," << c.y2 << c.z2 << ")";
    }
    return os;
  }
};

struct VertexConstraint {
  VertexConstraint(int time, int x, int y, int z) : time(time), x(x), y(y), z(z) {}
  int time;
  int x;
  int y;
  int z;

  bool operator<(const VertexConstraint& other) const {
    return std::tie(time, x, y, z) < std::tie(other.time, other.x, other.y, other.z);
  }

  bool operator==(const VertexConstraint& other) const {
    return std::tie(time, x, y, z) == std::tie(other.time, other.x, other.y, other.z);
  }

  friend std::ostream& operator<<(std::ostream& os, const VertexConstraint& c) {
    return os << "VC(" << c.time << "," << c.x << "," << c.y << c.z << ")";
  }
};

namespace std {
template <>
struct hash<VertexConstraint> {
  size_t operator()(const VertexConstraint& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    boost::hash_combine(seed, s.z);
    return seed;
  }
};
}  // namespace std

struct EdgeConstraint {
  EdgeConstraint(int time, int x1, int y1, int x2, int y2, int z1, int z2)
      : time(time), x1(x1), y1(y1), x2(x2), y2(y2), z1(z1), z2(z2) {}
  int time;
  int x1;
  int y1;
  int x2;
  int y2;
  int z1;
  int z2;

  bool operator<(const EdgeConstraint& other) const {
    return std::tie(time, x1, y1, x2, y2, z1, z2) <
           std::tie(other.time, other.x1, other.y1, other.x2, other.y2, other.z1, other.z2);
  }

  bool operator==(const EdgeConstraint& other) const {
    return std::tie(time, x1, y1, x2, y2, z1, z2) ==
           std::tie(other.time, other.x1, other.y1, other.x2, other.y2, other.z1, other.z2);
  }

  friend std::ostream& operator<<(std::ostream& os, const EdgeConstraint& c) {
    return os << "EC(" << c.time << "," << c.x1 << "," << c.y1 << "," << c.z1 << ","
              << c.x2 << "," << c.y2 << c.z2 << ")";
  }
};

namespace std {
template <>
struct hash<EdgeConstraint> {
  size_t operator()(const EdgeConstraint& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x1);
    boost::hash_combine(seed, s.y1);
    boost::hash_combine(seed, s.x2);
    boost::hash_combine(seed, s.y2);
    boost::hash_combine(seed, s.z1);
    boost::hash_combine(seed, s.z2);
    return seed;
  }
};
}  // namespace std

struct Constraints {
  std::unordered_set<VertexConstraint> vertexConstraints;
  std::unordered_set<EdgeConstraint> edgeConstraints;

  void add(const Constraints& other) {
    vertexConstraints.insert(other.vertexConstraints.begin(),
                             other.vertexConstraints.end());
    edgeConstraints.insert(other.edgeConstraints.begin(),
                           other.edgeConstraints.end());
  }

  bool overlap(const Constraints& other) {
    std::vector<VertexConstraint> vertexIntersection;
    std::vector<EdgeConstraint> edgeIntersection;
    std::set_intersection(vertexConstraints.begin(), vertexConstraints.end(),
                          other.vertexConstraints.begin(),
                          other.vertexConstraints.end(),
                          std::back_inserter(vertexIntersection));
    std::set_intersection(edgeConstraints.begin(), edgeConstraints.end(),
                          other.edgeConstraints.begin(),
                          other.edgeConstraints.end(),
                          std::back_inserter(edgeIntersection));
    return !vertexIntersection.empty() || !edgeIntersection.empty();
  }

  friend std::ostream& operator<<(std::ostream& os, const Constraints& c) {
    for (const auto& vc : c.vertexConstraints) {
      os << vc << std::endl;
    }
    for (const auto& ec : c.edgeConstraints) {
      os << ec << std::endl;
    }
    return os;
  }
};

struct Location {
  Location(int x, int y, int z) : x(x), y(y), z(z) {}
  int x;
  int y;
  int z;

  bool operator<(const Location& other) const {
    return std::tie(x, y, z) < std::tie(other.x, other.y, other.z);
  }

  bool operator==(const Location& other) const {
    return std::tie(x, y, z) == std::tie(other.x, other.y, other.z);
  }

  friend std::ostream& operator<<(std::ostream& os, const Location& c) {
    return os << "(" << c.x << "," << c.y << c.z << ")";
  }
};

namespace std {
template <>
struct hash<Location> {
  size_t operator()(const Location& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    boost::hash_combine(seed, s.z);
    return seed;
  }
};
}  // namespace std

///
class Environment {
 public:
  Environment(size_t dimx, size_t dimy, size_t dimz,
              std::unordered_set<Location> obstacles,
              std::vector<Location> goals)
      : m_dimx(dimx),
        m_dimy(dimy),
        m_dimz(dimz),
        m_obstacles(std::move(obstacles)),
        m_goals(std::move(goals)),
        m_agentIdx(0),
        m_constraints(nullptr),
        m_lastGoalConstraint(-1),
        m_highLevelExpanded(0),
        m_lowLevelExpanded(0) {}

  Environment(const Environment&) = delete;
  Environment& operator=(const Environment&) = delete;

  //find last goal constraint!
  void setLowLevelContext(size_t agentIdx, const Constraints* constraints) {
    assert(constraints);
    m_agentIdx = agentIdx;
    m_constraints = constraints;
    m_lastGoalConstraint = -1;
    for (const auto& vc : constraints->vertexConstraints) {
      if (vc.x == m_goals[m_agentIdx].x && vc.y == m_goals[m_agentIdx].y && vc.z == m_goals[m_agentIdx].z) {
        m_lastGoalConstraint = std::max(m_lastGoalConstraint, vc.time);
      }
    }
  }

  int admissibleHeuristic(const State& s) {
    return std::abs(s.x - m_goals[m_agentIdx].x) +
           std::abs(s.y - m_goals[m_agentIdx].y) +
           std::abs(s.z - m_goals[m_agentIdx].z);
  }

  // low-level, get numConflict(equal state) from given solution
  int focalStateHeuristic(
      const State& s, int /*gScore*/,
      const std::vector<PlanResult<State, Action, int> >& solution) {
    int numConflicts = 0;
    for (size_t i = 0; i < solution.size(); ++i) {
      if (i != m_agentIdx && !solution[i].states.empty()) {
        State state2 = getState(i, solution, s.time);
        if (s.equalExceptTime(state2)) {
          ++numConflicts;
        }
      }
    }
    return numConflicts;
  }

  // low-level, get numConflict(s1a <-> s1b) from given solution
  int focalTransitionHeuristic(
      const State& s1a, const State& s1b, int /*gScoreS1a*/, int /*gScoreS1b*/,
      const std::vector<PlanResult<State, Action, int> >& solution) {
    int numConflicts = 0;
    for (size_t i = 0; i < solution.size(); ++i) {
      if (i != m_agentIdx && !solution[i].states.empty()) {
        State s2a = getState(i, solution, s1a.time);
        State s2b = getState(i, solution, s1b.time);
        if (s1a.equalExceptTime(s2b) && s1b.equalExceptTime(s2a)) {
          ++numConflicts;
        }
      }
    }
    return numConflicts;
  }

  // Count all conflicts
  int focalHeuristic(
      const std::vector<PlanResult<State, Action, int> >& solution) {
    int numConflicts = 0;

    int max_t = 0;
    for (const auto& sol : solution) {
      max_t = std::max<int>(max_t, sol.states.size() - 1);
    }

    for (int t = 0; t < max_t; ++t) {
      // check drive-drive vertex collisions
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1 = getState(i, solution, t);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2 = getState(j, solution, t);
          if (state1.equalExceptTime(state2)) {
            ++numConflicts;
          }
        }
      }
      // drive-drive edge (swap)
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1a = getState(i, solution, t);
        State state1b = getState(i, solution, t + 1);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2a = getState(j, solution, t);
          State state2b = getState(j, solution, t + 1);
          if (state1a.equalExceptTime(state2b) &&
              state1b.equalExceptTime(state2a)) {
            ++numConflicts;
          }
        }
      }
    }
    return numConflicts;
  }

  bool isSolution(const State& s) {
    return s.x == m_goals[m_agentIdx].x && s.y == m_goals[m_agentIdx].y && s.z == m_goals[m_agentIdx].z &&
           s.time > m_lastGoalConstraint;
  }

  void getNeighbors(const State& s,
                    std::vector<Neighbor<State, Action, int> >& neighbors) {
    // std::cout << "#VC " << constraints.vertexConstraints.size() << std::endl;
    // for(const auto& vc : constraints.vertexConstraints) {
    //   std::cout << "  " << vc.time << "," << vc.x << "," << vc.y << "," << vc.z <<
    //   std::endl;
    // }
    neighbors.clear();
    {
      State n(s.time + 1, s.x, s.y, s.z);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Wait, 1));
      }
    }
    {
      State n(s.time + 1, s.x - 1, s.y, s.z);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Left, 1));
      }
    }
    {
      State n(s.time + 1, s.x + 1, s.y, s.z);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Right, 1));
      }
    }
    {
      State n(s.time + 1, s.x, s.y + 1, s.z);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Up, 1));
      }
    }
    {
      State n(s.time + 1, s.x, s.y - 1, s.z);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Down, 1));
      }
    }
    {
      State n(s.time + 1, s.x, s.y, s.z + 1);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Top, 1));
      }
    }
    {
      State n(s.time + 1, s.x, s.y, s.z - 1);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Bottom, 1));
      }
    }
  }

  bool getFirstConflict(
      const std::vector<PlanResult<State, Action, int> >& solution,
      Conflict& result) {
    int max_t = 0;
    for (const auto& sol : solution) {
      max_t = std::max<int>(max_t, sol.states.size() - 1);
    }

    for (int t = 0; t < max_t; ++t) {
      // check drive-drive vertex collisions
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1 = getState(i, solution, t);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2 = getState(j, solution, t);
          if (state1.equalExceptTime(state2)) {
            result.time = t;
            result.agent1 = i;
            result.agent2 = j;
            result.type = Conflict::Vertex;
            result.x1 = state1.x;
            result.y1 = state1.y;
            result.z1 = state1.z;
            // std::cout << "VC " << t << "," << state1.x << "," << state1.y << "," << state1.z <<
            // std::endl;
            return true;
          }
        }
      }
      // drive-drive edge (swap)
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1a = getState(i, solution, t);
        State state1b = getState(i, solution, t + 1);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2a = getState(j, solution, t);
          State state2b = getState(j, solution, t + 1);
          if (state1a.equalExceptTime(state2b) &&
              state1b.equalExceptTime(state2a)) {
            result.time = t;
            result.agent1 = i;
            result.agent2 = j;
            result.type = Conflict::Edge;
            result.x1 = state1a.x;
            result.y1 = state1a.y;
            result.z1 = state1a.z;
            result.x2 = state1b.x;
            result.y2 = state1b.y;
            result.z2 = state1b.z;
            return true;
          }
        }
      }
    }

    return false;
  }

  void createConstraintsFromConflict(
      const Conflict& conflict, std::map<size_t, Constraints>& constraints) {
    if (conflict.type == Conflict::Vertex) {
      Constraints c1;
      c1.vertexConstraints.emplace(
          VertexConstraint(conflict.time, conflict.x1, conflict.y1, conflict.z1));
      constraints[conflict.agent1] = c1;
      constraints[conflict.agent2] = c1;
    } else if (conflict.type == Conflict::Edge) {
      Constraints c1;
      c1.edgeConstraints.emplace(EdgeConstraint(
          conflict.time, conflict.x1, conflict.y1, conflict.z1, conflict.x2, conflict.y2, conflict.z2));
      constraints[conflict.agent1] = c1;
      Constraints c2;
      c2.edgeConstraints.emplace(EdgeConstraint(
          conflict.time, conflict.x2, conflict.y2, conflict.z2, conflict.x1, conflict.y1, conflict.z1));
      constraints[conflict.agent2] = c2;
    }
  }

  void onExpandHighLevelNode(int /*cost*/) { m_highLevelExpanded++; }

  void onExpandLowLevelNode(const State& /*s*/, int /*fScore*/,
                            int /*gScore*/) {
    m_lowLevelExpanded++;
  }

  int highLevelExpanded() { return m_highLevelExpanded; }

  int lowLevelExpanded() const { return m_lowLevelExpanded; }

 private:
  State getState(size_t agentIdx,
                 const std::vector<PlanResult<State, Action, int> >& solution,
                 size_t t) {
    assert(agentIdx < solution.size());
    if (t < solution[agentIdx].states.size()) {
      return solution[agentIdx].states[t].first;
    }
    assert(!solution[agentIdx].states.empty());
    return solution[agentIdx].states.back().first;
  }

  bool stateValid(const State& s) {
    assert(m_constraints);
    const auto& con = m_constraints->vertexConstraints;
    return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy && s.z >= 0 && s.z < m_dimz &&
           m_obstacles.find(Location(s.x, s.y, s.z)) == m_obstacles.end() &&
           con.find(VertexConstraint(s.time, s.x, s.y, s.z)) == con.end();
  }

  bool transitionValid(const State& s1, const State& s2) {
    assert(m_constraints);
    const auto& con = m_constraints->edgeConstraints;
    return con.find(EdgeConstraint(s1.time, s1.x, s1.y, s1.z, s2.x, s2.y, s2.z)) ==
           con.end();
  }

 private:
  int m_dimx;
  int m_dimy;
  int m_dimz;
  std::unordered_set<Location> m_obstacles;
  std::vector<Location> m_goals;
  size_t m_agentIdx;
  const Constraints* m_constraints;
  int m_lastGoalConstraint;
  int m_highLevelExpanded;
  int m_lowLevelExpanded;
};

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string inputFile;
  std::string outputFile;
  float w;
  desc.add_options()("help", "produce help message")(
      "input,i", po::value<std::string>(&inputFile)->required(),
      "input file (YAML)")("output,o",
                           po::value<std::string>(&outputFile)->required(),
                           "output file (YAML)")(
      "suboptimality,w", po::value<float>(&w)->default_value(1.0),
      "suboptimality bound");

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  YAML::Node config = YAML::LoadFile(inputFile);

  std::unordered_set<Location> obstacles;
  std::vector<Location> goals;
  std::vector<State> startStates;

  const auto& dim = config["map"]["dimensions"];
  int dimx = dim[0].as<int>();
  int dimy = dim[1].as<int>();
  int dimz = dim[2].as<int>();

  for (const auto& node : config["map"]["obstacles"]) {
    obstacles.insert(Location(node[0].as<int>(), node[1].as<int>(), node[2].as<int>()));
  }

  for (const auto& node : config["agents"]) {
    const auto& start = node["start"];
    const auto& goal = node["goal"];
    startStates.emplace_back(State(0, start[0].as<int>(), start[1].as<int>(), start[2].as<int>()));
    // std::cout << "s: " << startStates.back() << std::endl;
    goals.emplace_back(Location(goal[0].as<int>(), goal[1].as<int>(), goal[2].as<int>()));
  }

  Environment mapf(dimx, dimy, dimz, obstacles, goals);
  ECBS<State, Action, int, Conflict, Constraints, Environment> cbs(mapf, w);
  std::vector<PlanResult<State, Action, int> > solution;

  Timer timer;
  bool success = cbs.search(startStates, solution);
  timer.stop();

  if (success) {
    std::cout << "Planning successful! " << std::endl;
    int cost = 0;
    int makespan = 0;
    for (const auto& s : solution) {
      cost += s.cost;
      makespan = std::max<int>(makespan, s.cost);
    }

    std::ofstream out(outputFile);
    out << "statistics:" << std::endl;
    out << "  cost: " << cost << std::endl;
    out << "  makespan: " << makespan << std::endl;
    out << "  runtime: " << timer.elapsedSeconds() << std::endl;
    out << "  highLevelExpanded: " << mapf.highLevelExpanded() << std::endl;
    out << "  lowLevelExpanded: " << mapf.lowLevelExpanded() << std::endl;
    out << "schedule:" << std::endl;
    for (size_t a = 0; a < solution.size(); ++a) {
      // std::cout << "Solution for: " << a << std::endl;
      // for (size_t i = 0; i < solution[a].actions.size(); ++i) {
      //   std::cout << solution[a].states[i].second << ": " <<
      //   solution[a].states[i].first << "->" << solution[a].actions[i].first
      //   << "(cost: " << solution[a].actions[i].second << ")" << std::endl;
      // }
      // std::cout << solution[a].states.back().second << ": " <<
      // solution[a].states.back().first << std::endl;

      out << "  agent" << a << ":" << std::endl;
      for (const auto& state : solution[a].states) {
        out << "    - x: " << state.first.x << std::endl
            << "      y: " << state.first.y << std::endl
            << "      z: " << state.first.z << std::endl
            << "      t: " << state.second << std::endl;
      }
    }
  } else {
    std::cout << "Planning NOT successful!" << std::endl;
  }

  return 0;
}


#include <type_traits>
#include <complex>
#include <vector>
#include <armadillo>

// This trick taken from https://stackoverflow.com/a/41495065.

template <typename>
struct is_arma_container_t : public std::false_type
{ };

template <typename T>
struct is_arma_container_t<arma::Row<T>> : public std::true_type
{ };

template <typename T>
struct is_arma_container_t<arma::Col<T>> : public std::true_type
{ };

template <typename T>
struct is_arma_container_t<arma::Mat<T>> : public std::true_type
{ };

template <typename T>
struct is_arma_container_t<arma::Cube<T>> : public std::true_type
{ };

template <typename T>
constexpr bool is_arma_container(T const &) {
    return is_arma_container_t<T>::value;
}

int main() {
    arma::mat mydmat;
    arma::cube mydcube;
    arma::Col<std::complex<double>> mycdcol;
    std::vector<double> mystldvec;
    double myd;

    std::cout << is_arma_container(mydmat) << std::endl;
    std::cout << is_arma_container(mydcube) << std::endl;
    std::cout << is_arma_container(mycdcol) << std::endl;
    std::cout << is_arma_container(mystldvec) << std::endl;
    std::cout << is_arma_container(myd) << std::endl;

    return 0;
}


#define BOOST_TEST_MODULE "test_parse_range"
#include <boost/test/included/unit_test.hpp>
#include <jarngreipr/util/parse_range.hpp>

std::string operator"" _str(const char* c, std::size_t l)
{
    return std::string(c);
}

BOOST_AUTO_TEST_CASE(test_parse_range_simple)
{
    {
        const auto range = jarngreipr::parse_range("[1,10]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[1,  10]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[1, 10)"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("(1, 10)"_str);
        BOOST_TEST(range.size() == 8u);
        for(std::int64_t i=0; i<8; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }
    {
        const auto range = jarngreipr::parse_range("(1, 10]"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }

    {
        const auto range = jarngreipr::parse_range("[ 1,10 ]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[  1,  10 ]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[ 1, 10 )"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("( 1, 10 )"_str);
        BOOST_TEST(range.size() == 8u);
        for(std::int64_t i=0; i<8; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }
    {
        const auto range = jarngreipr::parse_range("( 1, 10 ]"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }
}

BOOST_AUTO_TEST_CASE(test_parse_range_signs)
{
    {
        const auto range = jarngreipr::parse_range("[+1, +10]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[+1, +10)"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("(+1, +10)"_str);
        BOOST_TEST(range.size() == 8u);
        for(std::int64_t i=0; i<8; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }
    {
        const auto range = jarngreipr::parse_range("(+1, +10]"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[-10, -1]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i-10); // from -10 to -1
        }
    }
    {
        const auto range = jarngreipr::parse_range("[-10, -1)"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i-10);
        }
    }
    {
        const auto range = jarngreipr::parse_range("(-10, -1)"_str);
        BOOST_TEST(range.size() == 8u);
        for(std::int64_t i=0; i<8; ++i)
        {
            BOOST_TEST(range.at(i) == i-9);
        }
    }
    {
        const auto range = jarngreipr::parse_range("(-10, -1]"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i-9);
        }
    }

    {
        const auto range = jarngreipr::parse_range("[ +1, +10 ]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[ +1, +10 )"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+1);
        }
    }
    {
        const auto range = jarngreipr::parse_range("( +1, +10 )"_str);
        BOOST_TEST(range.size() == 8u);
        for(std::int64_t i=0; i<8; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }
    {
        const auto range = jarngreipr::parse_range("( +1, +10 ]"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i+2);
        }
    }
    {
        const auto range = jarngreipr::parse_range("[ -10, -1 ]"_str);
        BOOST_TEST(range.size() == 10u);
        for(std::int64_t i=0; i<10; ++i)
        {
            BOOST_TEST(range.at(i) == i-10); // from -10 to -1
        }
    }
    {
        const auto range = jarngreipr::parse_range("[ -10, -1 )"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i-10);
        }
    }
    {
        const auto range = jarngreipr::parse_range("( -10, -1 )"_str);
        BOOST_TEST(range.size() == 8u);
        for(std::int64_t i=0; i<8; ++i)
        {
            BOOST_TEST(range.at(i) == i-9);
        }
    }
    {
        const auto range = jarngreipr::parse_range("( -10, -1 ]"_str);
        BOOST_TEST(range.size() == 9u);
        for(std::int64_t i=0; i<9; ++i)
        {
            BOOST_TEST(range.at(i) == i-9);
        }
    }
}

BOOST_AUTO_TEST_CASE(test_parse_range_chains)
{
    {
        const auto range = jarngreipr::parse_chain_range("A:B"_str);
        BOOST_TEST(range.size() == 2u);
        BOOST_TEST(range.at(0) == "A");
        BOOST_TEST(range.at(1) == "B");
    }
    {
        const auto range = jarngreipr::parse_chain_range("C:F"_str);
        BOOST_TEST(range.size() == 4u);
        BOOST_TEST(range.at(0) == "C");
        BOOST_TEST(range.at(1) == "D");
        BOOST_TEST(range.at(2) == "E");
        BOOST_TEST(range.at(3) == "F");
    }
}


#include <yawiel/core/util/ngram_counter.hpp>
#include <boost/test/unit_test.hpp>
#include <unordered_map>
#include <vector>

using namespace yawiel;
using namespace yawiel::text;
using namespace yawiel::util;
using namespace std;

BOOST_AUTO_TEST_SUITE(NGramCounterTest);

BOOST_AUTO_TEST_CASE(BasicCounter)
{
  const std::string text = "this is a test a test";
  Corpus<std::string> corpus;
  corpus.LoadString(text);
  NGramCounter<std::string> counter(corpus);
  counter.ComputeCounts(2);
  auto counts = counter.GetCounts(2);

  vector<size_t> bi1 {0, 1};
  vector<size_t> bi2 {1, 2};
  vector<size_t> bi3 {2, 3};
  vector<size_t> bi4 {3, 2};

  BOOST_REQUIRE_EQUAL(counts[bi1], 1);
  BOOST_REQUIRE_EQUAL(counts[bi2], 1);
  BOOST_REQUIRE_EQUAL(counts[bi3], 2);
  BOOST_REQUIRE_EQUAL(counts[bi4], 1);
}

BOOST_AUTO_TEST_CASE(SerializedCounter)
{
  const std::string text = "this is a test a test";
  const std::string dummyText = "nothing";
  const std::string fileName = "test_serialized_counter.bin";
  Corpus<std::string> corpusText;
  corpusText.LoadString(text);
  Corpus<std::string> corpusDummy;
  corpusDummy.LoadString(text);
  NGramCounter<std::string> counter1(corpusText);
  NGramCounter<std::string> counter2(corpusDummy);
  counter1.ComputeCounts(2);
  auto counts1 = counter1.GetCounts(2);

  // Save counter.
  std::ofstream outputFile(fileName);
  {
    boost::archive::binary_oarchive outputArchive(outputFile);
    outputArchive << BOOST_SERIALIZATION_NVP(counter1);
  }
  // Load counter.
  std::ifstream inputFile(fileName);
  assert(inputFile.good());
  {
    boost::archive::binary_iarchive inputArchive(inputFile);
    inputArchive >> BOOST_SERIALIZATION_NVP(counter2);
  }
  auto counts2 = counter2.GetCounts(2);

  // Check counts are the same.
  vector<size_t> bi1 {0, 1};
  vector<size_t> bi2 {1, 2};
  vector<size_t> bi3 {2, 3};
  vector<size_t> bi4 {3, 2};

  BOOST_REQUIRE_EQUAL(counts1[bi1], counts2[bi1]);
  BOOST_REQUIRE_EQUAL(counts1[bi2], counts2[bi2]);
  BOOST_REQUIRE_EQUAL(counts1[bi3], counts2[bi3]);
  BOOST_REQUIRE_EQUAL(counts1[bi4], counts2[bi4]);
}

BOOST_AUTO_TEST_SUITE_END();