/*
 * Copyright (c) 2017 Louis Langholtz https://github.com/louis-langholtz/Box2D
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

/**
 * @file
 * @brief Units file.
 */

#ifndef Units_hpp
#define Units_hpp

#include <Box2D/Common/RealNum.hpp>
#include <Box2D/Common/Templates.hpp>

// #define USE_BOOST_UNITS
#ifdef USE_BOOST_UNITS
#include <boost/units/io.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/time.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/acceleration.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/velocity.hpp>
#include <boost/units/systems/si/mass.hpp>
#include <boost/units/systems/si/momentum.hpp>
#include <boost/units/systems/si/inverse_mass.hpp>
#include <boost/units/systems/si/area.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/angular_momentum.hpp>
#include <boost/units/systems/si/angular_velocity.hpp>
#include <boost/units/systems/si/angular_acceleration.hpp>
#include <boost/units/systems/si/second_moment_of_area.hpp>
#include <boost/units/systems/si/surface_density.hpp>
#include <boost/units/systems/si/moment_of_inertia.hpp>
#include <boost/units/systems/si/inverse_moment_of_inertia.hpp>
#include <boost/units/systems/si/force.hpp>
#include <boost/units/systems/si/torque.hpp>
#include <boost/units/systems/angle/degrees.hpp>
#endif

namespace box2d
{
#ifdef USE_BOOST_UNITS

    using Time = boost::units::quantity<boost::units::si::time, Real>;
    constexpr auto Second = Time{boost::units::si::second * Real{1}};

    using Frequency = boost::units::quantity<boost::units::si::frequency, Real>;
    constexpr auto Hertz = Frequency{boost::units::si::hertz * Real{1}};

    using Length = boost::units::quantity<boost::units::si::length, Real>;
    constexpr auto Meter = Length{boost::units::si::meter * Real{1}};

    using LinearVelocity = boost::units::quantity<boost::units::si::velocity, Real>;
    constexpr auto MeterPerSecond = LinearVelocity{boost::units::si::meter_per_second * Real{1}};

    using LinearAcceleration = boost::units::quantity<boost::units::si::acceleration, Real>;
    constexpr auto MeterPerSquareSecond = LinearAcceleration{boost::units::si::meter_per_second_squared * Real{1}};

    using Mass = boost::units::quantity<boost::units::si::mass, Real>;
    constexpr auto Kilogram = Mass{boost::units::si::kilogram * Real{1}};

    using InvMass = boost::units::quantity<boost::units::si::inverse_mass, Real>;

    using Area = boost::units::quantity<boost::units::si::area, Real>;
    constexpr auto SquareMeter = Area{boost::units::si::square_meter * Real{1}};

    using Density = boost::units::quantity<boost::units::si::surface_density, Real>;
    constexpr auto KilogramPerSquareMeter = Density{boost::units::si::kilogram_per_square_meter * Real{1}};

    using Angle = boost::units::quantity<boost::units::si::plane_angle, Real>;
    constexpr auto Radian = Angle{boost::units::si::radian * Real{1}};
    constexpr auto Degree = Angle{boost::units::degree::degree * Real{1}};
    constexpr auto SquareRadian = Radian * Radian;

    using AngularVelocity = boost::units::quantity<boost::units::si::angular_velocity, Real>;
    constexpr auto RadianPerSecond = AngularVelocity{boost::units::si::radian_per_second * Real{1}};
    constexpr auto DegreePerSecond = AngularVelocity{RadianPerSecond * Degree / Radian};

    using AngularAcceleration = boost::units::quantity<boost::units::si::angular_acceleration, Real>;
    constexpr auto RadianPerSquareSecond = Radian / (Second * Second);

    using Force = boost::units::quantity<boost::units::si::force, Real>;
    constexpr auto Newton = Force{boost::units::si::newton * Real{1}};

    using Torque = boost::units::quantity<boost::units::si::torque, Real>;
    constexpr auto NewtonMeter = Torque{boost::units::si::newton_meter * Real{1}};

    using SecondMomentOfArea = boost::units::quantity<boost::units::si::second_moment_of_area, Real>;

    using RotInertia = boost::units::quantity<boost::units::si::moment_of_inertia, Real>;
    using InvRotInertia = boost::units::quantity<boost::units::si::inverse_moment_of_inertia, Real>;

    using Momentum = boost::units::quantity<boost::units::si::momentum, Real>;
    constexpr auto NewtonSecond = Newton * Second;

    /// @brief Angular momentum.
    /// @note Units of L^2 M T^-1 QP^-1.
    using AngularMomentum = boost::units::quantity<boost::units::si::angular_momentum, Real>;

#else // USE_BOOST_UNITS

    using Time = Real;
    constexpr auto Second = Real{1};

    using Frequency = Real;
    constexpr auto Hertz = Real{1};

    using Length = Real;
    constexpr auto Meter = Real{1};

    using LinearVelocity = Real;
    constexpr auto MeterPerSecond = Real{1};

    using LinearAcceleration = Real;
    constexpr auto MeterPerSquareSecond = Real{1};

    using Mass = Real;
    constexpr auto Kilogram = Real{1};

    using InvMass = Real;

    using Area = Real;
    constexpr auto SquareMeter = Real{1};

    using Density = Real;
    constexpr auto KilogramPerSquareMeter = Real{1};

    using Angle = Real;
    constexpr auto Radian = Real{1};
    constexpr auto Degree = Pi / Real{180};
    constexpr auto SquareRadian = Radian * Radian;

    using AngularVelocity = Real;
    constexpr auto RadianPerSecond = Real{1};
    constexpr auto DegreePerSecond = Degree;

    using AngularAcceleration = Real;
    constexpr auto RadianPerSquareSecond = Real{1};

    using Force = Real;
    constexpr auto Newton = Real{1};

    using Torque = Real;
    constexpr auto NewtonMeter = Real{1};

    using SecondMomentOfArea = Real;

    using RotInertia = Real;
    using InvRotInertia = Real;

    using Momentum = Real;
    constexpr auto NewtonSecond = Real{1};

    using AngularMomentum = Real;

#endif // USE_BOOST_UNITS

    constexpr inline Real StripUnit(const Real value)
    {
        return value;
    }

#ifdef USE_BOOST_UNITS

    constexpr inline Real StripUnit(const Angle value)
    {
        return Real{value / Radian};
    }

    constexpr inline Real StripUnit(const Length value)
    {
        return Real{value / Meter};
    }

    constexpr inline Real StripUnit(const Area value)
    {
        return Real{value / SquareMeter};
    }

    constexpr inline Real StripUnit(const Mass value)
    {
        // InvMass has units of M^-1
        return Real{value / Kilogram};
    }

    constexpr inline Real StripUnit(const InvMass value)
    {
        // InvMass has units of M^-1
        return Real{value * Kilogram};
    }

    constexpr inline Real StripUnit(const RotInertia value)
    {
        return Real{value * SquareRadian / (SquareMeter * Kilogram)};
    }

    constexpr inline Real StripUnit(const InvRotInertia value)
    {
        // InvRotInertia has units of L^-2 M^-1 QP^2
        return Real{value * SquareMeter * Kilogram / SquareRadian};
    }

    constexpr inline Real StripUnit(const Momentum value)
    {
        // Momentum has units of M L T^-1
        return Real{value * Second / (Kilogram * Meter)};
    }

    constexpr inline Real StripUnit(const LinearVelocity value)
    {
        return Real{value / MeterPerSecond};
    }

    constexpr inline Real StripUnit(const AngularVelocity value)
    {
        return Real{value / RadianPerSecond};
    }

    constexpr inline Real StripUnit(const Density value)
    {
        return Real{value / KilogramPerSquareMeter};
    }

    constexpr inline Real StripUnit(const Force value)
    {
        // Force has units of Newtons - which are M L T^2
        return Real{value / Newton};
    }

    constexpr inline Real StripUnit(const Torque value)
    {
        return Real{value / NewtonMeter};
    }

    template <>
    constexpr Angle GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Radian;
    }

    template <>
    constexpr inline bool IsValid(const Angle& x) noexcept
    {
        return IsValid(Real{x / Radian});
    }

    template <>
    constexpr Frequency GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Hertz;
    }

    template <>
    constexpr inline bool IsValid(const Frequency& x) noexcept
    {
        return IsValid(Real{x / Hertz});
    }

    template <>
    constexpr AngularVelocity GetInvalid() noexcept
    {
        return GetInvalid<Real>() * RadianPerSecond;
    }

    template <>
    constexpr inline bool IsValid(const AngularVelocity& x) noexcept
    {
        return IsValid(Real{x / RadianPerSecond});
    }

    template <>
    constexpr Time GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Second;
    }

    template <>
    constexpr inline bool IsValid(const Time& x) noexcept
    {
        return IsValid(Real{x / Second});
    }

    template <>
    constexpr Length GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Meter;
    }

    template <>
    constexpr inline bool IsValid(const Length& x) noexcept
    {
        return IsValid(Real{x / Meter});
    }

    template <>
    constexpr Mass GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Kilogram;
    }

    template <>
    constexpr inline bool IsValid(const Mass& x) noexcept
    {
        return IsValid(Real{x / Kilogram});
    }

    template <>
    constexpr InvMass GetInvalid() noexcept
    {
        return GetInvalid<Real>() / Kilogram;
    }

    template <>
    constexpr inline bool IsValid(const InvMass& x) noexcept
    {
        return IsValid(Real{x * Kilogram});
    }

    template <>
    constexpr Momentum GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Kilogram * MeterPerSecond;
    }

    template <>
    constexpr inline bool IsValid(const Momentum& x) noexcept
    {
        return IsValid(Real{x / (Kilogram * MeterPerSecond)});
    }

    template <>
    constexpr Force GetInvalid() noexcept
    {
        return GetInvalid<Real>() * Newton;
    }

    template <>
    constexpr inline bool IsValid(const Force& x) noexcept
    {
        return IsValid(Real{x / Newton});
    }

    template <>
    constexpr Torque GetInvalid() noexcept
    {
        return GetInvalid<Real>() * NewtonMeter;
    }

    template <>
    constexpr inline bool IsValid(const Torque& x) noexcept
    {
        return IsValid(Real{x / NewtonMeter});
    }

    template <>
    constexpr LinearVelocity GetInvalid() noexcept
    {
        return GetInvalid<Real>() * MeterPerSecond;
    }

    template <>
    constexpr inline bool IsValid(const LinearVelocity& x) noexcept
    {
        return IsValid(Real{x / MeterPerSecond});
    }

    template <>
    constexpr LinearAcceleration GetInvalid() noexcept
    {
        return GetInvalid<Real>() * MeterPerSquareSecond;
    }

    template <>
    constexpr inline bool IsValid(const LinearAcceleration& x) noexcept
    {
        return IsValid(Real{x / MeterPerSquareSecond});
    }

    template <>
    constexpr AngularAcceleration GetInvalid() noexcept
    {
        return GetInvalid<Real>() * RadianPerSquareSecond;
    }

    template <>
    constexpr inline bool IsValid(const AngularAcceleration& x) noexcept
    {
        return IsValid(Real{x / RadianPerSquareSecond});
    }

    template <>
    constexpr RotInertia GetInvalid() noexcept
    {
        // RotInertia is L^2  M    QP^-2
        return GetInvalid<Real>() * SquareMeter * Kilogram / SquareRadian;
    }

    template <>
    constexpr inline bool IsValid(const RotInertia& value) noexcept
    {
        return IsValid(Real{value / (SquareMeter * Kilogram / SquareRadian)});
    }

#endif

} // namespace box2d

#endif /* Units_hpp */


//          Copyright (C) 2012, Michele Caini.
// 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)

//          Two Graphs Common Spanning Trees Algorithm
//      Based on academic article of Mint, Read and Tarjan
//     Efficient Algorithm for Common Spanning Tree Problem
// Electron. Lett., 28 April 1983, Volume 19, Issue 9, p.346-347

#ifndef BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP
#define BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP

#include <boost/config.hpp>

#include <boost/bimap.hpp>
#include <boost/type_traits.hpp>
#include <boost/concept/requires.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <vector>
#include <stack>
#include <map>

namespace boost
{

namespace detail {

  template
    <
      typename TreeMap,
      typename PredMap,
      typename DistMap,
      typename LowMap,
      typename Buffer
    >
  struct bridges_visitor: public default_dfs_visitor
  {
    bridges_visitor(
        TreeMap tree,
        PredMap pred,
        DistMap dist,
        LowMap low,
        Buffer& buffer
      ): mTree(tree), mPred(pred), mDist(dist), mLow(low), mBuffer(buffer)
    { mNum = -1; }

    template <typename Vertex, typename Graph>
    void initialize_vertex(const Vertex& u, const Graph& g)
    {
      put(mPred, u, u);
      put(mDist, u, -1);
    }

    template <typename Vertex, typename Graph>
    void discover_vertex(const Vertex& u, const Graph& g)
    {
      put(mDist, u, ++mNum);
      put(mLow, u, get(mDist, u));
    }

    template <typename Edge, typename Graph>
    void tree_edge(const Edge& e, const Graph& g)
    {
      put(mPred, target(e, g), source(e, g));
      put(mTree, target(e, g), e);
    }

    template <typename Edge, typename Graph>
    void back_edge(const Edge& e, const Graph& g)
    {
      put(mLow, source(e, g),
        (std::min)(get(mLow, source(e, g)), get(mDist, target(e, g))));
    }

    template <typename Vertex, typename Graph>
    void finish_vertex(const Vertex& u, const Graph& g)
    {
      Vertex parent = get(mPred, u);
      if(get(mLow, u) > get(mDist, parent))
        mBuffer.push(get(mTree, u));
      put(mLow, parent,
        (std::min)(get(mLow, parent), get(mLow, u)));
    }

    TreeMap mTree;
    PredMap mPred;
    DistMap mDist;
    LowMap mLow;
    Buffer& mBuffer;
    int mNum;
  };

  template <typename Buffer>
  struct cycle_finder: public base_visitor< cycle_finder<Buffer> >
  {
    typedef on_back_edge event_filter;
    cycle_finder(): mBuffer(0) { }
    cycle_finder(Buffer* buffer)
      : mBuffer(buffer) { }
    template <typename Edge, typename Graph>
    void operator()(const Edge& e, const Graph& g)
      {
        if(mBuffer)
          mBuffer->push(e);
      }
    Buffer* mBuffer;
  };

  template <typename DeletedMap>
  struct deleted_edge_status
  {
    deleted_edge_status() { }
    deleted_edge_status(DeletedMap map): mMap(map) { }
    template <typename Edge>
    bool operator()(const Edge& e) const
      { return (!get(mMap, e)); }
    DeletedMap mMap;
  };

  template <typename InLMap>
  struct inL_edge_status
  {
    inL_edge_status() { }
    inL_edge_status(InLMap map): mMap(map) { }
    template <typename Edge>
    bool operator()(const Edge& e) const
      { return get(mMap, e); }
    InLMap mMap;
  };

  template <
    typename Graph,
    typename Func,
    typename Seq,
    typename Map
  >
  void rec_two_graphs_common_spanning_trees
    (
      const Graph& iG,
      bimap<
          bimaps::set_of<int>,
          bimaps::set_of< typename graph_traits<Graph>::edge_descriptor >
        > iG_bimap,
      Map aiG_inL,
      Map diG,
      const Graph& vG,
      bimap<
          bimaps::set_of<int>,
          bimaps::set_of< typename graph_traits<Graph>::edge_descriptor >
        > vG_bimap,
      Map avG_inL,
      Map dvG,
      Func func,
      Seq inL
    )
  {
    typedef graph_traits<Graph> GraphTraits;

    typedef typename GraphTraits::vertex_descriptor vertex_descriptor;
    typedef typename GraphTraits::edge_descriptor edge_descriptor;

    typedef typename Seq::size_type seq_size_type;

    int edges = num_vertices(iG) - 1;
//
//  [ Michele Caini ]
//
//  Using the condition (edges != 0) leads to the accidental submission of
//    sub-graphs ((V-1+1)-fake-tree, named here fat-tree).
//  Remove this condition is a workaround for the problem of fat-trees.
//  Please do not add that condition, even if it improves performance.
//
//  Here is proposed the previous guard (that was wrong):
//     for(seq_size_type i = 0; (i < inL.size()) && (edges != 0); ++i)
//
    {
      for(seq_size_type i = 0; i < inL.size(); ++i)
        if(inL[i])
          --edges;

      if(edges < 0)
        return;
    }

    bool is_tree = (edges == 0);
    if(is_tree) {
      func(inL);
    } else {
      std::map<vertex_descriptor, default_color_type> vertex_color;
      std::map<edge_descriptor, default_color_type> edge_color;

      std::stack<edge_descriptor> iG_buf, vG_buf;
      bool found = false;

      seq_size_type m;
      for(seq_size_type j = 0; j < inL.size() && !found; ++j) {
        if(!inL[j]
            && !get(diG, iG_bimap.left.at(j))
            && !get(dvG, vG_bimap.left.at(j)))
        {
          put(aiG_inL, iG_bimap.left.at(j), true);
          put(avG_inL, vG_bimap.left.at(j), true);

          undirected_dfs(
              make_filtered_graph(iG,
                detail::inL_edge_status< associative_property_map<
                  std::map<edge_descriptor, bool> > >(aiG_inL)),
              make_dfs_visitor(
                detail::cycle_finder< std::stack<edge_descriptor> > (&iG_buf)),
              associative_property_map<
                std::map<vertex_descriptor, default_color_type> >(vertex_color),
              associative_property_map<
                std::map<edge_descriptor, default_color_type> >(edge_color)
            );
          undirected_dfs(
              make_filtered_graph(vG,
                detail::inL_edge_status< associative_property_map<
                  std::map<edge_descriptor, bool> > >(avG_inL)),
              make_dfs_visitor(
                detail::cycle_finder< std::stack<edge_descriptor> > (&vG_buf)),
              associative_property_map<
                std::map<vertex_descriptor, default_color_type> >(vertex_color),
              associative_property_map<
                std::map<edge_descriptor, default_color_type> >(edge_color)
            );

          if(iG_buf.empty() && vG_buf.empty()) {
            inL[j] = true;
            found = true;
            m = j;
          } else {
            while(!iG_buf.empty()) iG_buf.pop();
            while(!vG_buf.empty()) vG_buf.pop();
            put(aiG_inL, iG_bimap.left.at(j), false);
            put(avG_inL, vG_bimap.left.at(j), false);
          }
        }
      }

      if(found) {

        std::stack<edge_descriptor> iG_buf_copy, vG_buf_copy;
        for(seq_size_type j = 0; j < inL.size(); ++j) {
          if(!inL[j]
              && !get(diG, iG_bimap.left.at(j))
              && !get(dvG, vG_bimap.left.at(j)))
          {

            put(aiG_inL, iG_bimap.left.at(j), true);
            put(avG_inL, vG_bimap.left.at(j), true);

            undirected_dfs(
                make_filtered_graph(iG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(aiG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&iG_buf)),
               associative_property_map< std::map<
                  vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );
            undirected_dfs(
                make_filtered_graph(vG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(avG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&vG_buf)),
                associative_property_map< std::map<
                  vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );

            if(!iG_buf.empty() || !vG_buf.empty()) {
              while(!iG_buf.empty()) iG_buf.pop();
              while(!vG_buf.empty()) vG_buf.pop();
              put(diG, iG_bimap.left.at(j), true);
              put(dvG, vG_bimap.left.at(j), true);
              iG_buf_copy.push(iG_bimap.left.at(j));
              vG_buf_copy.push(vG_bimap.left.at(j));
            }

            put(aiG_inL, iG_bimap.left.at(j), false);
            put(avG_inL, vG_bimap.left.at(j), false);
          }
        }

        // REC
        detail::rec_two_graphs_common_spanning_trees<Graph, Func, Seq, Map>
          (iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL, dvG, func, inL);

        while(!iG_buf_copy.empty()) {
          put(diG, iG_buf_copy.top(), false);
          put(dvG, vG_bimap.left.at(
            iG_bimap.right.at(iG_buf_copy.top())), false);
          iG_buf_copy.pop();
        }
        while(!vG_buf_copy.empty()) {
          put(dvG, vG_buf_copy.top(), false);
          put(diG, iG_bimap.left.at(
            vG_bimap.right.at(vG_buf_copy.top())), false);
          vG_buf_copy.pop();
        }

        inL[m] = false;
        put(aiG_inL, iG_bimap.left.at(m), false);
        put(avG_inL, vG_bimap.left.at(m), false);

        put(diG, iG_bimap.left.at(m), true);
        put(dvG, vG_bimap.left.at(m), true);

        std::map<vertex_descriptor, edge_descriptor> tree_map;
        std::map<vertex_descriptor, vertex_descriptor> pred_map;
        std::map<vertex_descriptor, int> dist_map, low_map;

        detail::bridges_visitor<
            associative_property_map<
                std::map<vertex_descriptor, edge_descriptor>
              >,
            associative_property_map<
                std::map<vertex_descriptor, vertex_descriptor>
              >,
            associative_property_map< std::map<vertex_descriptor, int> >,
            associative_property_map< std::map<vertex_descriptor, int> >,
            std::stack<edge_descriptor>
          >
        iG_vis(
            associative_property_map<
              std::map< vertex_descriptor, edge_descriptor> >(tree_map),
            associative_property_map<
              std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(dist_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(low_map),
            iG_buf
          ),
        vG_vis(
            associative_property_map<
              std::map< vertex_descriptor, edge_descriptor> >(tree_map),
            associative_property_map<
              std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(dist_map),
            associative_property_map<
              std::map< vertex_descriptor, int> >(low_map),
            vG_buf
          );

        undirected_dfs(make_filtered_graph(iG,
              detail::deleted_edge_status< associative_property_map<
              std::map<edge_descriptor, bool> > >(diG)),
            iG_vis,
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );
        undirected_dfs(make_filtered_graph(vG,
              detail::deleted_edge_status< associative_property_map<
              std::map<edge_descriptor, bool> > >(dvG)),
            vG_vis,
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );

        found = false;
        std::stack<edge_descriptor> iG_buf_tmp, vG_buf_tmp;
        while(!iG_buf.empty() && !found) {
          if(!inL[iG_bimap.right.at(iG_buf.top())]) {
            put(aiG_inL, iG_buf.top(), true);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf.top())), true);

            undirected_dfs(
                make_filtered_graph(iG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(aiG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&iG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );
            undirected_dfs(
                make_filtered_graph(vG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(avG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&vG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );

            if(!iG_buf_tmp.empty() || !vG_buf_tmp.empty()) {
              found = true;
            } else {
              while(!iG_buf_tmp.empty()) iG_buf_tmp.pop();
              while(!vG_buf_tmp.empty()) vG_buf_tmp.pop();
              iG_buf_copy.push(iG_buf.top());
            }

            put(aiG_inL, iG_buf.top(), false);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf.top())), false);
          }
          iG_buf.pop();
        }
        while(!vG_buf.empty() && !found) {
          if(!inL[vG_bimap.right.at(vG_buf.top())]) {
            put(avG_inL, vG_buf.top(), true);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf.top())), true);

            undirected_dfs(
                make_filtered_graph(iG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(aiG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&iG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );
            undirected_dfs(
                make_filtered_graph(vG,
                  detail::inL_edge_status< associative_property_map<
                    std::map<edge_descriptor, bool> > >(avG_inL)),
                make_dfs_visitor(
                  detail::cycle_finder<
                    std::stack<edge_descriptor> > (&vG_buf_tmp)),
                associative_property_map<
                  std::map<
                    vertex_descriptor, default_color_type> >(vertex_color),
                associative_property_map<
                  std::map<edge_descriptor, default_color_type> >(edge_color)
              );

            if(!iG_buf_tmp.empty() || !vG_buf_tmp.empty()) {
              found = true;
            } else {
              while(!iG_buf_tmp.empty()) iG_buf_tmp.pop();
              while(!vG_buf_tmp.empty()) vG_buf_tmp.pop();
              vG_buf_copy.push(vG_buf.top());
            }

            put(avG_inL, vG_buf.top(), false);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf.top())), false);
          }
          vG_buf.pop();
        }

        if(!found) {

          while(!iG_buf_copy.empty()) {
            inL[iG_bimap.right.at(iG_buf_copy.top())] = true;
            put(aiG_inL, iG_buf_copy.top(), true);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf_copy.top())), true);
            iG_buf.push(iG_buf_copy.top());
            iG_buf_copy.pop();
          }
          while(!vG_buf_copy.empty()) {
            inL[vG_bimap.right.at(vG_buf_copy.top())] = true;
            put(avG_inL, vG_buf_copy.top(), true);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf_copy.top())), true);
            vG_buf.push(vG_buf_copy.top());
            vG_buf_copy.pop();
          }

          // REC
          detail::rec_two_graphs_common_spanning_trees<
              Graph, Func, Seq, Map>
            (iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL, dvG, func, inL);

          while(!iG_buf.empty()) {
            inL[iG_bimap.right.at(iG_buf.top())] = false;
            put(aiG_inL, iG_buf.top(), false);
            put(avG_inL, vG_bimap.left.at(
              iG_bimap.right.at(iG_buf.top())), false);
            iG_buf.pop();
          }
          while(!vG_buf.empty()) {
            inL[vG_bimap.right.at(vG_buf.top())] = false;
            put(avG_inL, vG_buf.top(), false);
            put(aiG_inL, iG_bimap.left.at(
              vG_bimap.right.at(vG_buf.top())), false);
            vG_buf.pop();
          }

        }

        put(diG, iG_bimap.left.at(m), false);
        put(dvG, vG_bimap.left.at(m), false);

      }
    }
  }

} // namespace detail

template <typename Coll, typename Seq>
struct tree_collector
{

public:
  BOOST_CONCEPT_ASSERT((BackInsertionSequence<Coll>));
  BOOST_CONCEPT_ASSERT((RandomAccessContainer<Seq>));
  BOOST_CONCEPT_ASSERT((CopyConstructible<Seq>));

  typedef typename Coll::value_type coll_value_type;
  typedef typename Seq::value_type seq_value_type;

  BOOST_STATIC_ASSERT((is_same<coll_value_type, Seq>::value));
  BOOST_STATIC_ASSERT((is_same<seq_value_type, bool>::value));

  tree_collector(Coll& seqs): mSeqs(seqs) { }

  inline void operator()(Seq seq)
    { mSeqs.push_back(seq); }

private:
  Coll& mSeqs;

};

template <
  typename Graph,
  typename Order,
  typename Func,
  typename Seq
>
BOOST_CONCEPT_REQUIRES(
  ((RandomAccessContainer<Order>))
  ((IncidenceGraphConcept<Graph>))
  ((UnaryFunction<Func, void, Seq>))
  ((Mutable_RandomAccessContainer<Seq>))
  ((VertexAndEdgeListGraphConcept<Graph>)),
  (void)
)
two_graphs_common_spanning_trees
  (
    const Graph& iG,
    Order iG_map,
    const Graph& vG,
    Order vG_map,
    Func func,
    Seq inL
  )
{
  typedef graph_traits<Graph> GraphTraits;

  typedef typename GraphTraits::directed_category directed_category;
  typedef typename GraphTraits::vertex_descriptor vertex_descriptor;
  typedef typename GraphTraits::edge_descriptor edge_descriptor;

  typedef typename GraphTraits::edges_size_type edges_size_type;
  typedef typename GraphTraits::edge_iterator edge_iterator;

  typedef typename Seq::const_iterator seq_const_iterator;
  typedef typename Seq::difference_type seq_diff_type;
  typedef typename Seq::value_type seq_value_type;
  typedef typename Seq::size_type seq_size_type;
  typedef typename Seq::iterator seq_iterator;

  typedef typename Order::const_iterator order_const_iterator;
  typedef typename Order::difference_type order_diff_type;
  typedef typename Order::value_type order_value_type;
  typedef typename Order::size_type order_size_type;
  typedef typename Order::iterator order_iterator;

  BOOST_STATIC_ASSERT((is_same<order_value_type, edge_descriptor>::value));
  BOOST_CONCEPT_ASSERT((Convertible<order_size_type, edges_size_type>));

  BOOST_CONCEPT_ASSERT((Convertible<seq_size_type, edges_size_type>));
  BOOST_STATIC_ASSERT((is_same<seq_value_type, bool>::value));

  BOOST_STATIC_ASSERT((is_same<directed_category, undirected_tag>::value));

  if(num_vertices(iG) != num_vertices(vG))
    return;

  if(inL.size() != num_edges(iG)
      || inL.size() != num_edges(vG))
    return;

  if(iG_map.size() != num_edges(iG)
      || vG_map.size() != num_edges(vG))
    return;

  typedef bimaps::bimap<
      bimaps::set_of< int >,
      bimaps::set_of< order_value_type >
    > bimap_type;
  typedef typename bimap_type::value_type bimap_value;

  bimap_type iG_bimap, vG_bimap;
  for(order_size_type i = 0; i < iG_map.size(); ++i)
    iG_bimap.insert(bimap_value(i, iG_map[i]));
  for(order_size_type i = 0; i < vG_map.size(); ++i)
    vG_bimap.insert(bimap_value(i, vG_map[i]));

  edge_iterator current, last;
  boost::tuples::tie(current, last) = edges(iG);
  for(; current != last; ++current)
    if(iG_bimap.right.find(*current) == iG_bimap.right.end())
      return;
  boost::tuples::tie(current, last) = edges(vG);
  for(; current != last; ++current)
    if(vG_bimap.right.find(*current) == vG_bimap.right.end())
      return;

  std::stack<edge_descriptor> iG_buf, vG_buf;

  std::map<vertex_descriptor, edge_descriptor> tree_map;
  std::map<vertex_descriptor, vertex_descriptor> pred_map;
  std::map<vertex_descriptor, int> dist_map, low_map;

  detail::bridges_visitor<
      associative_property_map<
          std::map<vertex_descriptor, edge_descriptor>
        >,
      associative_property_map<
          std::map<vertex_descriptor, vertex_descriptor>
        >,
      associative_property_map< std::map<vertex_descriptor, int> >,
      associative_property_map< std::map<vertex_descriptor, int> >,
      std::stack<edge_descriptor>
    >
  iG_vis(
      associative_property_map<
        std::map< vertex_descriptor, edge_descriptor> >(tree_map),
      associative_property_map<
        std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
      associative_property_map<std::map< vertex_descriptor, int> >(dist_map),
      associative_property_map<std::map< vertex_descriptor, int> >(low_map),
      iG_buf
    ),
  vG_vis(
      associative_property_map<
        std::map< vertex_descriptor, edge_descriptor> >(tree_map),
      associative_property_map<
        std::map< vertex_descriptor, vertex_descriptor> >(pred_map),
      associative_property_map<std::map< vertex_descriptor, int> >(dist_map),
      associative_property_map<std::map< vertex_descriptor, int> >(low_map),
      vG_buf
    );

  std::map<vertex_descriptor, default_color_type> vertex_color;
  std::map<edge_descriptor, default_color_type> edge_color;

  undirected_dfs(iG, iG_vis,
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );
  undirected_dfs(vG, vG_vis,
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );

  while(!iG_buf.empty()) {
    inL[iG_bimap.right.at(iG_buf.top())] = true;
    iG_buf.pop();
  }
  while(!vG_buf.empty()) {
    inL[vG_bimap.right.at(vG_buf.top())] = true;
    vG_buf.pop();
  }

  std::map<edge_descriptor, bool> iG_inL, vG_inL;
  associative_property_map< std::map<edge_descriptor, bool> >
    aiG_inL(iG_inL), avG_inL(vG_inL);

  for(seq_size_type i = 0; i < inL.size(); ++i)
  {
    if(inL[i]) {
      put(aiG_inL, iG_bimap.left.at(i), true);
      put(avG_inL, vG_bimap.left.at(i), true);
    } else {
      put(aiG_inL, iG_bimap.left.at(i), false);
      put(avG_inL, vG_bimap.left.at(i), false);
    }
  }

  undirected_dfs(
      make_filtered_graph(iG,
        detail::inL_edge_status< associative_property_map<
          std::map<edge_descriptor, bool> > >(aiG_inL)),
      make_dfs_visitor(
        detail::cycle_finder< std::stack<edge_descriptor> > (&iG_buf)),
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );
  undirected_dfs(
      make_filtered_graph(vG,
        detail::inL_edge_status< associative_property_map<
          std::map<edge_descriptor, bool> > >(avG_inL)),
      make_dfs_visitor(
        detail::cycle_finder< std::stack<edge_descriptor> > (&vG_buf)),
      associative_property_map<
        std::map<vertex_descriptor, default_color_type> >(vertex_color),
      associative_property_map<
        std::map<edge_descriptor, default_color_type> >(edge_color)
    );

  if(iG_buf.empty() && vG_buf.empty()) {

    std::map<edge_descriptor, bool> iG_deleted, vG_deleted;
    associative_property_map< std::map<edge_descriptor, bool> > diG(iG_deleted);
    associative_property_map< std::map<edge_descriptor, bool> > dvG(vG_deleted);

    boost::tuples::tie(current, last) = edges(iG);
    for(; current != last; ++current)
      put(diG, *current, false);
    boost::tuples::tie(current, last) = edges(vG);
    for(; current != last; ++current)
      put(dvG, *current, false);

    for(seq_size_type j = 0; j < inL.size(); ++j) {
      if(!inL[j]) {
        put(aiG_inL, iG_bimap.left.at(j), true);
        put(avG_inL, vG_bimap.left.at(j), true);

        undirected_dfs(
            make_filtered_graph(iG,
              detail::inL_edge_status< associative_property_map<
                std::map<edge_descriptor, bool> > >(aiG_inL)),
            make_dfs_visitor(
              detail::cycle_finder< std::stack<edge_descriptor> > (&iG_buf)),
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );
        undirected_dfs(
            make_filtered_graph(vG,
              detail::inL_edge_status< associative_property_map<
                std::map<edge_descriptor, bool> > >(avG_inL)),
            make_dfs_visitor(
              detail::cycle_finder< std::stack<edge_descriptor> > (&vG_buf)),
            associative_property_map<
              std::map<vertex_descriptor, default_color_type> >(vertex_color),
            associative_property_map<
              std::map<edge_descriptor, default_color_type> >(edge_color)
          );

        if(!iG_buf.empty() || !vG_buf.empty()) {
          while(!iG_buf.empty()) iG_buf.pop();
          while(!vG_buf.empty()) vG_buf.pop();
          put(diG, iG_bimap.left.at(j), true);
          put(dvG, vG_bimap.left.at(j), true);
        }

        put(aiG_inL, iG_bimap.left.at(j), false);
        put(avG_inL, vG_bimap.left.at(j), false);
      }
    }

    int cc = 0;

    std::map<vertex_descriptor, int> com_map;
    cc += connected_components(
        make_filtered_graph(iG,
          detail::deleted_edge_status<associative_property_map<
            std::map<edge_descriptor, bool> > >(diG)),
        associative_property_map<std::map<vertex_descriptor, int> >(com_map)
      );
    cc += connected_components(
        make_filtered_graph(vG,
          detail::deleted_edge_status<associative_property_map<
            std::map<edge_descriptor, bool> > >(dvG)),
        associative_property_map< std::map<vertex_descriptor, int> >(com_map)
      );

    if(cc != 2)
      return;

    // REC
    detail::rec_two_graphs_common_spanning_trees<Graph, Func, Seq,
        associative_property_map< std::map<edge_descriptor, bool> > >
      (iG, iG_bimap, aiG_inL, diG, vG, vG_bimap, aiG_inL, dvG, func, inL);

  }

}

template <
  typename Graph,
  typename Func,
  typename Seq
>
BOOST_CONCEPT_REQUIRES(
  ((IncidenceGraphConcept<Graph>))
  ((EdgeListGraphConcept<Graph>)),
  (void)
)
two_graphs_common_spanning_trees
  (
    const Graph& iG,
    const Graph& vG,
    Func func,
    Seq inL
  )
{
  typedef graph_traits<Graph> GraphTraits;

  typedef typename GraphTraits::edge_descriptor edge_descriptor;
  typedef typename GraphTraits::edges_size_type edges_size_type;
  typedef typename GraphTraits::edge_iterator edge_iterator;

  std::vector<edge_descriptor> iGO, vGO;
  edge_iterator curr, last;

  boost::tuples::tie(curr, last) = edges(iG);
  for(; curr != last; ++curr)
    iGO.push_back(*curr);

  boost::tuples::tie(curr, last) = edges(vG);
  for(; curr != last; ++curr)
    vGO.push_back(*curr);

  two_graphs_common_spanning_trees(iG, iGO, vG, vGO, func, inL);
}

} // namespace boost

#endif // BOOST_GRAPH_TWO_GRAPHS_COMMON_SPANNING_TREES_HPP


// practice 5-6

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/size.hpp>

using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

template<class T> struct dim
{
	BOOST_STATIC_ASSERT(0);
};

template<class A, int N1, int N2, int N3>
struct dim< A[N1][N2][N3] > :
	mpl::vector_c<int, N1, N2, N3>
{
	typedef typename A value_type;
};

template < class T >
struct dimensions :
	dim<T>::type
{};

struct test :
	dimensions< float[10][5][3] >
{

};

// dimensions seq<array>
//size<seq>
//at_c<seq,0>::type::value
typedef dimensions< char[10][9][8] > seq;
BOOST_STATIC_ASSERT( mpl::size<seq>::value == 3 );

//BOOST_STATIC_ASSERT( mpl::at<seq, mpl::int_<0> >::type::value );
// BOOST_STATIC_ASSERT(
// 		mpl::at<
// 		mpl::vector_c<int, 10,1,2>
// 		, mpl::int_<0>
// 		>::type::value == 10 );

struct none{};

template<class T=int>
struct wrapAssert : T
{
	typedef typename T type;
	BOOST_STATIC_ASSERT(0);
};

struct test2 :
	mpl::eval_if<
		mpl::equal<
			mpl::at<seq, mpl::int_<0> >::type
			, mpl::int_<10>
		>
		, none
		, wrapAssert<>
	>
{};

struct test3 :
	mpl::eval_if<
	mpl::equal<
	mpl::at<seq, mpl::int_<1> >::type
	, mpl::int_<5>
	>
	, none
	, wrapAssert<>
	>
{};

struct test4 :
	mpl::eval_if<
	mpl::equal<
	mpl::at<seq, mpl::int_<2> >::type
	, mpl::int_<3>
	>
	, none
	, wrapAssert<>
	>
{};

struct test5 :
	mpl::push_back< seq >::type
{};

void main()
{
// 	int n1 = test::type::n1;
// 	int n2 = test::type::n2;
//	test::type::value_type a;

}


#include <boost/program_options.hpp>
#include <string>
#include <d3d_base/grid.h>
#include <iostream>
#include <d3d_base/configFile.h>
#include <d3d_fusion/volumetricFusionTools.h>
#include <algorithm>

void projectToHalspaceSpereIntersection(float& x, float& y, float& z, float nx, float ny, float nz, float dist, float radius)
{
    // check if point inside halfspace
    float pd = nx*x + ny*y + nz*z;
    if (pd < dist) {
        // project to sphere
        float norm = std::sqrt(x*x + y*y + z*z);
        float denom = std::max(1.f, norm/radius);
        x /= denom;
        y /= denom;
        z /= denom;
    } else {
        // project on halfplane
        float distDiff = pd-dist;
        x -= distDiff*nx;
        y -= distDiff*ny;
        z -= distDiff*nz;

        // check if inside sphere, if so we are done, otherwise project on circle (assuming radius > dist)
        float norm = std::sqrt(x*x + y*y + z*z);
        if (norm > radius) {
            // precompute for more efficient implemenetation
            float cicleRadius = std::sqrt(radius*radius - dist*dist);

            // shift to origin
            x -= dist*nx;
            y -= dist*ny;
            z -= dist*nz;

            // rescale such that norm = circleRadius
            norm = std::sqrt(x*x + y*y + z*z);
            x *= cicleRadius/norm;
            y *= cicleRadius/norm;
            z *= cicleRadius/norm;

            // shift to plane
            x += dist*nx;
            y += dist*ny;
            z += dist*nz;
        }
    }
}

struct Hist {
    float counts[8];
};

struct NormalInfo {
    bool isIsotropic;
    Eigen::Vector3f n1;
    Eigen::Vector3f n2;
    Eigen::Vector3f n3;
};

D3D::Grid<float> runTVHist(D3D::Grid<Hist>& dataCost, D3D::Grid<NormalInfo>& normals, Eigen::Vector3f& minCorner, Eigen::Vector3f& size,
                           Eigen::Matrix4f boxToGlobal, Eigen::Vector3f color, std::string vrmlOutputFile,
                           const int numIter, const float lambda, const float theta)
{
    D3D::Grid<float> u(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth());
    for (unsigned z = 0; z < u.getDepth(); z++)
        for (unsigned y = 0; y < u.getHeight(); y++)
            for (unsigned x = 0; x < u.getWidth(); x++)
            {
                u(x,y,z) = 0.f;
            }

    D3D::Grid<float> uBar = u.clone();

    D3D::Grid<float> lambda11(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda12(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda13(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda21(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda22(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda23(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);

    D3D::Grid<float> lambdaBar11 = lambda11.clone();
    D3D::Grid<float> lambdaBar12 = lambda12.clone();
    D3D::Grid<float> lambdaBar13 = lambda13.clone();
    D3D::Grid<float> lambdaBar21 = lambda21.clone();
    D3D::Grid<float> lambdaBar22 = lambda22.clone();
    D3D::Grid<float> lambdaBar23 = lambda23.clone();

    // dual variables
    D3D::Grid<float> p11(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p12(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p13(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p21(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p22(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p23(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p31(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p32(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p33(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);

    const int xDim = u.getWidth();
    const int yDim = u.getHeight();
    const int zDim = u.getDepth();

    // uses diagonal preconditioned first order pirmal dual

    const float sigma = 0.99f/3.f;
    const float tau = 0.99f/12.f;
    const float centers[8] = {-1.f, -5.f/7.f, -3.f/7.f, -1.f/7.f, 1.f/7.f, 3.f/7.f, 5.f/7.f, 1.f};

    for (int c = 0; c < numIter; c++)
    {
        std::cout << ".";
        std::cout.flush();

        // update dual variables
#pragma omp parallel for
        for (int z = 0; z < zDim; z++)
        {
            int const Z1 = (z < zDim-1) ? (z+1) : z;
            for (int y = 0; y < yDim; y++)
            {
                int const Y1 = (y < yDim-1) ? (y+1) : y;
                for (int x = 0; x < xDim; x++)
                {
                    float const u0 = uBar(x,y,z);

                    int const X1 = (x < xDim -1) ? (x+1) : x;

                    float const u_x = (uBar(X1, y, z) - u0);
                    float const u_y = (uBar(x, Y1, z) - u0);
                    float const u_z = (uBar(x, y, Z1) - u0);

                    float& p1w = p11(x,y,z);
                    float& p1h = p12(x,y,z);
                    float& p1d = p13(x,y,z);
                    float& p2w = p21(x,y,z);
                    float& p2h = p22(x,y,z);
                    float& p2d = p23(x,y,z);
                    float& p3w = p31(x,y,z);
                    float& p3h = p32(x,y,z);
                    float& p3d = p33(x,y,z);

                    p1w += sigma*(u_x + lambdaBar11(x,y,z));
                    p1h += sigma*(u_y + lambdaBar12(x,y,z));
                    p1d += sigma*(u_z + lambdaBar13(x,y,z));

                    p2w += sigma*(-lambdaBar11(x,y,z) + lambdaBar21(x,y,z));
                    p2h += sigma*(-lambdaBar12(x,y,z) + lambdaBar22(x,y,z));
                    p2d += sigma*(-lambdaBar13(x,y,z) + lambdaBar23(x,y,z));

                    p3w += sigma*(-lambdaBar21(x,y,z));
                    p3h += sigma*(-lambdaBar22(x,y,z));
                    p3d += sigma*(-lambdaBar23(x,y,z));

                    if (normals(x,y,z).isIsotropic) {
                        float tv = sqrtf(p1w*p1w + p1h*p1h + p1d*p1d);
                        float denom = std::max(1.f, tv/2.f);
                        p1w /= denom;
                        p1h /= denom;
                        p1d /= denom;

                        tv = sqrtf(p2w*p2w + p2h*p2h + p2d*p2d);
                        denom = std::max(1.f, tv/2.f);
                        p2w /= denom;
                        p2h /= denom;
                        p2d /= denom;

                        tv = sqrtf(p3w*p3w + p3h*p3h + p3d*p3d);
                        denom = std::max(1.f, tv/2.f);
                        p3w /= denom;
                        p3h /= denom;
                        p3d /= denom;
                    } else {
                        Eigen::Vector3f n1 = -normals(x,y,z).n1;
                        Eigen::Vector3f n2 = -normals(x,y,z).n2;
                        Eigen::Vector3f n3 = -normals(x,y,z).n3;
                        projectToHalspaceSpereIntersection(p1w, p1h, p1d, n1(0), n1(1), n1(2), 0.5, 2.0);
                        projectToHalspaceSpereIntersection(p2w, p2h, p2d, n2(0), n2(1), n2(2), 0.5, 2.0);
                        projectToHalspaceSpereIntersection(p3w, p3h, p3d, n3(0), n3(1), n3(2), 0.5, 2.0);
                    }
                }
            }
        }

        // update primal
#pragma omp parallel for
        for (int z = 0; z < zDim; z++)
        {
            int const Z0 = z-1;
            for (int y = 0; y < yDim; y++)
            {
                int const Y0 = y-1;
                for (int x = 0; x < xDim; x++)
                {
                    int const X0 = x-1;

                    float const div = (X0 < xDim -1 ? p11(x,y,z) : 0.0f) - (X0 >= 0 ? p11(X0, y, z) : 0.0f) +
                                      (Y0 < yDim -1 ? p12(x,y,z) : 0.0f) - (Y0 >= 0 ? p12(x, Y0, z) : 0.0f) +
                                      (Z0 < zDim -1 ? p13(x,y,z) : 0.0f) - (Z0 >= 0 ? p13(x, y, Z0) : 0.0f);

                    float const u0 = u(x,y,z);
                    float U = u(x,y,z) + tau*div;

//                    // check most likely bin, is in [1, 8] as u0 in [-1, 1]
//                    int mlBin = int(7.f * (u0 + 1.f) / 2.f + 1.f);

//                    float step = 0.f;
//                    for (int b = 0; b <= mlBin; ++b) {
//                        step -= float(dataCost(x, y, z).counts[b]);
//                    }
//                    for (int b = mlBin+1; b < 10; ++b) {
//                        step += float(dataCost(x, y, z).counts[b]);
//                    }

//                    float proxMin = u0 + tau*lambda*step;

                    float proxMin, step;

//                    if (proxMin <= centers[mlBin] || centers[mlBin+1] <= proxMin) {
                        // outside of range
                        bool found = false;
                        for (int bin = 0; bin < 7; bin++) {
//                            if (bin == mlBin)
//                                continue;
                            step = 0.f;
                            for (int b = 0; b <= bin; ++b) {
                                step -= dataCost(x, y, z).counts[b];
                            }
                            for (int b = bin+1; b < 8; ++b) {
                                step += dataCost(x, y, z).counts[b];
                            }

                            proxMin = U + tau*lambda*step;

                            if (centers[bin] < proxMin && proxMin < centers[bin+1]) {
                                // inside of range, val is the minimum
                                found = true;
                                break;
                            }
                        }
                        if (!found) {
                            // minimum must be one of the bin centers
                            float proxMinEnergy = std::numeric_limits<float>::max();
                            for (int bin = 0; bin < 8; bin++) {
                                float dataterm = 0.f;
                                for (int i = 0; i < 8; ++i) {
                                    dataterm += dataCost(x, y, z).counts[i] * fabs(centers[bin]-centers[i]);
                                }
                                float proxEnergy = (U-centers[bin])*(U-centers[bin])/(2.f*tau) + lambda*dataterm;
                                if (proxEnergy < proxMinEnergy) {
                                    proxMinEnergy = proxEnergy;
                                    proxMin = centers[bin];
                                }
                            }
                        }
//                    }

                    U = proxMin;

                    u(x,y,z) = U;
                    uBar(x,y,z) = U + theta*(U - u0);

                    float const L11 = lambda11(x,y,z) - tau*(p11(x,y,z) - p21(x,y,z));
                    float const L12 = lambda12(x,y,z) - tau*(p12(x,y,z) - p22(x,y,z));
                    float const L13 = lambda13(x,y,z) - tau*(p13(x,y,z) - p23(x,y,z));

                    lambdaBar11(x,y,z) = L11 + theta*(L11 - lambda11(x,y,z));
                    lambdaBar12(x,y,z) = L12 + theta*(L12 - lambda12(x,y,z));
                    lambdaBar13(x,y,z) = L13 + theta*(L13 - lambda13(x,y,z));

                    lambda11(x,y,z) = L11;
                    lambda12(x,y,z) = L12;
                    lambda13(x,y,z) = L13;

                    float const L21 = lambda21(x,y,z) - tau*(p21(x,y,z) - p31(x,y,z));
                    float const L22 = lambda22(x,y,z) - tau*(p22(x,y,z) - p32(x,y,z));
                    float const L23 = lambda23(x,y,z) - tau*(p23(x,y,z) - p33(x,y,z));

                    lambdaBar21(x,y,z) = L21 + theta*(L21 - lambda21(x,y,z));
                    lambdaBar22(x,y,z) = L22 + theta*(L22 - lambda22(x,y,z));
                    lambdaBar23(x,y,z) = L23 + theta*(L23 - lambda23(x,y,z));

                    lambda21(x,y,z) = L21;
                    lambda22(x,y,z) = L22;
                    lambda23(x,y,z) = L23;
                }
            }
        }

        if ((c+1) % 50 == 0) {
            D3D::saveVolumeAsVRMLMesh(u, 0.0f, minCorner, size, boxToGlobal, color, vrmlOutputFile, true);
            u.saveAsDataFile("tvHistNormalFusionU.dat");
            std::cout << std::endl;

        }
    }
    return u;
}

int main(int argc, char* argv[])
{
    std::string configFile;
    std::string intDepthGridFile;
    std::string clusteredNormalsFile;
    std::string vrmlOutputFile;

    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
            ("help", "Produce help message")
            ("intDepthGridFile", boost::program_options::value<std::string>(&intDepthGridFile)->default_value("intCSDFGrid.dat"), "Depth map list file")
            ("clusteredNormalsFile", boost::program_options::value<std::string>(&clusteredNormalsFile)->default_value("clusteredNormals.dat"), "Clustered normals file")
            ("configFile", boost::program_options::value<std::string>(&configFile)->default_value("conf.txt"), "Config file")
            ("vrmlOutputFile", boost::program_options::value<std::string>(&vrmlOutputFile)->default_value("tvHistNormalModel.wrl"), "Output data file with integrated depth data.")
            ;

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 1;
    }

    // config file
    D3D::ConfigFile conf(configFile);

    if (!conf.isFileRead())
    {
        D3D_THROW_EXCEPTION("Could not open config file.")
    }

    // load the box parameters
    std::string bpMinXStr = conf.get("BP_MIN_X");
    if (bpMinXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_X not specified in the config file.")
    }
    float bpMinX = atof(bpMinXStr.c_str());

    std::string bpSizeXStr = conf.get("BP_SIZE_X");
    if (bpSizeXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_X not specified in the config file.")
    }
    float bpSizeX = atof(bpSizeXStr.c_str());

    std::string bpMinYStr = conf.get("BP_MIN_Y");
    if (bpMinYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_Y not specified in the config file.")
    }
    float bpMinY = atof(bpMinYStr.c_str());

    std::string bpSizeYStr = conf.get("BP_SIZE_Y");
    if (bpSizeYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_Y not specified in the config file.")
    }
    float bpSizeY = atof(bpSizeYStr.c_str());

    std::string bpMinZStr = conf.get("BP_MIN_Z");
    if (bpMinZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_Z not specified in the config file.")
    }
    float bpMinZ = atof(bpMinZStr.c_str());

    std::string bpSizeZStr = conf.get("BP_SIZE_Z");
    if (bpSizeZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_Z not specified in the config file.")
    }
    float bpSizeZ = atof(bpSizeZStr.c_str());

    std::string bpResXStr = conf.get("BP_RES_X");
    if (bpResXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_X not specified in the config file.")
    }
    int bpResX = atoi(bpResXStr.c_str());

    std::string bpResYStr = conf.get("BP_RES_Y");
    if (bpResYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_Y not specified in the config file.")
    }
    int bpResY = atoi(bpResYStr.c_str());

    std::string bpResZStr = conf.get("BP_RES_Z");
    if (bpResZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_Z not specified in the config file.")
    }
    int bpResZ = atoi(bpResZStr.c_str());

    std::string bpOriginStr = conf.get("BP_ORIGIN");
    Eigen::Vector3f bpOrigin;
    if (bpOriginStr.empty())
    {
        bpOrigin = Eigen::Vector3f::Zero();
    }
    else
    {
        std::stringstream bpOriginStream(bpOriginStr);
        bpOriginStream >> bpOrigin(0);
        bpOriginStream >> bpOrigin(1);
        bpOriginStream >> bpOrigin(2);

        if (bpOriginStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_ORIGIN");
        }
    }

    std::string bpXAxisStr = conf.get("BP_X_AXIS");
    Eigen::Vector3f bpXAxis;
    bool xAxisSet = false;
    if (!bpXAxisStr.empty())
    {
        xAxisSet = true;

        std::stringstream bpXAxisStream(bpXAxisStr);
        bpXAxisStream >> bpXAxis(0);
        bpXAxisStream >> bpXAxis(1);
        bpXAxisStream >> bpXAxis(2);

        if (bpXAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_X_AXIS");
        }
    }

    std::string bpYAxisStr = conf.get("BP_Y_AXIS");
    Eigen::Vector3f bpYAxis;
    bool yAxisSet = false;
    if (!bpYAxisStr.empty())
    {
        yAxisSet = true;

        std::stringstream bpYAxisStream(bpYAxisStr);
        bpYAxisStream >> bpYAxis(0);
        bpYAxisStream >> bpYAxis(1);
        bpYAxisStream >> bpYAxis(2);

        if (bpYAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_Y_AXIS");
        }
    }

    std::string bpZAxisStr = conf.get("BP_Z_AXIS");
    Eigen::Vector3f bpZAxis;
    bool zAxisSet = false;
    if (!bpZAxisStr.empty())
    {
        zAxisSet = true;

        std::stringstream bpZAxisStream(bpZAxisStr);
        bpZAxisStream >> bpZAxis(0);
        bpZAxisStream >> bpZAxis(1);
        bpZAxisStream >> bpZAxis(2);

        if (bpZAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_Z_AXIS");
        }
    }

    if (xAxisSet == false && yAxisSet == false && zAxisSet == false)
    {
        bpXAxis(0) = 1; bpXAxis(1) = 0; bpXAxis(2) = 0;
        bpYAxis(0) = 0; bpYAxis(1) = 1; bpYAxis(2) = 0;
        bpZAxis(0) = 0; bpZAxis(1) = 0; bpZAxis(2) = 1;
    }
    else if (!(xAxisSet && yAxisSet && zAxisSet))
    {
        D3D_THROW_EXCEPTION("Either all or none out of BP_X_AXIS, BP_Y_AXIS and BP_Z_AXIS have to be specified.")
    }

    // rectify the axes
    // normlaize
    bpXAxis /= bpXAxis.norm();
    bpYAxis /= bpYAxis.norm();
    bpZAxis /= bpZAxis.norm();

    Eigen::Vector3f rightAngleZAxis = bpXAxis.cross(bpYAxis);
    if (rightAngleZAxis.dot(bpZAxis) < 0.95)
    {
        D3D_THROW_EXCEPTION("The given axes are not orthogonal to each other.")
    }

    bpZAxis = rightAngleZAxis;

    Eigen::Vector3f rightAngleYAxis = bpZAxis.cross(bpXAxis);
    if (rightAngleYAxis.dot(bpYAxis) < 0.95)
    {
        D3D_THROW_EXCEPTION("The given axes are not orthogonal to each other.");
    }

    bpYAxis = rightAngleYAxis;

    Eigen::Matrix4f boxToGlobal = Eigen::Matrix4f::Identity();
    boxToGlobal.topLeftCorner(3,3).col(0) = bpXAxis;
    boxToGlobal.topLeftCorner(3,3).col(1) = bpYAxis;
    boxToGlobal.topLeftCorner(3,3).col(2) = bpZAxis;
    boxToGlobal.topRightCorner(3,1).col(0) = bpOrigin;

    std::string hfNumIterStr = conf.get("HF_NUM_ITER");
    if (hfNumIterStr.empty())
    {
        D3D_THROW_EXCEPTION("Paramter HF_NUM_ITER is not specified in the config file.")
    }
    int ffNumIter = atoi(hfNumIterStr.c_str());

    std::string hfLambdaStr = conf.get("HF_LAMBDA");
    if (hfLambdaStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter HF_LAMBDA is not specified in the config file.")
    }
    float hfLambda = atof(hfLambdaStr.c_str());

    // load grid
    D3D::Grid<Hist> dataCost;
    dataCost.loadFromDataFile(intDepthGridFile);

    std::cout << "Data Cost Loaded: width = " << dataCost.getWidth() << ", height = " << dataCost.getHeight() << ", depth = " << dataCost.getDepth() << std::endl;

    if (bpResX != (int) dataCost.getWidth() || bpResY != (int) dataCost.getHeight() || bpResZ != (int) dataCost.getDepth())
    {
        D3D_THROW_EXCEPTION("Resolution specified in the config file does not match the dimension of the grid with integrated depth maps.")
    }

    // load clustered normals
    D3D::Grid<NormalInfo> clusteredNormals;
    clusteredNormals.loadFromDataFile(clusteredNormalsFile);

    std::cout << "Clustered Normals Loaded: width = " << clusteredNormals.getWidth() << ", height = " << clusteredNormals.getHeight() << ", depth = " << clusteredNormals.getDepth() << std::endl;

    if (bpResX != (int) clusteredNormals.getWidth() || bpResY != (int) clusteredNormals.getHeight() || bpResZ != (int) clusteredNormals.getDepth())
    {
        D3D_THROW_EXCEPTION("Resolution specified in the config file does not match the dimension of the grid with the clustered normals.")
    }

    Eigen::Vector3f minCorner;
    minCorner(0) = bpMinX;
    minCorner(1) = bpMinY;
    minCorner(2) = bpMinZ;
    Eigen::Vector3f size;
    size(0) = bpSizeX;
    size(1) = bpSizeY;
    size(2) = bpSizeZ;
    Eigen::Vector3f color;
    color(0) = 0.5;
    color(1) = 0.5;
    color(2) = 0.5;

    runTVHist(dataCost, clusteredNormals, minCorner, size, boxToGlobal, color, vrmlOutputFile, ffNumIter, hfLambda, 1);
}


#ifndef HOPS_DATA_HPP
#define HOPS_DATA_HPP

#include <hops/FileWriter/FileWriter.hpp>
#include <hops/FileWriter/FileWriterFactory.hpp>
#include <hops/FileWriter/FileWriterType.hpp>
#include <hops/MarkovChain/MarkovChain.hpp>
#include <hops/Statistics/ExpectedSquaredJumpDistance.hpp>
#include <hops/Statistics/EffectiveSampleSize.hpp>
#include <hops/Statistics/PotentialScaleReductionFactor.hpp>
#include <hops/Utility/ChainData.hpp>

#include <Eigen/Core>

#include <vector>
#include <memory>

namespace hops {
    class Data {
    public:
        Data(long dimension = 0) : dimension(dimension) {
            //
        }

        Data(const std::vector<std::shared_ptr<MarkovChain>>& markovChains, long dimension = 0) : dimension(dimension) {
            linkWithChains(markovChains);
        }

        void setDimension(long dimension) {
            this->dimension = dimension;
        }

        void linkWithChains(const std::vector<std::shared_ptr<MarkovChain>>& markovChains) {
            chains.resize(markovChains.size());
            for (size_t i = 0; i < markovChains.size(); ++i) {
                markovChains[i]->installDataObject(chains[i]);
            }
        }

        std::vector<const std::vector<double>*> getAcceptanceRates() {
            std::vector<const std::vector<double>*> acceptanceRates(chains.size());
            for (size_t i = 0; i < acceptanceRates.size(); ++i) {
                acceptanceRates[i] = chains[i].acceptanceRates.get();
            }
            return acceptanceRates;
        }

        std::vector<const std::vector<double>*> getNegativeLogLikelihood() {
            std::vector<const std::vector<double>*> negativeLogLikelihood(chains.size());
            for (size_t i = 0; i < negativeLogLikelihood.size(); ++i) {
                negativeLogLikelihood[i] = chains[i].negativeLogLikelihood.get();
            }
            return negativeLogLikelihood;
        }

        std::vector<const std::vector<Eigen::VectorXd>*> getStates() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
            }
            return states;
        }

        std::vector<const std::vector<long>*> getTimestamps() {
            std::vector<const std::vector<long>*> timestamps(chains.size());
            for (size_t i = 0; i < timestamps.size(); ++i) {
                timestamps[i] = chains[i].timestamps.get();
            }
            return timestamps;
        }

        void computeTotalNumberOfSamples() {
            totalNumberOfSamples = 0;
            for (size_t i = 0; i < chains.size(); ++i) {
                totalNumberOfSamples += chains[i].getStates().size();
            }
        }

        void computeAcceptanceRate() {
            acceptanceRate = Eigen::VectorXd(chains.size());
            for (size_t i = 0; i < chains.size(); ++i) {
                acceptanceRate(i) = chains[i].getAcceptanceRates().back();
            }
        }

        void computeEffectiveSampleSize() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
			if (!chains.size()) {
				throw EmptyChainDataException();
			}

            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
				if (!states[i]) {
					throw EmptyChainDataException();
				}
            }
            std::vector<double> effectiveSampleSize = ::hops::computeEffectiveSampleSize(states);
            this->effectiveSampleSize = Eigen::Map<Eigen::VectorXd>(effectiveSampleSize.data(), dimension);
        }

        void computeExpectedSquaredJumpDistance() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
			if (!chains.size()) {
				throw EmptyChainDataException();
			}

            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
				if (!states[i]) {
					throw EmptyChainDataException();
				}
            }
            std::vector<double> expectedSquaredJumpDistance = ::hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(states);
            this->expectedSquaredJumpDistance = Eigen::Map<Eigen::VectorXd>(expectedSquaredJumpDistance.data(), chains.size());
        }

        void computePotentialScaleReductionFactor() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
			if (!chains.size()) {
				throw EmptyChainDataException();
			}

            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
				if (!states[i]) {
					throw EmptyChainDataException();
				}
            }
            std::vector<double> potentialScaleReductionFactor = ::hops::computePotentialScaleReductionFactor(states);
            this->potentialScaleReductionFactor = Eigen::Map<Eigen::VectorXd>(potentialScaleReductionFactor.data(), dimension);
        }

        void computeTotalTimeTaken() {
            totalTimeTaken = Eigen::VectorXd(chains.size());
            for (size_t i = 0; i < chains.size(); ++i) {
                totalTimeTaken(i) = chains[i].getTimestamps().back() - chains[i].getTimestamps().front();
            }
        }

        void reset() {
            for (size_t i = 0; i < chains.size(); ++i) {
                chains[i].reset();
            }
        }

        void write(const std::string& outputDirectory, bool discardRawData = false) const {
            if (!discardRawData) {
                for (size_t i = 0; i < chains.size(); ++i) {
                    auto fileWriter = FileWriterFactory::createFileWriter(outputDirectory + "/chain" + std::to_string(i), FileWriterType::CSV);
                    chains[i].write(fileWriter.get());
                }
            }

			auto statisticsWriter = FileWriterFactory::createFileWriter(outputDirectory + "/statistics", FileWriterType::CSV);

            if (acceptanceRate.size() > 0) {
                statisticsWriter->write("acceptanceRate", Eigen::MatrixXd(acceptanceRate.transpose()));
            }

            if (expectedSquaredJumpDistance.size() > 0) {
                statisticsWriter->write("expectedSquaredJumpDistance", Eigen::MatrixXd(expectedSquaredJumpDistance.transpose()));
            }

            if (effectiveSampleSize.size() > 0) {
                statisticsWriter->write("effectiveSampleSize", Eigen::MatrixXd(effectiveSampleSize.transpose()));
            }

            if (potentialScaleReductionFactor.size() > 0) {
                statisticsWriter->write("potentialScaleReductionFactor", Eigen::MatrixXd(potentialScaleReductionFactor.transpose()));
            }

            if (totalNumberOfSamples > 0) {
                statisticsWriter->write("totalNumberOfSamples", Eigen::MatrixXd(totalNumberOfSamples * Eigen::MatrixXd::Identity(1,1)));
            }

            if (totalTimeTaken.size() > 0) {
                statisticsWriter->write("totalTimeTaken", Eigen::MatrixXd(totalTimeTaken.transpose()));
            }

            if (totalNumberOfTuningSamples > 0) {
                auto tuningWriter = FileWriterFactory::createFileWriter(outputDirectory + "/tuning", FileWriterType::CSV);
                tuningWriter->write("totalNumberOfTuningSamples", std::vector<long>{static_cast<long>(totalNumberOfTuningSamples)});
                tuningWriter->write("stepSize", std::vector<double>{tunedStepSize});
                tuningWriter->write("objectiveValue", std::vector<double>{tunedObjectiveValue});
                tuningWriter->write("totalTimeTaken", std::vector<double>{totalTuningTimeTaken});

                if (tuningData.size() > 0) {
                    tuningWriter->write("data", tuningData);
                }

                if (tuningPosterior.size() > 0) {
                    tuningWriter->write("posterior", tuningPosterior);
                }
            }
        }

        void setTuningMethod(const std::string& tuningMethod) {
            this->tuningMethod = tuningMethod;
        }

        void setTotalNumberOfTuningSamples(unsigned long totalNumberOfTuningSamples) {
            this->totalNumberOfTuningSamples = totalNumberOfTuningSamples;
        }

        void setTunedStepSize(double tunedStepSize) {
            this->tunedStepSize = tunedStepSize;
        }

        void setTunedObjectiveValue(double tunedObjectiveValue) {
            this->tunedObjectiveValue = tunedObjectiveValue;
        }

        void setTotalTuningTimeTaken(double totalTuningTimeTaken) {
            this->totalTuningTimeTaken = totalTuningTimeTaken;
        }

        void setTuningData(const Eigen::MatrixXd& tuningData) {
            this->tuningData = tuningData;
        }

        void setTuningPosterior(const Eigen::MatrixXd& tuningPosterior) {
            this->tuningPosterior = tuningPosterior;
        }

    private:
        std::vector<ChainData> chains;

        double totalNumberOfSamples;
        Eigen::VectorXd acceptanceRate;
        Eigen::VectorXd expectedSquaredJumpDistance;
        Eigen::VectorXd effectiveSampleSize;
        Eigen::VectorXd potentialScaleReductionFactor;
        Eigen::VectorXd totalTimeTaken;

        // tuning data
        std::string tuningMethod;
        unsigned long totalNumberOfTuningSamples = 0;
        double tunedStepSize;
        double tunedObjectiveValue;
        double totalTuningTimeTaken;

        Eigen::MatrixXd tuningData;
        Eigen::MatrixXd tuningPosterior;

        std::vector<std::vector<double>> sampleVariances;
        std::vector<std::vector<double>> intraChainExpectations;
        std::vector<double> interChainExpectation;
        unsigned long numSeen = 0;

        long dimension = 0;
        friend Eigen::VectorXd computeAcceptanceRate(Data& data);
        friend Eigen::VectorXd computeExpectedSquaredJumpDistance(Data& data);
        friend Eigen::VectorXd computeEffectiveSampleSize(Data& data);
        friend Eigen::VectorXd computePotentialScaleReductionFactor(Data& data);
        friend double computeTotalNumberOfSamples(Data& data);
        friend Eigen::VectorXd computeTotalTimeTaken(Data& data);
    };

    inline Eigen::VectorXd computeAcceptanceRate(Data& data) {
        data.computeAcceptanceRate();
        return data.acceptanceRate;
    }

    inline Eigen::VectorXd computeExpectedSquaredJumpDistance(Data& data) {
        data.computeExpectedSquaredJumpDistance();
        return data.expectedSquaredJumpDistance;
    }

    inline Eigen::VectorXd computeEffectiveSampleSize(Data& data) {
        data.computeEffectiveSampleSize();
        return data.effectiveSampleSize;
    }

    inline Eigen::VectorXd computePotentialScaleReductionFactor(Data& data) {
        data.computePotentialScaleReductionFactor();
        return data.potentialScaleReductionFactor;
    }

    inline double computeTotalNumberOfSamples(Data& data) {
        data.computeTotalNumberOfSamples();
        return data.totalNumberOfSamples;
    }

    inline Eigen::VectorXd computeTotalTimeTaken(Data& data) {
        data.computeTotalTimeTaken();
        return data.totalTimeTaken;
    }
}

#endif // HOPS_DATA_HPP


#include <iostream>
#include <tr1/memory>

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

#include "filelib.h"
#include "dict.h"
#include "sampler.h"
#include "ccrp.h"

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

Dict d; // global dictionary

string Join(char joiner, const vector<int>& phrase) {
  ostringstream os;
  for (int i = 0; i < phrase.size(); ++i) {
    if (i > 0) os << joiner;
    os << d.Convert(phrase[i]);
  }
  return os.str();
}

ostream& operator<<(ostream& os, const vector<int>& phrase) {
  for (int i = 0; i < phrase.size(); ++i)
    os << (i == 0 ? "" : " ") << d.Convert(phrase[i]);
  return os;
}

struct UnigramLM {
  explicit UnigramLM(const string& fname) {
    ifstream in(fname.c_str());
    assert(in);
  }

  double logprob(int word) const {
    assert(word < freqs_.size());
    return freqs_[word];
  }

  vector<double> freqs_;
};

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("samples,s",po::value<unsigned>()->default_value(1000),"Number of samples")
        ("input,i",po::value<string>(),"Read file from")
        ("random_seed,S",po::value<uint32_t>(), "Random seed")
        ("write_cdec_grammar,g", po::value<string>(), "Write cdec grammar to this file")
        ("write_cdec_weights,w", po::value<string>(), "Write cdec weights to this file")
        ("poisson_length,p", "Use a Poisson distribution as the length of a phrase in the base distribuion")
        ("no_hyperparameter_inference,N", "Disable hyperparameter inference");
  po::options_description clo("Command line options");
  clo.add_options()
        ("config", po::value<string>(), "Configuration file")
        ("help,h", "Print this help message and exit");
  po::options_description dconfig_options, dcmdline_options;
  dconfig_options.add(opts);
  dcmdline_options.add(opts).add(clo);

  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  if (conf->count("config")) {
    ifstream config((*conf)["config"].as<string>().c_str());
    po::store(po::parse_config_file(config, dconfig_options), *conf);
  }
  po::notify(*conf);

  if (conf->count("help") || (conf->count("input") == 0)) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

void ReadCorpus(const string& filename, vector<vector<int> >* c, set<int>* vocab) {
  c->clear();
  istream* in;
  if (filename == "-")
    in = &cin;
  else
    in = new ifstream(filename.c_str());
  assert(*in);
  string line;
  while(*in) {
    getline(*in, line);
    if (line.empty() && !*in) break;
    c->push_back(vector<int>());
    vector<int>& v = c->back();
    d.ConvertWhitespaceDelimitedLine(line, &v);
    for (int i = 0; i < v.size(); ++i) vocab->insert(v[i]);
  }
  if (in != &cin) delete in;
}

double log_poisson(unsigned x, const double& lambda) {
  assert(lambda > 0.0);
  return log(lambda) * x - lgamma(x + 1) - lambda;
}

struct UniphraseLM {
  UniphraseLM(const vector<vector<int> >& corpus,
              const set<int>& vocab,
              const po::variables_map& conf) :
    phrases_(1,1,1,1),
    gen_(1,1,1,1),
    corpus_(corpus),
    uniform_word_(1.0 / vocab.size()),
    gen_p0_(0.5),
    p_end_(0.5),
    use_poisson_(conf.count("poisson_length") > 0) {}

  double p0(const vector<int>& phrase) const {
    static vector<double> p0s(10000, 0.0);
    assert(phrase.size() < 10000);
    double& p = p0s[phrase.size()];
    if (p) return p;
    p = exp(log_p0(phrase));
    if (!p) {
      cerr << "0 prob phrase: " << phrase << "\nAssigning std::numeric_limits<double>::min()\n";
      p = std::numeric_limits<double>::min();
    }
    return p;
  }

  double log_p0(const vector<int>& phrase) const {
    double len_logprob;
    if (use_poisson_)
      len_logprob = log_poisson(phrase.size(), 1.0);
    else
      len_logprob = log(1 - p_end_) * (phrase.size() -1) + log(p_end_);
    return log(uniform_word_) * phrase.size() + len_logprob;
  }

  double llh() const {
    double llh = gen_.log_crp_prob();
    llh += gen_.num_tables(false) * log(gen_p0_) +
           gen_.num_tables(true) * log(1 - gen_p0_);
    double llhr = phrases_.log_crp_prob();
    for (CCRP<vector<int> >::const_iterator it = phrases_.begin(); it != phrases_.end(); ++it) {
      llhr += phrases_.num_tables(it->first) * log_p0(it->first);
      //llhr += log_p0(it->first);
      if (!isfinite(llh)) {
        cerr << it->first << endl;
        cerr << log_p0(it->first) << endl;
        abort();
      }
    }
    return llh + llhr;
  }

  void Sample(unsigned int samples, bool hyp_inf, MT19937* rng) {
    cerr << "Initializing...\n";
    z_.resize(corpus_.size());
    int tc = 0;
    for (int i = 0; i < corpus_.size(); ++i) {
      const vector<int>& line = corpus_[i];
      const int ls = line.size();
      const int last_pos = ls - 1;
      vector<bool>& z = z_[i];
      z.resize(ls);
      int prev = 0;
      for (int j = 0; j < ls; ++j) {
        z[j] = rng->next() < 0.5;
        if (j == last_pos) z[j] = true;  // break phrase at the end of the sentence
        if (z[j]) {
          const vector<int> p(line.begin() + prev, line.begin() + j + 1);
          phrases_.increment(p, p0(p), rng);
          //cerr << p << ": " << p0(p) << endl;
          prev = j + 1;
          gen_.increment(false, gen_p0_, rng);
          ++tc; // remove
        }
      }
      ++tc;
      gen_.increment(true, 1.0 - gen_p0_, rng); // end of utterance
    }
    cerr << "TC: " << tc << endl;
    cerr << "Initial LLH: " << llh() << endl;
    cerr << "Sampling...\n";
    cerr << gen_ << endl;
    for (int s = 1; s < samples; ++s) {
      cerr << '.';
      if (s % 10 == 0) {
        cerr << " [" << s;
        if (hyp_inf) ResampleHyperparameters(rng);
        cerr << " LLH=" << llh() << "]\n";
        vector<int> z(z_[0].size(), 0);
        //for (int j = 0; j < z.size(); ++j) z[j] = z_[0][j];
        //SegCorpus::Write(corpus_[0], z, d);
      }
      for (int i = 0; i < corpus_.size(); ++i) {
        const vector<int>& line = corpus_[i];
        const int ls = line.size();
        const int last_pos = ls - 1;
        vector<bool>& z = z_[i];
        int prev = 0;
        for (int j = 0; j < last_pos; ++j) { // don't resample last position
          int next = j+1;  while(!z[next]) { ++next; }
          const vector<int> p1p2(line.begin() + prev, line.begin() + next + 1);
          const vector<int> p1(line.begin() + prev, line.begin() + j + 1);
          const vector<int> p2(line.begin() + j + 1, line.begin() + next + 1);

          if (z[j]) {
            phrases_.decrement(p1, rng);
            phrases_.decrement(p2, rng);
            gen_.decrement(false, rng);
            gen_.decrement(false, rng);
          } else {
            phrases_.decrement(p1p2, rng);
            gen_.decrement(false, rng);
          }

          const double d1 = phrases_.prob(p1p2, p0(p1p2)) * gen_.prob(false, gen_p0_);
          double d2 = phrases_.prob(p1, p0(p1)) * gen_.prob(false, gen_p0_);
          phrases_.increment(p1, p0(p1), rng);
          gen_.increment(false, gen_p0_, rng);
          d2 *= phrases_.prob(p2, p0(p2)) * gen_.prob(false, gen_p0_);
          phrases_.decrement(p1, rng);
          gen_.decrement(false, rng);
          z[j] = rng->SelectSample(d1, d2);

          if (z[j]) {
            phrases_.increment(p1, p0(p1), rng);
            phrases_.increment(p2, p0(p2), rng);
            gen_.increment(false, gen_p0_, rng);
            gen_.increment(false, gen_p0_, rng);
            prev = j + 1;
          } else {
            phrases_.increment(p1p2, p0(p1p2), rng);
            gen_.increment(false, gen_p0_, rng);
          }
        }
      }
    }
//    cerr << endl << endl << gen_ << endl << phrases_ << endl;
    cerr << gen_.prob(false, gen_p0_) << " " << gen_.prob(true, 1 - gen_p0_) << endl;
  }

  void WriteCdecGrammarForCurrentSample(ostream* os) const {
    CCRP<vector<int> >::const_iterator it = phrases_.begin();
    for (; it != phrases_.end(); ++it) {
      (*os) << "[X] ||| " << Join(' ', it->first) << " ||| "
                          << Join('_', it->first) << " ||| C=1 P="
                          << log(phrases_.prob(it->first, p0(it->first))) << endl;
    }
  }

  double OOVUnigramLogProb() const {
    vector<int> x(1,99999999);
    return log(phrases_.prob(x, p0(x)));
  }

  void ResampleHyperparameters(MT19937* rng) {
    phrases_.resample_hyperparameters(rng);
    gen_.resample_hyperparameters(rng);
    cerr << " d=" << phrases_.discount() << ",c=" << phrases_.concentration();
  }

  CCRP<vector<int> > phrases_;
  CCRP<bool> gen_;
  vector<vector<bool> > z_;   // z_[i] is there a phrase boundary after the ith word
  const vector<vector<int> >& corpus_;
  const double uniform_word_;
  const double gen_p0_;
  const double p_end_; // in base length distribution, p of the end of a phrase
  const bool use_poisson_;
};

int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  shared_ptr<MT19937> prng;
  if (conf.count("random_seed"))
    prng.reset(new MT19937(conf["random_seed"].as<uint32_t>()));
  else
    prng.reset(new MT19937);
  MT19937& rng = *prng;

  vector<vector<int> > corpus;
  set<int> vocab;
  ReadCorpus(conf["input"].as<string>(), &corpus, &vocab);
  cerr << "Corpus size: " << corpus.size() << " sentences\n";
  cerr << "Vocabulary size: " << vocab.size() << " types\n";

  UniphraseLM ulm(corpus, vocab, conf);
  ulm.Sample(conf["samples"].as<unsigned>(), conf.count("no_hyperparameter_inference") == 0, &rng);
  cerr << "OOV unigram prob: " << ulm.OOVUnigramLogProb() << endl;

  for (int i = 0; i < corpus.size(); ++i)
//    SegCorpus::Write(corpus[i], shmmlm.z_[i], d);
 ;
  if (conf.count("write_cdec_grammar")) {
    string fname = conf["write_cdec_grammar"].as<string>();
    cerr << "Writing model to " << fname << " ...\n";
    WriteFile wf(fname);
    ulm.WriteCdecGrammarForCurrentSample(wf.stream());
  }

  if (conf.count("write_cdec_weights")) {
    string fname = conf["write_cdec_weights"].as<string>();
    cerr << "Writing weights to " << fname << " .\n";
    WriteFile wf(fname);
    ostream& os = *wf.stream();
    os << "# make C smaller to use more phrases\nP 1\nPassThrough " << ulm.OOVUnigramLogProb() << "\nC -3\n";
  }

  return 0;
}


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2009 Trustees of Indiana University.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek, Michael Hansen
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#ifndef BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP
#define BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP

#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/relax.hpp>
#include <boost/pending/relaxed_heap.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost {

  // No init version
  template <typename Graph, typename DijkstraVisitor,
            typename PredecessorMap, typename DistanceMap,
            typename WeightMap, typename VertexIndexMap,
            typename DistanceCompare, typename DistanceWeightCombine,
            typename DistanceInfinity, typename DistanceZero>
  void dijkstra_shortest_paths_no_color_map_no_init
    (const Graph& graph,
     typename graph_traits<Graph>::vertex_descriptor start_vertex,
     PredecessorMap predecessor_map,
     DistanceMap distance_map,
     WeightMap weight_map,
     VertexIndexMap index_map,
     DistanceCompare distance_compare,
     DistanceWeightCombine distance_weight_combine,
     DistanceInfinity distance_infinity,
     DistanceZero distance_zero,
     DijkstraVisitor visitor)
  {
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename property_traits<DistanceMap>::value_type Distance;
    typedef typename property_traits<WeightMap>::value_type Weight;

    typedef indirect_cmp<DistanceMap, DistanceCompare> DistanceIndirectCompare;
    DistanceIndirectCompare
      distance_indirect_compare(distance_map, distance_compare);

    // Choose vertex queue type
#if BOOST_GRAPH_DIJKSTRA_USE_RELAXED_HEAP
    typedef relaxed_heap<Vertex, DistanceIndirectCompare, VertexIndexMap>
      VertexQueue;
    VertexQueue vertex_queue(num_vertices(graph),
                             distance_indirect_compare,
                             index_map);
#else
    // Default - use d-ary heap (d = 4)
    typedef
      detail::vertex_property_map_generator<Graph, VertexIndexMap, std::size_t>
      IndexInHeapMapHelper;
    typedef typename IndexInHeapMapHelper::type IndexInHeapMap;
    typedef
      d_ary_heap_indirect<Vertex, 4, IndexInHeapMap, DistanceMap, DistanceCompare>
      VertexQueue;

    boost::scoped_array<std::size_t> index_in_heap_map_holder;
    IndexInHeapMap index_in_heap =
      IndexInHeapMapHelper::build(graph, index_map,
                                  index_in_heap_map_holder);
    VertexQueue vertex_queue(distance_map, index_in_heap, distance_compare);
#endif

    // Add vertex to the queue
    vertex_queue.push(start_vertex);

    // Starting vertex will always be the first discovered vertex
    visitor.discover_vertex(start_vertex, graph);

    while (!vertex_queue.empty()) {
      Vertex min_vertex = vertex_queue.top();
      vertex_queue.pop();

      visitor.examine_vertex(min_vertex, graph);

      // Check if any other vertices can be reached
      Distance min_vertex_distance = get(distance_map, min_vertex);

      if (!distance_compare(min_vertex_distance, distance_infinity)) {
        // This is the minimum vertex, so all other vertices are unreachable
        return;
      }

      // Examine neighbors of min_vertex
      typedef typename graph_traits<Graph>::edge_descriptor Edge;
      BGL_FORALL_OUTEDGES_T(min_vertex, current_edge, graph, Graph) {
        visitor.examine_edge(current_edge, graph);

        // Check if the edge has a negative weight
        if (distance_compare(get(weight_map, current_edge), distance_zero)) {
          boost::throw_exception(negative_edge());
        }

        // Extract the neighboring vertex and get its distance
        Vertex neighbor_vertex = target(current_edge, graph);
        Distance neighbor_vertex_distance = get(distance_map, neighbor_vertex);
        bool is_neighbor_undiscovered =
          !distance_compare(neighbor_vertex_distance, distance_infinity);

        // Attempt to relax the edge
        bool was_edge_relaxed = relax(current_edge, graph, weight_map,
          predecessor_map, distance_map,
          distance_weight_combine, distance_compare);

        if (was_edge_relaxed) {
          vertex_queue.update(neighbor_vertex);
          visitor.edge_relaxed(current_edge, graph);
        } else {
          visitor.edge_not_relaxed(current_edge, graph);
        }

        if (is_neighbor_undiscovered) {
          visitor.discover_vertex(neighbor_vertex, graph);
          vertex_queue.push(neighbor_vertex);
        }
      } // end out edge iteration

      visitor.finish_vertex(min_vertex, graph);
    } // end while queue not empty
  }

  // Full init version
  template <typename Graph, typename DijkstraVisitor,
            typename PredecessorMap, typename DistanceMap,
            typename WeightMap, typename VertexIndexMap,
            typename DistanceCompare, typename DistanceWeightCombine,
            typename DistanceInfinity, typename DistanceZero>
  void dijkstra_shortest_paths_no_color_map
    (const Graph& graph,
     typename graph_traits<Graph>::vertex_descriptor start_vertex,
     PredecessorMap predecessor_map,
     DistanceMap distance_map,
     WeightMap weight_map,
     VertexIndexMap index_map,
     DistanceCompare distance_compare,
     DistanceWeightCombine distance_weight_combine,
     DistanceInfinity distance_infinity,
     DistanceZero distance_zero,
     DijkstraVisitor visitor)
  {
    // Initialize vertices
    BGL_FORALL_VERTICES_T(current_vertex, graph, Graph) {
      visitor.initialize_vertex(current_vertex, graph);

      // Default all distances to infinity
      put(distance_map, current_vertex, distance_infinity);

      // Default all vertex predecessors to the vertex itself
      put(predecessor_map, current_vertex, current_vertex);
    }

    // Set distance for start_vertex to zero
    put(distance_map, start_vertex, distance_zero);

    // Pass everything on to the no_init version
    dijkstra_shortest_paths_no_color_map_no_init(graph,
      start_vertex, predecessor_map, distance_map, weight_map,
      index_map, distance_compare, distance_weight_combine,
      distance_infinity, distance_zero, visitor);
  }

  namespace detail {

    // Handle defaults for PredecessorMap, DistanceCompare,
    // DistanceWeightCombine, DistanceInfinity and DistanceZero
    template <typename Graph, typename DistanceMap, typename WeightMap,
              typename VertexIndexMap, typename Params>
    inline void
    dijkstra_no_color_map_dispatch2
      (const Graph& graph,
       typename graph_traits<Graph>::vertex_descriptor start_vertex,
       DistanceMap distance_map, WeightMap weight_map,
       VertexIndexMap index_map, const Params& params)
    {
      // Default for predecessor map
      dummy_property_map predecessor_map;

      typedef typename property_traits<DistanceMap>::value_type DistanceType;
      dijkstra_shortest_paths_no_color_map
        (graph, start_vertex,
         choose_param(get_param(params, vertex_predecessor), predecessor_map),
         distance_map, weight_map, index_map,
         choose_param(get_param(params, distance_compare_t()),
                      std::less<DistanceType>()),
         choose_param(get_param(params, distance_combine_t()),
                      closed_plus<DistanceType>()),
         choose_param(get_param(params, distance_inf_t()),
                      (std::numeric_limits<DistanceType>::max)()),
         choose_param(get_param(params, distance_zero_t()),
                      DistanceType()),
         choose_param(get_param(params, graph_visitor),
                      make_dijkstra_visitor(null_visitor())));
    }

    template <typename Graph, typename DistanceMap, typename WeightMap,
              typename IndexMap, typename Params>
    inline void
    dijkstra_no_color_map_dispatch1
      (const Graph& graph,
       typename graph_traits<Graph>::vertex_descriptor start_vertex,
       DistanceMap distance_map, WeightMap weight_map,
       IndexMap index_map, const Params& params)
    {
      // Default for distance map
      typedef typename property_traits<WeightMap>::value_type DistanceType;
      typename std::vector<DistanceType>::size_type
        vertex_count = is_default_param(distance_map) ? num_vertices(graph) : 1;

      std::vector<DistanceType> default_distance_map(vertex_count);

      detail::dijkstra_no_color_map_dispatch2
        (graph, start_vertex, choose_param(distance_map,
         make_iterator_property_map(default_distance_map.begin(), index_map,
                                    default_distance_map[0])),
         weight_map, index_map, params);
    }
  } // namespace detail

  // Named parameter version
  template <typename Graph, typename Param, typename Tag, typename Rest>
  inline void
  dijkstra_shortest_paths_no_color_map
    (const Graph& graph,
     typename graph_traits<Graph>::vertex_descriptor start_vertex,
     const bgl_named_params<Param, Tag, Rest>& params)
  {
    // Default for edge weight and vertex index map is to ask for them
    // from the graph. Default for the visitor is null_visitor.
    detail::dijkstra_no_color_map_dispatch1
      (graph, start_vertex,
       get_param(params, vertex_distance),
       choose_const_pmap(get_param(params, edge_weight), graph, edge_weight),
       choose_const_pmap(get_param(params, vertex_index), graph, vertex_index),
       params);
  }

} // namespace boost

#endif // BOOST_GRAPH_DIJKSTRA_NO_COLOR_MAP_HPP