// Copyright (C) 2014 The Regents of the University of California (Regents).
// 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 The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

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

#include "theia/matching/cascade_hashing_feature_matcher.h"
#include "theia/matching/distance.h"
#include "theia/matching/feature_matcher.h"
#include "theia/matching/image_pair_match.h"
#include "theia/matching/in_memory_features_and_matches_database.h"

#include "gtest/gtest.h"

namespace theia {

using Eigen::VectorXf;

static const int kNumDescriptors = 10;
static const int kNumDescriptorDimensions = 10;

TEST(CascadeHashingFeatureMatcherTest, NoOptions) {
  // Set up descriptors.
  KeypointsAndDescriptors features1, features2;
  features1.image_name = "1";
  features2.image_name = "2";
  features1.descriptors.resize(kNumDescriptors);
  features2.descriptors.resize(kNumDescriptors);
  for (int i = 0; i < kNumDescriptors; i++) {
    // Avoid a zero vector.
    features1.descriptors[i] = VectorXf::Constant(kNumDescriptorDimensions, 1);
    features2.descriptors[i] = VectorXf::Constant(kNumDescriptorDimensions, 1);
    features1.descriptors[i].normalize();
    features2.descriptors[i].normalize();
  }

  // Set options.
  FeatureMatcherOptions options;
  options.min_num_feature_matches = 0;
  options.keep_only_symmetric_matches = false;
  options.use_lowes_ratio = false;
  options.perform_geometric_verification = false;

  // Add features.
  features1.keypoints.resize(features1.descriptors.size());
  features2.keypoints.resize(features2.descriptors.size());

  InMemoryFeaturesAndMatchesDatabase database;
  database.PutFeatures("1", features1);
  database.PutFeatures("2", features2);

  CascadeHashingFeatureMatcher matcher(options, &database);
  matcher.AddImage("1");
  matcher.AddImage("2");

  // Match features
  matcher.MatchImages();

  // Check that the results are valid.
  EXPECT_GT(database.NumMatches(), 0);
}

TEST(CascadeHashingFeatureMatcherTest, RatioTest) {
  // Set up descriptors.
  KeypointsAndDescriptors features1, features2;
  features1.image_name = "1";
  features2.image_name = "2";
  features1.descriptors.resize(1);
  features2.descriptors.resize(2);
  features1.descriptors[0] =
      VectorXf::Constant(kNumDescriptorDimensions, 1).normalized();

  // Set the two descriptors to be very close to each other so that they do not
  // pass the ratio test.
  features2.descriptors[0] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  features2.descriptors[0](0) = 0.9;
  features2.descriptors[0].normalize();
  features2.descriptors[1] = VectorXf::Constant(kNumDescriptorDimensions, 1);
  features2.descriptors[1](0) = 0.89;
  features2.descriptors[1].normalize();

  // Set options.
  FeatureMatcherOptions options;
  options.min_num_feature_matches = 0;
  options.keep_only_symmetric_matches = false;
  options.use_lowes_ratio = true;
  options.perform_geometric_verification = false;

  // Add features.
  features1.keypoints.resize(features1.descriptors.size());
  features2.keypoints.resize(features2.descriptors.size());

  InMemoryFeaturesAndMatchesDatabase database;
  database.PutFeatures("1", features1);
  database.PutFeatures("2", features2);

  CascadeHashingFeatureMatcher matcher(options, &database);
  matcher.AddImage("1");
  matcher.AddImage("2");

  // Match features.
  matcher.MatchImages();

  // Check that the results are valid.
  EXPECT_GT(database.NumMatches(), 0);
}

}  // namespace theia


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

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/constexpr.hpp>
#include <boost/hana/ext/std/integral_constant.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/maybe.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;

int main() {
    //! [main]
    BOOST_HANA_CONSTEXPR_ASSERT(find(list(1.0, 2, '3'), trait_<std::is_integral>) == just(2));
    BOOST_HANA_CONSTANT_ASSERT(find(list(1.0, 2, '3'), trait_<std::is_class>) == nothing);
    //! [main]
}


/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2013, Rice University
*  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 the Rice University 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.
*********************************************************************/

/* Author: Bryant Gipson, Mark Moll */

#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/geometric/planners/rrt/RRT.h>
#include <ompl/geometric/planners/kpiece/KPIECE1.h>
#include <ompl/geometric/planners/est/EST.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <ompl/geometric/planners/stride/STRIDE.h>
#include <ompl/tools/benchmark/Benchmark.h>

#include <boost/math/constants/constants.hpp>
#include <boost/format.hpp>
#include <fstream>

// a 2D line segment
struct Segment
{
    Segment(double p0_x, double p0_y, double p1_x, double p1_y)
        : x0(p0_x), y0(p0_y), x1(p1_x), y1(p1_y)
    {
    }
    double x0, y0, x1, y1;
};

// the robot and environment are modeled both as a vector of segments.
typedef std::vector<Segment> Environment;

// simply use a random projection
class KinematicChainProjector : public ompl::base::ProjectionEvaluator
{
public:
    KinematicChainProjector(const ompl::base::StateSpace *space)
        : ompl::base::ProjectionEvaluator(space)
    {
        int dimension = std::max(2, (int)ceil(log((double) space->getDimension())));
        projectionMatrix_.computeRandom(space->getDimension(), dimension);
    }
    virtual unsigned int getDimension(void) const
    {
        return projectionMatrix_.mat.size1();
    }
    void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
    {
        std::vector<double> v(space_->getDimension());
        space_->copyToReals(v, state);
        projectionMatrix_.project(&v[0], projection);
    }
protected:
    ompl::base::ProjectionMatrix projectionMatrix_;
};

class KinematicChainSpace : public ompl::base::CompoundStateSpace
{
public:
    KinematicChainSpace(unsigned int numLinks, double linkLength, Environment *env = NULL)
        : ompl::base::CompoundStateSpace(), linkLength_(linkLength), environment_(env)
    {
        for (unsigned int i = 0; i < numLinks; ++i)
            addSubspace(ompl::base::StateSpacePtr(new ompl::base::SO2StateSpace()), 1.);
        lock();
    }

    void registerProjections()
    {
        registerDefaultProjection(ompl::base::ProjectionEvaluatorPtr(
            new KinematicChainProjector(this)));
    }

    double distance(const ompl::base::State *state1, const ompl::base::State *state2) const
    {
        const StateType *cstate1 = state1->as<StateType>();
        const StateType *cstate2 = state2->as<StateType>();
        double theta1 = 0., theta2 = 0., dx = 0., dy = 0., dist = 0.;

        for (unsigned int i = 0; i < getSubspaceCount(); ++i)
        {
            theta1 += cstate1->as<ompl::base::SO2StateSpace::StateType>(i)->value;
            theta2 += cstate2->as<ompl::base::SO2StateSpace::StateType>(i)->value;
            dx += cos(theta1) - cos(theta2);
            dy += sin(theta1) - sin(theta2);
            dist += sqrt(dx * dx + dy * dy);
        }
        return dist * linkLength_;
    }
    double linkLength() const
    {
        return linkLength_;
    }
    const Environment* environment() const
    {
        return environment_;
    }

protected:
    double linkLength_;
    Environment* environment_;
};

class KinematicChainValidityChecker : public ompl::base::StateValidityChecker
{
public:
    KinematicChainValidityChecker(const ompl::base::SpaceInformationPtr &si)
        : ompl::base::StateValidityChecker(si)
    {
    }

    bool isValid(const ompl::base::State *state) const
    {
        const KinematicChainSpace* space = si_->getStateSpace()->as<KinematicChainSpace>();
        const KinematicChainSpace::StateType *s = state->as<KinematicChainSpace::StateType>();
        unsigned int n = si_->getStateDimension();
        Environment segments;
        double linkLength = space->linkLength();
        double theta = 0., x = 0., y = 0., xN, yN;

        segments.reserve(n + 1);
        for(unsigned int i = 0; i < n; ++i)
        {
            theta += s->as<ompl::base::SO2StateSpace::StateType>(i)->value;
            xN = x + cos(theta) * linkLength;
            yN = y + sin(theta) * linkLength;
            segments.push_back(Segment(x, y, xN, yN));
            x = xN;
            y = yN;
        }
        xN = x + cos(theta) * 0.001;
        yN = y + sin(theta) * 0.001;
        segments.push_back(Segment(x, y, xN, yN));
        return selfIntersectionTest(segments)
            && environmentIntersectionTest(segments, *space->environment());
    }

protected:
    // return true iff env does *not* include a pair of intersecting segments
    bool selfIntersectionTest(const Environment& env) const
    {
        for (unsigned int i = 0; i < env.size(); ++i)
            for (unsigned int j = i + 1; j < env.size(); ++j)
                if (intersectionTest(env[i], env[j]))
                    return false;
        return true;
    }
    // return true iff no segment in env0 intersects any segment in env1
    bool environmentIntersectionTest(const Environment& env0, const Environment& env1) const
    {
        for (unsigned int i = 0; i < env0.size(); ++i)
            for (unsigned int j = 0; j < env1.size(); ++j)
                if (intersectionTest(env0[i], env1[j]))
                    return false;
        return true;
    }
    // return true iff segment s0 intersects segment s1
    bool intersectionTest(const Segment& s0, const Segment& s1) const
    {
        // adopted from:
        // http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect/1201356#1201356
        double s10_x = s0.x1 - s0.x0;
        double s10_y = s0.y1 - s0.y0;
        double s32_x = s1.x1 - s1.x0;
        double s32_y = s1.y1 - s1.y0;
        double denom = s10_x * s32_y - s32_x * s10_y;
        if (fabs(denom) < std::numeric_limits<double>::epsilon())
            return false; // Collinear
        bool denomPositive = denom > 0;

        double s02_x = s0.x0 - s1.x0;
        double s02_y = s0.y0 - s1.y0;
        double s_numer = s10_x * s02_y - s10_y * s02_x;
        if ((s_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
            return false; // No collision
        double t_numer = s32_x * s02_y - s32_y * s02_x;
        if ((t_numer < std::numeric_limits<float>::epsilon()) == denomPositive)
            return false; // No collision
        if (((s_numer - denom > -std::numeric_limits<float>::epsilon()) == denomPositive)
            || ((t_numer - denom > std::numeric_limits<float>::epsilon()) == denomPositive))
            return false; // No collision
        return true;
    }
};

Environment createHornEnvironment(unsigned int d, double eps)
{
    std::ofstream envFile("environment.dat");
    std::vector<Segment> env;
    double w = 1. / (double)d, x = w, y = -eps, xN, yN, theta = 0.,
        scale = w * (1. + boost::math::constants::pi<double>() * eps);

    envFile << x << " " << y << std::endl;
    for(unsigned int i = 0; i < d - 1; ++i)
    {
        theta += boost::math::constants::pi<double>() / (double) d;
        xN = x + cos(theta) * scale;
        yN = y + sin(theta) * scale;
        env.push_back(Segment(x, y, xN, yN));
        x = xN;
        y = yN;
        envFile << x << " " << y << std::endl;
    }

    theta = 0.;
    x = w;
    y = eps;
    envFile << x << " " << y << std::endl;
    scale = w * (1.0 - boost::math::constants::pi<double>() * eps);
    for(unsigned int i = 0; i < d - 1; ++i)
    {
        theta += boost::math::constants::pi<double>() / d;
        xN = x + cos(theta) * scale;
        yN = y + sin(theta) * scale;
        env.push_back(Segment(x, y, xN, yN));
        x = xN;
        y = yN;
        envFile << x << " " << y << std::endl;
    }
    envFile.close();
    return env;
}

int main(int argc, char **argv)
{
    if (argc < 2)
    {
        std::cout << "Usage:\n" << argv[0] << " <num_links>\n";
        exit(0);
    }

    unsigned int numLinks = boost::lexical_cast<unsigned int>(std::string(argv[1]));
    Environment env = createHornEnvironment(numLinks, log((double)numLinks) / (double)numLinks);
    ompl::base::StateSpacePtr chain(new KinematicChainSpace(numLinks, 1. / (double)numLinks, &env));
    ompl::geometric::SimpleSetup ss(chain);

    ss.setStateValidityChecker(ompl::base::StateValidityCheckerPtr(
        new KinematicChainValidityChecker(ss.getSpaceInformation())));

    ompl::base::ScopedState<> start(chain), goal(chain);
    std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (double)numLinks);
    std::vector<double> goalVec(numLinks, 0.);

    startVec[0] = 0.;
    goalVec[0] = boost::math::constants::pi<double>() - .001;
    chain->setup();
    chain->copyFromReals(start.get(), startVec);
    chain->copyFromReals(goal.get(), goalVec);
    ss.setStartAndGoalStates(start, goal);

    // SEKRIT BONUS FEATURE:
    // if you specify a second command line argument, it will solve the
    // problem just once with STRIDE and print out the solution path.
    if (argc > 2)
    {
        ss.setPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
        ss.setup();
        ss.print();
        ss.solve(3600);
        ss.simplifySolution();

        ompl::geometric::PathGeometric path = ss.getSolutionPath();
        std::vector<double> v;
        for(unsigned int i = 0; i < path.getStateCount(); ++i)
        {
            chain->copyToReals(v, path.getState(i));
            std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cout, " "));
            std::cout << std::endl;
        }
        exit(0);
    }

    // by default, use the Benchmark class
    double runtime_limit = 60, memory_limit = 1024;
    int run_count = 20;
    ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
    ompl::tools::Benchmark b(ss, "KinematicChain");
    b.addExperimentParameter("num_links", "INTEGER", boost::lexical_cast<std::string>(numLinks));

    b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
    b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())));
    b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getSpaceInformation())));
    b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::RRT(ss.getSpaceInformation())));
    b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::PRM(ss.getSpaceInformation())));
    b.benchmark(request);
    b.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());

    exit(0);
}


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

/** \file calibrateCamera.cpp
    \brief This file calibrates the camera intrinsics from a BAG file.
  */

#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <string>
#include <sstream>
#include <algorithm>

#include <boost/filesystem.hpp>

#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <rosbag/message_instance.h>

#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>

#include <sm/BoostPropertyTree.hpp>

#include <opencv2/highgui/highgui.hpp>

#include <aslam/calibration/core/IncrementalEstimator.h>
#include <aslam/calibration/base/Timestamp.h>

#include "aslam/calibration/camera/CameraCalibrator.h"

using namespace aslam::calibration;
using namespace sm;

int main(int argc, char** argv) {
  if (argc != 3) {
    std::cerr << "Usage: " << argv[0] << " <ros_bag_file> <conf_file>"
      << std::endl;
    return -1;
  }

  // loading configuration
  std::cout << "Loading configuration parameters..." << std::endl;
  BoostPropertyTree config;
  config.loadXml(argv[2]);

  // create the camera calibrator
  CameraCalibrator calibrator(PropertyTree(config, "camera/calibrator"));

  // load ros bag file
  rosbag::Bag bag(argv[1]);
  std::vector<std::string> topics;
  const std::string rosTopic = config.getString("camera/rosTopic");
  topics.push_back(rosTopic);
  rosbag::View view(bag, rosbag::TopicQuery(topics));

  // initializing geometry from the dataset
  std::cout << "Initializing geometry..." << std::endl;
  for (auto it = view.begin(); it != view.end(); ++it) {
    if (it->getTopic() == rosTopic) {
      sensor_msgs::ImagePtr image(it->instantiate<sensor_msgs::Image>());
      auto cvImage = cv_bridge::toCvCopy(image);
      if (calibrator.initGeometry(cvImage->image))
        break;
    }
  }

  // processing ros bag file
  std::cout << "Processing BAG file..." << std::endl;
  size_t viewCounter = 0;
  for (auto it = view.begin(); it != view.end(); ++it) {
    std::cout << std::fixed << std::setw(3)
      << viewCounter++ / (double)view.size() * 100 << " %" << '\r';
    if (it->getTopic() == rosTopic) {
      sensor_msgs::ImagePtr image(it->instantiate<sensor_msgs::Image>());
      auto cvImage = cv_bridge::toCvCopy(image);
      const size_t numObservations =
        calibrator.getEstimatorObservations().size();
      calibrator.addImage(cvImage->image, image->header.stamp.toNSec());
      if (config.getBool("camera/calibrator/saveEstimatorImages")) {
        if (calibrator.getEstimatorObservations().size() != numObservations) {
          cv::Mat checkerboardImage;
          calibrator.getLastCheckerboardImage(checkerboardImage);
          if (!boost::filesystem::exists("images"))
            boost::filesystem::create_directory("images");
          std::stringstream stream;
          stream << "images/" << config.getString("camera/cameraId") << "-"
            << image->header.stamp.toNSec() << ".png";
          cv::imwrite(stream.str().c_str(), checkerboardImage);
        }
      }
    }
  }
  calibrator.processBatch();

  std::cout << "final parameters: " << std::endl;
  std::cout << "projection: " << calibrator.getProjection().transpose()
    << std::endl;
  std::cout << "projection standard deviation: "
    << calibrator.getProjectionStandardDeviation().transpose() << std::endl;
  std::cout << "distortion: " << calibrator.getDistortion().transpose()
    << std::endl;
  std::cout << "distortion standard deviation: "
    << calibrator.getDistortionStandardDeviation().transpose() << std::endl;
  std::cout << "unobservable basis: " << std::endl << calibrator.getNobsBasis()
    << std::endl;
  std::cout << "unobservable basis (scaled): " << std::endl
    << calibrator.getNobsBasis(true) << std::endl;
  std::cout << "initial cost: " << calibrator.getInitialCost() << std::endl;
  std::cout << "final cost: " << calibrator.getFinalCost() << std::endl;
  std::cout << "number of images for estimation: "
    << calibrator.getEstimatorObservations().size() << std::endl;
  std::cout << "total number of images: " << view.size() << std::endl;
  Eigen::VectorXd mean, variance, standardDeviation;
  double maxXError, maxYError;
  size_t numOutliers;
  calibrator.getStatistics(mean, variance, standardDeviation, maxXError,
    maxYError, numOutliers);
  std::cout << "reprojection error mean: " << mean.transpose()
    << std::endl;
  std::cout << "reprojection error standard deviation: " <<
    standardDeviation.transpose() << std::endl;
  std::cout << "max x reprojection error: " << maxXError << std::endl;
  std::cout << "max y reprojection error: " << maxYError << std::endl;
  std::cout << "number of outliers: " << numOutliers << std::endl;

  // write calibration to xml file
  BoostPropertyTree calibrationData("intrinsics");
  calibrator.write(calibrationData);
  std::stringstream stream;
  stream << config.getString("camera/cameraId") << "-"
    << config.getString("camera/calibrator/cameraProjectionType") << "-"
    << Timestamp::getDate(view.getBeginTime().toSec()) << "-"
    << calibrator.getOptions().batchNumImages << "-"
    << calibrator.getEstimator()->getOptions().infoGainDelta<< ".xml";
  calibrationData.saveXml(stream.str());

  // output errors
  if (config.getBool("camera/calibrator/outputErrors")) {
    std::vector<Eigen::Vector2d> errors;
    std::vector<double> errorsMd2;
    calibrator.getErrors(errors, errorsMd2);
    std::ofstream errorsFile("errors.txt");
    errorsFile << std::fixed << std::setprecision(18);
    std::for_each(errors.cbegin(), errors.cend(), [&](
      decltype(*errors.cbegin()) x){errorsFile << x.transpose() << std::endl;});
    std::ofstream errorsMd2File("errorsMd2.txt");
    errorsMd2File << std::fixed << std::setprecision(18);
    std::for_each( errorsMd2.cbegin(), errorsMd2.cend(), [&](
      decltype(* errorsMd2.cbegin()) x){errorsMd2File << x << std::endl;});
  }

  return 0;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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)
//
/// \file rewriter_test.cpp
/// \brief Test for PBES rewriters.

#define BOOST_TEST_MODULE rewriter_test
#include <boost/test/included/unit_test_framework.hpp>

#include "mcrl2/data/list.h"
#include "mcrl2/pbes/detail/normalize_and_or.h"
#include "mcrl2/pbes/detail/parse.h"
#include "mcrl2/pbes/rewrite.h"
#include "mcrl2/pbes/rewriter.h"
#include "mcrl2/pbes/rewriters/data2pbes_rewriter.h"
#include "mcrl2/pbes/rewriters/one_point_rule_rewriter.h"
#include "mcrl2/pbes/txt2pbes.h"
#include "mcrl2/utilities/detail/test_operation.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

const std::string VARIABLE_SPECIFICATION =
  "datavar         \n"
  "  b:  Bool;     \n"
  "  b1: Bool;     \n"
  "  b2: Bool;     \n"
  "  b3: Bool;     \n"
  "                \n"
  "  n:  Nat;      \n"
  "  n1: Nat;      \n"
  "  n2: Nat;      \n"
  "  n3: Nat;      \n"
  "                \n"
  "  p:  Pos;      \n"
  "  p1: Pos;      \n"
  "  p2: Pos;      \n"
  "  p3: Pos;      \n"
  "                \n"
  "predvar         \n"
  "  X;            \n"
  "  Y: Nat;       \n"
  "  W: Bool;      \n"
  "  Z: Bool, Nat; \n"
  "  X0;           \n"
  "  X1: Bool;     \n"
  "  X2: Nat, Nat; \n"
  "  X3: Bool, Nat;\n"
  "  X4: Nat, Bool;\n"
  ;

// normalize operator
template <typename Function>
struct normalizer
{
  Function& f;

  normalizer(Function& f0)
    : f(f0)
  {}

  pbes_expression operator()(const pbes_expression& t) const
  {
    return pbes_system::detail::normalize_and_or(f(t));
  }
};

// utility function for creating a normalizer
template <typename Function>
normalizer<Function> N(Function& f)
{
  return normalizer<Function>(f);
}

/// \brief A rewriter that simplifies boolean expressions.
template <typename Rewriter>
class rewriter_with_substitution
{
  protected:
    Rewriter& R;
    data::mutable_indexed_substitution<> sigma;

  public:
    /// \brief The term type
    typedef typename Rewriter::term_type term_type;

    /// \brief The variable type
    typedef typename Rewriter::variable_type variable_type;

    rewriter_with_substitution(Rewriter& R_, const std::string& substitution_text)
      : R(R_)
    {
      data::detail::parse_substitution(substitution_text, sigma);
    }

    /// \brief Rewrites a boolean expression.
    /// \param x A term
    /// \return The rewrite result.
    term_type operator()(const term_type& x)
    {
      return R(x, sigma);
    }
};

template <typename Rewriter>
rewriter_with_substitution<Rewriter> make_rewriter_with_substitution(Rewriter& R, const std::string& sigma)
{
  return rewriter_with_substitution<Rewriter>(R, sigma);
}

// default PBES expression parser
class default_parser
{
  public:
    pbes_expression operator()(const std::string& expr)
    {
      return pbes_system::parse_pbes_expression(expr);
    }
};

// PBES expression parser
class parser
{
  protected:
    std::string m_var_decl;
    std::string m_data_spec;

  public:

    parser(const std::string& var_decl = VARIABLE_SPECIFICATION, const std::string& data_spec = "")
      : m_var_decl(var_decl),
        m_data_spec(data_spec)
    {}

    pbes_expression operator()(const std::string& expr)
    {
      return pbes_system::parse_pbes_expression(expr, m_var_decl, m_data_spec);
    }
};

struct equal_to
{
  bool operator()(const pbes_expression& x, const pbes_expression& y) const
  {
    if (x == y)
    {
      return true;
    }
    if (is_true(x) && is_true(y))
    {
      return true;
    }
    if (is_false(x) && is_false(y))
    {
      return true;
    }
    return false;
  }
};

template <typename Rewriter1, typename Rewriter2>
void test_simplify(Rewriter1 R1, Rewriter2 R2, std::string expr1, std::string expr2)
{
  BOOST_CHECK(utilities::detail::test_operation(
    expr1,
    expr2,
    parser(),
    equal_to(),
    R1,
    "simplify",
    R2,
    "datarewr"
  ));
}

template <typename Rewriter1, typename Rewriter2>
void test_rewriters(Rewriter1 R1, Rewriter2 R2, std::string expr1, std::string expr2, const std::string& var_decl = VARIABLE_SPECIFICATION, const std::string& data_spec = "")
{
  BOOST_CHECK(utilities::detail::test_operation(
    expr1,
    expr2,
    parser(var_decl, data_spec),
    equal_to(),
    R1,
    "R1",
    R2,
    "R2"
  ));
}

BOOST_AUTO_TEST_CASE(test_simplifying_rewriter)
{
  data::data_specification data_spec = data::data_specification();
  data_spec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(data_spec);
  pbes_system::simplify_quantifiers_data_rewriter<data::rewriter> R(datar);
  pbes_system::data_rewriter<data::rewriter> r(datar);

  test_simplify(R, r, "val(n >= 0) || Y(n)"                                             , "true");
  test_simplify(R, r, "false"                                                           , "false");
  test_simplify(R, r, "true"                                                            , "true");
  test_simplify(R, r, "true && true"                                                    , "true");
  test_simplify(R, r, "(true && true) && true"                                          , "true");
  test_simplify(R, r, "true && false"                                                   , "false");
  test_simplify(R, r, "true => val(b)"                                                  , "val(b)");
  test_simplify(R, r, "X && true"                                                       , "X");
  test_simplify(R, r, "true && X"                                                       , "X");
  test_simplify(R, r, "X && false"                                                      , "false");
  test_simplify(R, r, "X && val(false)"                                                 , "false");
  test_simplify(R, r, "false && X"                                                      , "false");
  test_simplify(R, r, "X && (false && X)"                                               , "false");
  test_simplify(R, r, "X && (X0 && X)"                                                  , "X && (X0 && X)");
  test_simplify(R, r, "X && (X && X0)"                                                  , "X && (X && X0)");
  test_simplify(R, r, "Y(1+2)"                                                          , "Y(3)");
  test_simplify(R, r, "true || true"                                                    , "true");
  test_simplify(R, r, "(true || true) || true"                                          , "true");
  test_simplify(R, r, "true || false"                                                   , "true");
  test_simplify(R, r, "false => X"                                                      , "true");
  test_simplify(R, r, "Y(n+n)"                                                          , "Y(n+n)");
  test_simplify(R, r, "Y(n+p)"                                                          , "Y(n+p)");
  test_simplify(R, r, "forall m:Nat. false"                                             , "false");
  test_simplify(R, r, "X && X"                                                          , "X");
  test_simplify(R, r, "val(true)"                                                       , "true");
  test_simplify(R, r, "false => (exists m:Nat. exists k:Nat. val(m*m == k && k > 20))"  , "true");
  test_simplify(R, r, "exists m:Nat.true"                                               , "true");
  test_simplify(R, r, "forall m:Nat. val(m < 0 && m > 3)"                               , "false");
  test_simplify(R, r, "forall m:Nat. val(m < 0 && m > 3) => Y(n)"                       , "true");
  test_simplify(R, r, "forall m:Nat. Y(n)"                                              , "Y(n)");
  test_simplify(R, r, "forall m:Nat. val(m < 0 && m > 3) || Y(n)"                       , "Y(n)");
  test_simplify(R, r, "!!X"                                                             , "X");
  test_simplify(R, r, "forall m:Nat. X"                                                 , "X");
  test_simplify(R, r, "forall m,n:Nat. Y(n)"                                            , "forall n:Nat. Y(n)");
  test_simplify(R, r, "forall m,n:Nat. Y(m)"                                            , "forall m:Nat. Y(m)");
  test_simplify(R, r, "forall b: Bool. forall n: Nat. val(n > 3) || Y(n)"               , "forall n: Nat. val(n > 3) || Y(n)");
  test_simplify(R, r, "forall n: Nat. forall b: Bool. val(n > 3) || Y(n)"               , "forall n: Nat. val(n > 3) || Y(n)");
  test_simplify(R, r, "forall n: Nat. val(b) && Y(n)"                                   , "val(b) && forall n: Nat. Y(n)");
  test_simplify(R, r, "forall n: Nat. val(b)"                                           , "val(b)");
  test_simplify(R, r, "forall m: Nat. val(m > 2) && X"                                  , "(forall m: Nat. val(m > 2)) && X");

  // ticket 1273
  test_simplify(R, r, "exists m: Nat. val(n == 0)"                                      , "val(n == 0)");

  // test_expressions(R, "Y(n+p) && Y(p+n)"                                                , "Y(n+p)");
  // test_expressions(R, "exists m:Nat. val( m== p) && Y(m)"                               , "Y(p)");
  // test_expressions(R, "X && (Y(p) || X)"                                                , "X");
  // test_expressions(R, "X || (Y(p) && X)"                                                , "X");
  // test_expressions(R, "val(b || !b)"                                                    , "val(true)");
  // test_expressions(R, "Y(n1 + n2)"                                                      , "Y(n2 + n1)");
}

BOOST_AUTO_TEST_CASE(test_enumerate_quantifiers_rewriter)
{
  data::data_specification dataspec = data::data_specification();
  dataspec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(dataspec);
  pbes_system::data_rewriter<data::rewriter> r(datar);
  enumerate_quantifiers_rewriter R(datar, dataspec);

  // test_rewriters(N(R), N(r),  "(Y(0) && Y(1)) => (Y(1) && Y(0))"                                , "true");
  test_rewriters(N(R), N(r),  "forall n, m: Nat. val(n > 3) || val(b)"                          , "val(b)");
  test_rewriters(N(R), N(r),  "forall b: Bool. forall n: Nat. val(n > 3) || Y(n)"               , "Y(2) && Y(1) && Y(3) && Y(0)");
  test_rewriters(N(R), N(r),  "(Y(0) && Y(1)) => (Y(0) && Y(1))"                                , "true");
  test_rewriters(N(R), N(r),  "exists b: Bool. val(if(b, false, b))"                            , "false");
  test_rewriters(N(R), N(r),  "exists b: Bool. W(b)"                                            , "W(true) || W(false)");
  test_rewriters(N(R), N(r),  "forall n: Nat.val(!(n < 1)) || Y(n)"                             , "Y(0)");
  test_rewriters(N(R), N(r),  "false"                                                           , "false");
  test_rewriters(N(R), N(r),  "true"                                                            , "true");
  test_rewriters(N(R), N(r),  "true && true"                                                    , "true");
  test_rewriters(N(R), N(r),  "(true && true) && true"                                          , "true");
  test_rewriters(N(R), N(r),  "true && false"                                                   , "false");
  test_rewriters(N(R), N(r),  "true => val(b)"                                                  , "val(b)");
  test_rewriters(N(R), N(r),  "X && true"                                                       , "X");
  test_rewriters(N(R), N(r),  "true && X"                                                       , "X");
  test_rewriters(N(R), N(r),  "X && false"                                                      , "false");
  test_rewriters(N(R), N(r),  "X && val(false)"                                                 , "false");
  test_rewriters(N(R), N(r),  "false && X"                                                      , "false");
  test_rewriters(N(R), N(r),  "X && (false && X)"                                               , "false");
  test_rewriters(N(R), N(r),  "Y(1+2)"                                                          , "Y(3)");
  test_rewriters(N(R), N(r),  "true || true"                                                    , "true");
  test_rewriters(N(R), N(r),  "(true || true) || true"                                          , "true");
  test_rewriters(N(R), N(r),  "true || false"                                                   , "true");
  test_rewriters(N(R), N(r),  "false => X"                                                      , "true");
  test_rewriters(N(R), N(r),  "Y(n+n)"                                                          , "Y(n+n)");
  test_rewriters(N(R), N(r),  "Y(n+p)"                                                          , "Y(n+p)");
  test_rewriters(N(R), N(r),  "forall m:Nat. false"                                             , "false");
  test_rewriters(N(R), N(r),  "X && X"                                                          , "X");
  test_rewriters(N(R), N(r),  "val(true)"                                                       , "true");
  test_rewriters(N(R), N(r),  "false => (exists m:Nat. exists k:Nat. val(m*m == k && k > 20))"  , "true");
  test_rewriters(N(R), N(r),  "exists m:Nat.true"                                               , "true");
  test_rewriters(N(R), N(r),  "forall m:Nat.val(m < 3)"                                         , "false");
  test_rewriters(N(R), N(r),  "exists m:Nat.val(m > 3)"                                         , "true");
  test_rewriters(N(R), N(r),  "forall m:Nat. X"                                                 , "X");
  test_rewriters(N(R), N(r),  "exists d: Nat. X && val(d == 0)"                                 , "X");
  test_rewriters(N(R), N(r),  "forall m: Nat. (val(!(m < 3)) || Y(m + 1))"                      , "Y(1) && Y(2) && Y(3)");
  test_rewriters(N(R), N(r),  "val(!true) || (((forall m: Nat. val(!(m < 3)) && X3(false, m + 1)) && X2(1, 1) && val(!false) || val(false)) || (exists m: Nat. val(m < 3) || (forall k: Nat. val(k < 3) && val(k < 2)))) && X1(false)", "X1(false)");
  test_rewriters(N(R), N(r),  "forall n: Nat. (val(n < 3) && (exists n: Nat. val(n < 3)))", "false");
  test_rewriters(N(R), N(r),  "Y(n + 1) || forall n: Nat. val(n < 3)", "Y(n + 1)");
  test_rewriters(N(R), N(r),  "forall b: Bool . val(b) || X1(b)", "X1(false)");
  test_rewriters(N(R), N(r),  "forall n: Nat . val(n > 1)", "false");
  test_rewriters(N(R), N(r),  "forall n: Nat . val(n > 0)", "false");
  test_rewriters(N(R), N(r),  "forall p: Pos . val(p > 0)", "true");
}

template <typename Rewriter1, typename Rewriter2>
void test_expressions(Rewriter1& R1, std::string expr1, Rewriter2& R2, std::string expr2, std::string var_decl = VARIABLE_SPECIFICATION, std::string substitutions = "", std::string data_spec = "")
{
  if (substitutions == "")
  {
    test_rewriters(N(R1), N(R2), expr1, expr2, var_decl, data_spec);
  }
  else
  {
    rewriter_with_substitution<Rewriter1> r1(R1, substitutions);
    test_rewriters(N(r1), N(R2), expr1, expr2, var_decl, data_spec);
  }
}

void test_enumerate_quantifiers_rewriter(const std::string& expr1, const std::string& expr2, const std::string& var_decl, const std::string& sigma, const std::string& data_spec)
{
  std::cout << "<enumerate_quantifiers_rewriter>" << std::endl;

  data::data_specification dspec = data::parse_data_specification(data_spec);
  data::rewriter datar(dspec);
  pbes_system::enumerate_quantifiers_rewriter R(datar, dspec);
  test_expressions(R, expr1, R, expr2, var_decl, sigma, data_spec);
}

BOOST_AUTO_TEST_CASE(test_enumerate_quantifiers_rewriter2)
{
  std::string var_decl;
  std::string data_spec;
  std::string sigma;
  std::string expr1;
  std::string expr2;

  //------------------------//
  data_spec = "sort Enum = struct e1 | e2;\n";
  var_decl =
    "datavar         \n"
    "predvar         \n"
    "  X: Enum;      \n"
    ;
  expr1 = "exists m:Enum.(X(m))";
  expr2 = "X(e1) || X(e2)";
  sigma = "";
  test_enumerate_quantifiers_rewriter(expr1, expr2, var_decl, sigma, data_spec);

  data_spec = "sort Enum = struct e1 | e2;\n";
  var_decl =
    "datavar         \n"
    "predvar         \n"
    "  X: Enum;      \n"
    ;
  expr1 = "exists m1,m2:Enum.(X(m1) || X(m2))";
  expr2 = "X(e1) || X(e2)";
  sigma = "";
  //test_enumerate_quantifiers_rewriter(expr1, expr2, var_decl, sigma, data_spec);
  // This test fails with output
  // x = exists m1,m2: Enum. X(m1) || X(m2)
  // y = X(e1) || X(e2)
  // R1(x) = ((X(e1) || X(e1)) || X(e2)) || X(e2)
  // R2(y) = X(e1) || X(e2)
}

BOOST_AUTO_TEST_CASE(test_enumerate_quantifiers_rewriter_finite)
{
  data::data_specification data_spec = data::data_specification();
  data_spec.add_context_sort(data::sort_list::list(data::sort_nat::nat()));
  data::rewriter datar(data_spec);
  pbes_system::enumerate_quantifiers_rewriter R(datar, data_spec, false);
  pbes_system::simplify_data_rewriter<data::rewriter> S(datar);

  test_expressions(R, "forall n:Nat, b:Bool.Z(b,n)", S, "forall n:Nat.Z(false,n) && Z(true,n)");
  test_expressions(R, "forall n:Nat. Y(n)", S, "forall n:Nat. Y(n)");

  std::string expr1;
  std::string expr2;
  std::string sigma;
  std::string var_decl;

  //------------------------//
  var_decl =
    "datavar          \n"
    "  m: Nat;        \n"
    "  q: List(Nat);  \n"
    "                 \n"
    "predvar          \n"
    "  X: Nat, List(Nat), Nat;\n"
    ;
  expr1 = "forall k: Nat. val(!(k < m)) || X(m, q, q . (k mod 4))";
  expr2 = "forall k: Nat. val(!(k < m)) || X(m, q, q . (k mod 4))";
  sigma = "";
  test_expressions(R, expr1, S, expr2, var_decl, sigma);
}

BOOST_AUTO_TEST_CASE(test_substitutions1)
{
  std::cout << "<test_substitutions1>" << std::endl;

  data::data_specification specification;
  specification.add_context_sort(data::sort_pos::pos());
  data::rewriter  datar(specification);
  pbes_system::simplify_data_rewriter<data::rewriter> r(datar);

  data::mutable_map_substitution<> sigma;
  sigma[data::parse_variable("m: Pos")] = datar(data::parse_data_expression("3"));
  sigma[data::parse_variable("n: Pos")] = datar(data::parse_data_expression("4"));

  std::cout << "<test_substitutions1aaa>" << std::endl;
  std::string var_decl =
    "datavar         \n"
    "  m, n:  Pos;   \n"
    "                \n"
    "predvar         \n"
    "  X: Pos;       \n"
    ;
  pbes_system::pbes_expression d1 = pbes_system::parse_pbes_expression("X(m+n)", var_decl);
  pbes_system::pbes_expression d2 = pbes_system::parse_pbes_expression("X(7)", var_decl);
  BOOST_CHECK(r(d1, sigma) == r(d2));
}

BOOST_AUTO_TEST_CASE(test_substitutions2)
{
  std::cout << "<test_substitutions2>" << std::endl;
  data::data_specification data_spec;
  data_spec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(data_spec);
  pbes_system::enumerate_quantifiers_rewriter R(datar, data_spec);

  std::string var_decl;
  std::string sigma;
  std::string expr1;
  std::string expr2;
  std::string expr3;

  //------------------------//
  var_decl =
    "datavar         \n"
    "  m, n:  Pos;   \n"
    "                \n"
    "predvar         \n"
    "  X: Pos;       \n"
    ;
  expr1 = "X(m+n)";
  expr2 = "X(7)";
  sigma = "m: Pos := 3; n: Pos := 4";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  //------------------------//
  var_decl =
    "datavar         \n"
    "  n: Nat;       \n"
    "                \n"
    "predvar         \n"
    "  X: Bool, Nat; \n"
    ;
  expr1 = "forall c: Bool. X(c, n)";
  expr2 = "X(false, 0) && X(true, 0)";
  sigma = "b: Bool := false; n: Nat := 0";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  //------------------------//
  var_decl =
    "datavar         \n"
    "predvar         \n"
    "  X: Nat;       \n"
    ;
  expr1 = "exists b: Bool, c: Bool. val(b && c)";
  expr2 = "val(true)";
  sigma = "";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  //------------------------//
  var_decl =
    "datavar         \n"
    "predvar         \n"
    "  X: Nat;       \n"
    ;
  expr1 = "exists b: Bool.exists c:Bool. val(b && c)";
  expr2 = "val(true)";
  sigma = "";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  //------------------------//
  var_decl =
    "datavar         \n"
    "  b: Bool;      \n"
    "predvar         \n"
    "  X: Nat;       \n"
    ;
  expr1 = "!!val(!!b)";
  expr2 = "val(true)";
  sigma = "b:Bool := true";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);
}

BOOST_AUTO_TEST_CASE(test_substitutions3)
{
  std::cout << "<test_substitutions3>" << std::endl;
  std::string DATA_SPEC =
    "sort D = struct d1 | d2;                                                                                \n"
    "     DBuf = List(D);                                                                                \n"
    "     BBuf = List(Bool);                                                                                \n"
    "                                                                                \n"
    "map  n: Pos;                                                                                \n"
    "     empty: BBuf;                                                                                \n"
    "     insert: D # Nat # DBuf -> DBuf;                                                                                \n"
    "     insert: Bool # Nat # BBuf -> BBuf;                                                                                \n"
    "     nextempty_mod: Nat # BBuf # Nat # Pos -> Nat;                                                                         \n"
    "     q1,q2: DBuf;                                                                                \n"
    "                                                                                \n"
    "var  d,d': D;                                                                                \n"
    "     i,j,m: Nat;                                                                                \n"
    "     q: DBuf;                                                                                \n"
    "     c,c': Bool;                                                                                \n"
    "     n': Pos;                                                                                \n"
    "     b: BBuf;                                                                                \n"
    "eqn  q1  =  [d1, d1];                                                                                \n"
    "     q2  =  [d1, d1];                                                                                \n"
    "     n  =  2;                                                                                \n"
    "     q1  =  [d1, d1];                                                                                \n"
    "     q2  =  [d1, d1];                                                                                \n"
    "     empty  =  [false, false];                                                                                \n"
    "     i == 0  ->  insert(d, i, q)  =  d |> tail(q);                                                                         \n"
    "     i > 0  ->  insert(d, i, d' |> q)  =  d' |> insert(d, Int2Nat(i - 1), q);                                              \n"
    "     i == 0  ->  insert(c, i, b)  =  c |> tail(b);                                                                         \n"
    "     i > 0  ->  insert(c, i, c' |> b)  =  c' |> insert(c, Int2Nat(i - 1), b);                                              \n"
    "     b . (i mod n') && m > 0  ->  nextempty_mod(i, b, m, n')  =  nextempty_mod((i + 1) mod 2 * n', b, Int2Nat(m - 1), n'); \n"
    "     !(b . (i mod n') && m > 0)  ->  nextempty_mod(i, b, m, n')  =  i mod 2 * n';                                          \n"
    ;
  data::data_specification data_spec = data::parse_data_specification(DATA_SPEC);
  data::rewriter datar(data_spec);
  pbes_system::enumerate_quantifiers_rewriter r(datar, data_spec);

  std::string var_decl =
    "datavar                                                     \n"
    "  l_S:Nat    ;                                              \n"
    "  m_S:Nat    ;                                              \n"
    "  bst_K:Bool ;                                              \n"
    "  bst1_K:Bool;                                              \n"
    "  k_K:Nat    ;                                              \n"
    "  bst2_L:Bool;                                              \n"
    "  bst3_L:Bool;                                              \n"
    "  k_L:Nat    ;                                              \n"
    "  l'_R:Nat   ;                                              \n"
    "  b_R:BBuf   ;                                              \n"
    "                                                            \n"
    "predvar                                                     \n"
    "  X: Nat, Nat, Bool, Bool, Nat, Bool, Bool, Nat, Nat, BBuf; \n"
    ;

  pbes_system::pbes_expression phi = pbes_system::parse_pbes_expression("forall k_S2_00: Nat. val(!(k_S2_00 < m_S && !bst_K && !bst1_K)) || X(l_S, m_S, false, true, (l_S + k_S2_00) mod 4, bst2_L, bst3_L, k_L, l'_R, b_R)", var_decl, DATA_SPEC);

  // Note: the variables below do not exist elsewhere. So if a mutable_indexed_substitution is used,
  // they are immediately destroyed after parsing. The resulting substitution is not safe to use,
  // since it refers to variables that no longer exist.
  data::mutable_map_substitution<> sigma;
  sigma[data::parse_variable("l_S:Nat")]             = data::parse_data_expression("0");
  sigma[data::parse_variable("m_S:Nat")]             = data::parse_data_expression("0");
  sigma[data::parse_variable("bst_K:Bool")]          = data::parse_data_expression("false");
  sigma[data::parse_variable("bst1_K:Bool")]         = data::parse_data_expression("false");
  sigma[data::parse_variable("k_K:Nat")]             = data::parse_data_expression("0");
  sigma[data::parse_variable("bst2_L:Bool")]         = data::parse_data_expression("false");
  sigma[data::parse_variable("bst3_L:Bool")]         = data::parse_data_expression("false");
  sigma[data::parse_variable("k_L:Nat")]             = data::parse_data_expression("0");
  sigma[data::parse_variable("l'_R:Nat")]            = data::parse_data_expression("0");
  sigma[data::parse_variable("b_R:BBuf", data_spec)] = data::parse_data_expression("[false, false]");

  pbes_system::pbes_expression x = r(phi, sigma);
}

BOOST_AUTO_TEST_CASE(test_substitutions4)
{
  std::cout << "<test_substitutions4>" << std::endl;
  data::data_specification data_spec;
  data_spec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(data_spec);
  pbes_system::simplify_data_rewriter<data::rewriter> R(datar);

  std::string var_decl;
  std::string sigma;
  std::string expr1;
  std::string expr2;
  std::string expr3;

  //------------------------//
  var_decl =
    "datavar         \n"
    "  m, n:  Pos;   \n"
    "                \n"
    "predvar         \n"
    "  X: Pos;       \n"
    ;
  expr1 = "val(m < n) && X(m + n)";
  expr2 = "X(7)";
  sigma = "m: Pos := 3; n: Pos := 4";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  //------------------------//
  var_decl =
    "datavar         \n"
    "  n1, n2: Pos;  \n"
    "                \n"
    "predvar         \n"
    "  X: Pos;       \n"
    ;
  expr1 = "val(n1 > n2)";
  expr2 = "false";
  sigma = "n2:Pos := 2; n1:Pos := 1";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  //------------------------//
  var_decl =
    "datavar         \n"
    "  n1, n2: Pos;  \n"
    "                \n"
    "predvar         \n"
    "  X: Pos;       \n"
    ;
  expr1 = "!(exists m: Nat. val(!(n1 > n2)))";
  expr2 = "false";
  sigma = "n2:Pos := 2; n1:Pos := 1";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  expr1 = "!(forall m: Nat. false) && !(exists m: Nat. false) && !(forall m: Nat. false) && !(exists m: Nat. val(!(n1 > n2))) && !(forall m: Nat. false) && !(exists m: Nat. val(!(n1 > n2)))";
  expr2 = "false";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);
}

BOOST_AUTO_TEST_CASE(test_substitutions5)
{
  std::cout << "<test_substitutions5>" << std::endl;
  data::data_specification data_spec;
  data_spec.add_context_sort(data::sort_nat::nat());
  data::rewriter datar(data_spec);
  pbes_system::data_rewriter<data::rewriter> R(datar);

  std::string var_decl;
  std::string sigma;
  std::string expr1;
  std::string expr2;
  std::string expr3;

  //------------------------//
  var_decl =
    "datavar         \n"
    "  n: Pos;       \n"
    "                \n"
    "predvar         \n"
    "  X: Pos;       \n"
    ;
  expr1 = "val(n == 2)";
  expr2 = "true";
  sigma = "n:Pos := 2";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);

  expr1 = "val(2 == 2)";
  test_expressions(R, expr1, R, expr2, var_decl, sigma);
}

BOOST_AUTO_TEST_CASE(test_data2pbes)
{
  std::cout << "<test_data2pbes>" << std::endl;
  pbes_system::pbes_expression x;
  pbes_system::pbes_expression y;

  x = pbes_system::parse_pbes_expression("forall n: Nat. val(n != 3 && n == 5)");
  y = pbes_system::detail::data2pbes(x);
  std::clog << "x = " << pbes_system::pp(x) << std::endl;
  std::clog << "y = " << pbes_system::pp(y) << std::endl;
  BOOST_CHECK(pbes_system::pp(y) == "forall n: Nat. val(n != 3) && val(n == 5)");

  x = pbes_system::parse_pbes_expression("val(forall n: Nat. n != 3 && n == 5)");
  y = pbes_system::detail::data2pbes(x);
  std::clog << "x = " << pbes_system::pp(x) << std::endl;
  std::clog << "y = " << pbes_system::pp(y) << std::endl;
  BOOST_CHECK(pbes_system::pp(y) == "forall n: Nat. val(n != 3) && val(n == 5)");
}

BOOST_AUTO_TEST_CASE(test_simplify_rewriter)
{
  std::string text =
    "pbes nu X(b: Bool) =                                  \n"
    "       val(b) && (X(b) || X(!b)) && (X(b) || X(!!b)); \n"
    "                                                      \n"
    "init X(true);                                         \n"
    ;
  text =  "pbes mu X0(n: Nat) =\n"
          "           (exists t: Nat. val(t < 3) || !X0(1)) => val(n > 0);\n"
          "     nu X1 = true;\n"
          "init X0(0);\n"
          ;

  pbes p = txt2pbes(text, false);
  pbes_system::simplify_rewriter R;
  pbes_system::pbes_rewrite(p, R);
}


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

/*
 * index-encoding.cpp
 *
 *  Created on: Dec 2, 2014
 *      Author: David A. Boyuka II
 */

#include <iostream>
#include <boost/smart_ptr.hpp>

#include "pique/encoding/index-encoding.hpp"
#include "pique/util/dynamic-dispatch.hpp"

#include "pique/indexing/binned-index.hpp"
#include "pique/setops/setops.hpp"

#include "pique/encoding/eq/eq-encoding.hpp"
#include "pique/encoding/range/range-encoding.hpp"
#include "pique/encoding/interval/interval-encoding.hpp"
#include "pique/encoding/binarycomp/binarycomp-encoding.hpp"
#include "pique/encoding/hier/hier-encoding.hpp"

using EncType = IndexEncoding::Type;

typedef typename
	MakeValueToTypeDispatch< EncType >
	::WithValues< EncType::EQUALITY, EncType::RANGE, EncType::INTERVAL, EncType::BINARY_COMPONENT, EncType::HIERARCHICAL >
	::WithTypes< EqualityIndexEncoding, RangeIndexEncoding, IntervalIndexEncoding, BinaryComponentIndexEncoding, HierarchicalIndexEncoding >
	::type EncTypeDispatch;

boost::shared_ptr< IndexEncoding > IndexEncoding::get_instance(Type type) {
	return EncTypeDispatch::template dispatchMatching< boost::shared_ptr< IndexEncoding > >(
			type,
			boost::make_shared_dispatch< IndexEncoding >(),
			nullptr);
}

auto IndexEncoding::get_encoded_index(
	boost::shared_ptr< const IndexEncoding > index_enc,
	boost::shared_ptr< const BinnedIndex > in_index,
	const AbstractSetOperations &setops
	) -> boost::shared_ptr< BinnedIndex >
{
	// Assume the input index is equality-encoded, so that encoded regions == bin regions
	assert(in_index->get_encoding()->get_type() == Type::EQUALITY);

	region_vector_t bin_regions;
	in_index->get_regions(bin_regions);

	region_vector_t encoded_regions = index_enc->get_encoded_regions(bin_regions, setops);
	return in_index->derive_new_index(index_enc, encoded_regions);
}

auto IndexEncoding::get_encoded_regions_impl(region_vector_t bins, const AbstractSetOperations& setops) const -> region_vector_t {
	const std::vector< bin_id_vector_t > enc_region_defs = this->get_encoded_region_definitions(bins.size());

	region_vector_t enc_regions;
	for (const bin_id_vector_t &enc_region_def : enc_region_defs) {
		region_vector_t bins_to_merge;
		for (const bin_id_t bin : enc_region_def)
			bins_to_merge.push_back(bins[bin]);

		boost::shared_ptr< RegionEncoding > enc_region =
			setops.dynamic_nary_set_op(bins_to_merge.begin(), bins_to_merge.end(), NArySetOperation::UNION);

		enc_regions.push_back(enc_region);
	}

	return enc_regions;
}


// Copyright (C) 2003 Roland Richter <roland@flll.jku.at>
// Permission to copy, use, modify, sell and distribute this software
// is granted provided this copyright notice appears in all copies.
// This software is provided "as is" without express or implied
// warranty, and with no claim as to its suitability for any purpose.
//
//
// in Sept 2009, Erwann Rogard's modified version:
//  - this->foo() rather than foo() because compile errors with gcc 4.0/Mac OS
//  - deleted reachable functionality
//  - Uses a modulo 1 base iterator (e) to mark an end iterator
//    sequence =  {0,1,2,3,e}, step = 2;
//    offset:       base iterators:
//    0 :           {0,2,3}
//    1 :           {1,3,e}
//
// Warning : don't try step < 0 yet

#ifndef BOOST_VIEW_DETAIL_STEP_COUNT_DOWN_ITERATOR_HPP_ER_2009
#define BOOST_VIEW_DETAIL_STEP_COUNT_DOWN_ITERATOR_HPP_ER_2009

#include <iostream> // TODO remove
#include <string>
#include <stdexcept>
#include <boost/format.hpp>

#include <boost/iterator/iterator_adaptor.hpp>
#include <boost/next_prior.hpp>

namespace boost {
namespace view{
namespace detail{   // to avoid any conflict with existing files in view

template< class I
        , class V   = use_default
        , class C   = use_default
        , class R   = use_default
        , class D   = use_default
>
class step_count_down_iterator
  : public iterator_adaptor<
        step_count_down_iterator<I, V, C, R, D>,
        I,
        V,
        C,
        R,
        D
    >
{
    typedef std::string str_;
    typedef std::size_t size_;
public:

  typedef iterator_adaptor<
        step_count_down_iterator<I, V, C, R, D>,
        I,
        V,
        C,
        R,
        D
  > super_type;

    typedef typename super_type::difference_type difference_type;
    typedef typename super_type::reference reference;

    step_count_down_iterator(): step_( 1 ),count_down_(0)
    {}

    // Constructs an iterator that traverses the subsequence
    // {(i+k), ..., (i+(n-1)k),(i+(n-1)k+1)}
    // where n is the largest number such that (i+(n-1)k+1)<=e
    explicit step_count_down_iterator( I i, I e, difference_type theStep = 1 )
    : super_type(i), step_( theStep ),count_down_(0)
    {
        difference_type d = std::distance(i,e);
        if(d>0){
            this->count_down_ = (d-1)/this->step()+1;
        }

        BOOST_ASSERT( std::distance(i,e)>=0);
        BOOST_ASSERT( this->step() != 0 );
    }

    step_count_down_iterator( const step_count_down_iterator& rhs )
        : super_type( rhs ), step_( rhs.step_ ), count_down_(rhs.count_down_)
    {}

    //operator I() const
    //  { return this->base(); }

    difference_type
    step()const{ return this->step_ ;}

    difference_type count_down()const{ return this->count_down_; }

private:
    friend class iterator_core_access;

    // TODO
    /// If the step if negative behaves like a reverse_iterator
    reference dereference() const
    {
        //if( this->step() < 0 )
        //    return *boost::prior( this->base() );
        //else
        return *this->base();
    }

    void increment()
    {
        if(this->count_down()==0){
            throw std::out_of_range(
                "view::detail::step_count_down_iterator"
            );
        }else{
            this->base_reference() = boost::next(
                this->base(),
                (this->count_down()>1)? this->step() : 1
            );
        }
        --this->count_down_;
    }

    void decrement()
    {
        this->base_reference() = boost::prior(
            this->base(),
            (this->count_down() > 0) ? this->step() : 1
        );
        ++this->count_down_;
    }

    void advance( difference_type n )
    {
        if(n==0){
            return;
        }
        difference_type n2 = this->count_down() - n;
        if(n2<0){
            throw std::out_of_range(
                "view::detail::step_count_down_iterator"
            );
        }
        if(n>0){
            this->base_reference() = boost::next(
                this->base(),
                (n2>0)
                    ?(n * this->step()):((n-1) * this->step()+1)
            );
        }else{
            this->base_reference() = boost::next(
                this->base(),
                (this->count_down()>0)
                    ?(n * this->step()):-1+(n+1) * this->step()
            );
        }
        this->count_down_ -= n;
    }

    difference_type distance_to( const step_count_down_iterator& rhs ) const
    {
        BOOST_ASSERT(
            boost::next(
                *this,
                this->count_down()
            ) == boost::next(
                rhs,
                rhs.count_down()
            )
        );
        return this->count_down()-rhs.count_down();
    }

    bool equal( const step_count_down_iterator& rhs ) const
    {
        return(
            this->base() == rhs.base()
                && this->step() == rhs.step() );
    }

private:
  difference_type step_;
  difference_type count_down_;
};

// Returns an iterator that traverses the subsequence
// {(i+k), ..., (i+(n-1)k),(i+(n-1)k+1)}
// where n is the largest number such that (i+(n-1)k+1)<=e
template< class I, class D >
inline step_count_down_iterator<I>
make_step_count_down_iterator( I i, I e, D k)
{
  typedef step_count_down_iterator<I> result_;
  return result_( i, e, k );
}

// Returns the step_iterator whose base is the maximum multiple number of steps
// away from b, less or equal to e
template<typename I,typename D>
step_count_down_iterator<I>
make_end_step_count_down_iterator(I b,I e,D step){
    typedef typename iterator_difference<I>::type diff_;
    typedef step_count_down_iterator<I> res_;
    //TODO be able lift these restrictions
    BOOST_ASSERT(step>=0);
    diff_ d = std::distance(b,e);
    BOOST_ASSERT(d>=0);
    if(d>0){
        d = (d-1)/step;
        b = boost::next(b,d * step+1);
        return res_(b,b,step);
    }else{
        return res_(e,e,step);
    }
}

}// detail
}// view
}// boost

#endif


#ifndef STATDISTANCE_HPP
#define STATDISTANCE_HPP

#include <cmath>
#include <string>
#include <vector>
#include <boost/random.hpp>
#include <boost/math/special_functions/bessel.hpp>

//todo : should change name and implementaion for general case

namespace stat {

class statdistance_equil{
public :
  static std::string name() { return "Statistical Distance from Equilibrium : calc by histogram"; }
  statdistance(unsigned int n_bin, unsigned int N_loop) :  n_bin_(n_bin), N_(N_loop) {KL_ = BC_ = 0.0;}

  template <class exactcalc>
  void inputdat(std::vector<double> const& range, std::vector<double> const& hist) const {
    for(int i = 0 ; i < n_bin ; ++i){
    BC_ += std::exp( - 0.5 * (E/T + exact.logZ(T))) / sqrtN_;
    KL_ += (E/T + exact.logZ(T)) / N_;
  }

  double KLdivergence() const {
    return KL_;
  }

  double BhattacharyyaCoefficient() const {
    return BC_;
  }

  double BhattacharyyaDistance() const {
   return -1.0 * std::log(BC_);
  }

  double HellingerDistance() const {
    return 1 - BC_;
  }

  void reset() const {
    BC_ = KL_ = 0;
  }

private :
  unsigned int n_bin_;
  unsigned int N_;
  double BC_;
  double KL_;
}; //end class

class statdistance_equil{

}; // end class

}
#endif // EXACTCALC_HPP


/**
*   Box.hpp
*
*   Copyright 2015 The Climate Corporation
*   Copyright 2015 Tony Stone
*
*   Licensed under the Apache License, Version 2.0 (the "License");
*   you may not use this file except in compliance with the License.
*   You may obtain a copy of the License at
*
*   http://www.apache.org/licenses/LICENSE-2.0
*
*   Unless required by applicable law or agreed to in writing, software
*   distributed under the License is distributed on an "AS IS" BASIS,
*   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
*   See the License for the specific language governing permissions and
*   limitations under the License.
*
*   Created by Tony Stone on 6/16/15.
*
*   MODIFIED 2015 BY Tony Stone. Modifications licensed under Apache License, Version 2.0.
*
*/
#pragma once

#ifndef __Box_HPP_
#define __Box_HPP_

#include "Geometry.hpp"
#include "Point.hpp"

#include <boost/concept/assert.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/geometries/concepts/point_concept.hpp>
#include <boost/geometry/algorithms/convert.hpp>

namespace geofeatures {

    /**
     * @class       Box
     *
     * @brief       A Box with 2 corner points, min and max.
     *
     * @author      Tony Stone
     * @date        6/16/15
     */
    class Box : public Geometry {

    public:
        // construction/destruction
        inline Box() noexcept : Geometry(), minCorner_(), maxCorner_() {}
        inline Box(Point const& minCorner, Point const& maxCorner) noexcept : Geometry(), minCorner_(minCorner), maxCorner_(maxCorner) {}
        inline virtual ~Box() noexcept {};

        inline Point const& minCorner() const noexcept { return minCorner_; }
        inline Point const& maxCorner() const noexcept { return maxCorner_; }

        inline Point& minCorner() noexcept { return minCorner_; }
        inline Point& maxCorner() noexcept { return maxCorner_; }

    private:
        Point minCorner_;
        Point maxCorner_;
    };

}   // namespace geofeatures

namespace geofeatures_boost {
    namespace geometry {
        namespace traits {
            // Adapt geofeatures::Box to Boost.Geometry

            template <> struct tag<geofeatures::Box>
            {  typedef box_tag type; };

            template <> struct point_type<geofeatures::Box>
            { typedef geofeatures::Point type; };

            template <std::size_t Dimension>
            struct indexed_access<geofeatures::Box, min_corner, Dimension>
            {
                typedef typename geometry::coordinate_type<geofeatures::Point>::type coordinate_type;

                static inline coordinate_type get(geofeatures::Box const& b)
                {
                    return geometry::get<Dimension>(b.minCorner());
                }

                static inline void set(geofeatures::Box& b, coordinate_type const& value)
                {
                    geometry::set<Dimension>(b.minCorner(), value);
                }
            };

            template <std::size_t Dimension>
            struct indexed_access<geofeatures::Box, max_corner, Dimension>
            {
                typedef typename geometry::coordinate_type<geofeatures::Point>::type coordinate_type;

                static inline coordinate_type get(geofeatures::Box const& b)
                {
                    return geometry::get<Dimension>(b.maxCorner());
                }

                static inline void set(geofeatures::Box& b, coordinate_type const& value)
                {
                    geometry::set<Dimension>(b.maxCorner(), value);
                }
            };

        }  // namespace traits
    } // namespace geometry
} // namespace boost

#endif //__Box_HPP_


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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 BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MANTISSA_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_MANTISSA_HPP_INCLUDED

#include <boost/simd/ieee/functions/mantissa.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/bitwise_and.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mantissamask.hpp>
#include <boost/simd/sdk/config.hpp>

#ifndef BOOST_SIMD_NO_INVALIDS
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/is_invalid.hpp>
#endif

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( mantissa_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<integer_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      return a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( mantissa_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      A0 r = b_or(b_and(a0,Mantissamask<A0>()),One<A0>());
      #ifndef BOOST_SIMD_NO_INVALIDS
      return if_else(logical_or(is_invalid(a0),is_eqz(a0)),a0,r);
      #else
      return if_else(is_eqz(a0),a0,r);
      #endif
    }
  };

} } }

#endif


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE polararsite_test

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/polarsite.h"

using namespace votca::xtp;

BOOST_AUTO_TEST_SUITE(polararsite_test)

BOOST_AUTO_TEST_CASE(constructors_test) { PolarSite ps(1, "ps1"); }

BOOST_AUTO_TEST_CASE(getters_test) {
  PolarSite ps(1, "ps2");
  BOOST_CHECK_EQUAL(ps.getId(), 1);
  BOOST_CHECK_EQUAL(ps.getElement(), "ps2");
}

BOOST_AUTO_TEST_CASE(multipole_test) {
  PolarSite ps(1, "ps2");
  Eigen::VectorXd multipole = Eigen::VectorXd::Zero(9);
  multipole << 1, 2, 3, 4, 8, 7, 2, 3.3, -0.5;
  ps.setMultipole(multipole);
  bool check_mpoles = multipole.isApprox(ps.getPermMultipole(), 0.0001);
  BOOST_CHECK_EQUAL(check_mpoles, true);

  bool check_rank = (ps.getRank() == 2);
  BOOST_CHECK_EQUAL(check_rank, true);
}

BOOST_AUTO_TEST_CASE(translate_test) {
  PolarSite ps(1, "ps2");
  Eigen::Vector3d shift;
  shift << 0, 0, 5;
  ps.Translate(shift);
  BOOST_CHECK_EQUAL(shift.isApprox(ps.getPos(), 1e-5), true);
}

BOOST_AUTO_TEST_CASE(rotation_test) {
  PolarSite ps(1, "ps2", Eigen::Vector3d::UnitY());

  Eigen::Matrix3d R = Eigen::Matrix3d::Zero();  // Rotation around z axes
  R << 0, -1, 0, 1, 0, 0, 0, 0, 1;

  Eigen::VectorXd multipoles = Eigen::VectorXd::Zero(9);
  multipoles << 1, 1, 0, 0, 0, 1, 0, 0,
      0;  // q=1, mu_x=1 and Q_21c=1 the rest is 0
  ps.setMultipole(multipoles);
  ps.Rotate(R, Eigen::Vector3d::Zero());
  bool equalpos = ps.getPos().isApprox(Eigen::Vector3d(-1, 0, 0), 1e-5);
  if (!equalpos) {
    std::cout << "Result " << std::endl;
    std::cout << ps.getPos() << std::endl;
    std::cout << "Reference" << std::endl;
    std::cout << Eigen::Vector3d(-1, 0, 0) << std::endl;
  }
  BOOST_CHECK_EQUAL(equalpos, true);

  Eigen::VectorXd rotmultipoles = Eigen::VectorXd::Zero(9);
  rotmultipoles << 1, 0, 1, 0, 0, 0, 1, 0, 0;  // q=1, mu_y=1 and Q_21s=1 is 0
  bool equalmultipoles = rotmultipoles.isApprox(ps.getPermMultipole(), 1e-5);
  if (!equalmultipoles) {
    std::cout << "Result " << std::endl;
    std::cout << ps.getPermMultipole() << std::endl;
    std::cout << "Reference" << std::endl;
    std::cout << rotmultipoles << std::endl;
  }
  BOOST_CHECK_EQUAL(equalmultipoles, true);
}

BOOST_AUTO_TEST_CASE(interaction_test) {
  PolarSite ps1(1, "ps1");
  PolarSite ps2(2, "ps2", Eigen::Vector3d::UnitX());

  Eigen::VectorXd mp1 = Eigen::VectorXd::Zero(1);
  Eigen::VectorXd mp2 = Eigen::VectorXd::Zero(1);
  mp1 << 1;
  mp2 << -1;
  ps1.setPolarisable(false);
  ps2.setPolarisable(false);
  ps1.setMultipole(mp1);
  ps2.setMultipole(mp2);

  double Energyref = -1;
  double Energy = ps1.InteractStatic(ps2);
  BOOST_CHECK_EQUAL(std::abs(Energy - Energyref) < 1e-9, true);

  bool check_field = ps1.getField().isApprox(ps2.getField(), 1e-5);
  if (!check_field) {
    std::cout << "Field at ps1" << std::endl;
    std::cout << ps1.getField() << std::endl;
    std::cout << "Field at ps2" << std::endl;
    std::cout << ps2.getField() << std::endl;
  }
  BOOST_CHECK_EQUAL(check_field, true);

  bool check_potential =
      std::abs(ps1.getPotential() + ps2.getPotential()) < 1e-5;
  if (!check_potential) {
    std::cout << "Potential at ps1" << std::endl;
    std::cout << ps1.getPotential() << std::endl;
    std::cout << "Potential at ps2" << std::endl;
    std::cout << ps2.getPotential() << std::endl;
  }
  BOOST_CHECK_EQUAL(check_potential, true);
  PolarSite ps3(3, "ps3");
  PolarSite ps4(4, "ps4", Eigen::Vector3d::UnitZ());
  Eigen::VectorXd multipole = Eigen::VectorXd::Zero(9);
  multipole << 1, 2, 3, 4, 8, 7, 2, 3.3, -0.5;

  ps3.setPolarisable(true);
  ps4.setPolarisable(true);
  ps3.setMultipole(multipole);
  ps4.setMultipole(multipole);
  ps3.InteractStatic(ps4);
}

BOOST_AUTO_TEST_CASE(induction_test) {
  PolarSite ps1(1, "ps1");
  PolarSite ps2(2, "ps2", Eigen::Vector3d::UnitX());

  Eigen::VectorXd mp1 = Eigen::VectorXd::Zero(1);
  Eigen::VectorXd mp2 = Eigen::VectorXd::Zero(1);
  mp1 << 1;
  mp2 << -1;
  ps1.setPolarisable(true);
  ps2.setPolarisable(true);
  ps1.setMultipole(mp1);
  ps2.setMultipole(mp2);
  Eigen::Matrix3d poltensor = Eigen::Matrix3d::Zero();
  poltensor << 2, 1, 0, 1, 3, 1, 0, 1, 2.5;

  ps1.setpolarization(poltensor);
  ps2.setpolarization(poltensor);

  // double Energy= ps1.InteractStatic(ps2);
  ps1.Induce(1);
  ps2.Induce(1);
  // double alpha=0.39;
  // double InductionEnergy=ps1.InteractInduction(ps2,alpha);
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright (C) 2018 Fondazione Istituto Italiano di Tecnologia iCub Facility
 * Authors: see AUTHORS file.
 * CopyPolicy: Released under the terms of the LGPLv2.1 or any later version, see LGPL.TXT or LGPL3.TXT
 */

// gazebo
#include <gazebo/physics/Model.hh>
#include <gazebo/physics/Link.hh>
#include <gazebo/common/Events.hh>
#include <gazebo/physics/World.hh>

// ignition
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Quaternion.hh>

// GazeboYarpPlugins
#include <GazeboYarpPlugins/common.h>

// yarp
#include <yarp/os/Log.h>
#include <yarp/os/LogStream.h>
#include <yarp/math/FrameTransform.h>

// boost
#include <boost/bind.hpp>

#include "ModelPosePublisher.hh"

GZ_REGISTER_MODEL_PLUGIN(gazebo::GazeboYarpModelPosePublisher)

namespace gazebo {

GazeboYarpModelPosePublisher::~GazeboYarpModelPosePublisher()
{
    // Close the driver
    m_drvTransformClient.close();
}

void GazeboYarpModelPosePublisher::Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
    // Check yarp network availability
    if (!m_yarp.checkNetwork(GazeboYarpPlugins::yarpNetworkInitializationTimeout)) {
        yError() << "GazeboYarpModelPosePublisher::Load error:"
		 << "yarp network does not seem to be available, is the yarpserver running?";
        return;
    }

    // Store pointer to the model
    m_model = _parent;

    // Load update period
    if (_sdf->HasElement("period")) {
	// Get the parameter
	sdf::ParamPtr periodPtr = _sdf->GetElement("period")->GetValue();

	// Check if it is a strictly positive double
	if (!periodPtr->Get<double>(m_period) || m_period <= 0) {
		yError() << "GazeboYarpModelPosePublisher::Load error:"
			 << "parameter 'period' for model"
			 << m_model->GetName() << "should be a strictly positive number";
		return;
	}
    } else {
	// Default to 10 ms
	m_period = 0.01;
    }

    // Prepare properties for the PolyDriver
    yarp::os::Property propTfClient;
    propTfClient.put("device", "transformClient");
    // The local port depends on the model name that is unique
    // also in the case of multiple insertions of the same model
    // in Gazebo
    propTfClient.put("local", "/" + m_model->GetName() + "/transformClient");
    propTfClient.put("remote", "/transformServer");
    // This is the update period of the transformClient in ms
    propTfClient.put("period", m_period * 1000);

    // Open the driver and obtain a a IFrameTransform view
    m_tfClient = nullptr;
    bool ok = m_drvTransformClient.open(propTfClient);
    ok = ok && m_drvTransformClient.view(m_tfClient) && m_tfClient != nullptr;

    // Return if the driver open failed
    // or the view retrieval failed
    // or the IFrameTransform pointer is not valid
    if (!ok) {
	yError() << "GazeboYarpModelPosePublisher::Load error:"
	         << "failure in opening iFrameTransform interface for model"
		 << m_model->GetName();
	return;
    }

    // Clear last update time
    m_lastUpdateTime = gazebo::common::Time(0.0);

    // Listen to the update event
    auto worldUpdateBind = boost::bind(&GazeboYarpModelPosePublisher::OnWorldUpdate, this);
    m_worldUpdateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(worldUpdateBind);
}

void GazeboYarpModelPosePublisher::PublishTransform()
{
    // Get the current pose of the canonical link of the model
#if GAZEBO_MAJOR_VERSION >= 8
    ignition::math::Pose3d curPose = m_model->GetLink()->WorldPose();
#else
    gazebo::math::Pose curPoseGazebo = m_model->GetLink()->GetWorldPose();
    // Convert to Ignition so that the same interface
    // can be used in the rest of the function
    ignition::math::Pose3d curPose = curPoseGazebo.Ign();
#endif

    // Get the positional and rotational parts
    ignition::math::Vector3d pos = curPose.Pos();
    ignition::math::Quaterniond rot = curPose.Rot();

    // Convert the pose to a homogeneous transformation matrix
    // (the first two arguments can be blank)
    yarp::math::FrameTransform  inertialToModel("", "",
						pos.X(),
						pos.Y(),
						pos.Z(),
						rot.X(),
						rot.Y(),
						rot.Z(),
						rot.W());

    // Set a new transform using
    // /inertial for source frame name and
    // /<model_name>/frame for the target frame name
    m_tfClient->setTransform("/" + m_model->GetName() + "/frame",
			     "/inertial",
			     inertialToModel.toMatrix());
}

void GazeboYarpModelPosePublisher::OnWorldUpdate()
{
    // Get current time
#if GAZEBO_MAJOR_VERSION >= 8
    gazebo::common::Time currentTime = m_model->GetWorld()->SimTime();
#else
    gazebo::common::Time currentTime = m_model->GetWorld()->GetSimTime();
#endif

    // Check if an update period is elapsed
    if(currentTime - m_lastUpdateTime >= m_period) {

	// Store current time for next update
	m_lastUpdateTime = currentTime;

	// Publish the transform with the current pose
	PublishTransform();
    }
}

}


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/slice.hpp>
#include <boost/simd/function/combine.hpp>
#include <boost/simd/function/deinterleave_first.hpp>
#include <boost/simd/function/deinterleave_second.hpp>
#include <boost/simd/detail/dispatch/meta/as_floating.hpp>
#include <array>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( deinterleave_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                          , bs::pack_<bd::type64_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE std::array<A0,2> operator()(A0 const& a0, A0 const& a1 ) const BOOST_NOEXCEPT
    {
      auto b0 = bitwise_cast<bd::as_floating_t<A0>>(a0);
      auto b1 = bitwise_cast<bd::as_floating_t<A0>>(a1);
      auto p0 = _mm256_permute2f128_pd(b0,b1,0x20);
      auto p1 = _mm256_permute2f128_pd(b0,b1,0x31);

      A0 f = bitwise_cast<A0>( _mm256_unpacklo_pd( p0, p1 ) );
      A0 s = bitwise_cast<A0>( _mm256_unpackhi_pd( p0, p1 ) );

      return {f,s};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( deinterleave_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                          , bs::pack_<bd::type32_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE std::array<A0,2> operator()(A0 const& a0, A0 const& a1 ) const BOOST_NOEXCEPT
    {
      auto b0 = bitwise_cast<bd::as_floating_t<A0>>(a0);
      auto b1 = bitwise_cast<bd::as_floating_t<A0>>(a1);
      auto x  = _mm256_permute2f128_ps(b0,b1,0x20);
      auto y  = _mm256_permute2f128_ps(b0,b1,0x31);
      auto u0 = _mm256_unpacklo_ps(x,y);
      auto u1 = _mm256_unpackhi_ps(x,y);

      A0 f = bitwise_cast<A0>( _mm256_unpacklo_ps( u0, u1 ) );
      A0 s = bitwise_cast<A0>( _mm256_unpackhi_ps( u0, u1 ) );

      return {f,s};
    }
  };

  BOOST_DISPATCH_OVERLOAD ( deinterleave_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                          , bs::pack_<bd::integer_<A0>, bs::avx_>
                         )
  {
    BOOST_FORCEINLINE std::array<A0,2> operator()(A0 const& a0, A0 const& a1 ) const BOOST_NOEXCEPT
    {
      auto s00 = slice_low(a0), s01 = slice_high(a0);
      auto s10 = slice_low(a1), s11 = slice_high(a1);

      auto f = combine(deinterleave_first (s00,s01), deinterleave_first (s10,s11));
      auto s = combine(deinterleave_second(s00,s01), deinterleave_second(s10,s11));

      return {f,s};
    }
  };
} } }

#endif


#include <petscmat.h>
#include <petscsys.h>
#include <petscviewer.h>
#include <petsctao.h>

#include <boost/functional/hash.hpp>
#include <unordered_map>
#include <memory>
#include "TaoVQEBackend.hpp"
#include "MPIProvider.hpp"

using IndexPair = std::pair<std::uint64_t, std::uint64_t>;

namespace xacc {
namespace vqe {

typedef struct {
  int nParameters;
  std::shared_ptr<ComputeEnergyVQETask> computeTask;
  double currentEnergy = 0.0;
  Eigen::VectorXd angles;
} AppCtx;

PetscErrorCode nelderMeadFunction(Tao tao, Vec X, PetscReal *f, Vec G,
		void *ptr) {
	AppCtx *user = (AppCtx *) ptr;
	const double* x;

	/* Get pointers to vector data */
	VecGetArrayRead(X, &x);

	// Need to broadcast data pointer
	auto params = Eigen::Map<const Eigen::VectorXd>(x, user->nParameters);
	auto e = user->computeTask->execute(params).energy;
	*f = e;
	user->currentEnergy = e;
	user->angles = params;
	/* Restore vectors */
	VecRestoreArrayRead(X, &x);

	return 0;
}

PetscErrorCode poundersFunction(Tao tao, Vec X, Vec F, void * ptr) {
	AppCtx *user = (AppCtx *) ptr;
	PetscInt i;
	PetscReal* f, *x;

	VecGetArray(X, &x);
	VecGetArray(F, &f);

	auto params = Eigen::Map<const Eigen::VectorXd>(x, user->nParameters);
	auto e = user->computeTask->execute(params).energy;
	f[0] = e + 5.0;
	user->currentEnergy = e;
	user->angles = params;
	VecRestoreArray(X, &x);
	VecRestoreArray(F, &f);
	return 0;
}

const VQETaskResult TaoVQEBackend::minimize(Eigen::VectorXd parameters) {

	xacc::info("Running VQE via PETSc Tao.");
	static char help[] = "";
	std::vector<std::string> argvVec;
	std::vector<char*> cstrs;
	argvVec.insert(argvVec.begin(), "appExec");
	for (auto& s : argvVec) {
		cstrs.push_back(&s.front());
	}
	int argc = argvVec.size();
	auto argv = cstrs.data();
	PetscInitialize(&argc, &argv, (char*) 0, (char*) 0);

	auto nParameters = program->getNParameters();

	Vec x;
	Tao tao;
	PetscBool flg;
	AppCtx user;

	computeTask = std::make_shared<ComputeEnergyVQETask>(program);
	user.nParameters = nParameters;
	user.computeTask = computeTask;
	VecCreateSeq(PETSC_COMM_WORLD, nParameters, &x);
	TaoCreate(PETSC_COMM_WORLD, &tao);
	std::string t = "nm";
	if (xacc::optionExists("tao-type")) {
		t = xacc::getOption("tao-type");
	}
	TaoSetType(tao, t == "nm" ? TAONM : TAOPOUNDERS);

	/* Set solution vec and an initial guess */
	PetscInt *indices = new PetscInt[nParameters];
	for (int i = 0; i < nParameters; i++)
		indices[i] = i;

	// Set the Parameter vector
	VecSetValues(x, nParameters, indices, parameters.data(), INSERT_VALUES);
	VecAssemblyBegin(x);
	VecAssemblyEnd(x);

	TaoSetInitialVector(tao, x);

	if (t == "pounders") {
		xacc::info("Running Pounders");
		Vec F;
		VecCreateSeq(PETSC_COMM_WORLD, 1, &F);
		TaoSetSeparableObjectiveRoutine(tao, F, poundersFunction,
				(void*) &user);
	} else {
		/* Set routines for function, gradient, hessian evaluation */
		TaoSetObjectiveAndGradientRoutine(tao, nelderMeadFunction, &user);
	}

	TaoSolve(tao);

	TaoDestroy(&tao);
	VecDestroy(&x);
	PetscFinalize();

	VQETaskResult result;
	result.energy = user.currentEnergy;
	result.angles = user.angles;
	result.nQpuCalls = computeTask->totalQpuCalls;
	result.vqeIterations = computeTask->vqeIteration;
	return result;
}

}
}