/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#ifdef BOOST_TEST_MAIN
#  undef BOOST_TEST_MAIN
#endif
#include <mc_control/api.h>
#include <mc_control/mc_controller.h>
#include <mc_rtc/logging.h>
#include <mc_tasks/CoMTask.h>

#include <boost/test/unit_test.hpp>

namespace mc_control
{

struct MC_CONTROL_DLLAPI TestObserverController : public MCController
{
public:
  TestObserverController(std::shared_ptr<mc_rbdyn::RobotModule> rm, double dt) : MCController(rm, dt)
  {
    // Check that the default constructor loads the robot + ground environment
    BOOST_CHECK_EQUAL(robots().robots().size(), 2);
    // Check that JVRC-1 was loaded
    BOOST_CHECK_EQUAL(robot().name(), "jvrc1");
    solver().addConstraintSet(contactConstraint);
    solver().addConstraintSet(dynamicsConstraint);
    postureTask->stiffness(1);
    postureTask->weight(1);
    solver().addTask(postureTask.get());
    solver().setContacts(
        {mc_rbdyn::Contact(robots(), "LeftFoot", "AllGround"), mc_rbdyn::Contact(robots(), "RightFoot", "AllGround")});

    /* Create and add the CoM task with the default stiffness/weight */
    comTask = std::make_shared<mc_tasks::CoMTask>(robots(), 0);
    comTask->stiffness(10);
    solver().addTask(comTask);

    LOG_SUCCESS("Created TestObserverController")
  }

  virtual bool run() override
  {
    bool ret = MCController::run();
    BOOST_CHECK(ret);
    nrIter++;
    if(nrIter == 1000)
    {
      /* Check that the task is "finished" */
      BOOST_CHECK_SMALL(comTask->eval().norm(), 1e-2);
      BOOST_CHECK_SMALL(comTask->speed().norm(), 1e-3);

      /* Apply dimWeight and give a "crazy" position target */
      comTask->dimWeight(Eigen::Vector3d(1., 1., 0.));
      comTask->move_com(Eigen::Vector3d(0., 0., 100.));
      postureTask->posture(robot().mbc().q);
    }
    if(nrIter == 2000)
    {
      BOOST_CHECK_SMALL(comTask->eval().norm(), 1e-2);
      BOOST_CHECK_SMALL(comTask->speed().norm(), 1e-3);

      /* Raise the CoM again, using only the leg joints */
      comTask->reset();
      comTask->dimWeight(Eigen::Vector3d(1., 1., 1.));
      comTask->selectActiveJoints(solver(), active_joints);
      comTask->com(comTask->com() + Eigen::Vector3d(0., 0., 0.05));
    }
    if(nrIter == 3000)
    {
      /* Check that the task is "finished" */
      BOOST_CHECK_SMALL(comTask->eval().norm(), 2e-2);
      BOOST_CHECK_SMALL(comTask->speed().norm(), 1e-2);

      /* Lower the CoM, forbid right knee movement in all tasks */
      comTask->reset();
      comTask->selectUnactiveJoints(solver(), {"R_KNEE"});
      orig_rkj = robot().mbc().q[robot().jointIndexByName("R_KNEE")][0];
      comTask->com(comTask->com() + Eigen::Vector3d(0., 0., -0.05));

      /* Also reset the joint target in posture task */
      postureTask->reset();
      postureTask->jointStiffness(solver(), {{"R_KNEE", 1e5}});
    }
    if(nrIter == 4000)
    {
      /* Check that the task is "finished" */
      BOOST_CHECK_SMALL(comTask->speed().norm(), 1e-2);

      /* And that RLEG_JOINT3 didn't move. Note that the error is not so
       * small because of other tasks' interaction */
      double current_rkj = robot().mbc().q[robot().jointIndexByName("R_KNEE")][0];
      BOOST_CHECK_SMALL(fabs(orig_rkj - current_rkj), 1e-2);
    }
    return ret;
  }

  virtual void reset(const ControllerResetData & reset_data) override
  {
    MCController::reset(reset_data);
    comTask->reset();
    /* Lower the CoM */
    comTask->com(comTask->com() + Eigen::Vector3d(0., 0., -0.05));

    // clang-format off
    std::vector<std::string> enabledObservers = { @ENABLED_OBSERVERS@ };
    std::vector<std::string> runObservers = { @RUN_OBSERVERS@ };
    std::vector<std::string> updateObservers = { @UPDATE_OBSERVERS@ };
    // clang-format on

    auto hasObserver = [this](const std::string & name) {
      for(const auto & obs : observers_)
      {
        if(obs->name() == name)
        {
          return true;
        }
      }
      return false;
    };
    auto hasPipelineObserver = [this](const std::string & name, bool update) {
      for(const auto & obs : pipelineObservers_)
      {
        if(obs.first->name() == name)
        {
          return update ? obs.second : true;
        }
      }
      return false;
    };
    for(const auto & enabled : enabledObservers)
    {
      BOOST_REQUIRE(hasObserver(enabled));
    }
    for(const auto & run : runObservers)
    {
      BOOST_REQUIRE(hasPipelineObserver(run, false));
    }
    for(const auto & update : updateObservers)
    {
      BOOST_REQUIRE(hasPipelineObserver(update, false));
    }
  }

private:
  unsigned int nrIter = 0;
  std::shared_ptr<mc_tasks::CoMTask> comTask = nullptr;
  std::vector<std::string> active_joints = {"Root",      "R_HIP_P",   "R_HIP_R",  "R_HIP_Y", "R_KNEE",
                                            "R_ANKLE_R", "R_ANKLE_P", "L_HIP_P",  "L_HIP_R", "L_HIP_Y",
                                            "L_KNEE",    "L_ANKLE_R", "L_ANKLE_P"};
  double orig_rkj = 0;
};

} // namespace mc_control

SIMPLE_CONTROLLER_CONSTRUCTOR("TestObserverController", mc_control::TestObserverController)


/**
 * @file rl_environment_test.hpp
 * @author Shangtong Zhang
 * @author Rohan Raj
 *
 * Basic test for the components of reinforcement learning algorithms.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core.hpp>

#include <mlpack/methods/reinforcement_learning/environment/mountain_car.hpp>
#include <mlpack/methods/reinforcement_learning/environment/continuous_mountain_car.hpp>
#include <mlpack/methods/reinforcement_learning/environment/cart_pole.hpp>
#include <mlpack/methods/reinforcement_learning/environment/double_pole_cart.hpp>
#include <mlpack/methods/reinforcement_learning/environment/continuous_double_pole_cart.hpp>
#include <mlpack/methods/reinforcement_learning/environment/acrobot.hpp>
#include <mlpack/methods/reinforcement_learning/environment/pendulum.hpp>
#include <mlpack/methods/reinforcement_learning/replay/random_replay.hpp>
#include <mlpack/methods/reinforcement_learning/policy/greedy_policy.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::rl;

BOOST_AUTO_TEST_SUITE(RLComponentsTest)

/**
 * Constructs a Pendulum instance and check if the main routine works as it
 * should be working.
 */
BOOST_AUTO_TEST_CASE(SimplePendulumTest)
{
  Pendulum task = Pendulum();
  task.MaxSteps() = 5;

  Pendulum::State state = task.InitialSample();
  Pendulum::Action action;
  action.action[0] = math::Random(-2.0, 2.0);
  double reward = task.Sample(state, action);

  // The reward is always negative. Check if not lower than lowest possible.
  BOOST_REQUIRE(reward >= -(pow(M_PI, 2) + 6.404));

  BOOST_REQUIRE(!task.IsTerminal(state));

  while (!task.IsTerminal(state))
    task.Sample(state, action, state);

  // Check if the number of steps performed is less or equal as the maximum
  // allowed, since we use a random action there is no guarantee that we will
  // reach the maximum number of steps.
  BOOST_REQUIRE_LE(task.StepsPerformed(), 5);

  // The action is simply the torque. Check if dimension is 1.
  BOOST_REQUIRE_EQUAL(1, action.size);
}

/**
 * Constructs a Continuous MountainCar instance and check if the main rountine
 * works as it should be.
 */
BOOST_AUTO_TEST_CASE(SimpleContinuousMountainCarTest)
{
  ContinuousMountainCar task = ContinuousMountainCar();
  task.MaxSteps() = 5;

  ContinuousMountainCar::State state = task.InitialSample();
  ContinuousMountainCar::Action action;
  action.action[0] = math::Random(-1.0, 1.0);
  double reward = task.Sample(state, action);
  // Maximum reward possible is 100.
  BOOST_REQUIRE(reward <= 100.0);
  BOOST_REQUIRE(!task.IsTerminal(state));

  while (!task.IsTerminal(state))
    task.Sample(state, action, state);

  // Check if the number of steps performed is the same as the maximum allowed.
  BOOST_REQUIRE_EQUAL(task.StepsPerformed(), 5);

  // Check if the size of the action space is 1.
  BOOST_REQUIRE_EQUAL(1, action.size);
}

/**
 * Constructs a Acrobot instance and check if the main rountine works as
 * it should be.
 */
BOOST_AUTO_TEST_CASE(SimpleAcrobotTest)
{
  Acrobot task = Acrobot();
  task.MaxSteps() = 5;

  Acrobot::State state = task.InitialSample();
  Acrobot::Action action = Acrobot::Action::negativeTorque;
  double reward = task.Sample(state, action);

  BOOST_REQUIRE_EQUAL(reward, -1.0);
  BOOST_REQUIRE(!task.IsTerminal(state));

  while (!task.IsTerminal(state))
    task.Sample(state, action, state);

  // Check if the number of steps performed is the same as the maximum allowed.
  BOOST_REQUIRE_EQUAL(task.StepsPerformed(), 5);

  // Check if the size of the action space is 3.
  BOOST_REQUIRE_EQUAL(3, Acrobot::Action::size);
}

/**
 * Constructs a MountainCar instance and check if the main rountine works as
 * it should be.
 */
BOOST_AUTO_TEST_CASE(SimpleMountainCarTest)
{
  MountainCar task = MountainCar();
  task.MaxSteps() = 5;

  MountainCar::State state = task.InitialSample();
  MountainCar::Action action = MountainCar::Action::backward;
  double reward = task.Sample(state, action);

  BOOST_REQUIRE_EQUAL(reward, -1.0);
  BOOST_REQUIRE(!task.IsTerminal(state));

  while (!task.IsTerminal(state))
    task.Sample(state, action, state);

  // Check if the number of steps performed is the same as the maximum allowed.
  BOOST_REQUIRE_EQUAL(task.StepsPerformed(), 5);

  // Check if the size of the action space is 3.
  BOOST_REQUIRE_EQUAL(3, MountainCar::Action::size);
}

/**
 * Constructs a CartPole instance and check if the main routine works as
 * it should be.
 */
BOOST_AUTO_TEST_CASE(SimpleCartPoleTest)
{
  CartPole task = CartPole();
  task.MaxSteps() = 5;

  CartPole::State state = task.InitialSample();
  CartPole::Action action = CartPole::Action::backward;
  double reward = task.Sample(state, action);

  BOOST_REQUIRE_EQUAL(reward, 1.0);
  BOOST_REQUIRE(!task.IsTerminal(state));

  while (!task.IsTerminal(state))
    task.Sample(state, action, state);

  // Check if the number of steps performed is the same as the maximum allowed.
  BOOST_REQUIRE_EQUAL(task.StepsPerformed(), 5);

  BOOST_REQUIRE_EQUAL(2, CartPole::Action::size);
}

/**
 * Constructs a DoublePoleCart instance and check if the main routine works as
 * it should be.
 */
BOOST_AUTO_TEST_CASE(DoublePoleCartTest)
{
  DoublePoleCart task = DoublePoleCart();
  task.MaxSteps() = 5;

  DoublePoleCart::State state = task.InitialSample();
  DoublePoleCart::Action action = DoublePoleCart::Action::backward;
  double reward = task.Sample(state, action);

  BOOST_REQUIRE_EQUAL(reward, 1.0);
  BOOST_REQUIRE(!task.IsTerminal(state));

  while (!task.IsTerminal(state))
    task.Sample(state, action, state);

  // Check if the number of steps performed is the same as the maximum allowed.
  BOOST_REQUIRE_EQUAL(task.StepsPerformed(), 5);
  BOOST_REQUIRE_EQUAL(2, DoublePoleCart::Action::size);
}

/**
 * Constructs a ContinuousDoublePoleCart instance and check if the main
 * routine works as it should be.
 */
BOOST_AUTO_TEST_CASE(ContinuousDoublePoleCartTest)
{
  ContinuousDoublePoleCart task = ContinuousDoublePoleCart();
  task.MaxSteps() = 5;

  ContinuousDoublePoleCart::State state = task.InitialSample();
  ContinuousDoublePoleCart::Action action;
  action.action[0] = math::Random(-1.0, 1.0);
  double reward = task.Sample(state, action);

  BOOST_REQUIRE_EQUAL(reward, 1.0);
  BOOST_REQUIRE(!task.IsTerminal(state));

  while (!task.IsTerminal(state))
    task.Sample(state, action, state);

  // Check if the number of steps performed is the same as the maximum allowed.
  BOOST_REQUIRE_EQUAL(task.StepsPerformed(), 5);
  BOOST_REQUIRE_EQUAL(1, action.size);
}

/**
 * Construct a random replay instance and check if it works as
 * it should be.
 */
BOOST_AUTO_TEST_CASE(RandomReplayTest)
{
  RandomReplay<MountainCar> replay(1, 3);
  MountainCar env;
  MountainCar::State state = env.InitialSample();
  MountainCar::Action action = MountainCar::Action::forward;
  MountainCar::State nextState;
  double reward = env.Sample(state, action, nextState);
  replay.Store(state, action, reward, nextState, env.IsTerminal(nextState));
  arma::mat sampledState;
  arma::icolvec sampledAction;
  arma::colvec sampledReward;
  arma::mat sampledNextState;
  arma::icolvec sampledTerminal;

  //! So far there should be only one record in the memory
  replay.Sample(sampledState, sampledAction, sampledReward, sampledNextState,
      sampledTerminal);

  CheckMatrices(state.Encode(), sampledState);
  BOOST_REQUIRE_EQUAL(action, arma::as_scalar(sampledAction));
  BOOST_REQUIRE_CLOSE(reward, arma::as_scalar(sampledReward), 1e-5);
  CheckMatrices(nextState.Encode(), sampledNextState);
  BOOST_REQUIRE_EQUAL(false, arma::as_scalar(sampledTerminal));
  BOOST_REQUIRE_EQUAL(1, replay.Size());

  //! Overwrite the memory with a nonsense record
  for (size_t i = 0; i < 5; ++i)
    replay.Store(nextState, action, reward, state, true);

  BOOST_REQUIRE_EQUAL(3, replay.Size());

  //! Sample several times, the original record shouldn't appear
  for (size_t i = 0; i < 30; ++i)
  {
    replay.Sample(sampledState, sampledAction, sampledReward, sampledNextState,
        sampledTerminal);

    CheckMatrices(state.Encode(), sampledNextState);
    CheckMatrices(nextState.Encode(), sampledState);
    BOOST_REQUIRE_EQUAL(true, arma::as_scalar(sampledTerminal));
  }
}

/**
 * Construct a greedy policy instance and check if it works as
 * it should be.
 */
BOOST_AUTO_TEST_CASE(GreedyPolicyTest)
{
  GreedyPolicy<CartPole> policy(1.0, 10, 0.0, 0.99);
  for (size_t i = 0; i < 15; ++i)
    policy.Anneal();
  BOOST_REQUIRE_CLOSE(0.0, policy.Epsilon(), 1e-5);
  arma::colvec actionValue = arma::randn<arma::colvec>(CartPole::Action::size);
  CartPole::Action action = policy.Sample(actionValue);
  BOOST_REQUIRE_CLOSE(actionValue[action], actionValue.max(), 1e-5);
}

BOOST_AUTO_TEST_SUITE_END()


//
//=======================================================================
// Copyright (c) 2004 Kristopher Beevers
//
// 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_ASTAR_SEARCH_HPP
#define BOOST_GRAPH_ASTAR_SEARCH_HPP

#include <functional>
#include <vector>
#include <boost/limits.hpp>
#include <boost/throw_exception.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/property_maps/constant_property_map.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/concept/assert.hpp>

namespace boost
{

template < class Heuristic, class Graph > struct AStarHeuristicConcept
{
    void constraints()
    {
        BOOST_CONCEPT_ASSERT((CopyConstructibleConcept< Heuristic >));
        h(u);
    }
    Heuristic h;
    typename graph_traits< Graph >::vertex_descriptor u;
};

template < class Graph, class CostType > class astar_heuristic
{
public:
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef Vertex argument_type;
    typedef CostType result_type;
    astar_heuristic() {}
    CostType operator()(Vertex u) { return static_cast< CostType >(0); }
};

template < class Visitor, class Graph > struct AStarVisitorConcept
{
    void constraints()
    {
        BOOST_CONCEPT_ASSERT((CopyConstructibleConcept< Visitor >));
        vis.initialize_vertex(u, g);
        vis.discover_vertex(u, g);
        vis.examine_vertex(u, g);
        vis.examine_edge(e, g);
        vis.edge_relaxed(e, g);
        vis.edge_not_relaxed(e, g);
        vis.black_target(e, g);
        vis.finish_vertex(u, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits< Graph >::vertex_descriptor u;
    typename graph_traits< Graph >::edge_descriptor e;
};

template < class Visitors = null_visitor >
class astar_visitor : public bfs_visitor< Visitors >
{
public:
    astar_visitor() {}
    astar_visitor(Visitors vis) : bfs_visitor< Visitors >(vis) {}

    template < class Edge, class Graph >
    void edge_relaxed(Edge e, const Graph& g)
    {
        invoke_visitors(this->m_vis, e, g, on_edge_relaxed());
    }
    template < class Edge, class Graph >
    void edge_not_relaxed(Edge e, const Graph& g)
    {
        invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed());
    }

private:
    template < class Edge, class Graph > void tree_edge(Edge e, const Graph& g)
    {
    }
    template < class Edge, class Graph >
    void non_tree_edge(Edge e, const Graph& g)
    {
    }
};
template < class Visitors >
astar_visitor< Visitors > make_astar_visitor(Visitors vis)
{
    return astar_visitor< Visitors >(vis);
}
typedef astar_visitor<> default_astar_visitor;

namespace detail
{

    template < class AStarHeuristic, class UniformCostVisitor,
        class UpdatableQueue, class PredecessorMap, class CostMap,
        class DistanceMap, class WeightMap, class ColorMap,
        class BinaryFunction, class BinaryPredicate >
    struct astar_bfs_visitor
    {

        typedef typename property_traits< CostMap >::value_type C;
        typedef typename property_traits< ColorMap >::value_type ColorValue;
        typedef color_traits< ColorValue > Color;
        typedef
            typename property_traits< DistanceMap >::value_type distance_type;

        astar_bfs_visitor(AStarHeuristic h, UniformCostVisitor vis,
            UpdatableQueue& Q, PredecessorMap p, CostMap c, DistanceMap d,
            WeightMap w, ColorMap col, BinaryFunction combine,
            BinaryPredicate compare, C zero)
        : m_h(h)
        , m_vis(vis)
        , m_Q(Q)
        , m_predecessor(p)
        , m_cost(c)
        , m_distance(d)
        , m_weight(w)
        , m_color(col)
        , m_combine(combine)
        , m_compare(compare)
        , m_zero(zero)
        {
        }

        template < class Vertex, class Graph >
        void initialize_vertex(Vertex u, const Graph& g)
        {
            m_vis.initialize_vertex(u, g);
        }
        template < class Vertex, class Graph >
        void discover_vertex(Vertex u, const Graph& g)
        {
            m_vis.discover_vertex(u, g);
        }
        template < class Vertex, class Graph >
        void examine_vertex(Vertex u, const Graph& g)
        {
            m_vis.examine_vertex(u, g);
        }
        template < class Vertex, class Graph >
        void finish_vertex(Vertex u, const Graph& g)
        {
            m_vis.finish_vertex(u, g);
        }
        template < class Edge, class Graph >
        void examine_edge(Edge e, const Graph& g)
        {
            if (m_compare(get(m_weight, e), m_zero))
                BOOST_THROW_EXCEPTION(negative_edge());
            m_vis.examine_edge(e, g);
        }
        template < class Edge, class Graph >
        void non_tree_edge(Edge, const Graph&)
        {
        }

        template < class Edge, class Graph >
        void tree_edge(Edge e, const Graph& g)
        {
            using boost::get;
            bool m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                m_combine, m_compare);

            if (m_decreased)
            {
                m_vis.edge_relaxed(e, g);
                put(m_cost, target(e, g),
                    m_combine(
                        get(m_distance, target(e, g)), m_h(target(e, g))));
            }
            else
                m_vis.edge_not_relaxed(e, g);
        }

        template < class Edge, class Graph >
        void gray_target(Edge e, const Graph& g)
        {
            using boost::get;
            bool m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                m_combine, m_compare);

            if (m_decreased)
            {
                put(m_cost, target(e, g),
                    m_combine(
                        get(m_distance, target(e, g)), m_h(target(e, g))));
                m_Q.update(target(e, g));
                m_vis.edge_relaxed(e, g);
            }
            else
                m_vis.edge_not_relaxed(e, g);
        }

        template < class Edge, class Graph >
        void black_target(Edge e, const Graph& g)
        {
            using boost::get;
            bool m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                m_combine, m_compare);

            if (m_decreased)
            {
                m_vis.edge_relaxed(e, g);
                put(m_cost, target(e, g),
                    m_combine(
                        get(m_distance, target(e, g)), m_h(target(e, g))));
                m_Q.push(target(e, g));
                put(m_color, target(e, g), Color::gray());
                m_vis.black_target(e, g);
            }
            else
                m_vis.edge_not_relaxed(e, g);
        }

        AStarHeuristic m_h;
        UniformCostVisitor m_vis;
        UpdatableQueue& m_Q;
        PredecessorMap m_predecessor;
        CostMap m_cost;
        DistanceMap m_distance;
        WeightMap m_weight;
        ColorMap m_color;
        BinaryFunction m_combine;
        BinaryPredicate m_compare;
        C m_zero;
    };

} // namespace detail

template < typename VertexListGraph, typename AStarHeuristic,
    typename AStarVisitor, typename PredecessorMap, typename CostMap,
    typename DistanceMap, typename WeightMap, typename ColorMap,
    typename VertexIndexMap, typename CompareFunction, typename CombineFunction,
    typename CostInf, typename CostZero >
inline void astar_search_no_init(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, AStarVisitor vis, PredecessorMap predecessor,
    CostMap cost, DistanceMap distance, WeightMap weight, ColorMap color,
    VertexIndexMap index_map, CompareFunction compare, CombineFunction combine,
    CostInf /*inf*/, CostZero zero)
{
    typedef typename graph_traits< VertexListGraph >::vertex_descriptor Vertex;
    typedef boost::vector_property_map< std::size_t, VertexIndexMap >
        IndexInHeapMap;
    IndexInHeapMap index_in_heap(index_map);
    typedef d_ary_heap_indirect< Vertex, 4, IndexInHeapMap, CostMap,
        CompareFunction >
        MutableQueue;
    MutableQueue Q(cost, index_in_heap, compare);

    detail::astar_bfs_visitor< AStarHeuristic, AStarVisitor, MutableQueue,
        PredecessorMap, CostMap, DistanceMap, WeightMap, ColorMap,
        CombineFunction, CompareFunction >
        bfs_vis(h, vis, Q, predecessor, cost, distance, weight, color, combine,
            compare, zero);

    breadth_first_visit(g, s, Q, bfs_vis, color);
}

namespace graph_detail
{
    template < typename A, typename B > struct select1st
    {
        typedef std::pair< A, B > argument_type;
        typedef A result_type;
        A operator()(const std::pair< A, B >& p) const { return p.first; }
    };
}

template < typename VertexListGraph, typename AStarHeuristic,
    typename AStarVisitor, typename PredecessorMap, typename CostMap,
    typename DistanceMap, typename WeightMap, typename CompareFunction,
    typename CombineFunction, typename CostInf, typename CostZero >
inline void astar_search_no_init_tree(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, AStarVisitor vis, PredecessorMap predecessor,
    CostMap cost, DistanceMap distance, WeightMap weight,
    CompareFunction compare, CombineFunction combine, CostInf /*inf*/,
    CostZero zero)
{
    typedef typename graph_traits< VertexListGraph >::vertex_descriptor Vertex;
    typedef typename property_traits< DistanceMap >::value_type Distance;
    typedef d_ary_heap_indirect< std::pair< Distance, Vertex >, 4,
        null_property_map< std::pair< Distance, Vertex >, std::size_t >,
        function_property_map< graph_detail::select1st< Distance, Vertex >,
            std::pair< Distance, Vertex > >,
        CompareFunction >
        MutableQueue;
    MutableQueue Q(make_function_property_map< std::pair< Distance, Vertex > >(
                       graph_detail::select1st< Distance, Vertex >()),
        null_property_map< std::pair< Distance, Vertex >, std::size_t >(),
        compare);

    vis.discover_vertex(s, g);
    Q.push(std::make_pair(get(cost, s), s));
    while (!Q.empty())
    {
        Vertex v;
        Distance v_rank;
        boost::tie(v_rank, v) = Q.top();
        Q.pop();
        vis.examine_vertex(v, g);
        BGL_FORALL_OUTEDGES_T(v, e, g, VertexListGraph)
        {
            Vertex w = target(e, g);
            vis.examine_edge(e, g);
            Distance e_weight = get(weight, e);
            if (compare(e_weight, zero))
                BOOST_THROW_EXCEPTION(negative_edge());
            bool decreased
                = relax(e, g, weight, predecessor, distance, combine, compare);
            combine(get(distance, v), e_weight);
            if (decreased)
            {
                vis.edge_relaxed(e, g);
                Distance w_rank = combine(get(distance, w), h(w));
                put(cost, w, w_rank);
                vis.discover_vertex(w, g);
                Q.push(std::make_pair(w_rank, w));
            }
            else
            {
                vis.edge_not_relaxed(e, g);
            }
        }
        vis.finish_vertex(v, g);
    }
}

// Non-named parameter interface
template < typename VertexListGraph, typename AStarHeuristic,
    typename AStarVisitor, typename PredecessorMap, typename CostMap,
    typename DistanceMap, typename WeightMap, typename VertexIndexMap,
    typename ColorMap, typename CompareFunction, typename CombineFunction,
    typename CostInf, typename CostZero >
inline void astar_search(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, AStarVisitor vis, PredecessorMap predecessor,
    CostMap cost, DistanceMap distance, WeightMap weight,
    VertexIndexMap index_map, ColorMap color, CompareFunction compare,
    CombineFunction combine, CostInf inf, CostZero zero)
{

    typedef typename property_traits< ColorMap >::value_type ColorValue;
    typedef color_traits< ColorValue > Color;
    typename graph_traits< VertexListGraph >::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
    {
        put(color, *ui, Color::white());
        put(distance, *ui, inf);
        put(cost, *ui, inf);
        put(predecessor, *ui, *ui);
        vis.initialize_vertex(*ui, g);
    }
    put(distance, s, zero);
    put(cost, s, h(s));

    astar_search_no_init(g, s, h, vis, predecessor, cost, distance, weight,
        color, index_map, compare, combine, inf, zero);
}

// Non-named parameter interface
template < typename VertexListGraph, typename AStarHeuristic,
    typename AStarVisitor, typename PredecessorMap, typename CostMap,
    typename DistanceMap, typename WeightMap, typename CompareFunction,
    typename CombineFunction, typename CostInf, typename CostZero >
inline void astar_search_tree(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, AStarVisitor vis, PredecessorMap predecessor,
    CostMap cost, DistanceMap distance, WeightMap weight,
    CompareFunction compare, CombineFunction combine, CostInf inf,
    CostZero zero)
{

    typename graph_traits< VertexListGraph >::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
    {
        put(distance, *ui, inf);
        put(cost, *ui, inf);
        put(predecessor, *ui, *ui);
        vis.initialize_vertex(*ui, g);
    }
    put(distance, s, zero);
    put(cost, s, h(s));

    astar_search_no_init_tree(g, s, h, vis, predecessor, cost, distance, weight,
        compare, combine, inf, zero);
}

// Named parameter interfaces
template < typename VertexListGraph, typename AStarHeuristic, typename P,
    typename T, typename R >
void astar_search(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, const bgl_named_params< P, T, R >& params)
{
    using namespace boost::graph::keywords;
    typedef bgl_named_params< P, T, R > params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)

    // Distance type is the value type of the distance map if there is one,
    // otherwise the value type of the weight map.
    typedef
        typename boost::detail::override_const_property_result< arg_pack_type,
            boost::graph::keywords::tag::weight_map, edge_weight_t,
            VertexListGraph >::type weight_map_type;
    typedef typename boost::property_traits< weight_map_type >::value_type D;
    const D inf = arg_pack[_distance_inf || detail::get_max< D >()];
    const D zero_actual = D();
    const D zero_d = arg_pack[_distance_zero | zero_actual];
    null_visitor null_vis;
    astar_visitor< null_visitor > default_visitor(null_vis);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::visitor, dummy_property_map& >::type vis
        = arg_pack[_visitor | default_visitor];
    dummy_property_map dummy_prop;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::predecessor_map,
        dummy_property_map& >::type pred_map
        = arg_pack[_predecessor_map | dummy_prop];
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::rank_map, D >
        rank_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::rank_map, D >::map_type r_map
        = rank_map_gen(g, arg_pack);
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::distance_map, D >
        dist_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::distance_map, D >::map_type dist_map
        = dist_map_gen(g, arg_pack);
    weight_map_type w_map = detail::override_const_property(
        arg_pack, _weight_map, g, edge_weight);
    typename boost::detail::override_const_property_result< arg_pack_type,
        boost::graph::keywords::tag::vertex_index_map, vertex_index_t,
        VertexListGraph >::type v_i_map
        = detail::override_const_property(
            arg_pack, _vertex_index_map, g, vertex_index);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::color_map,
        boost::default_color_type >::map_type c_map
        = boost::detail::make_color_map_from_arg_pack(g, arg_pack);
    std::less< D > default_compare;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_compare, std::less< D >& >::type
        dist_comp
        = arg_pack[_distance_compare | default_compare];
    closed_plus< D > default_combine(inf);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_combine, closed_plus< D >& >::type
        dist_comb
        = arg_pack[_distance_combine | default_combine];
    astar_search(g, s, h, vis, pred_map, r_map, dist_map, w_map, v_i_map, c_map,
        dist_comp, dist_comb, inf, zero_d);
}

template < typename VertexListGraph, typename AStarHeuristic, typename P,
    typename T, typename R >
void astar_search_tree(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, const bgl_named_params< P, T, R >& params)
{
    using namespace boost::graph::keywords;
    typedef bgl_named_params< P, T, R > params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)

    // Distance type is the value type of the distance map if there is one,
    // otherwise the value type of the weight map.
    typedef
        typename boost::detail::override_const_property_result< arg_pack_type,
            boost::graph::keywords::tag::weight_map, edge_weight_t,
            VertexListGraph >::type weight_map_type;
    typedef typename boost::property_traits< weight_map_type >::value_type D;
    const D inf = arg_pack[_distance_inf || detail::get_max< D >()];
    const D zero_actual = D();
    const D zero_d = arg_pack[_distance_zero | zero_actual];
    null_visitor null_vis;
    astar_visitor< null_visitor > default_visitor(null_vis);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::visitor, dummy_property_map& >::type vis
        = arg_pack[_visitor | default_visitor];
    dummy_property_map dummy_prop;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::predecessor_map,
        dummy_property_map& >::type pred_map
        = arg_pack[_predecessor_map | dummy_prop];
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::rank_map, D >
        rank_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::rank_map, D >::map_type r_map
        = rank_map_gen(g, arg_pack);
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::distance_map, D >
        dist_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::distance_map, D >::map_type dist_map
        = dist_map_gen(g, arg_pack);
    weight_map_type w_map = detail::override_const_property(
        arg_pack, _weight_map, g, edge_weight);
    std::less< D > default_compare;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_compare, std::less< D >& >::type
        dist_comp
        = arg_pack[_distance_compare | default_compare];
    closed_plus< D > default_combine(inf);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_combine, closed_plus< D >& >::type
        dist_comb
        = arg_pack[_distance_combine | default_combine];
    astar_search_tree(g, s, h, vis, pred_map, r_map, dist_map, w_map, dist_comp,
        dist_comb, inf, zero_d);
}

template < typename VertexListGraph, typename AStarHeuristic, typename P,
    typename T, typename R >
void astar_search_no_init(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, const bgl_named_params< P, T, R >& params)
{
    using namespace boost::graph::keywords;
    typedef bgl_named_params< P, T, R > params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)
    typedef
        typename boost::detail::override_const_property_result< arg_pack_type,
            boost::graph::keywords::tag::weight_map, edge_weight_t,
            VertexListGraph >::type weight_map_type;
    typedef typename boost::property_traits< weight_map_type >::value_type D;
    const D inf = arg_pack[_distance_inf || detail::get_max< D >()];
    const D zero_actual = D();
    const D zero_d = arg_pack[_distance_zero | zero_actual];
    null_visitor null_vis;
    astar_visitor< null_visitor > default_visitor(null_vis);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::visitor, dummy_property_map& >::type vis
        = arg_pack[_visitor | default_visitor];
    dummy_property_map dummy_prop;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::predecessor_map,
        dummy_property_map& >::type pred_map
        = arg_pack[_predecessor_map | dummy_prop];
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::rank_map, D >
        rank_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::rank_map, D >::map_type r_map
        = rank_map_gen(g, arg_pack);
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::distance_map, D >
        dist_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::distance_map, D >::map_type dist_map
        = dist_map_gen(g, arg_pack);
    weight_map_type w_map = detail::override_const_property(
        arg_pack, _weight_map, g, edge_weight);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::color_map,
        boost::default_color_type >::map_type c_map
        = boost::detail::make_color_map_from_arg_pack(g, arg_pack);
    typename boost::detail::override_const_property_result< arg_pack_type,
        boost::graph::keywords::tag::vertex_index_map, vertex_index_t,
        VertexListGraph >::type v_i_map
        = detail::override_const_property(
            arg_pack, _vertex_index_map, g, vertex_index);
    std::less< D > default_compare;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_compare, std::less< D >& >::type
        dist_comp
        = arg_pack[_distance_compare | default_compare];
    closed_plus< D > default_combine(inf);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_combine, closed_plus< D >& >::type
        dist_comb
        = arg_pack[_distance_combine | default_combine];
    astar_search_no_init(g, s, h, vis, pred_map, r_map, dist_map, w_map, c_map,
        v_i_map, dist_comp, dist_comb, inf, zero_d);
}

template < typename VertexListGraph, typename AStarHeuristic, typename P,
    typename T, typename R >
void astar_search_no_init_tree(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    AStarHeuristic h, const bgl_named_params< P, T, R >& params)
{
    using namespace boost::graph::keywords;
    typedef bgl_named_params< P, T, R > params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)
    typedef
        typename boost::detail::override_const_property_result< arg_pack_type,
            boost::graph::keywords::tag::weight_map, edge_weight_t,
            VertexListGraph >::type weight_map_type;
    typedef typename boost::property_traits< weight_map_type >::value_type D;
    const D inf = arg_pack[_distance_inf || detail::get_max< D >()];
    const D zero_actual = D();
    const D zero_d = arg_pack[_distance_zero | zero_actual];
    null_visitor null_vis;
    astar_visitor< null_visitor > default_visitor(null_vis);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::visitor, dummy_property_map& >::type vis
        = arg_pack[_visitor | default_visitor];
    dummy_property_map dummy_prop;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::predecessor_map,
        dummy_property_map& >::type pred_map
        = arg_pack[_predecessor_map | dummy_prop];
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::rank_map, D >
        rank_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::rank_map, D >::map_type r_map
        = rank_map_gen(g, arg_pack);
    boost::detail::make_property_map_from_arg_pack_gen<
        boost::graph::keywords::tag::distance_map, D >
        dist_map_gen(zero_actual);
    typename boost::detail::map_maker< VertexListGraph, arg_pack_type,
        boost::graph::keywords::tag::distance_map, D >::map_type dist_map
        = dist_map_gen(g, arg_pack);
    weight_map_type w_map = detail::override_const_property(
        arg_pack, _weight_map, g, edge_weight);
    std::less< D > default_compare;
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_compare, std::less< D >& >::type
        dist_comp
        = arg_pack[_distance_compare | default_compare];
    closed_plus< D > default_combine(inf);
    typename boost::parameter::binding< arg_pack_type,
        boost::graph::keywords::tag::distance_combine, closed_plus< D >& >::type
        dist_comb
        = arg_pack[_distance_combine | default_combine];
    astar_search_no_init_tree(g, s, h, vis, pred_map, r_map, dist_map, w_map,
        dist_comp, dist_comb, inf, zero_d);
}

} // namespace boost

#endif // BOOST_GRAPH_ASTAR_SEARCH_HPP


#include <iostream>
#include "mcp3427.hpp"
#include <unistd.h>

#include <boost/program_options.hpp>
#include <boost/chrono.hpp>
#include <boost/thread/thread.hpp>

#include <random>
#include <string>
#include <thread>
#include <chrono>
#include <iostream>
#include <cstdlib>
#include <cstring>
#include <ctime>
#include <cmath>
#include "mqtt/async_client.h"

class ptx_convert {
	bool usa = false;
	const double A = 3.9083e-3;
	const double B = -5.775e-7;
	double R0 = 100.0;

	public:
	ptx_convert(double R0, bool usa){
		this->usa = usa;
		this->R0 = R0;
	}
	double operator()(double r){
		using namespace std;
		double c = (-R0 * A + sqrt(pow(R0,2) * pow(A,2) - 4 * R0 * B * (R0 - r)))/(2*R0*B);
        if(usa)
            return c*9.0/5.0 + 32.0;
        else
            return c;
	}
};

int main(int ac, char **av)
{
	namespace po = boost::program_options;

	const std::string TOPIC { "data/rand" };
	const int	 QOS = 1;
	const auto PERIOD = std::chrono::seconds(5);
	const int MAX_BUFFERED_MSGS = 120;	// 120 * 5sec => 10min off-line buffering
	const std::string PERSIST_DIR { "data-persist" };
	const double alpha = 0.003851;

	// Declare the supported options.
	po::options_description desc("Allowed options");
	desc.add_options()
		("help", "produce help message")
		("server", 	po::value<std::string>()->default_value("localhost"), "mqtt server")
		("port", 	po::value<int>()->default_value(1883),"mqtt port")
		("i2cbus", 	po::value<std::string>()->default_value("/dev/i2c-1"), "i2c bus, e.g. /dev/i2c-1")
		("delay",	po::value<int>()->default_value(100), "delay between readings (integer milliseconds)")
		("topic1",	po::value<std::string>()->default_value("adc/1"), "mqtt publish topic for adc1")
		("topic2",	po::value<std::string>()->default_value("adc/2"), "mqtt publish topic for adc2")
		("topic3",	po::value<std::string>()->default_value("adc/3"), "mqtt publish topic for adc3")
		("topic4",	po::value<std::string>()->default_value("adc/4"), "mqtt publish topic for adc4")
		("factor1",	po::value<double>()->default_value(1.0), "output factor for pt100/1")
		("factor2",	po::value<double>()->default_value(1.0), "output factor for pt100/2")
		("offset1",	po::value<double>()->default_value(0.0), "resistance offset for pt100/1")
		("offset2",	po::value<double>()->default_value(0.0), "resistance offset for pt100/2")
		("avg1",	po::value<int>()->default_value(1), "runtime average 'n' readings for adc1")
		("avg2",	po::value<int>()->default_value(1), "runtime average 'n' readings for adc2")
		("verbose",	"report to stdout")
		("pt100",	"treat ADC as PT100 readout")
		("csv",	"csv readout to stdout")
        ("usa",	"readout units in Fahrenheit")
		("addr1",	po::value<int>()->default_value(0x68),"i2c address of ADC1")
		("addr2",	po::value<int>()->default_value(0x6a),"i2c address of ADC2")
	;

	po::variables_map vm;
	po::store(po::parse_command_line(ac, av, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		std::cout << desc << "\n";
		return 1;
	}
	int delay = 0;
	std::string topic1,topic2,topic3,topic4;

	std::string i2cbus(vm["i2cbus"].as<std::string>());
	std::string server(vm["server"].as<std::string>());

	int avg1, avg2, addr1, addr2, port;
	double factor1, factor2, offset1, offset2;

	port = vm["port"].as<int>();
	topic1 = vm["topic1"].as<std::string>();
	topic2 = vm["topic2"].as<std::string>();
	topic3 = vm["topic3"].as<std::string>();
	topic4 = vm["topic4"].as<std::string>();
	factor1 = vm["factor1"].as<double>();
	factor2 = vm["factor2"].as<double>();
	offset1 = vm["offset1"].as<double>();
	offset2 = vm["offset2"].as<double>();
	avg1 = vm["avg1"].as<int>();
	avg2 = vm["avg2"].as<int>();
	addr1 = vm["addr1"].as<int>();
	addr2 = vm["addr2"].as<int>();
	bool verbose = false;
	bool mode_pt100 = false;
	bool usa = false;
	bool mode_csv = false;

	delay = vm["delay"].as<int>();

	if (vm.count("verbose")){
		std::cout << "verbose set\n";
		verbose = true;
	}

	if(verbose){
		std::cout << "mqtt server set to " << vm["server"].as<std::string>() << "\n";
		std::cout << "mqtt port set to " << vm["port"].as<int>() << "\n";
		std::cout << "i2cbus is set to " << vm["i2cbus"].as<std::string>() << "\n";
		std::cout << "delay is set to " << vm["delay"].as<int>() << "\n";
		std::cout << "mqtt topic for adc1/1 or pt100/1 is set to " << vm["topic1"].as<std::string>() << "\n";
		std::cout << "mqtt topic for adc1/2 or pt100/2 is set to " << vm["topic2"].as<std::string>() << "\n";
		std::cout << "mqtt topic for adc2/1 is set to " << vm["topic3"].as<std::string>() << "\n";
		std::cout << "mqtt topic for adc2/2 is set to " << vm["topic4"].as<std::string>() << "\n";
		std::cout << "factor1 is set to " << vm["factor1"].as<double>() << "\n";
		std::cout << "factor2 is set to " << vm["factor2"].as<double>() << "\n";
		std::cout << "offset1 is set to " << vm["offset1"].as<double>() << "\n";
		std::cout << "offset2 is set to " << vm["offset2"].as<double>() << "\n";
		std::cout << "avg1 is set to " << vm["avg1"].as<int>() << "\n";
		std::cout << "avg2 is set to " << vm["avg2"].as<int>() << "\n";
		std::cout << "addr1 is set to " << vm["addr1"].as<int>() << "\n";
		std::cout << "addr2 is set to " << vm["addr2"].as<int>() << "\n";
	}

	if (vm.count("pt100")){
		mode_pt100 = true;
		if(verbose)
			std::cout << "pt100 mode is set\n";
	}

	if (vm.count("csv")){
		mode_csv = true;
		if(verbose)
			std::cout << "csv output mode is set\n";
	}

	if (vm.count("usa")){
		usa = true;
		if(verbose)
			std::cout << "pt100 temperature in Fahrenheit mode is set\n";
	}
	ptx_convert pt100(100, usa);

	// sensors
	mcp3427 adc(i2cbus, addr1);
	mcp3427 adc2(i2cbus, addr2);

	// mqtt
	std::string address = std::string("tcp://") + server + std::string(":") + std::to_string(port);
	mqtt::async_client cli(address, "", MAX_BUFFERED_MSGS, PERSIST_DIR);

	mqtt::connect_options connOpts;
	connOpts.set_keep_alive_interval(MAX_BUFFERED_MSGS * PERIOD);
	connOpts.set_clean_session(true);
	connOpts.set_automatic_reconnect(true);

	mqtt::topic pub_topic1(cli, topic1, QOS, true);
	mqtt::topic pub_topic2(cli, topic2, QOS, true);
	mqtt::topic pub_topic3(cli, topic3, QOS, true);
	mqtt::topic pub_topic4(cli, topic4, QOS, true);

	std::cout << "Connecting to server '" << address << "'..." << std::flush;
	cli.connect(connOpts)->wait();
	std::cout << "OK\n" << std::endl;
	std::string payload;

	double wire1, wire2, res1, res2;
	int n=0;
	do{
		if(mode_pt100){
			if(!(n & 0xf)){
				wire1 = adc.get_float(1) * 1000.0;
				if(adc2)
					wire2 = adc2.get_float(1) * 1000.0;
			}
			n++;

			res1 = adc.get_float(0) * 1000.0 - wire1 * 2 + offset1;
			res1 = pt100(res1);
			payload = std::to_string(res1);
			if(verbose)
				std::cout << topic1 << " " << payload << std::endl;
			pub_topic1.publish(payload);

			if(adc2){
				res2 = adc2.get_float(0) * 1000.0 - wire2 * 2 + offset2;
				res2 = pt100(res2);
				payload = std::to_string(res2);
				if(verbose)
					std::cout << topic2 << " " << payload << std::endl;
				pub_topic2.publish(payload);
			}

		} else {
			res1 = adc.get_float(0);
			payload = std::to_string(res1);
			if(verbose)
				std::cout << topic1 << " " << payload << std::endl;

			pub_topic1.publish(payload);
			if(adc2){
				res2 = adc2.get_float(0);
				payload = std::to_string(res2);
				if(verbose)
					std::cout << topic2 << " " << payload << std::endl;
				pub_topic2.publish(payload);
			}
			if(mode_csv)
				std::cout << res1 << ", " << res2;

			res1 = adc.get_float(1);
			payload = std::to_string(res1);
			if(verbose)
				std::cout << topic3 << " " << payload << std::endl;
			pub_topic3.publish(payload);
			if(adc2){
				res2 = adc2.get_float(1);
				payload = std::to_string(res2);
				if(verbose)
					std::cout << topic4 << " " << payload << std::endl;
				pub_topic4.publish(payload);
			}
			if(mode_csv)
				std::cout << ", " << res1 << ", " << res2 << std::endl;
		}
		boost::this_thread::sleep_for(boost::chrono::milliseconds(delay));

	} while (1);

}


///////////////////////////////////////////////////////////////////////////////
//                                                                           //
//                 polar v1.0 - August the 15th 2019                         //
//                                                                           //
///////////////////////////////////////////////////////////////////////////////

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

#ifndef VIBSTATES_H
#define VIBSTATES_H

#include <Eigen/Core>
#include <iostream>
#include <fstream>
#include <vector>
#include "units.hpp"

class VibPolarizability;

class VibStates
{

friend class VibPolarizability;

private:
   // attributes
   Eigen::VectorXd wavenumbers,
                            IR,
                           VCD;

// electric dipole derivative wrt normal coordinates;
// atomic units: (e / m**0.5)
   std::vector<Eigen::Vector3d> dmu_dq;

// magnetic dipole derivative wrt normal coordinates;
// atomic units: (e**3 / (k0 * hbar * m**0.5))
   std::vector<Eigen::Vector3d>  dm_dqdot;

// this comes from VCD class, where it is computed in cgs units
   Eigen::VectorXd  m10_robust;
   Eigen::VectorXd mu01_robust;

public:

   VibStates() = default;
   explicit VibStates(const std::string&);
   void PrintReport() const;

   // getters
   unsigned int GetNumberOfStates() const;
   Eigen::VectorXd              GetWavenumbers() const;
   std::vector<Eigen::Vector3d> GetElectricDipoleDerivative() const;
   std::vector<Eigen::Vector3d> GetMagneticDipoleDerivative() const;
   Eigen::VectorXd Get_m10_Robust() const;
   Eigen::VectorXd Get_mu01_Robust() const;
};

#endif // VIBSTATES_H


/*
The MIT License

Copyright (c) 2015-2017 Albert Murienne

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include "network.h"
#include "tensor_solver.h"
#include "tensor_utils.h"
#include "tensor_loss_functions.h"
#include "tensor_gradient_checker.h"
#include "conv_layer.h"
#include "full_layer.h"
#include "pool_layer.h"
#include "input_layer.h"
#include "output_layer.h"
#include "dropout_layer.h"

#include <boost/range/adaptor/reversed.hpp>

namespace neurocl { namespace convnet {

std::atomic_size_t network::m_training_samples{ 0 };

//#define VERBOSE_NETWORK

network::network()
{
    m_solver = tensor_solver_factory::build();
}

network::~network()
{
    //dump_image_features();
}

void network::set_training( bool training )
{
    layer::set_training( training );
}

void network::add_layers( const std::vector<layer_descr>& layers )
{
    size_t conv_idx = 0;
    size_t pool_idx = 0;
    size_t drop_idx = 0;
    size_t full_idx = 0;

    size_t cache_size = m_solver->get_cache_size();

    for ( auto& _layer : layers )
    {
        std::shared_ptr<layer> l;
        switch( _layer.type )
        {
        case INPUT_LAYER:
            {
                std::shared_ptr<input_layer> in = std::make_shared<input_layer>();
                in->populate( _layer.sizeX, _layer.sizeY, _layer.sizeZ );
                l = in;
            }
            break;
        case CONV_LAYER:
            {
                std::shared_ptr<conv_layer_iface> c =
                    std::make_shared< conv_layer<tensor_activations::relu> >( "c" + std::to_string(++conv_idx) );
                c->set_filter_size( _layer.sizeF );
                c->populate( m_layers.back(), _layer.sizeX, _layer.sizeY, _layer.sizeZ, cache_size );
                l = c;
            }
            break;
        case POOL_LAYER:
            {
                std::shared_ptr<pool_layer> s = std::make_shared<pool_layer>( "s" + std::to_string(++pool_idx) );
                s->populate( m_layers.back(), _layer.sizeX, _layer.sizeY, _layer.sizeZ );
                l = s;
            }
            break;
        case DROPOUT_LAYER:
            {
                std::shared_ptr<dropout_layer> d = std::make_shared<dropout_layer>( "d" + std::to_string(++drop_idx) );
                d->populate( m_layers.back(), _layer.sizeX, _layer.sizeY, _layer.sizeZ );
                l = d;
            }
            break;
        case FULL_LAYER:
            {
                std::shared_ptr<full_layer_iface> f =
                    std::make_shared< full_layer<tensor_activations::relu> >( "f" + std::to_string(++full_idx) );
                f->populate( m_layers.back(), _layer.sizeX, _layer.sizeY, _layer.sizeZ, cache_size );
                l = f;
            }
            break;
        case OUTPUT_LAYER:
            {
                std::shared_ptr<output_layer_iface> out =
                    std::make_shared< output_layer< tensor_activations::softmax_cross_entropy,
                                                    tensor_loss_functions::cross_entropy_softmax >
                                    >();
                out->populate( m_layers.back(), _layer.sizeX, _layer.sizeY, _layer.sizeZ, cache_size );
                l = out;
            }
            break;
        }
        m_layers.emplace_back( l );
    }
}

void network::set_input(  const size_t& in_size, const float* in )
{
    // TODO-CNN : for now works only because input layer has no depth for now!

    std::shared_ptr<layer> layer;
    std::shared_ptr<input_layer> input_layer =
        std::dynamic_pointer_cast<neurocl::convnet::input_layer>( m_layers.front() );

    if ( in_size > input_layer->size() )
        throw network_exception( "sample size exceeds allocated layer size!" );

#ifdef VERBOSE_NETWORK
    LOGGER(info) << "network::set_input - input (" << in << ") size = " << in_size << std::endl;
#endif

    input_layer->fill( 0, 0, in_size, in );
}

void network::set_output( const size_t& out_size, const float* out )
{
    // TODO-CNN : for now works only because output layer has no depth for now!

    std::shared_ptr<output_layer_iface> output_layer =
        std::dynamic_pointer_cast<neurocl::convnet::output_layer_iface>( m_layers.back() );

    if ( out_size > output_layer->size() )
        throw network_exception( "output size exceeds allocated layer size!" );

#ifdef VERBOSE_NETWORK
    LOGGER(info) << "network::set_output - output (" << out << ") size = " << out_size << std::endl;
#endif

    output_layer->fill( 0, 0, out_size, out );
}

const layer_ptr network::get_layer_ptr( const size_t layer_idx )
{
    if ( layer_idx >= m_layers.size() )
	{
        LOGGER(error) << "network::get_layer_ptr - cannot access layer " << layer_idx << std::endl;
        throw network_exception( "invalid layer index" );
    }

    std::shared_ptr<layer> _layer = m_layers[layer_idx];

    LOGGER(info) << "network::get_layer_ptr - getting layer  " << _layer->type() << std::endl;

    layer_ptr l( _layer->nb_weights(), _layer->nb_bias() );
    _layer->fill_w( l.weights.get() );
    _layer->fill_b( l.bias.get() );

    return l;
}

void network::set_layer_ptr( const size_t layer_idx, const layer_ptr& l )
{
    if ( layer_idx >= m_layers.size() )
    {
        LOGGER(error) << "network::set_layer_ptr - cannot access layer " << layer_idx << std::endl;
        throw network_exception( "invalid layer index" );
    }

    std::shared_ptr<layer> _layer = m_layers[layer_idx];

    if ( ( _layer->nb_weights() != l.num_weights ) || ( _layer->nb_bias() != l.num_bias ) )
    {
        LOGGER(error) << "network::set_layer_ptr - inconsistent layer " << layer_idx << " size" << std::endl;
        throw network_exception( "inconsistent layer size" );
    }

    LOGGER(info) << "network::set_layer_ptr - setting layer  " << _layer->type() << std::endl;

    _layer->fill_w( _layer->nb_weights(), l.weights.get() );
    _layer->fill_b( _layer->nb_bias(), l.bias.get() );
}

const output_ptr network::output()
{
    // TODO-CNN : for now works only because last layer has no depth for now

    std::shared_ptr<output_layer_iface> output_layer =
        std::dynamic_pointer_cast<neurocl::convnet::output_layer_iface>( m_layers.back() );

    output_ptr o( output_layer->width() * output_layer->height() );
    output_layer->fill( 0, 0, o.outputs.get() );

    return o;
}

void network::clear_gradients()
{
    for ( auto _layer : m_layers )
    {
        _layer->clear_gradients();
    }

    m_training_samples = 0;
}

void network::feed_forward()
{
    for ( auto _layer : m_layers )
    {
#ifdef VERBOSE_NETWORK
        std::cout << "--> feed forwarding " << _layer->type() << " layer" << std::endl;
#endif
        _layer->feed_forward();
    }
}

void network::back_propagate()
{
    for ( auto _layer : boost::adaptors::reverse( m_layers ) )
    {
#ifdef VERBOSE_NETWORK
        std::cout << "--> back propagating " << _layer->type() << " layer" << std::endl;
#endif
        _layer->back_propagate();
    }

    for ( auto _layer : m_layers )
    {
#ifdef VERBOSE_NETWORK
        std::cout << "--> updating gradients " << _layer->type() << " layer" << std::endl;
#endif
        _layer->update_gradients();
    }

    ++m_training_samples;
}

void network::gradient_descent()
{
    m_solver->set_size( m_training_samples );

    for ( auto _layer : m_layers )
    {
        _layer->gradient_descent( m_solver );
    }
}

float network::loss()
{
    return std::static_pointer_cast<output_layer_iface>( m_layers.back() )->loss();
}

void network::gradient_check( const output_ptr& out_ref )
{
    // NOTE : network input & output have to be previously set!!

    auto err_func = []( output_ptr a, output_ptr b ) {
        float _acc = 0.f;
        float* pa = a.outputs.get();
        float* pb = b.outputs.get();
        for ( size_t i=0; i<a.num_outputs; i++ )
            _acc += (pa[i] - pb[i])*(pa[i] - pb[i]);

        return _acc / ( 2.f * static_cast<float>( a.num_outputs ) ); // TODO : mse error, use a generic loss class?
     };

    float epsilon = 1e-4f;

    for ( auto _layer : m_layers )
    {
        std::unique_ptr<tensor_gradient_checker> grad_check = _layer->get_gradient_checker();
        if ( !grad_check )
            continue;

        for ( size_t i = 0; i<grad_check->size(); i++ )
        {
            // Store weight
            grad_check->store();

            // Compute output with +epsilon increment
            grad_check->mod( +epsilon );
            feed_forward();
            back_propagate();

            const output_ptr out_p = output();
            float err_p = err_func( out_p, out_ref );

            // Compute output with -epsilon increment
            grad_check->mod( -epsilon );
            feed_forward();
            back_propagate();

            const output_ptr out_m = output();
            float err_m = err_func( out_m, out_ref );

            grad_check->set_grad( ( err_p - err_m ) / ( 2.f * epsilon ) );

            // Restore weight
            grad_check->restore();

            // switch to next parameter
            grad_check->next();

            clear_gradients();
        }

        // compute "standard" deltas
        feed_forward();
        back_propagate();

        LOGGER(info) << "network::gradient_check - layer " << _layer->type() << " gradient check error : " << grad_check->error() << std::endl;
    }
}

void network::dump_image_features()
{
    //tensor_utils::visualizer::dump_features( "features_dump", m_layers[1]->type(), m_layers[1]->weights({}) );
    //return;

    for ( auto _layer : m_layers )
    {
        // dump only if the layer has weights
        if ( _layer->nb_weights() )
        	tensor_utils::visualizer::dump_features( "features_dump", _layer->type(), _layer->weights({}) );
    }
}

} /*namespace neurocl*/ } /*namespace convnet*/


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file orea/aggregation/exposurecalculator.hpp
    \brief Exposure calculator
    \ingroup analytics
*/

#pragma once

#include <orea/aggregation/collatexposurehelper.hpp>
#include <orea/cube/cubeinterpretation.hpp>
#include <orea/cube/npvcube.hpp>
#include <ored/portfolio/portfolio.hpp>

#include <boost/shared_ptr.hpp>

namespace ore {
namespace analytics {
using namespace QuantLib;
using namespace QuantExt;
using namespace data;
using namespace std;

//! XVA Calculator base class
/*!
  Derived classes implement a constructor with the relavant additional input data
  and a build function that performs the XVA calculations for all netting sets and
  along all paths.
*/
class ExposureCalculator {
public:
    ExposureCalculator(
        //! Driving portfolio consistent with the cube below
        const boost::shared_ptr<Portfolio>& portfolio,
        //! NPV cube resulting from the Monte Carlo simulation loop
        const boost::shared_ptr<NPVCube>& cube,
        //! Interpreter for cube storage (where to find which data items)
        const boost::shared_ptr<CubeInterpretation> cubeInterpretation,
        //! Market data object to access e.g. discounting and funcing curves
        const boost::shared_ptr<Market>& market,
	    //! Flag to indicate exposure termination at the next break date
        const bool exerciseNextBreak,
        //! Expression currency for all results
        const string& baseCurrency,
        //! Market configuration to use
        const string& configuration,
        //! Quantile for Potential Future Exposure output
        const Real quantile,
        //! Collateral calculation type to be used, see class %CollateralExposureHelper
        const CollateralExposureHelper::CalculationType calcType,
	    //! Flag to indicate exposure evaluation with dynamic credit
        const bool multiPath,
        //! Flag to indicate flipped xva calculation
        const bool flipViewXVA
    );

    virtual ~ExposureCalculator() {}

    //! Compute exposures along all paths and fill result structurues
    virtual void build();

    enum ExposureIndex {
        EPE = 0,
        ENE,
        allocatedEPE,
        allocatedENE
    };
    const Size EXPOSURE_CUBE_DEPTH = 4;

    boost::shared_ptr<Portfolio> portfolio() { return portfolio_; }
    boost::shared_ptr<NPVCube> npvCube() { return cube_; }
    boost::shared_ptr<CubeInterpretation> cubeInterpretation() { return cubeInterpretation_; }
    boost::shared_ptr<Market> market() { return market_; }
    bool exerciseNextBreak() { return exerciseNextBreak_; }
    string baseCurrency() { return baseCurrency_; }
    string configuration() { return configuration_; }
    Real quantile() { return quantile_; }
    CollateralExposureHelper::CalculationType calcType() { return calcType_; }
    bool isRegularCubeStorage() { return isRegularCubeStorage_; }
    bool multiPath() { return multiPath_; }

    vector<Date> dates() { return dates_; }
    Date today() { return today_; }
    DayCounter dc() { return dc_; }
    vector<string> nettingSetIds() { return nettingSetIds_; }
    map<string, Real> nettingSetValueToday() { return nettingSetValueToday_; }
    map<string, Date> nettingSetMaturity() { return nettingSetMaturity_; }
    vector<Real> times() { return times_; };

    const boost::shared_ptr<NPVCube>& exposureCube() { return exposureCube_; }
    const map<string, vector<vector<Real>>>& nettingSetDefaultValue() { return nettingSetDefaultValue_; }
    const map<string, vector<vector<Real>>>& nettingSetCloseOutValue() { return nettingSetCloseOutValue_; }

    vector<Real> epe(const string& tid) { return getMeanExposure(tid, ExposureIndex::EPE); }
    vector<Real> ene(const string& tid) { return getMeanExposure(tid, ExposureIndex::ENE); }
    vector<Real> allocatedEpe(const string& tid) { return getMeanExposure(tid, ExposureIndex::allocatedEPE); }
    vector<Real> allocatedEne(const string& tid) { return getMeanExposure(tid, ExposureIndex::allocatedENE); }
    vector<Real>& ee_b(const string& tid) { return ee_b_[tid]; }
    vector<Real>& eee_b(const string& tid) { return eee_b_[tid]; }
    vector<Real>& pfe(const string& tid) { return pfe_[tid]; }
    Real& epe_b(const string& tid) { return epe_b_[tid]; }
    Real& eepe_b(const string& tid) { return eepe_b_[tid]; }

protected:
    const boost::shared_ptr<Portfolio> portfolio_;
    const boost::shared_ptr<NPVCube> cube_;
    const boost::shared_ptr<CubeInterpretation> cubeInterpretation_;
    const boost::shared_ptr<Market> market_;
    const bool exerciseNextBreak_;
    const string baseCurrency_;
    const string configuration_;
    const Real quantile_;
    const CollateralExposureHelper::CalculationType calcType_;
    const bool multiPath_;
    bool isRegularCubeStorage_;

    vector<Date> dates_;
    const Date today_;
    const DayCounter dc_;
    vector<string> nettingSetIds_;
    map<string, Real> nettingSetValueToday_;
    map<string, Date> nettingSetMaturity_;
    vector<Real> times_;

    boost::shared_ptr<NPVCube> exposureCube_;
    map<string, vector<vector<Real>>> nettingSetDefaultValue_, nettingSetCloseOutValue_;

    map<string, std::vector<Real>> ee_b_;
    map<string, std::vector<Real>> eee_b_;
    map<string, std::vector<Real>> pfe_;
    map<string, Real> epe_b_;
    map<string, Real> eepe_b_;
    vector<Real> getMeanExposure(const string& tid, ExposureIndex index);
    bool flipViewXVA_;
};

} // namespace analytics
} // namespace ore


#ifndef __PCA_HH__
#define __PCA_HH__

#include <cstdlib>
#include <Eigen/Dense>
#include "iterator.hh"
#include "parse.hh"
#include "pcaoptions.hh"

namespace hashpca
{
  template<typename Hash,
           typename Iterator>
  std::pair<double, uint64_t>
  pca_accumulate (std::istream&            in,
                  hashpca::MatrixXd&       Y,
                  const hashpca::MatrixXd& Omega,
                  Iterator&                iterator,
                  bool                     center,
                  Eigen::VectorXd&         sum)
    {
      std::pair<double, uint64_t> rv (0.0, 0);
      veedubparse::StandardParse<Hash> parse;
      SugaryVectorXd xOmega (Omega.cols ());

      if (center)
        sum.setZero (Omega.rows ());

      parse (in,
             [ & ] (bool ok,
                    const veedubparse::GeneralExample& ex) {
               double imp;

               if (ok &&
                  [] (char* s, double& z) {
                    if (! s) { z = 1; return true; }
                    char* ep; z = ::strtod (s, &ep);
                    return ep != s && (*ep == '\0' || isspace (*ep));
                  } (ex.importance, imp))
                 {
                   auto x = iterator (ex);

                   xOmega = x.transpose () * Omega;
                   Y += imp * x * xOmega;
                   if (center)
                     sum += imp * x.transpose ();
                   rv.first += imp;
                 }
                 else {
                   ++rv.second;
                 }
              });

      double invn = 1.0 / rv.first;

      if (center)
        {
          Eigen::VectorXd c (Omega.cols ());
          c = sum.transpose () * Omega;
          Y.noalias () -= invn * sum * c;
        }

      Y *= invn;

      return rv;
    }

  template<typename Hash,
           typename Iterator>
  int
  computeu (std::istream&            in,
            std::ostream&            out,
            const hashpca::MatrixXd& V,
            const Eigen::VectorXd&   mean,
            const Eigen::VectorXd&   sinv,
            Iterator                 iterator,
            PcaOptions               options)
    {
      veedubparse::StandardParse<Hash> parse;
      SugaryVectorXd Vtx (V.cols ());
      Eigen::VectorXd u (V.cols ());

      parse (in,
             [ & ] (bool ok,
                    const veedubparse::GeneralExample& ex) {
               if (ok)
                 {
                   auto x = iterator (ex);

                   Vtx = x.transpose () * V;
                   switch (options.whiten)
                     {
                       case PcaOptions::ALL:
                         u = sinv.asDiagonal () * (Vtx - mean);
                         break;
                       case PcaOptions::FIRST:
                         u = sinv (0) * (Vtx - mean);
                         break;
                       case PcaOptions::NONE:
                         u = Vtx - mean;
                         break;
                     }

                   if (options.tanhify)
                     u = u.unaryExpr ([] (double x) { return tanh (0.85 * x); });

                   if (options.normalize)
                     u.normalize ();

                   if (ex.tag)
                     out << ex.tag;

                   if (options.withprefix)
                     for (unsigned int i = 0; i < V.cols (); ++i)
                       out << " " << (i+1) << ":" << u (i);
                   else
                     for (unsigned int i = 0; i < V.cols (); ++i)
                       {
                         if (i > 0) out << " ";
                         out << u (i);
                       }

                   out << std::endl;

                   if (options.flush)
                     out << std::flush;
                 }
              });

      return 0;
    }
}

#endif // __PCA_HH__


//---------------------------------------------------------------------------//
//!
//! \file   Geometry_Navigator.hpp
//! \author Alex Robinson
//! \brief  The geometry model navigator base class declaration
//!
//---------------------------------------------------------------------------//

#ifndef GEOMETRY_NAVIGATOR_HPP
#define GEOMETRY_NAVIGATOR_HPP

// Std Lib Includes
#include <functional>
#include <sstream>

#include <boost/units/systems/cgs/length.hpp>
#include <boost/units/systems/cgs/volume.hpp>

// FRENSIE Includes
#include "Geometry_PointLocation.hpp"
#include "Geometry_Ray.hpp"
#include "Utility_ArrayView.hpp"
#include "Utility_Set.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_SerializationHelpers.hpp"

namespace Geometry{

/*! The navigator base class
 * \details Navigators are used to do geometric queries on a model and
 * to ray trace on a model.
 */
class Navigator
{

public:

  //! The entity (cell or surface) id type
  typedef uint64_t EntityId;

  //! The length unit
  typedef boost::units::cgs::length LengthUnit;

  //! The length quantity
  typedef boost::units::quantity<LengthUnit> Length;

  //! The ray type used by the navigator
  typedef UnitAwareRay<LengthUnit> Ray;

  //! The cell id set
  typedef std::set<EntityId> CellIdSet;

  //! The advance callback
  typedef std::function<void(const Length)> AdvanceCompleteCallback;

  //! Constructor
  Navigator( const AdvanceCompleteCallback& advance_complete_callback =
             AdvanceCompleteCallback() );

  //! Destructor
  virtual ~Navigator()
  { /* ... */ }

  /*! Get the location of a point w.r.t. a given cell
   *
   * The direction can be used to help determine if the point is inside or
   * outside of the cell when close to a boundary.
   */
  virtual PointLocation getPointLocation(
                                     const Length position[3],
                                     const double direction[3],
                                     const EntityId cell ) const = 0;

  /*! Get the location of a cell w.r.t. a given cell
   *
   * The direction can be used to help determine if the point is inside or
   * outside of the cell when close to a boundary.
   */
  PointLocation getPointLocation( const Ray& ray,
                                  const EntityId cell ) const;

  /*! Get the surface normal at a point on the surface
   *
   * The dot product of the direction and normal must be positive defined.
   */
  virtual void getSurfaceNormal( const EntityId surface_id,
                                 const Length position[3],
                                 const double direction[3],
                                 double normal[3] ) const = 0;

  /*! Get the surface normal at a point on the surface
   *
   * The dot product of the direction and normal will be positive defined.
   */
  void getSurfaceNormal( const EntityId surface_id,
                         const Ray& ray,
                         double normal[3] ) const;

  /*! Find the cell that contains a given ray
   *
   * The direction can be used to help determine the cell that contains
   * the point when close to a boundary. The cells in the found cell cache
   * will be checked first. If a new cell is found it will be added to the
   * cache. A std::runtime_error (or class derived from it) will be thrown if
   * an error occurs.
   */
  virtual EntityId findCellContainingRay(
                                       const Length position[3],
                                       const double direction[3],
                                       CellIdSet& found_cell_cache ) const = 0;

  /*! Find the cell that contains a given ray
   *
   * The direction can be used to help determine the cell that contains
   * the point when close to a boundary. The cells in the found cell cache
   * will be checked first. If a new cell is found it will be added to the
   * cache. A std::runtime_error (or class derived from it) will be thrown if
   * an error occurs.
   */
  EntityId findCellContainingRay(
                                           const Ray& ray,
                                           CellIdSet& found_cell_cache ) const;

  /*! Find the cell that contains a given ray
   *
   * The direction can be used to help determine the cell that contains
   * the point when close to a boundary. A std::runtime_error (or class
   * derived from it) must be thrown if an error occurs.
   */
  virtual EntityId findCellContainingRay(
                                         const Length position[3],
                                         const double direction[3] ) const = 0;

  /*! Find the cell that contains a given ray
   *
   * The direction can be used to help determine the cell that contains
   * the point when close to a boundary. A std::runtime_error (or class
   * derived from it) must be thrown if an error occurs.
   * A std::runtime_error (or class derived from it) will be thrown if an error
   * occurs.
   */
  EntityId findCellContainingRay( const Ray& ray ) const;

  //! Check if an internal ray has been set
  virtual bool isStateSet() const = 0;

  /*! Set the internal ray with unknown starting cell
   *
   * A std::runtime_error (or class derived from it) must be thrown if
   * an error occurs.
   */
  virtual void setState( const Length x_position,
                         const Length y_position,
                         const Length z_position,
                         const double x_direction,
                         const double y_direction,
                         const double z_direction ) = 0;

  /*! Set the internal ray with unknown starting cell
   *
   * A std::runtime_error (or class derived from it) must be thrown if
   * an error occurs.
   */
  void setState( const Length position[3], const double direction[3] );

  /*! Set the internal ray with unknown starting cell
   *
   * A std::runtime_error (or class derived from it) must be thrown if
   * an error occurs.
   */
  void setState( const Ray& ray );

  /*! Set the internal ray with known starting cell
   *
   * Failure to provide the correct starting cell will result in undefined
   * behavior. A std::runtime_error (or class derived from it) must be thrown
   * if an error occurs.
   */
  virtual void setState( const Length x_position,
                         const Length y_position,
                         const Length z_position,
                         const double x_direction,
                         const double y_direction,
                         const double z_direction,
                         const EntityId start_cell ) = 0;

  /*! Set the internal ray with known starting cell
   *
   * Failure to provide the correct starting cell will result in undefined
   * behavior. A std::runtime_error (or class derived from it) must be thrown
   * if an error occurs.
   */
  void setState( const Length position[3],
                 const double direction[3],
                 const EntityId start_cell );

  /*! Set the internal ray with known starting cell
   *
   * Failure to provide the correct starting cell will result in undefined
   * behavior. A std::runtime_error (or class derived from it) must be thrown
   * if an error occurs.
   */
  void setState( const Ray& ray,
                 const EntityId start_cell );

  //! Get the internal ray position
  virtual const Length* getPosition() const = 0;

  //! Get the internal ray direction
  virtual const double* getDirection() const = 0;

  /*! Get the cell that contains the internal ray
   *
   * A std::runtime_error (or class derived from it) must be thrown
   * if an error occurs.
   */
  virtual EntityId getCurrentCell() const = 0;

  /*! Get the distance from the internal ray pos. to the nearest boundary in all directions
   *
   * A std::runtime_error (or class derived from it) must be thrown if a ray
   * tracing error occurs.
   */
  virtual Length getDistanceToClosestBoundary() = 0;

  /*! Fire the internal ray through the geometry
   *
   * A std::runtime_error (or class derived from it) must be thrown if a ray
   * tracing error occurs.
   */
  virtual Length fireRay( EntityId* surface_hit ) = 0;

  /*! Fire the internal ray through the geometry
   *
   * A std::runtime_error (or class derived from it) must be thrown if a ray
   * tracing error occurs.
   */
  Length fireRay( EntityId& surface_hit );

  /*! Fire the internal ray through the geometry
   *
   * A std::runtime_error (or class derived from it) must be thrown if a ray
   * tracing error occurs.
   */
  Length fireRay();

  /*! Advance the internal ray to the cell boundary
   *
   * If a reflecting surface is hit "true" will be returned. Passing NULL
   * to this function must be allowed.
   */
  bool advanceToCellBoundary( double* surface_normal );

  /*! Advance the internal ray to the cell boundary
   *
   * If a reflecting surface is hit "true" will be returned. Passing NULL
   * to this function must be allowed.
   */
  bool advanceToCellBoundary();

  //! Advance the internal ray by a substep (less than distance to boundary)
  void advanceBySubstep( const Length step_size );

  //! Change the internal ray direction
  virtual void changeDirection( const double x_direction,
                                const double y_direction,
                                const double z_direction ) = 0;

  //! Change the internal ray direction
  void changeDirection( const double direction[3] );

  //! The invalid cell id
  static EntityId invalidCellId();

  //! The invalid surface id
  static EntityId invalidSurfaceId();

  /*! Clone the navigator
   *
   * The returned pointer must be heap allocated.
   */
  virtual Navigator* clone( const AdvanceCompleteCallback& advance_complete_callback ) const = 0;

  /*! Clone the navigator
   *
   * The returned pointer must be heap allocated.
   */
  virtual Navigator* clone() const = 0;

protected:

  //! Copy constructor
  Navigator( const Navigator& other );

  // Convert an array to a string
  template<typename T>
  static std::string arrayToString( const T* data );

  //! Advance the internal ray to the cell boundary implementation
  virtual bool advanceToCellBoundaryImpl( double* surface_normal,
                                          Length& distance_traveled ) = 0;

  //! Advance the internal ray by a substep (less than distance to boundary) implementation
  virtual void advanceBySubstepImpl( const Length step_size ) = 0;

private:

  // The onAdvanceComplete method
  AdvanceCompleteCallback d_on_advance_complete;
};

// Get the location of a cell w.r.t. a given cell
inline PointLocation Navigator::getPointLocation(
                                          const Ray& ray,
                                          const EntityId cell ) const
{
  return this->getPointLocation( ray.getPosition(), ray.getDirection(), cell );
}

// Get the surface normal at a point on the surface
inline void Navigator::getSurfaceNormal(
                                        const EntityId surface_id,
                                        const Ray& ray,
                                        double normal[3] ) const
{
  this->getSurfaceNormal( surface_id,
                          ray.getPosition(),
                          ray.getDirection(),
                          normal );
}

/* SWIG has limitations with the auto function return type. To avoid a syntax
 * error the following functions will not be defined for SWIG.
 */
#if !defined SWIG
// Find the cell that contains a given ray
inline auto Navigator::findCellContainingRay(
                      const Ray& ray,
                      CellIdSet& found_cell_cache ) const -> EntityId
{
  return this->findCellContainingRay( ray.getPosition(),
                                      ray.getDirection(),
                                      found_cell_cache );
}

// Find the cell that contains a given ray
inline auto Navigator::findCellContainingRay( const Ray& ray ) const -> EntityId
{
  return this->findCellContainingRay( ray.getPosition(), ray.getDirection() );
}
#endif // end !defined SWIG

// Set the internal ray with unknown starting cell
inline void Navigator::setState( const Length position[3],
                                 const double direction[3] )
{
  this->setState( position[0], position[1], position[2],
                  direction[0], direction[1], direction[2] );
}

// Set the internal ray with unknown starting cell
inline void Navigator::setState( const Ray& ray )
{
  this->setState( ray.getPosition(), ray.getDirection() );
}

// Set the internal ray with known starting cell
inline void Navigator::setState( const Length position[3],
                                 const double direction[3],
                                 const EntityId start_cell )
{
  this->setState( position[0], position[1], position[2],
                  direction[0], direction[1], direction[2],
                  start_cell );
}

// Set the internal ray with known starting cell
inline void Navigator::setState( const Ray& ray,
                                 const EntityId start_cell )
{
  this->setState( ray.getPosition(), ray.getDirection() );
}

// Fire the internal ray through the geometry
inline auto Navigator::fireRay( EntityId& surface_hit ) -> Length
{
  return this->fireRay( &surface_hit );
}

// Fire the internal ray through the geometry
inline auto Navigator::fireRay() -> Length
{
  return this->fireRay( NULL );
}

// Advance the internal ray to the cell boundary
inline bool Navigator::advanceToCellBoundary( double* surface_normal )
{
  Length distance_traveled;
  const bool reflection = this->advanceToCellBoundaryImpl( surface_normal,
                                                           distance_traveled );

  if( d_on_advance_complete )
    d_on_advance_complete( distance_traveled );

  return reflection;
}

// Advance the internal ray to the cell boundary
inline bool Navigator::advanceToCellBoundary()
{
  return this->advanceToCellBoundary( NULL );
}

// Advance the internal ray by a substep (less than distance to boundary)
inline void Navigator::advanceBySubstep( const Length step_size )
{
  this->advanceBySubstepImpl( step_size );

  if( d_on_advance_complete )
    d_on_advance_complete( step_size );
}

// Change the internal ray direction
inline void Navigator::changeDirection( const double direction[3] )
{
  this->changeDirection( direction[0], direction[1], direction[2] );
}

// Convert an array to a string
template<typename T>
inline std::string Navigator::arrayToString( const T* data )
{
  return Utility::toString( Utility::ArrayView<const T>( data, data+3 ) );
}

/*! The geometry error
 * \details This error can be used to record geometry gaps/tacking errors
 */
class GeometryError : public std::runtime_error
{

public:

  GeometryError( const std::string& what )
    : std::runtime_error( what )
  { /* ... */ }
};

} // end Geometry namespace

#endif // end GEOMETRY_NAVIGATOR_HPP

//---------------------------------------------------------------------------//
// end Geometry_Navigator.hpp
//---------------------------------------------------------------------------//


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 6, 5> test_int_6_5(const Eigen::Matrix<int, 6, 5>& M) { return M; }
void export_int_6_5() { boost::python::def("test_int_6_5", test_int_6_5); }