#include <limits>
#include <vector>
#include <unordered_map>

#include "controllers/sfm_aligner.h"
#include "estimators/similarity_transform.h"
#include "estimators/ransac_similarity.h"
#include "estimators/sim3.h"
#include "optim/bundle_adjustment.h"
#include "util/timer.h"
#include "math/util.h"

#include <glog/logging.h>
#include <Eigen/Geometry>
#include <ceres/rotation.h>

namespace GraphSfM {

double MeanReprojectionResiduals(const std::vector<double>& residuals)
{
    double mean_residual = 0.0;
    for (auto residual : residuals) {
        mean_residual += residual;
    }
    return mean_residual / residuals.size();
}

std::vector<double> ComputeReprojectionResiduals(const std::vector<Eigen::Vector3d>& src_points,
                                    const std::vector<Eigen::Vector3d>& ref_points,
                                    const Eigen::Matrix3x4d& alignment)
{
    std::vector<double> residuals;
    for (uint i = 0; i < src_points.size(); i++) {
        const Eigen::Vector3d& tsrc_point = alignment * src_points[i].homogeneous();
        const Eigen::Vector3d& ref_point = ref_points[i];

        double residual = (tsrc_point - ref_point).norm();
        residuals.push_back(residual);
    }
    return residuals;
}

SfMAligner::SfMAligner(const std::vector<Reconstruction*>& reconstructions,
                       const BundleAdjustmentOptions& ba_options)
     : reconstructions_(reconstructions),
       ba_options_(ba_options)
{
    // some logic or parameters check
    CHECK_GT(reconstructions.size(), 0);
    CHECK_GT(reconstructions_.size(), 0);

    for (uint i = 0; i < reconstructions_.size(); i++) {
        LOG(INFO) << "Node id: " << i;
        CHECK_NOTNULL(reconstructions_[i]);
        LOG(INFO) << "Total images number: " << reconstructions_[i]->NumImages();
    }
}

bool SfMAligner::Align()
{
    // 1. Constructing a graph from reconstructions,
    // each node is a reconstruction, edges represents the connections between
    // reconstructions (by the means of common images or common 3D points), the weight
    // of edge represents the mean reprojection error.
    LOG(INFO) << "Constructing Reconstructions Graph...";
    ConstructReconsGraph();
    recons_graph_.ShowInfo();
    CHECK_EQ(recons_graph_.GetNodesNum(), reconstructions_.size());

    // The reconstruction graph should be at least a spanning tree,
    // or we couldn't stitch all reconstructions together due to
    // too large alignment error or disconnected components.
    if (recons_graph_.GetEdgesNum() < recons_graph_.GetNodesNum() - 1) {
        LOG(ERROR) << "Can't align all reconstructions together due to "
                   << "too large alignment error or disconnected components";
        return false;
    }

    // 2. Constructing a minimum spanning tree, thus we can select the
    // most accurate n - 1 edges for accurate alignment.
    LOG(INFO) << "Finding Minimum Spanning Tree...";
    std::vector<Edge> mst_edges = recons_graph_.Kruskal();
    if (mst_edges.size() < recons_graph_.GetNodesNum() - 1) {
        return false;
    }

    Graph<Node, Edge> mst;
    for (auto node : recons_graph_.GetNodes()) {
        mst.AddNode(node.second);
    }
    for (const auto edge : mst_edges) {
        mst.AddEdge(edge);
    }
    mst.ShowInfo();

    // 3. Finding an anchor node, an anchor node is a reference reconstruction
    // that all other reconstructions should be aligned to.
    LOG(INFO) << "Finding Anchor Node...";
    FindAnchorNode(&mst);

    // 4. Compute the final transformation to anchor node for each cluster
    LOG(INFO) << "Computing Final Similarity Transformations...";
    for (uint i = 0; i < reconstructions_.size(); i++) {
        if (i != anchor_node_.id) {
            this->ComputePath(i, anchor_node_.id);
        }
    }
    sim3_to_anchor_[anchor_node_.id] = Sim3();

    // 5. Merging all other reconstructions to anchor node
    LOG(INFO) << "Merging Reconstructions...";
    this->MergeReconstructions();

    // 6. Final Bundle Adjustment
    LOG(INFO) << "Final Global Bundle Adjustment";
    this->AdjustGlobalBundle();

    return true;
}

Node SfMAligner::GetAnchorNode() const
{
    return anchor_node_;
}

std::vector<Sim3> SfMAligner::GetSim3ToAnchor() const
{
    return sim3_to_anchor_;
}

void SfMAligner::ConstructReconsGraph()
{
    // 1. Add nodes
    for (size_t i = 0; i < reconstructions_.size(); i++) {
        Node node(i);
        // node.recon = reconstructions_[i];
        recons_graph_.AddNode(node);
    }

    // 2. Add edges
    for (uint i = 0; i < reconstructions_.size(); i++) {
        for (uint j = i + 1; j < reconstructions_.size(); j++) {
            const double weight = ComputeEdgeWeight(i, j);
            LOG(INFO) << "weight: " << weight;
            if (weight != std::numeric_limits<double>::max()) {
                recons_graph_.AddEdge(Edge(i, j, (float)weight));
            }
        }
    }
}

double SfMAligner::ComputeEdgeWeight(const uint i, const uint j)
{
    const Reconstruction& recon1 = *reconstructions_[i];
    const Reconstruction& recon2 = *reconstructions_[j];
    double weight = std::numeric_limits<double>::max();

    Eigen::Matrix3d R1 = Eigen::Matrix3d::Identity(3, 3),
                    R2 = Eigen::Matrix3d::Identity(3, 3);
    Eigen::Vector3d t1 = Eigen::Vector3d::Zero(),
                    t2 = Eigen::Vector3d::Zero();
    double s1 = 1.0, s2 = 1.0;

    // Find common registered images
    std::vector<image_t> common_reg_images = recon1.FindCommonRegImageIds(recon2);
    std::vector<Eigen::Vector3d> src_points, ref_points;
    std::vector<Eigen::Matrix3d> src_rotations, ref_rotations;
    for (const auto common_id : common_reg_images) {
        src_points.push_back(recon1.Image(common_id).ProjectionCenter());
        src_rotations.push_back(recon1.Image(common_id).RotationMatrix());
        ref_points.push_back(recon2.Image(common_id).ProjectionCenter());
        ref_rotations.push_back(recon2.Image(common_id).RotationMatrix());
    }
    LOG(INFO) << "Common registerd images number: " << common_reg_images.size();

    if (common_reg_images.size() < 2) {
        LOG(WARNING) << "Not found enough common registered images.";
        return std::numeric_limits<double>::max();
    } else if (common_reg_images.size() < 5) {
        ComputeSimilarityByCameraMotions(src_points, ref_points,
                                         src_rotations, ref_rotations, R1, t1, s1);
        ComputeSimilarityByCameraMotions(ref_points, src_points,
                                         ref_rotations, src_rotations, R2, t2, s2);

        double msd1 = CheckReprojError(src_points, ref_points, s1, R1, t1),
               msd2 = CheckReprojError(ref_points, src_points, s2, R2, t2);

        // angular residual should be considered
        const double angular_residual1 =
            CheckAngularResidual(src_rotations, ref_rotations, R1);
        const double angular_residual2 =
            CheckAngularResidual(ref_rotations, src_rotations, R2);

        weight = std::max(std::max(msd1, msd2),
                          std::max(angular_residual1, angular_residual2));

        if (weight != numeric_limits<double>::max()) {
            sim3_graph_[i][j] = Sim3(R1, t1, s1);
            sim3_graph_[j][i] = Sim3(R2, t2, s2);
        }
    } else {
        double msd1 = 0.0, msd2 = 0.0;
        FindSimilarityTransform(src_points, ref_points, R1, t1, s1, msd1);
        FindSimilarityTransform(ref_points, src_points, R2, t2, s2, msd2);

        // angular residual should be considered
        const double angular_residual1 =
            CheckAngularResidual(src_rotations, ref_rotations, R1);
        const double angular_residual2 =
            CheckAngularResidual(ref_rotations, src_rotations, R2);

        weight = std::max(std::max(msd1, msd2),
                          std::max(angular_residual1, angular_residual2));

        if (weight != numeric_limits<double>::max()) {
            sim3_graph_[i][j] = Sim3(R1, t1, s1);
            sim3_graph_[j][i] = Sim3(R2, t2, s2);
        }
    }

    return weight;
}

void SfMAligner::FindAnchorNode(Graph<Node, Edge>* graph)
{
    paths_.resize(recons_graph_.GetNodesNum());
    sim3_to_anchor_.resize(recons_graph_.GetNodesNum());

    // The anchor is found by merging all leaf nodes to their adjacent nodes,
    // until one node or two nodes left. If two nodes left, we choose the reconstruction
    // that has the largest size as the anchor.
    int layer = 1;
    uint anchor_index = 0;

    while (graph->GetNodesNum() > 1) {
        LOG(INFO) << "Merging the " << layer++ << "-th layer leaf nodes";

        graph->CountOutDegrees();
        graph->CountInDegrees();
        graph->CountDegrees();
        std::unordered_map<size_t, size_t> degrees = graph->GetDegrees();

        // Finding all leaf nodes. Leaf node in graph has degree equals to 1.
        std::vector<int> indexes;
        if (graph->GetNodesNum() == 2) {
            indexes.push_back(degrees.begin()->first);
        } else {
            for (auto it = degrees.begin(); it != degrees.end(); ++it) {
                LOG(INFO) << "node: " << it->first << ", "
                          << "degree: " << it->second;
                if (it->second == 1) indexes.push_back(it->first);
            }
        }
        if (indexes.empty()) break;

        for (auto idx : indexes) {
            if (idx == -1) break;
            const Edge& edge = graph->FindConnectedEdge(idx);

            LOG(INFO) << "Find node [degree = 1]: " << idx;
            LOG(INFO) << edge.src << "->" << edge.dst << ": " << edge.weight;

            // src is the node with degree = 1
            uint src = (idx == edge.src) ? edge.src : edge.dst;
            uint dst = (idx == edge.src) ? edge.dst : edge.src;
            Reconstruction* src_recon = reconstructions_[src];
            Reconstruction* dst_recon = reconstructions_[dst];

            LOG(INFO) << "Merge Clusters: " << src << "->" << dst << ": " << edge.weight;
            anchor_index = dst;
            const Sim3 sim = sim3_graph_[src][dst];
            paths_[src].insert(std::make_pair(dst, sim));

            graph->DeleteNode(src);
            graph->DeleteEdge(src, dst);
            graph->DeleteEdge(dst, src);
            graph->ShowInfo();
        }
    }

    anchor_node_.id = anchor_index;
}

void SfMAligner::ComputePath(int src, int dst)
{
    LOG(INFO) << "Computing Path: " << src << "->" << dst;
    std::queue<int> qu;
    qu.push(src);

    Eigen::Matrix3d r = Eigen::Matrix3d::Identity();
    Eigen::Vector3d t = Eigen::Vector3d::Zero();
    double s = 1.0;

    Sim3 sim(Eigen::Matrix3d::Identity(), Eigen::Vector3d::Identity(), 1.0);
    LOG(INFO) << "v: " << src;
    while (!qu.empty()) {
        int u = qu.front(); qu.pop();
        auto it = paths_[u].begin();
        int v = it->first;
        LOG(INFO) << "v: " << v;
        s = it->second.s * s;
        r = it->second.R * r.eval();
        t = it->second.s * it->second.R * t.eval() + it->second.t;
        if (v == dst) {
            sim.s = s; sim.R = r; sim.t = t;
            sim3_to_anchor_[src] = sim;
            return;
        }
        else qu.push(v);
    }
    LOG(INFO) << "\n";
}

void SfMAligner::MergeReconstructions()
{
    for (uint i = 0; i < reconstructions_.size(); i++) {
        if (i == anchor_node_.id) { continue; }

        Sim3 sim3 = sim3_to_anchor_[i];
        Eigen::Matrix3x4d alignment;
        alignment.block(0, 0, 3, 3) = sim3.s * sim3.R;
        alignment.block(0, 3, 3, 1) = sim3.t;

        reconstructions_[anchor_node_.id]->Merge(*reconstructions_[i],
                                                 alignment);
    }
}

bool SfMAligner::AdjustGlobalBundle()
{
    Reconstruction* final_recon = reconstructions_[anchor_node_.id];
    CHECK_NOTNULL(final_recon);

    const std::vector<image_t>& reg_image_ids = final_recon->RegImageIds();

    CHECK_GE(reg_image_ids.size(), 2) << "At least two images must be "
                                         "registered for global bundle-adjustment";

    // Avoid degeneracies in bundle adjustment.
    final_recon->FilterObservationsWithNegativeDepth();

    // Configure bundle adjustment
    BundleAdjustmentConfig ba_config;
    for (const image_t image_id : reg_image_ids) {
        ba_config.AddImage(image_id);
    }

    // Fix 7-DOFs of the bundle adjustment problem.
    ba_config.SetConstantPose(reg_image_ids[0]);
    ba_config.SetConstantTvec(reg_image_ids[1], {0});

    // Run bundle adjustment.
    BundleAdjuster bundle_adjuster(ba_options_, ba_config);
    if (!bundle_adjuster.Solve(final_recon)) {
        return false;
    }

    // Normalize scene for numerical stability and
    // to avoid large scale changes in viewer.
    final_recon->Normalize();

    return true;
}

void FindSimilarityTransform(const std::vector<Eigen::Vector3d>& observations1,
                             const std::vector<Eigen::Vector3d>& observations2,
                             Eigen::Matrix3d& R,
                             Eigen::Vector3d& t,
                             double& scale,
                             double& msd)
{
    std::vector<Eigen::Vector3d> inliers1, inliers2;
    double threshold = 0.1; // TODO: (chenyu) make this parameter an user option
    double p = 0.99;

    if (observations1.size() > 5) {
        LOG(INFO) << "Finding Similarity by RANSAC";
        RansacSimilarity(observations1, observations2, inliers1, inliers2, R, t, scale, threshold, p);
        VLOG(2) << "inliers size: " << inliers1.size();
        // Re-compute similarity by inliers
        Eigen::MatrixXd x1 = Eigen::MatrixXd::Zero(3, inliers1.size()),
                        x2 = Eigen::MatrixXd::Zero(3, inliers2.size());
        for(int i = 0; i < inliers1.size(); i++) {
            x1.col(i) = inliers1[i];
            x2.col(i) = inliers2[i];
        }
        GraphSfM::FindRTS(x1, x2, &scale, &t, &R);
        // Optional non-linear refinement of the found parameters
        GraphSfM::Refine_RTS(x1, x2, &scale, &t, &R);

        if (inliers1.size() < 4) { msd = numeric_limits<double>::max(); return; }
        // else msd = CheckReprojError(inliers1, inliers2, scale, R, t);
    }

    if (observations1.size() <= 5 || inliers1.size() <= 5) {
        Eigen::MatrixXd x1 = Eigen::MatrixXd::Zero(3, observations1.size()),
                        x2 = Eigen::MatrixXd::Zero(3, observations2.size());
        for (int i = 0; i < observations1.size(); i++) {
            x1.col(i) = observations1[i];
            x2.col(i) = observations2[i];
        }
        GraphSfM::FindRTS(x1, x2, &scale, &t, &R);
        GraphSfM::Refine_RTS(x1, x2, &scale, &t, &R);

        // msd = CheckReprojError(observations1, observations2, scale, R, t);
    }

    msd = CheckReprojError(observations1, observations2, scale, R, t);

    // LOG(INFO) << "scale: " << scale;
    // LOG(INFO) << "rotation: ";
    // LOG(INFO) << R(0, 0) << " " << R(0, 1) << " " << R(0, 2);
    // LOG(INFO) << R(1, 0) << " " << R(1, 1) << " " << R(1, 2);
    // LOG(INFO) << R(2, 0) << " " << R(2, 1) << " " << R(2, 2);
    // LOG(INFO) << "translation: " << t[0] << ", " << t[1] << ", " << t[2];
}

double CheckReprojError(const vector<Eigen::Vector3d>& src_observations,
                        const vector<Eigen::Vector3d>& dst_observations,
                        const double& scale,
                        const Eigen::Matrix3d& R,
                        const Eigen::Vector3d& t)
{
    double reproj_err = 0.0;
    const int size = src_observations.size();
    for (int i = 0; i < size; i++) {
        Eigen::Vector3d reproj_obv = scale * R * src_observations[i] + t;
        reproj_err += (reproj_obv - dst_observations[i]).norm();
    }

    LOG(INFO) << "Mean Reprojection Error: " << reproj_err / size
              << " (" << reproj_err << "/"
              << size << ")";
    return reproj_err / size;
}

double CheckAngularResidual(const std::vector<Eigen::Matrix3d>& src_rotations,
                           const std::vector<Eigen::Matrix3d>& dst_rotations,
                           const Eigen::Matrix3d& R)
{
    double angular_residual = 0.0;
    const int n = src_rotations.size();
    for (uint i = 0; i < n; i++) {
        Eigen::Matrix3d rel_rotation =
            dst_rotations[i].transpose() * src_rotations[i] * R.transpose();
        Eigen::Vector3d angle_axis;
        ceres::RotationMatrixToAngleAxis(rel_rotation.data(), angle_axis.data());
        angular_residual += RadToDeg(angle_axis.norm());
    }

    angular_residual /= n;
    LOG(INFO) << "Average Angular Residual: " << angular_residual << " degree.";
    return angular_residual;
}

bool ComputeSimilarityByCameraMotions(
    std::vector<Eigen::Vector3d>& camera_centers1,
    std::vector<Eigen::Vector3d>& camera_centers2,
    std::vector<Eigen::Matrix3d>& camera_rotations1,
    std::vector<Eigen::Matrix3d>& camera_rotations2,
    Eigen::Matrix3d& relative_r,
    Eigen::Vector3d& relative_t,
    double& scale)
{
    // my hybrid approach by combining "Divide and Conquer: Efficient Large-Scale
    // Structure from Motion Using Graph Partitioning" and RANSAC

    const int n = camera_centers1.size();
    std::vector<Eigen::Vector3d> ts1(n);
    std::vector<Eigen::Vector3d> ts2(n);

    for (uint i = 0; i < n; i++) {
        ts1[i] = -camera_rotations1[i] * camera_centers1[i];
        ts2[i] = -camera_rotations2[i] * camera_centers2[i];
    }

    // compute relative scale from a->b
    std::vector<double> scales;
    for (int i = 0; i < n; i++) {
        Eigen::Vector3d center_a1 = camera_centers1[i];
        Eigen::Vector3d center_b1 = camera_centers2[i];
        for (int j = i + 1; j < n; j++) {
            Eigen::Vector3d center_a2 = camera_centers1[j];
            Eigen::Vector3d center_b2 = camera_centers2[j];
            double scale_ab = (center_b1 - center_b2).norm() /
                             (center_a1 - center_a2).norm();
            scales.push_back(scale_ab);
        }
    }
    // retrieve the median of scales, according to
    // the equation (5) of the paper "Divide and Conquer: Efficient Large-Scale
    // Structure from Motion Using Graph Partitioning"
    std::sort(scales.begin(), scales.end());
    scale = scales[scales.size() / 2];

    // compute relative rotation & relative translation from a->b
    std::vector<Correspondence3D> corres3d;
    std::vector<CorrespondenceEuc> input_datas;
    for (int i = 0; i < camera_centers1.size() ;i++) {
        corres3d.emplace_back(camera_centers1[i], camera_centers2[i]);
        input_datas.push_back(make_pair(Euclidean3D(camera_rotations1[i], ts1[i]),
                                        Euclidean3D(camera_rotations2[i], ts2[i])));
    }
    EuclideanEstimator euc_estimator(scale, corres3d);

    Euclidean3D euc3d;
    RansacParameters params;
    params.rng = std::make_shared<RandomNumberGenerator>((unsigned int)time(NULL));
    params.error_thresh = 0.002;
    params.max_iterations = 1000;

    Prosac<EuclideanEstimator> prosac_euc3(params, euc_estimator);
    prosac_euc3.Initialize();
    RansacSummary summary;
    prosac_euc3.Estimate(input_datas, &euc3d, &summary);

    relative_r = euc3d.R;
    relative_t = euc3d.t;

    // LOG(INFO) << "scale: " << scale;
    // LOG(INFO) << "rotation: \n";
    // LOG(INFO) << relative_r(0, 0) << " " << relative_r(0, 1) << " " << relative_r(0, 2);
    // LOG(INFO) << relative_r(1, 0) << " " << relative_r(1, 1) << " " << relative_r(1, 2);
    // LOG(INFO) << relative_r(2, 0) << " " << relative_r(2, 1) << " " << relative_r(2, 2);
    // LOG(INFO) << "translation: "
            //   << relative_t[0] << ", " << relative_t[1] << ", " << relative_t[2];

    return true;
}

} // namespace GraphSfM


// Boost.Geometry

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

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_THOMAS_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_THOMAS_HPP

#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/strategies/distance.hpp>

#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/algorithms/detail/thomas_inverse.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

namespace strategy { namespace distance
{

/*!
\brief The solution of the inverse problem of geodesics on latlong coordinates,
       Forsyth-Andoyer-Lambert type approximation with second order terms.
\ingroup distance
\tparam Spheroid The reference spheroid model
\tparam CalculationType \tparam_calculation
\author See
    - Technical Report: PAUL D. THOMAS, MATHEMATICAL MODELS FOR NAVIGATION SYSTEMS, 1965
      http://www.dtic.mil/docs/citations/AD0627893
    - Technical Report: PAUL D. THOMAS, SPHEROIDAL GEODESICS, REFERENCE SYSTEMS, AND LOCAL GEOMETRY, 1970
      http://www.dtic.mil/docs/citations/AD703541
*/
template
<
    typename Spheroid,
    typename CalculationType = void
>
class thomas
{
public :
    template <typename Point1, typename Point2>
    struct calculation_type
        : promote_floating_point
          <
              typename select_calculation_type
                  <
                      Point1,
                      Point2,
                      CalculationType
                  >::type
          >
    {};

    typedef Spheroid model_type;

    inline thomas()
        : m_spheroid()
    {}

    explicit inline thomas(Spheroid const& spheroid)
        : m_spheroid(spheroid)
    {}

    template <typename Point1, typename Point2>
    inline typename calculation_type<Point1, Point2>::type
    apply(Point1 const& point1, Point2 const& point2) const
    {
        return geometry::detail::thomas_inverse
                <
                    typename calculation_type<Point1, Point2>::type,
                    true, false
                >::apply(get_as_radian<0>(point1),
                         get_as_radian<1>(point1),
                         get_as_radian<0>(point2),
                         get_as_radian<1>(point2),
                         m_spheroid).distance;
    }

    inline Spheroid const& model() const
    {
        return m_spheroid;
    }

private :
    Spheroid m_spheroid;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename Spheroid, typename CalculationType>
struct tag<thomas<Spheroid, CalculationType> >
{
    typedef strategy_tag_distance_point_point type;
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct return_type<thomas<Spheroid, CalculationType>, P1, P2>
    : thomas<Spheroid, CalculationType>::template calculation_type<P1, P2>
{};

template <typename Spheroid, typename CalculationType>
struct comparable_type<thomas<Spheroid, CalculationType> >
{
    typedef thomas<Spheroid, CalculationType> type;
};

template <typename Spheroid, typename CalculationType>
struct get_comparable<thomas<Spheroid, CalculationType> >
{
    static inline thomas<Spheroid, CalculationType> apply(thomas<Spheroid, CalculationType> const& input)
    {
        return input;
    }
};

template <typename Spheroid, typename CalculationType, typename P1, typename P2>
struct result_from_distance<thomas<Spheroid, CalculationType>, P1, P2 >
{
    template <typename T>
    static inline typename return_type<thomas<Spheroid, CalculationType>, P1, P2>::type
        apply(thomas<Spheroid, CalculationType> const& , T const& value)
    {
        return value;
    }
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_THOMAS_HPP


//Source: The LLVM Compiler Infrastructure - lib/addsf3.c
//Modified to truncate result of addition

#include <limits.h>
#include <boost/cstdint.hpp>
#include "FpAddTruncate.h"
#include "BitManip.h"

typedef uint32 rep_t;
typedef int32 srep_t;
typedef float fp_t;
#define REP_C UINT32_C
#define significandBits 23

#define typeWidth       (sizeof(rep_t)*CHAR_BIT)
#define exponentBits    (typeWidth - significandBits - 1)
#define maxExponent     ((1 << exponentBits) - 1)
#define exponentBias    (maxExponent >> 1)

#define implicitBit     (REP_C(1) << significandBits)
#define significandMask (implicitBit - 1U)
#define signBit         (REP_C(1) << (significandBits + exponentBits))
#define absMask         (signBit - 1U)
#define exponentMask    (absMask ^ significandMask)
#define oneRep          ((rep_t)exponentBias << significandBits)
#define infRep          exponentMask
#define quietBit        (implicitBit >> 1)
#define qnanRep         (exponentMask | quietBit)

static inline int rep_clz(rep_t a) {
	return __builtin_clz(a);
}

uint32 FpAddTruncate(uint32 a, uint32 b)
{
    const rep_t aAbs = a & absMask;
    const rep_t bAbs = b & absMask;

    // Detect if a or b is zero, infinity, or NaN.
    if (aAbs - 1U >= infRep - 1U || bAbs - 1U >= infRep - 1U) {

        // NaN + anything = qNaN
        if (aAbs > infRep) return (a | quietBit);
        // anything + NaN = qNaN
        if (bAbs > infRep) return (b | quietBit);

        if (aAbs == infRep) {
            // +/-infinity + -/+infinity = qNaN
            if ((a ^ b) == signBit) return qnanRep;
            // +/-infinity + anything remaining = +/- infinity
            else return a;
        }

        // anything remaining + +/-infinity = +/-infinity
        if (bAbs == infRep) return b;

        // zero + anything = anything
        if (!aAbs) {
            // but we need to get the sign right for zero + zero
            if (!bAbs) return (a & b);
            else return b;
        }

        // anything + zero = anything
        if (!bAbs) return a;
    }

    // Swap a and b if necessary so that a has the larger absolute value.
    if (bAbs > aAbs) {
        const uint32 temp = a;
        a = b;
        b = temp;
    }

    // Extract the exponent and significand from the (possibly swapped) a and b.
    int aExponent = a >> significandBits & maxExponent;
    int bExponent = b >> significandBits & maxExponent;
    rep_t aSignificand = a & significandMask;
    rep_t bSignificand = b & significandMask;

    // Normalize any denormals, and adjust the exponent accordingly.
    //if (aExponent == 0) aExponent = normalize(&aSignificand);
    //if (bExponent == 0) bExponent = normalize(&bSignificand);

    // The sign of the result is the sign of the larger operand, a.  If they
    // have opposite signs, we are performing a subtraction; otherwise addition.
    const rep_t resultSign = a & signBit;
    const bool subtraction = (a ^ b) & signBit;

    // Shift the significands to give us round, guard and sticky, and or in the
    // implicit significand bit.  (If we fell through from the denormal path it
    // was already set by normalize( ), but setting it twice won't hurt
    // anything.)
    aSignificand = (aSignificand | implicitBit) << 3;
    bSignificand = (bSignificand | implicitBit) << 3;

    // Shift the significand of b by the difference in exponents, with a sticky
    // bottom bit to get rounding correct.
    const unsigned int align = aExponent - bExponent;
    if (align) {
        if (align < typeWidth) {
            //const bool sticky = bSignificand << (typeWidth - align);
            bSignificand = bSignificand >> align;
        } else {
            bSignificand = 0; // sticky; b is known to be non-zero.
        }
    }

    if (subtraction) {
        aSignificand -= bSignificand;

        // If a == -b, return +zero.
        if (aSignificand == 0) return 0;

        // If partial cancellation occured, we need to left-shift the result
        // and adjust the exponent:
        if (aSignificand < implicitBit << 3) {
            const int shift = rep_clz(aSignificand) - rep_clz(implicitBit << 3);
            aSignificand <<= shift;
            aExponent -= shift;
        }
    }

    else /* addition */ {
        aSignificand += bSignificand;

        // If the addition carried up, we need to right-shift the result and
        // adjust the exponent:
        if (aSignificand & implicitBit << 4) {
            const bool sticky = aSignificand & 1;
            aSignificand = aSignificand >> 1 | sticky;
            aExponent += 1;
        }
    }

    // If we have overflowed the type, return +/- infinity:
    if (aExponent >= maxExponent) return infRep | resultSign;

    if (aExponent <= 0) {
        // Result is denormal before rounding; the exponent is zero and we
        // need to shift the significand.
        const int shift = 1 - aExponent;
        const bool sticky = aSignificand << (typeWidth - shift);
        aSignificand = aSignificand >> shift | sticky;
        aExponent = 0;
    }

    // Low three bits are round, guard, and sticky.
    const int roundGuardSticky = aSignificand & 0x7;

    // Shift the significand into place, and mask off the implicit bit.
    rep_t result = aSignificand >> 3 & significandMask;

    // Insert the exponent and sign.
    result |= (rep_t)aExponent << significandBits;
    result |= resultSign;

    return result;
}


#include "timetable/departure_table.hpp"

#include <algorithm>

#include <boost/assert.hpp>

namespace nepomuk
{
namespace timetable
{

bool Departure::operator<(Departure const &other) const { return end < other.end; }

date::Time Departure::get_next_departure(date::Time const starting_at) const
{
    // the next departue for a given starting time is defined by T = begin + x * headway with T >=
    // starting_at and x minimal among all x that fulfill T >= starting_at

    BOOST_ASSERT(starting_at <= end);

    // in case the trip is only serviced after the starting time, the very first service can be
    // reached. After this check starting_at is within the range [begin,end)
    if (starting_at <= begin)
        return begin;

    // compute the xth train that can be reached (ceil(delta_t/headway))
    const auto x = (starting_at - begin + headway - 1) / headway;

    return begin + x * headway;
}

DepartureTable::const_iterator_range DepartureTable::list(date::Time starting_at) const
{
    return {std::lower_bound(departures.begin(),
                             departures.end(),
                             starting_at,
                             [](auto const &departure, auto const &time) {
                                 return departure.end < time + (departure.headway);
                             }),
            departures.end()};
}

} // namespace timetable
} // namespace nepomuk


/*  This file is part of libDAI - http://www.libdai.org/
 *
 *  Copyright (c) 2006-2011, The libDAI authors. All rights reserved.
 *
 *  Use of this source code is governed by a BSD-style license that can be found in the LICENSE file.
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <iterator>
#include <algorithm>
#include <boost/program_options.hpp>
#include <dai/factorgraph.h>
#include <dai/weightedgraph.h>
#include <dai/util.h>
#include <dai/graph.h>
#include <dai/enum.h>
#include <dai/properties.h>

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

/// Possible factor types
DAI_ENUM(FactorType,ISINGGAUSS,ISINGUNIFORM,EXPGAUSS,POTTS);

/// Creates a factor graph from a pairwise interactions graph
/** \param G  Graph describing interactions between variables
 *  \param ft Type of factors to use for interactions
 *  \param states Number of states of the variables
 *  \param props Additional properties for generating the interactions
 */
FactorGraph createFG( const GraphAL &G, FactorType ft, size_t states, const PropertySet &props ) {
    size_t N = G.nrNodes();

    DAI_ASSERT(states <= 2 || ft != FactorType::ISINGGAUSS && ft != FactorType::ISINGUNIFORM );

    // Get inverse temperature
    Real beta = 1.0;
    if( ft != FactorType::ISINGGAUSS && ft != FactorType::ISINGUNIFORM )
        beta = props.getAs<Real>("beta");

    // Get properties for Ising factors
    Real mean_h = 0.0;
    Real sigma_h = 0.0;
    Real mean_J = 0.0;
    Real sigma_J = 0.0;
    if( ft == FactorType::ISINGGAUSS ) {
        mean_h = props.getAs<Real>("mean_th");
        sigma_h = props.getAs<Real>("sigma_th");
        mean_J = props.getAs<Real>("mean_w");
        sigma_J = props.getAs<Real>("sigma_w");
    }
    Real min_h = 0.0;
    Real min_J = 0.0;
    Real max_h = 0.0;
    Real max_J = 0.0;
    if( ft == FactorType::ISINGUNIFORM ) {
        min_h = props.getAs<Real>("min_th");
        min_J = props.getAs<Real>("min_w");
        max_h = props.getAs<Real>("max_th");
        max_J = props.getAs<Real>("max_w");
    }

    // Create variables
    vector<Var> vars;
    vars.reserve( N );
    for( size_t i = 0; i < N; i++ )
        vars.push_back( Var( i, states ) );

    // Create factors
    vector<Factor> factors;
    factors.reserve( G.nrEdges() + N );
    // Pairwise factors
    for( size_t i = 0; i < N; i++ )
        for( const Neighbor &j : G.nb(i) )
            if( i < j ) {
                if( ft == FactorType::POTTS )
                    factors.push_back( createFactorPotts( vars[i], vars[j], beta ) );
                else if( ft == FactorType::EXPGAUSS )
                    factors.push_back( createFactorExpGauss( VarSet( vars[i], vars[j] ), beta ) );
                else if( ft == FactorType::ISINGGAUSS ) {
                    Real J = rnd_stdnormal() * sigma_J + mean_J;
                    factors.push_back( createFactorIsing( vars[i], vars[j], J ) );
                } else if( ft == FactorType::ISINGUNIFORM ) {
                    Real J = min_J + rnd_uniform() * (max_J - min_J);
                    factors.push_back( createFactorIsing( vars[i], vars[j], J ) );
                }
            }
    // Unary factors
    if( ft == FactorType::ISINGGAUSS )
        for( size_t i = 0; i < N; i++ ) {
            Real h = rnd_stdnormal() * sigma_h + mean_h;
            factors.push_back( createFactorIsing( vars[i], h ) );
        }
    else if( ft == FactorType::ISINGUNIFORM )
        for( size_t i = 0; i < N; i++ ) {
            Real h = min_h + rnd_uniform() * (max_h - min_h);
            factors.push_back( createFactorIsing( vars[i], h ) );
        }

    return FactorGraph( factors.begin(), factors.end(), vars.begin(), vars.end(), factors.size(), vars.size() );
}

/// Return a random factor graph with higher-order interactions
/** \param N number of variables
 *  \param M number of factors
 *  \param k number of variables that each factor depends on
 *  \param beta standard-deviation of Gaussian log-factor entries
 */
FactorGraph createHOIFG( size_t N, size_t M, size_t k, Real beta ) {
    vector<Var> vars;
    vector<Factor> factors;

    vars.reserve(N);
    for( size_t i = 0; i < N; i++ )
        vars.push_back(Var(i,2));

    for( size_t I = 0; I < M; I++ ) {
        VarSet vars;
        while( vars.size() < k ) {
            do {
                size_t newind = (size_t)(N * rnd_uniform());
                Var newvar = Var(newind, 2);
                if( !vars.contains( newvar ) ) {
                    vars |= newvar;
                    break;
                }
            } while( 1 );
        }
        factors.push_back( createFactorExpGauss( vars, beta ) );
    }

    return FactorGraph( factors.begin(), factors.end(), vars.begin(), vars.end(), factors.size(), vars.size() );
}

/// Creates a regular random bipartite graph
/** \param N1 = number of nodes of type 1
 *  \param d1 = size of neighborhoods of nodes of type 1
 *  \param N2 = number of nodes of type 2
 *  \param d2 = size of neighborhoods of nodes of type 2
 *  \note asserts that N1 * d1 == N2 * d2
 */
BipartiteGraph createRandomBipartiteGraph( size_t N1, size_t N2, size_t d1, size_t d2 ) {
    BipartiteGraph G;

    DAI_ASSERT( N1 * d1 == N2 * d2 );

    // build lists of degree-repeated vertex numbers
    std::vector<size_t> stubs1( N1*d1, 0 );
    for( size_t n1 = 0; n1 < N1; n1++ )
        for( size_t t = 0; t < d1; t++ )
            stubs1[n1*d1 + t] = n1;

    // build lists of degree-repeated vertex numbers
    std::vector<size_t> stubs2( N2*d2, 0 );
    for( size_t n2 = 0; n2 < N2; n2++ )
        for( size_t t = 0; t < d2; t++ )
            stubs2[n2*d2 + t] = n2;

    // shuffle lists
    random_shuffle( stubs1.begin(), stubs1.end(), rnd );
    random_shuffle( stubs2.begin(), stubs2.end(), rnd );

    // add edges
    vector<Edge> edges;
    edges.reserve( N1*d1 );
    for( size_t e = 0; e < N1*d1; e++ )
        edges.push_back( Edge(stubs1[e], stubs2[e]) );

    // finish construction
    G.construct( N1, N2, edges.begin(), edges.end() );

    return G;
}

/// Returns x**n % p, assuming p is prime
int powmod (int x, int n, int p) {
    int y = 1;
    for( int m = 0; m < n; m++ )
        y = (x * y) % p;
    return y;
}

/// Returns order of x in GF(p) with p prime
size_t order (int x, int p) {
    x = x % p;
    DAI_ASSERT( x != 0 );
    size_t n = 0;
    size_t prod = 1;
    do {
        prod = (prod * x) % p;
        n++;
    } while( prod != 1 );
    return n;
}

/// Returns whether n is a prime number
bool isPrime (size_t n) {
    bool result = true;
    for( size_t k = 2; (k < n) && result; k++ )
        if( n % k == 0 )
            result = false;
    return result;
}

/// Constructs a regular LDPC graph with N=6, j=2, K=4, k=3
BipartiteGraph createSmallLDPCGraph() {
    BipartiteGraph G;
    size_t N=4, j=3, K=4; // k=3;

    vector<Edge> edges;
    edges.reserve( N*j );
    edges.push_back( Edge(0,0) ); edges.push_back( Edge(1,0) ); edges.push_back( Edge(2,0) );
    edges.push_back( Edge(0,1) ); edges.push_back( Edge(1,1) ); edges.push_back( Edge(3,1) );
    edges.push_back( Edge(0,2) ); edges.push_back( Edge(2,2) ); edges.push_back( Edge(3,2) );
    edges.push_back( Edge(1,3) ); edges.push_back( Edge(2,3) ); edges.push_back( Edge(3,3) );

    // finish construction
    G.construct( N, K, edges.begin(), edges.end() );

    return G;
}

/// Creates group-structured LDPC code
/** Use construction described in "A Class of Group-Structured LDPC Codes"
 *  by R. M. Tanner, D. Sridhara and T. Fuja
 *  Proceedings of ICSTA, 2001
 *
 *  Example parameters: (p,j,k) = (31,3,5)
 *                      (p,j,k) = (37,3,4)
 *                      (p,j,k) = (7,2,4)
 *                      (p,j,k) = (29,2,4)
 *
 *  j and k must be divisors of p-1
 */
BipartiteGraph createGroupStructuredLDPCGraph( size_t p, size_t j, size_t k ) {
    BipartiteGraph G;

    size_t n = j;
    size_t N = p * k;
    size_t K = p * j;

    size_t a, b;
    for( a = 2; a < p; a++ )
        if( order(a,p) == k )
            break;
    DAI_ASSERT( a != p );
    for( b = 2; b < p; b++ )
        if( order(b,p) == j )
            break;
    DAI_ASSERT( b != p );
    // cout << "# order(a=" << a << ") = " << order(a,p) << endl;
    // cout << "# order(b=" << b << ") = " << order(b,p) << endl;

    DAI_ASSERT( N * n == K * k );

    vector<Edge> edges;
    edges.reserve( N * n );

    for( size_t s = 0; s < j; s++ )
        for( size_t t = 0; t < k; t++ ) {
            size_t P = (powmod(b,s,p) * powmod(a,t,p)) % p;
            for( size_t m = 0; m < p; m++ )
                edges.push_back( Edge(t*p + m, s*p + ((m + P) % p)) );
        }

    // finish construction
    G.construct( N, K, edges.begin(), edges.end() );

    return G;
}

// Constructs a parity check table
void createParityCheck( Real *result, size_t n, Real eps ) {
    size_t N = 1 << n;
    for( size_t i = 0; i < N; i++ ) {
        size_t c = 0;
        for( size_t t = 0; t < n; t++ )
            if( i & (1 << t) )
                c ^= 1;
        if( c )
            result[i] = eps;
        else
            result[i] = 1.0 - eps;
    }
    return;
}

/// Predefined names of various factor graph types
const char *FULL_TYPE        = "FULL";
const char *DREG_TYPE        = "DREG";
const char *LOOP_TYPE        = "LOOP";
const char *TREE_TYPE        = "TREE";
const char *GRID_TYPE        = "GRID";
const char *GRID3D_TYPE      = "GRID3D";
const char *HOI_TYPE         = "HOI";
const char *LDPC_TYPE        = "LDPC";

/// Possible LDPC structures
DAI_ENUM(LDPCType,SMALL,GROUP,RANDOM);

/// Main function
int main( int argc, char *argv[] ) {
    try {
        // Variables for storing command line arguments
        size_t seed;
        size_t states = 2;
        string type;
        size_t d, N, K, k, j, n1, n2, n3, prime;
        bool periodic = false;
        FactorType ft;
        LDPCType ldpc;
        Real beta, sigma_w, sigma_th, mean_w, mean_th, min_w, min_th, max_w, max_th, noise;

        // Declare the supported options.
        po::options_description opts("General command line options");
        opts.add_options()
            ("help",     "produce help message")
            ("seed",     po::value<size_t>(&seed),   "random number seed (tries to read from /dev/urandom if not specified)")
            ("states",   po::value<size_t>(&states), "number of states of each variable (default=2 for binary variables)")
        ;

        // Graph structure options
        po::options_description opts_graph("Options for specifying graph structure");
        opts_graph.add_options()
            ("type",     po::value<string>(&type),   "factor graph type (one of 'FULL', 'DREG', 'LOOP', 'TREE', 'GRID', 'GRID3D', 'HOI', 'LDPC')")
            ("d",        po::value<size_t>(&d),      "variable connectivity (only for type=='DREG');\n\t<d><N> should be even")
            ("N",        po::value<size_t>(&N),      "number of variables (not for type=='GRID','GRID3D')")
            ("n1",       po::value<size_t>(&n1),     "width of grid (only for type=='GRID','GRID3D')")
            ("n2",       po::value<size_t>(&n2),     "height of grid (only for type=='GRID','GRID3D')")
            ("n3",       po::value<size_t>(&n3),     "length of grid (only for type=='GRID3D')")
            ("periodic", po::value<bool>(&periodic), "periodic grid? (only for type=='GRID','GRID3D'; default=0)")
            ("K",        po::value<size_t>(&K),      "number of factors (only for type=='HOI','LDPC')")
            ("k",        po::value<size_t>(&k),      "number of variables per factor (only for type=='HOI','LDPC')")
        ;

        // Factor options
        po::options_description opts_factors("Options for specifying factors");
        opts_factors.add_options()
            ("factors",  po::value<FactorType>(&ft), "factor type (one of 'EXPGAUSS','POTTS','ISINGGAUSS','ISINGUNIFORM')")
            ("beta",     po::value<Real>(&beta),     "inverse temperature (ignored for factors=='ISINGGAUSS','ISINGUNIFORM')")
            ("mean_w",   po::value<Real>(&mean_w),   "mean of pairwise interactions w_{ij} (only for factors=='ISINGGAUSS')")
            ("mean_th",  po::value<Real>(&mean_th),  "mean of unary interactions th_i (only for factors=='ISINGGAUSS')")
            ("sigma_w",  po::value<Real>(&sigma_w),  "stddev of pairwise interactions w_{ij} (only for factors=='ISINGGAUSS')")
            ("sigma_th", po::value<Real>(&sigma_th), "stddev of unary interactions th_i (only for factors=='ISINGGAUSS'")
            ("min_w",    po::value<Real>(&min_w),    "minimum of pairwise interactions w_{ij} (only for factors=='ISINGUNIFORM')")
            ("min_th",   po::value<Real>(&min_th),   "minimum of unary interactions th_i (only for factors=='ISINGUNIFORM')")
            ("max_w",    po::value<Real>(&max_w),    "maximum of pairwise interactions w_{ij} (only for factors=='ISINGUNIFORM')")
            ("max_th",   po::value<Real>(&max_th),   "maximum of unary interactions th_i (only for factors=='ISINGUNIFORM')")
        ;

        // LDPC options
        po::options_description opts_ldpc("Options for specifying LDPC code factor graphs");
        opts_ldpc.add_options()
            ("ldpc",     po::value<LDPCType>(&ldpc), "type of LDPC code (one of 'SMALL','GROUP','RANDOM')")
            ("j",        po::value<size_t>(&j),      "number of parity checks per bit (only for type=='LDPC')")
            ("noise",    po::value<Real>(&noise),    "bitflip probability for binary symmetric channel (only for type=='LDPC')")
            ("prime",    po::value<size_t>(&prime),  "prime number for construction of LDPC code (only for type=='LDPC' with ldpc='GROUP'))")
        ;

        // All options
        opts.add(opts_graph).add(opts_factors).add(opts_ldpc);

        // Parse command line arguments
        po::variables_map vm;
        po::store(po::parse_command_line(argc, argv, opts), vm);
        po::notify(vm);

        // Display help message if necessary
        if( vm.count("help") || !vm.count("type") ) {
            cout << "This program is part of libDAI - http://www.libdai.org/" << endl << endl;
            cout << "Usage: ./createfg [options]" << endl << endl;
            cout << "Creates a factor graph according to the specified options." << endl << endl;

            cout << endl << opts << endl;

            cout << "The following factor graph types with pairwise interactions can be created:" << endl;
            cout << "\t'FULL':   fully connected graph of <N> variables" << endl;
            cout << "\t'DREG':   random regular graph of <N> variables where each variable is connected with <d> others" << endl;
            cout << "\t'LOOP':   a single loop of <N> variables" << endl;
            cout << "\t'TREE':   random tree-structured (acyclic, connected) graph of <N> variables" << endl;
            cout << "\t'GRID':   2D grid of <n1>x<n2> variables" << endl;
            cout << "\t'GRID3D': 3D grid of <n1>x<n2>x<n3> variables" << endl;
            cout << "The following higher-order interactions factor graphs can be created:" << endl;
            cout << "\t'HOI':    random factor graph consisting of <N> variables and <K> factors," << endl;
            cout << "\t          each factor being an interaction of <k> variables." << endl;
            cout << "The following LDPC code factor graphs can be created:" << endl;
            cout << "\t'LDPC':   simulates LDPC decoding problem, using an LDPC code of <N> bits and <K>" << endl;
            cout << "\t          parity checks, with <k> bits per check and <j> checks per bit, transmitted" << endl;
            cout << "\t          on a binary symmetric channel with probability <noise> of flipping a bit." << endl;
            cout << "\t          The transmitted codeword has all bits set to zero. The argument 'ldpc'" << endl;
            cout << "\t          determines how the LDPC code is constructed: either using a group structure," << endl;
            cout << "\t          or randomly, or a fixed small code with (N,K,k,j) = (4,4,3,3)." << endl << endl;

            cout << "For all types except type=='LDPC', the factors have to be specified as well." << endl << endl;

            cout << "EXPGAUSS factors (the default) are created by drawing all log-factor entries" << endl;
            cout << "independently from a Gaussian with mean 0 and standard deviation <beta>." << endl << endl;

            cout << "In case of pairwise interactions, one can also choose POTTS factors, for which" << endl;
            cout << "the log-factors are simply delta functions multiplied by the strength <beta>." << endl << endl;

            cout << "For pairwise interactions and binary variables, one can also use ISINGGAUSS factors." << endl;
            cout << "Here variables x1...xN are assumed to be +1/-1--valued, and unary interactions" << endl;
            cout << "are of the form exp(th*xi) with th drawn from a Gaussian distribution with mean" << endl;
            cout << "<mean_th> and standard deviation <sigma_th>, and pairwise interactions are of the" << endl;
            cout << "form exp(w*xi*xj) with w drawn from a Gaussian distribution with mean <mean_w>" << endl;
            cout << "and standard deviation <sigma_w>." << endl;
            cout << "Alternatively, one can use ISINGUNIFORM factors: here th is drawn from a uniform" << endl;
            cout << "distribution on [<min_th>, <max_th>), and w is drawn from a uniform distribution" << endl;
            cout << "on [<min_w>, <max_w>)." << endl;
            return 1;
        }

        // Set default number of states
        if( !vm.count("states") )
            states = 2;

        // Set default factor type
        if( !vm.count("factors") )
            ft = FactorType::EXPGAUSS;
        // Check validness of factor type
        if( ft == FactorType::POTTS )
            if( type == HOI_TYPE )
                throw "For factors=='POTTS', interactions should be pairwise (type!='HOI')";
        if( ft == FactorType::ISINGGAUSS )
            if( ((states != 2) || (type == HOI_TYPE)) )
                throw "For factors=='ISINGGAUSS', variables should be binary (states==2) and interactions should be pairwise (type!='HOI')";
        if( ft == FactorType::ISINGUNIFORM )
            if( ((states != 2) || (type == HOI_TYPE)) )
                throw "For factors=='ISINGUNIFORM', variables should be binary (states==2) and interactions should be pairwise (type!='HOI')";

        // Read random seed
        if( !vm.count("seed") ) {
            ifstream infile;
            bool success;
            infile.open( "/dev/urandom" );
            success = infile.is_open();
            if( success ) {
                infile.read( (char *)&seed, sizeof(size_t) / sizeof(char) );
                success = infile.good();
                infile.close();
            }
            if( !success )
                throw "Please specify random number seed.";
        }
        rnd_seed( seed );

        // Set default periodicity
        if( !vm.count("periodic") )
            periodic = false;

        // Store some options in a PropertySet object
        PropertySet options;
        if( vm.count("mean_th") )
            options.set("mean_th", mean_th);
        if( vm.count("sigma_th") )
            options.set("sigma_th", sigma_th);
        if( vm.count("mean_w") )
            options.set("mean_w", mean_w);
        if( vm.count("sigma_w") )
            options.set("sigma_w", sigma_w);
        if( vm.count("beta") )
            options.set("beta", beta);
        if( vm.count("min_w") )
            options.set("min_w", min_w);
        if( vm.count("min_th") )
            options.set("min_th", min_th);
        if( vm.count("max_w") )
            options.set("max_w", max_w);
        if( vm.count("max_th") )
            options.set("max_th", max_th);

        // Output some comments
        cout << "# Factor graph made by " << argv[0] << endl;
        cout << "# type = " << type << endl;
        cout << "# states = " << states << endl;
        cout << "# factor type = " << ft << endl;

        // The factor graph to be constructed
        FactorGraph fg;

#define NEED_ARG(name, desc) do { if(!vm.count(name)) throw "Please specify " desc " with --" name; } while(0);

        if( type == FULL_TYPE || type == DREG_TYPE || type == LOOP_TYPE || type == TREE_TYPE || type == GRID_TYPE || type == GRID3D_TYPE ) {
            // Pairwise interactions

            // Check command line options
            if( type == GRID_TYPE ) {
                NEED_ARG("n1", "width of grid");
                NEED_ARG("n2", "height of grid");
                N = n1 * n2;
            } else if( type == GRID3D_TYPE ) {
                NEED_ARG("n1", "width of grid");
                NEED_ARG("n2", "height of grid");
                NEED_ARG("n3", "depth of grid");
                N = n1 * n2 * n3;
            } else
                NEED_ARG("N", "number of variables");

            if( ft == FactorType::ISINGGAUSS ) {
                NEED_ARG("mean_w", "mean of pairwise interactions");
                NEED_ARG("mean_th", "mean of unary interactions");
                NEED_ARG("sigma_w", "stddev of pairwise interactions");
                NEED_ARG("sigma_th", "stddev of unary interactions");
            } else if( ft == FactorType::ISINGUNIFORM ) {
                NEED_ARG("min_w", "minimum of pairwise interactions");
                NEED_ARG("min_th", "minimum of unary interactions");
                NEED_ARG("max_w", "maximum of pairwise interactions");
                NEED_ARG("max_th", "maximum of unary interactions");
            } else
                NEED_ARG("beta", "stddev of log-factor entries");

            if( type == DREG_TYPE )
                NEED_ARG("d", "connectivity (number of neighboring variables of each variable)");

            // Build pairwise interaction graph
            GraphAL G;
            if( type == FULL_TYPE )
                G = createGraphFull( N );
            else if( type == DREG_TYPE )
                G = createGraphRegular( N, d );
            else if( type == LOOP_TYPE )
                G = createGraphLoop( N );
            else if( type == TREE_TYPE )
                G = createGraphTree( N );
            else if( type == GRID_TYPE )
                G = createGraphGrid( n1, n2, periodic );
            else if( type == GRID3D_TYPE )
                G = createGraphGrid3D( n1, n2, n3, periodic );

            // Construct factor graph from pairwise interaction graph
            fg = createFG( G, ft, states, options );

            // Output some additional comments
            if( type == GRID_TYPE || type == GRID3D_TYPE ) {
                cout << "# n1 = " << n1 << endl;
                cout << "# n2 = " << n2 << endl;
                if( type == GRID3D_TYPE )
                    cout << "# n3 = " << n3 << endl;
            }
            if( type == DREG_TYPE )
                cout << "# d = " << d << endl;
            cout << "# options = " << options << endl;
        } else if( type == HOI_TYPE ) {
            // Higher order interactions

            // Check command line arguments
            NEED_ARG("N", "number of variables");
            NEED_ARG("K", "number of factors");
            NEED_ARG("k", "number of variables per factor");
            NEED_ARG("beta", "stddev of log-factor entries");

            // Create higher-order interactions factor graph
            do {
                fg = createHOIFG( N, K, k, beta );
            } while( !fg.isConnected() );

            // Output some additional comments
            cout << "# K = " << K << endl;
            cout << "# k = " << k << endl;
            cout << "# beta = " << beta << endl;
        } else if( type == LDPC_TYPE ) {
            // LDPC codes

            // Check command line arguments
            NEED_ARG("ldpc", "type of LDPC code");
            NEED_ARG("noise", "bitflip probability for binary symmetric channel");

            // Check more command line arguments (seperately for each LDPC type)
            if( ldpc == LDPCType::RANDOM ) {
                NEED_ARG("N", "number of variables");
                NEED_ARG("K", "number of factors");
                NEED_ARG("k", "number of variables per factor");
                NEED_ARG("j", "number of parity checks per bit");
                if( N * j != K * k )
                    throw "Parameters should satisfy N * j == K * k";
            } else if( ldpc == LDPCType::GROUP ) {
                NEED_ARG("prime", "prime number");
                NEED_ARG("k", "number of variables per factor");
                NEED_ARG("j", "number of parity checks per bit");

                if( !isPrime(prime) )
                    throw "Parameter <prime> should be prime";
                if( !((prime-1) % j == 0 ) )
                    throw "Parameters should satisfy (prime-1) % j == 0";
                if( !((prime-1) % k == 0 ) )
                    throw "Parameters should satisfy (prime-1) % k == 0";

                N = prime * k;
                K = prime * j;
            } else if( ldpc == LDPCType::SMALL ) {
                N = 4;
                K = 4;
                j = 3;
                k = 3;
            }

            // Output some additional comments
            cout << "# N = " << N << endl;
            cout << "# K = " << K << endl;
            cout << "# j = " << j << endl;
            cout << "# k = " << k << endl;
            if( ldpc == LDPCType::GROUP )
                cout << "# prime = " << prime << endl;
            cout << "# noise = " << noise << endl;

            // Construct likelihood and paritycheck factors
            Real likelihood[4] = {1.f - noise, noise, noise, 1.f - noise};
            Real *paritycheck = new Real[1 << k];
            createParityCheck(paritycheck, k, 0.0);

            // Create LDPC structure
            BipartiteGraph ldpcG;
            bool regular;
            do {
                if( ldpc == LDPCType::GROUP )
                    ldpcG = createGroupStructuredLDPCGraph( prime, j, k );
                else if( ldpc == LDPCType::RANDOM )
                    ldpcG = createRandomBipartiteGraph( N, K, j, k );
                else if( ldpc == LDPCType::SMALL )
                    ldpcG = createSmallLDPCGraph();

                regular = true;
                for( size_t i = 0; i < N; i++ )
                    if( ldpcG.nb1(i).size() != j )
                        regular = false;
                for( size_t I = 0; I < K; I++ )
                    if( ldpcG.nb2(I).size() != k )
                        regular = false;
            } while( !regular && !ldpcG.isConnected() );

            // Convert to FactorGraph
            vector<Factor> factors;
            for( size_t I = 0; I < K; I++ ) {
                VarSet vs;
                for( size_t _i = 0; _i < k; _i++ ) {
                    size_t i = ldpcG.nb2(I)[_i];
                    vs |= Var( i, 2 );
                }
                factors.push_back( Factor( vs, paritycheck ) );
            }
            delete paritycheck;

            // Generate noise vector
            vector<char> noisebits(N,0);
            size_t bitflips = 0;
            for( size_t i = 0; i < N; i++ ) {
                if( rnd_uniform() < noise ) {
                    noisebits[i] = 1;
                    bitflips++;
                }
            }
            cout << "# bitflips = " << bitflips << endl;

            // Simulate transmission of all-zero codeword
            vector<char> input(N,0);
            vector<char> output(N,0);
            for( size_t i = 0; i < N; i++ )
                output[i] = (input[i] + noisebits[i]) & 1;

            // Add likelihoods
            for( size_t i = 0; i < N; i++ )
               factors.push_back( Factor(Var(i,2), likelihood + output[i]*2) );

            // Construct Factor Graph
            fg = FactorGraph( factors );
        } else
            throw "Invalid type";

        // Output additional comments
        cout << "# N = " << fg.nrVars() << endl;
        cout << "# seed = " << seed << endl;

        // Output factor graph
        cout << fg;
    } catch( const char *e ) {
        /// Display error message
        cerr << "Error: " << e << endl;
        return 1;
    }

    return 0;
}