//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// 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_HPP
#define BOOST_GRAPH_DIJKSTRA_HPP

#include <functional>
#include <boost/limits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/relax.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/exception.hpp>
#include <boost/pending/relaxed_heap.hpp>

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING
#  include <boost/pending/mutable_queue.hpp>
#endif // BOOST_GRAPH_DIJKSTRA_TESTING

namespace boost {

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING
  static bool dijkstra_relaxed_heap = true;
#endif

  template <class Visitor, class Graph>
  struct DijkstraVisitorConcept {
    void constraints() {
      function_requires< 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.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 dijkstra_visitor : public bfs_visitor<Visitors> {
  public:
    dijkstra_visitor() { }
    dijkstra_visitor(Visitors vis)
      : bfs_visitor<Visitors>(vis) { }

    template <class Edge, class Graph>
    void edge_relaxed(Edge e, Graph& g) {
      invoke_visitors(this->m_vis, e, g, on_edge_relaxed());
    }
    template <class Edge, class Graph>
    void edge_not_relaxed(Edge e, Graph& g) {
      invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed());
    }
  private:
    template <class Edge, class Graph>
    void tree_edge(Edge u, Graph& g) { }
  };
  template <class Visitors>
  dijkstra_visitor<Visitors>
  make_dijkstra_visitor(Visitors vis) {
    return dijkstra_visitor<Visitors>(vis);
  }
  typedef dijkstra_visitor<> default_dijkstra_visitor;

  namespace detail {

    template <class UniformCostVisitor, class UpdatableQueue,
      class WeightMap, class PredecessorMap, class DistanceMap,
      class BinaryFunction, class BinaryPredicate>
    struct dijkstra_bfs_visitor
    {
      typedef typename property_traits<DistanceMap>::value_type D;

      dijkstra_bfs_visitor(UniformCostVisitor vis, UpdatableQueue& Q,
                           WeightMap w, PredecessorMap p, DistanceMap d,
                           BinaryFunction combine, BinaryPredicate compare,
                           D zero)
        : m_vis(vis), m_Q(Q), m_weight(w), m_predecessor(p), m_distance(d),
          m_combine(combine), m_compare(compare), m_zero(zero)  { }

      template <class Edge, class Graph>
      void tree_edge(Edge e, Graph& g) {
        m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                            m_combine, m_compare);
        if (m_decreased)
          m_vis.edge_relaxed(e, g);
        else
          m_vis.edge_not_relaxed(e, g);
      }
      template <class Edge, class Graph>
      void gray_target(Edge e, Graph& g) {
        m_decreased = relax(e, g, m_weight, m_predecessor, m_distance,
                            m_combine, m_compare);
        if (m_decreased) {
          m_Q.update(target(e, g));
          m_vis.edge_relaxed(e, g);
        } else
          m_vis.edge_not_relaxed(e, g);
      }

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

      UniformCostVisitor m_vis;
      UpdatableQueue& m_Q;
      WeightMap m_weight;
      PredecessorMap m_predecessor;
      DistanceMap m_distance;
      BinaryFunction m_combine;
      BinaryPredicate m_compare;
      bool m_decreased;
      D m_zero;
    };

  } // namespace detail

  // Call breadth first search with default color map.
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistZero>
  inline void
  dijkstra_shortest_paths_no_init
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistZero zero,
     DijkstraVisitor vis)
  {
    std::vector<default_color_type> color(num_vertices(g));
    default_color_type c = white_color;
    dijkstra_shortest_paths_no_init( g, s, predecessor, distance, weight,
      index_map, compare, combine, zero, vis,
        make_iterator_property_map(&color[0], index_map, c));
  }

  // Call breadth first search
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistZero, class ColorMap>
  inline void
  dijkstra_shortest_paths_no_init
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistZero zero,
     DijkstraVisitor vis, ColorMap color)
  {
    typedef indirect_cmp<DistanceMap, Compare> IndirectCmp;
    IndirectCmp icmp(distance, compare);

    typedef typename graph_traits<VertexListGraph>::vertex_descriptor Vertex;

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING
    if (!dijkstra_relaxed_heap) {
      typedef mutable_queue<Vertex, std::vector<Vertex>, IndirectCmp, IndexMap>
        MutableQueue;

      MutableQueue Q(num_vertices(g), icmp, index_map);

      detail::dijkstra_bfs_visitor<DijkstraVisitor, MutableQueue, WeightMap,
        PredecessorMap, DistanceMap, Combine, Compare>
      bfs_vis(vis, Q, weight, predecessor, distance, combine, compare, zero);

      breadth_first_visit(g, s, Q, bfs_vis, color);
      return;
    }
#endif // BOOST_GRAPH_DIJKSTRA_TESTING

    typedef relaxed_heap<Vertex, IndirectCmp, IndexMap> MutableQueue;

    MutableQueue Q(num_vertices(g), icmp, index_map);

    detail::dijkstra_bfs_visitor<DijkstraVisitor, MutableQueue, WeightMap,
      PredecessorMap, DistanceMap, Combine, Compare>
        bfs_vis(vis, Q, weight, predecessor, distance, combine, compare, zero);

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

  // Initialize distances and call breadth first search with default color map
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistInf, class DistZero>
  inline void
  dijkstra_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistInf inf, DistZero zero,
     DijkstraVisitor vis)
  {
    std::vector<default_color_type> color(num_vertices(g));
    default_color_type c = white_color;
    dijkstra_shortest_paths(g, s, predecessor, distance, weight, index_map,
                            compare, combine, inf, zero, vis,
                            make_iterator_property_map(&color[0], index_map,
                                                       c));
  }

  // Initialize distances and call breadth first search
  template <class VertexListGraph, class DijkstraVisitor,
            class PredecessorMap, class DistanceMap,
            class WeightMap, class IndexMap, class Compare, class Combine,
            class DistInf, class DistZero, class ColorMap>
  inline void
  dijkstra_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
     IndexMap index_map,
     Compare compare, Combine combine, DistInf inf, DistZero zero,
     DijkstraVisitor vis, ColorMap color)
  {
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;
    typename graph_traits<VertexListGraph>::vertex_iterator ui, ui_end;
    for (tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui) {
      vis.initialize_vertex(*ui, g);
      put(distance, *ui, inf);
      put(predecessor, *ui, *ui);
      put(color, *ui, Color::white());
    }
    put(distance, s, zero);

    dijkstra_shortest_paths_no_init(g, s, predecessor, distance, weight,
                            index_map, compare, combine, zero, vis, color);
  }

  namespace detail {

    // Handle defaults for PredecessorMap and
    // Distance Compare, Combine, Inf and Zero
    template <class VertexListGraph, class DistanceMap, class WeightMap,
              class IndexMap, class Params, class ColorMap>
    inline void
    dijkstra_dispatch2
      (const VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       DistanceMap distance, WeightMap weight, IndexMap index_map,
       const Params& params, ColorMap color)
    {
      // Default for predecessor map
      dummy_property_map p_map;

      typedef typename property_traits<DistanceMap>::value_type D;
      dijkstra_shortest_paths
        (g, s,
         choose_param(get_param(params, vertex_predecessor), p_map),
         distance, weight, index_map,
         choose_param(get_param(params, distance_compare_t()),
                      std::less<D>()),
         choose_param(get_param(params, distance_combine_t()),
                      closed_plus<D>()),
         choose_param(get_param(params, distance_inf_t()),
                      (std::numeric_limits<D>::max)()),
         choose_param(get_param(params, distance_zero_t()),
                      D()),
         choose_param(get_param(params, graph_visitor),
                      make_dijkstra_visitor(null_visitor())),
         color);
    }

    template <class VertexListGraph, class DistanceMap, class WeightMap,
              class IndexMap, class Params, class ColorMap>
    inline void
    dijkstra_dispatch1
      (const VertexListGraph& g,
       typename graph_traits<VertexListGraph>::vertex_descriptor s,
       DistanceMap distance, WeightMap weight, IndexMap index_map,
       const Params& params, ColorMap color)
    {
      // Default for distance map
      typedef typename property_traits<WeightMap>::value_type D;
      typename std::vector<D>::size_type
        n = is_default_param(distance) ? num_vertices(g) : 1;
      std::vector<D> distance_map(n);

      // Default for color map
      typename std::vector<default_color_type>::size_type
        m = is_default_param(color) ? num_vertices(g) : 1;
      std::vector<default_color_type> color_map(m);

      detail::dijkstra_dispatch2
        (g, s, choose_param(distance, make_iterator_property_map
                            (distance_map.begin(), index_map,
                             distance_map[0])),
         weight, index_map, params,
         choose_param(color, make_iterator_property_map
                      (color_map.begin(), index_map,
                       color_map[0])));
    }
  } // namespace detail

  // Named Parameter Variant
  template <class VertexListGraph, class Param, class Tag, class Rest>
  inline void
  dijkstra_shortest_paths
    (const VertexListGraph& g,
     typename graph_traits<VertexListGraph>::vertex_descriptor s,
     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_dispatch1
      (g, s,
       get_param(params, vertex_distance),
       choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
       choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
       params,
       get_param(params, vertex_color));
  }

} // namespace boost

#endif // BOOST_GRAPH_DIJKSTRA_HPP


//****************************************************************************
// (c) 2008, 2009 by the openOR Team
//****************************************************************************
// The contents of this file are available under the GPL v2.0 license
// or under the openOR comercial license. see
//   /Doc/openOR_free_license.txt or
//   /Doc/openOR_comercial_license.txt
// for Details.
//****************************************************************************
//! OPENOR_INTERFACE_FILE(openOR_core)
//****************************************************************************
/**
 * @file
 * @author Christian Winne
 * @ingroup openOR_core
 */

#ifndef openOR_core_Math_matrixconcept_hpp
#define openOR_core_Math_matrixconcept_hpp

#include <boost/type_traits/add_const.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>
#include <boost/concept/usage.hpp>
#include <boost/concept_check.hpp>
#include <boost/math/quaternion.hpp>

#include <openOR/Math/utilities.hpp>
#include <openOR/Math/traits.hpp>
#include <openOR/Math/vector.hpp>
#include <openOR/Math/ublasmatrix.hpp>
#include <openOR/Math/ublasvector.hpp>

namespace openOR {
   namespace Math {

      template <int I, int J, typename P>
         typename if_const < P,
                     typename MatrixTraits<typename boost::remove_const<P>::type>::Access::ConstAccessType,
                     typename MatrixTraits<typename boost::remove_const<P>::type>::Access::AccessType >
      ::type get(P& p) {
         return MatrixTraits<typename boost::remove_const<P>::type>::Access::template get<I, J>(p);
      }

      namespace Impl {

         template < class Mat,
         int BeginRow = 0,
         int EndRow = MatrixTraits<Mat>::RowDimension::value,
         int BeginCol = 0,
         int EndCol = MatrixTraits<Mat>::ColDimension::value >
         class MatrixCompileTimeIterator {
               static const int SizeRow = EndRow - BeginRow;
               static const int SizeCol = EndCol - BeginCol;

            public:

               template<template<class, int, int> class Op>
               inline void apply() const {
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  Apply0<0, SizeRow * SizeCol, Op>()();
               }

               template<template<class, class, int, int> class Op, class Mat2>
               inline void apply(Mat& mat, const Mat2& mat2) {
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less_equal<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  Apply2<0, SizeRow * SizeCol, Op, Mat2>()(mat, mat2);
               }

               template<template <class, int, int> class Op, typename Result, template<class> class Reducer>
               inline Result reduce(const Mat& mat) const {
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  return Reduce1 < 0, SizeRow * SizeCol - 1, Op, Result, Reducer > ()(mat);
               }

               template<template <class, class, int, int> class Op, class Mat2, typename Result, template<class> class Reducer>
               inline Result reduce(const Mat& mat, const Mat2& mat2) const {
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginRow>, boost::mpl::int_<EndRow> >));
                  BOOST_MPL_ASSERT((boost::mpl::less<boost::mpl::int_<BeginCol>, boost::mpl::int_<EndCol> >));
                  return Reduce2 < 0, SizeRow * SizeCol - 1, Op, Mat2, Result, Reducer > ()(mat, mat2);
               }

            private:

               template<int I, int Count, template<class, int, int> class Op>
               struct Apply0 {
                  inline void operator()() const {
                     Op < Mat, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()();
                     Apply0 < I + 1, Count, Op > ()();
                  }
               };

               template<int Count, template<class, int, int> class Op>
               struct Apply0<Count, Count, Op> {
                  inline void operator()() const {}
               };

               template<int I, int Count, template<class, class, int, int> class Op, class Mat2>
               struct Apply2 {
                  inline void operator()(Mat& mat, const Mat2& mat2) const {
                     Op < Mat, Mat2, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()(mat, mat2);
                     Apply2 < I + 1, Count, Op, Mat2 > ()(mat, mat2);
                  }
               };

               template<int Count, template<class, class, int, int> class Op, class Mat2>
               struct Apply2<Count, Count, Op, Mat2> {
                  inline void operator()(Mat& mat, const Mat2& mat2) const {}
               };

               template<int I, int Count, template <class, int, int> class Op, typename Result, template<class> class Reducer>
               struct Reduce1 {
                  inline Result operator()(const Mat& mat) const {
                     return Reducer<Result>()(Op < Mat, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()(mat),
                                              Reduce1 < I + 1, Count, Op, Result, Reducer > ()(mat));
                  }
               };

               template<int Count, template <class, int, int> class Op, typename Result, template<class> class Reducer>
               struct Reduce1<Count, Count, Op, Result, Reducer> {
                  inline Result operator()(const Mat& mat) {
                     return Op < Mat, BeginRow + (Count % SizeRow), BeginCol + (Count / SizeRow) > ()(mat);
                  }
               };

               template<int I, int Count, template <class, class, int, int> class Op, class Mat2, typename Result, template<class> class Reducer>
               struct Reduce2 {
                  inline Result operator()(const Mat& mat, const Mat2& mat2) {
                     return Reducer<Result>()(Op < Mat, Mat2, BeginRow + (I % SizeRow), BeginCol + (I / SizeRow) > ()(mat, mat2),
                                              Reduce2 < I + 1, Count, Op, Mat2, Result, Reducer > ()(mat, mat2));
                  }
               };

               template<int Count, template <class, class, int, int> class Op, class Mat2, typename Result, template<class> class Reducer>
               struct Reduce2<Count, Count, Op, Mat2, Result, Reducer> {
                  inline Result operator()(const Mat& mat, const Mat2& mat2) {
                     return Op < Mat, Mat2, BeginRow + (Count % SizeRow), BeginCol + (Count / SizeRow) > ()(mat, mat2);
                  }
               };

         };

      }

      namespace Concept {

         template <typename Type>
         class ConstMatrix {
            private:

               BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));

               typedef typename MatrixTraits<Type>::ValueType ValueType;
               typedef typename MatrixTraits<Type>::RowVectorType RowVectorType;
               typedef typename MatrixTraits<Type>::ColVectorType ColVectorType;
               enum { ROW_DIMENSION = MatrixTraits<Type>::RowDimension::value,
                      COL_DIMENSION = MatrixTraits<Type>::ColDimension::value
                 };

               template <class M, int I, int J>
               struct AccessCheck {
                  inline void operator()() const {
                     const M* p = 0;
                     typename MatrixTraits<M>::ValueType val(Math::get<I, J>(*p));
                     (void)sizeof(val); // To avoid "unused variable" warnings
                  }
               };

            public :
               /// BCCL macro to check the ConstPoint concept
               BOOST_CONCEPT_USAGE(ConstMatrix) {
                  Impl::MatrixCompileTimeIterator<Type, 0, ROW_DIMENSION, 0, COL_DIMENSION>().template apply<AccessCheck>();
                  Type* pMat = NULL;
                  const RowVectorType* pRowVector = NULL;
                  ColVectorType colvec(Math::prod(*pMat, *pRowVector));
               }
         };

         template<class Type>
         class Matrix : public boost::DefaultConstructible<Type>, public boost::CopyConstructible<Type>, public boost::Assignable<Type> {
            private:

               BOOST_CONCEPT_ASSERT((Concept::ConstMatrix<Type>));
               BOOST_MPL_ASSERT((typename MatrixTraits<Type>::IsMatrix));

               typedef typename MatrixTraits<Type>::ValueType ValueType;
               enum { ROW_DIMENSION = MatrixTraits<Type>::RowDimension::value,
                      COL_DIMENSION = MatrixTraits<Type>::ColDimension::value
                 };

               template <class M, int I, int J>
               struct AccessCheck {
                  void operator()() const {
                     M* p = NULL;
                     get<I, J>(*p) = typename MatrixTraits<M>::ValueType();
                  }
               };

               template<typename P, int RowCount, int ColCount>
               struct MatrixProductChecker {
                  static void check() {}
               };

               template<typename P, int Count>
               struct MatrixProductChecker<P, Count, Count> {
                  static void check() {
                     P* p = 0;
                     *p = Math::prod(*p, *p);
                  }
               };

            public:
               /// BCCL macro to check the Point concept
               BOOST_CONCEPT_USAGE(Matrix) {
                  Impl::MatrixCompileTimeIterator<Type, 0, ROW_DIMENSION, 0, COL_DIMENSION>().template apply<AccessCheck>();
                  MatrixProductChecker<Type, ROW_DIMENSION, COL_DIMENSION>::check();
                  ValueType v;
                  Type t;
                  t *= v;
                  t = v * t;
                  t = t * v;
                  t = t + t;
                  t = t - t;
                  t += t;
                  t -= t;
               }
         };

      }

   }
}

#endif


// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2014 The University of Sydney
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// 1. Redistributions of source code must retain the above copyright
//    notice, this list of conditions and the following disclaimer.
// 2. Redistributions in binary form must reproduce the above copyright
//    notice, this list of conditions and the following disclaimer in the
//    documentation and/or other materials provided with the distribution.
// 3. Neither the name of the University of Sydney nor the
//    names of its contributors may be used to endorse or promote products
//    derived from this software without specific prior written permission.
//
// NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE
// GRANTED BY THIS LICENSE.  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT
// HOLDERS AND CONTRIBUTORS \"AS IS\" AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
// MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
// BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY,
// WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
// OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN
// IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

#include <cmath>
#include <Eigen/Eigen>
#include <comma/base/exception.h>
#include "../rotation_matrix.h"
#include "polytope.h"

namespace snark { namespace geometry {

convex_polytope::convex_polytope( const std::vector< Eigen::VectorXd >& normals, const std::vector< double >& distances )
    : normals_( normals.size(), normals[0].size() )
    , distances_( distances.size() )
{
    if( !normals.size() || !normals[0].size() ) { COMMA_THROW( comma::exception, "normals cannot be empty or have zero dimensions" ); }
    if( normals.size() != distances.size() ) { COMMA_THROW( comma::exception, "normals and distances should be of same size, got "<< normals.size()<<" normals and " << distances.size() << " distances" ); }
    for( unsigned int i = 0; i < normals.size(); ++i )
    {
        normals_.row( i ) = normals[i].transpose().normalized();
        distances_( i ) = distances[i];
    }
}

convex_polytope::convex_polytope( const Eigen::MatrixXd& normals, const Eigen::VectorXd& distances )
    : normals_( normals )
    , distances_( distances )
{
    if( !normals.size() ) { COMMA_THROW( comma::exception, "normals cannot be empty" ); }
    if( normals.rows() != distances.size() ) { COMMA_THROW( comma::exception, "normals and distances should be of same size, got "<< normals.rows() << " normals and " << distances.size() << " distances" ); }
    for( unsigned int i = 0; i < normals.rows(); ++i ) { normals_.row( i ).normalize(); }
}

convex_polytope convex_polytope::transformed( const Eigen::Vector3d& translation, const snark::roll_pitch_yaw& orientation ) const
{
    convex_polytope p( *this );
    const Eigen::MatrixXd& rotation_matrix = snark::rotation_matrix::rotation( orientation );
    for( unsigned int i = 0; i < normals_.rows(); ++i ) // todo: quick and dirty; can it be done in a single operation?
    {
        p.normals_.row( i ) = rotation_matrix * normals_.row( i ).transpose();
        const Eigen::VectorXd& n = p.normals_.row( i );
        p.distances_[i] = n.dot( n * distances_[i] + translation );
    }
    return p;
}

bool convex_polytope::has( const Eigen::VectorXd &rhs, double epsilon ) const
{
    if( rhs.size() != normals_.cols() ) { COMMA_THROW( comma::exception, "expected same dimension as polytope: "<< normals_.cols()<<"; got dimension: " << rhs.size() ); }
    return ( ( normals_ * rhs - distances_ ).array() <= epsilon ).all();
}

} } // namespace snark{ { namespace geometry {


#include <iostream>

#include <vector>
using namespace std;

#include <boost/timer.hpp>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "DM_NRG.hpp"

void DM_NRG_CalcRhoN(vector<double> ParamsTemp,
		     bool UseFDM,CNRGbasisarray* pAcutN,
		     CNRGbasisarray* pAcutNp1,
		     CNRGbasisarray* pAbasisNp1,
		     CNRGmatrix* pRhoN,
		     CNRGmatrix* pRhoNp1){

  // New routine, which rotates the matrix first with uBLAS (!)
  // FASTER... like a Ferrari compared to an old Volkswagen beetle.

  vector< vector<int> > ChildSt_SameType;
  vector< vector<int> >::iterator arrayit;
  vector<int>::iterator iit;

  //Trace...
  double trace=0.0;
  double traceKK=0.0;
  double traceDD=0.0;

  // Check time
  boost::timer MyTime;
  double time_elapsed;

  bool debug=false;

  CNRGmatrix RhoNp1basis;

  double TempBar=ParamsTemp[0];
  double ZNdisc=ParamsTemp[1];
  double betabar=1/TempBar;

  cout << " DM-NRG: Calculating Rho N ..." << endl;

  cout << " Rotating Rho N... " << endl;

  MyTime.restart();
  RotateMatrix(pRhoNp1,pAcutNp1,(&RhoNp1basis));
  time_elapsed=MyTime.elapsed();

  cout << " ... done in " << time_elapsed << " secs. " << endl;

  int ThisBl=0;
//   if (pAcutN->Nshell==4){
//     cout << " Rho_N=5: " << endl;
//     pRhoNp1->PrintAllBlocks();
// //     pRhoNp1->PrintMatBlock(11,11);
// //     RhoNp1basis.PrintMatBlock(11,11);
//     ThisBl=11;
//     cout << " ibl = " << ThisBl << endl;
//     cout << " pAcutNp1 block structure: " << endl;
//     pAcutNp1->PrintBlockQNumbers(ThisBl);
//     cout << " Block " << ThisBl << " goes from ist = "
// 	 << pAcutNp1->GetBlockLimit(ThisBl,0)
// 	 << " to "
// 	 << pAcutNp1->GetBlockLimit(ThisBl,1)
// 	 << endl;
//     cout << " RhoNp1basis block structure: " << endl;
//     RhoNp1basis.PrintBlockQNumbers(ThisBl);
//     cout << " Block " << ThisBl << " goes from ist = "
// 	 << RhoNp1basis.GetBlockLimit(ThisBl,0)
// 	 << " to "
// 	 << RhoNp1basis.GetBlockLimit(ThisBl,1)
// 	 << endl;
//     ThisBl=18;
//     cout << " pAbasisNp1 block structure: " << endl;
// //     pAbasisNp1->PrintBlockQNumbers(ThisBl);
// //     cout << " Block " << ThisBl << " goes from ist = "
// // 	 << pAbasisNp1->GetBlockLimit(ThisBl,0)
// // 	 << " to "
// // 	 << pAbasisNp1->GetBlockLimit(ThisBl,1)
// // 	 << endl;
// //     pAbasisNp1->PrintBlockBasis(ThisBl);
//   }

  // Initial set up
  pRhoN->ClearAll();
  pRhoN->SyncNRGarray(*pAcutN);
  pRhoN->UpperTriangular=true;
  pRhoN->IsComplex=pRhoNp1->IsComplex;

  // Calculate matrix elements
  int i1=0;
  for (int ibl=0;ibl<pAcutN->NumBlocks();ibl++){

    // Rho is block diagonal
    pRhoN->MatBlockMap.push_back(ibl);
    pRhoN->MatBlockMap.push_back(ibl);
    pRhoN->MatBlockBegEnd.push_back(i1);

    int ist0=pAcutN->GetBlockLimit(ibl,0);
    int ist1=pAcutN->GetBlockLimit(ibl,1);

    // testing...

    // Get Sold from ibl

    int BlSize=pAcutN->GetBlockSize(ibl);
    if (debug)
      cout << " Setting Block " << ibl << " of " << pAcutN->NumBlocks()-1
	   << "  Size : " << BlSize << " ... ";

    MyTime.restart();
    // Loop in block states (and calculate rhoN(ist,jst))
    for (int ist=ist0;ist<=ist1;ist++){
      for (int jst=ist;jst<=ist1;jst++){

	// Ok, watch out for the " type" thing for SU(2) symmetry

	DM_NRG_SetChildSameType(ist,jst,pAcutN,pAbasisNp1,ChildSt_SameType);

	double auxMatEl=0.0;
	complex<double> cMatEl=ZeroC;
	// Loop over blocks in ChildSt_SameType (N+1)
	for (arrayit=ChildSt_SameType.begin();
	     arrayit<ChildSt_SameType.end();arrayit++){
	  iit=(*arrayit).begin();
	  int iblbasis_Np1=(*iit);
	  int istbasis_Np1=(*(iit+1));
	  int jstbasis_Np1=(*(iit+2));

	  // Get S_Np1 from iblbasis_Np1
	  // Get S_type= from istbasis_Np1
	  // Calculate Clebsh-Gordan coefs.

	  //
	  // No need to find corresponding block in AcutNp1:
	  //
	  // if block is not there, then
	  // RhoNp1basis.GetMatEl(istbasis_Np1,jstbasis_Np1)
	  // will return 0.0 (in principle!)
	  //

	  if (!(RhoNp1basis.IsComplex))
	    auxMatEl+=RhoNp1basis.GetMatEl(istbasis_Np1,jstbasis_Np1);
	  else
	    cMatEl+=RhoNp1basis.cGetMatEl(istbasis_Np1,jstbasis_Np1);

//  	  if ( (debug)&&(iblbasis_Np1==18)&&(ist==jst) ){
// 	    cout << " ibl = " << ibl
// 		 << " ist = " << ist
// 		 << " jst = " << jst
// 		 << " iblbasis_Np1 = " << iblbasis_Np1
// 		 << " istbasis_Np1 = " << istbasis_Np1
// 		 << " jstbasis_Np1 = " << jstbasis_Np1
// 		 << " mat el = " << RhoNp1basis.GetMatEl(istbasis_Np1,jstbasis_Np1)
// 		 << " auxMatEl = " << auxMatEl
// 		 << endl;
// 	  }

	}
	// end loop in Child states
	// What if states are NOT kept
	// // Diagonal only
	if ( (UseFDM)&&(jst==ist)&&(pAcutN->CheckKept(ist,false)) ){
// 	  cout << " Disc-disc : ist=" << ist <<  endl;
// 	  cout << " size ChildSt[ist]= " << pAcutN->ChildStates[ist].size();
	  if ( dNEqual(ZNdisc,0.0) )
	    auxMatEl=exp(-betabar*(pAcutN->dEn[ist]))/ZNdisc;
// 	  cout << " auxMatEl= " << auxMatEl;
//	  cout << endl;
	}
	// end if discarded, discarded

	if (ist==jst){
	  trace+=auxMatEl;
	  if (pAcutN->Kept[ist]) traceKK+=auxMatEl;
	  else traceDD+=auxMatEl;

	}
	// diagonal terms
	if (!(pRhoN->IsComplex))
	  pRhoN->MatEl.push_back(auxMatEl);
	else
	  pRhoN->MatElCplx.push_back(cMatEl);

	i1++;
      }
      // end loop in jst
    }
    // end loop in ist
    pRhoN->MatBlockBegEnd.push_back(i1-1);

    time_elapsed=MyTime.elapsed();
    if (debug)
     cout << " ... done in " << time_elapsed << " secs." << endl;

  }
  // end loop in pAcutN blocks

  cout << " ... Rho N  done. " << endl;
  cout << " Trace = " << trace
       << " TraceKK = " << traceKK
       << " TraceDD = " << traceDD << endl;

//   if (pAcutN->Nshell==4){
//     cout << " Rho_N=4: " << endl;
//     //pRhoN->PrintMatBlock(10,10);
//     ThisBl=10;
//     cout << " ibl = " << ThisBl << endl;
//     cout << " pRhoN block structure: " << endl;
//     pRhoN->PrintBlockQNumbers(ThisBl);
//     cout << " Block " << ThisBl << " goes from ist = "
// 	 << pRhoN->GetBlockLimit(ThisBl,0)
// 	 << " to "
// 	 << pRhoN->GetBlockLimit(ThisBl,1)
// 	 << endl;
//     //pRhoN->PrintMatBlock(13,13);
//     ThisBl=13;
//     cout << " ibl = " << ThisBl << endl;
//     cout << " pRhoN block structure: " << endl;
//     pRhoN->PrintBlockQNumbers(ThisBl);
//     cout << " Block " << ThisBl << " goes from ist = "
// 	 << pRhoN->GetBlockLimit(ThisBl,0)
// 	 << " to "
// 	 << pRhoN->GetBlockLimit(ThisBl,1)
// 	 << endl;
//   }

}
// end set RhoN new

/////////////////////////////////////////
/////   Version with SU(2) symmetry /////
/////////////////////////////////////////

void DM_NRG_CalcRhoN_withSU2(vector<double> ParamsTemp,
			     bool UseFDM,
			     CNRGbasisarray* pAcutN,
			     CNRGbasisarray* pAcutNp1,
			     CNRGbasisarray* pAbasisNp1,
			     CNRGbasisarray* pSingleSite,
			     CNRGmatrix* pRhoN,
			     CNRGmatrix* pRhoNp1){

  // New routine, which rotates the matrix first with uBLAS (!)
  // FASTER... like a Ferrari compared to an old Volkswagen beetle.

  vector< vector<int> > ChildSt_SameType;
  vector< vector<int> >::iterator arrayit;
  vector<int>::iterator iit;

  // Check time
  boost::timer MyTime;
  double time_elapsed;

  //Trace...
  double trace=0.0;

  bool debug=false;

  CNRGmatrix RhoNp1basis;

  double TempBar=ParamsTemp[0];
  double ZNdisc=ParamsTemp[1];
  double betabar=1/TempBar;

  cout << " DM-NRG: Calculating Rho N (w SU(2))..." << endl;

  cout << " Rotating Rho N... " << endl;

  MyTime.restart();
  RotateMatrix(pRhoNp1,pAcutNp1,(&RhoNp1basis));
  time_elapsed=MyTime.elapsed();

  cout << " ... done in " << time_elapsed << " secs. " << endl;

  int ThisBl=0;

  // Initial set up
  pRhoN->ClearAll();
  pRhoN->SyncNRGarray(*pAcutN);
  pRhoN->UpperTriangular=true;

  // Calculate matrix elements
  int i1=0;
  for (int ibl=0;ibl<pAcutN->NumBlocks();ibl++){

    // Rho is block diagonal
    pRhoN->MatBlockMap.push_back(ibl);
    pRhoN->MatBlockMap.push_back(ibl);
    pRhoN->MatBlockBegEnd.push_back(i1);

    int ist0=pAcutN->GetBlockLimit(ibl,0);
    int ist1=pAcutN->GetBlockLimit(ibl,1);

    // testing...

    // Get Sold from ibl
    double Sold=0.0;
    if (pAcutN->totalS){
      Sold=pAcutN->GetQNumber(ibl,pAcutN->Sqnumbers[0]);
      // only a single SU(2) for now
    }

    int BlSize=pAcutN->GetBlockSize(ibl);
    if (debug)
      cout << " Setting Block " << ibl << " of " << pAcutN->NumBlocks()-1
	   << "  Size : " << BlSize << " ... ";

    MyTime.restart();
    // Loop in block states (and calculate rhoN(ist,jst))
    for (int ist=ist0;ist<=ist1;ist++){
      for (int jst=ist;jst<=ist1;jst++){

	// Ok, watch out for the " type" thing for SU(2) symmetry
	DM_NRG_SetChildSameType(ist,jst,pAcutN,pAbasisNp1,ChildSt_SameType);

	double auxMatEl=0.0;
	complex<double> cMatEl=ZeroC;
	// Loop over blocks in ChildSt_SameType (N+1)
	for (arrayit=ChildSt_SameType.begin();
	     arrayit<ChildSt_SameType.end();arrayit++){
	  iit=(*arrayit).begin();
	  int iblbasis_Np1=(*iit);
	  int istbasis_Np1=(*(iit+1));
	  int jstbasis_Np1=(*(iit+2));

	  // Get S_Np1 from iblbasis_Np1
	  double Si=0.0;
	  if (pAbasisNp1->totalS){
	    Si=pAbasisNp1->GetQNumber(iblbasis_Np1,pAbasisNp1->Sqnumbers[0]);
	    // only a single SU(2) for now
	  }
	  // Get S_type= from istbasis_Np1
	  int itype_Np1=pAbasisNp1->iType[istbasis_Np1];
	  double Stilde=0.0;
	  if (pSingleSite->totalS){
	    //Stilde=pSingleSite->GetQNumber(ibls,pSingleSite->Sqnumbers[0]);
	    Stilde=pSingleSite->GetQNumberFromSt(itype_Np1,pSingleSite->Sqnumbers[0]);
	    // only a single SU(2) for now
	  }

	  // Calculate Clebsh-Gordan coefs.

	  double CGfactor=0.0;
	  for (double Sz=-Si;Sz<=Si;Sz+=1.0){
	    double auxCG=CGordan(Sold,Sold,Stilde,Sz-Sold,Si,Sz);
	    CGfactor+=auxCG*auxCG; // Square it!! (oct 2010)
	  }
	  // end calcCG

	  //
	  // No need to find corresponding block in AcutNp1:
	  //
	  // if block is not there, then
	  // RhoNp1basis.GetMatEl(istbasis_Np1,jstbasis_Np1)
	  // will return 0.0 (in principle!)
	  //
	  //auxMatEl+=CGfactor*RhoNp1basis.GetMatEl(istbasis_Np1,jstbasis_Np1);
	  if (!(RhoNp1basis.IsComplex))
	    auxMatEl+=CGfactor*RhoNp1basis.GetMatEl(istbasis_Np1,jstbasis_Np1);
	  else
	    cMatEl+=CGfactor*RhoNp1basis.cGetMatEl(istbasis_Np1,jstbasis_Np1);

	}
	// end loop in Child states
	// What if states are NOT kept
	// // Diagonal only
	// What about the CB coefficient? Should we do (???)
	//auxExp=(2.0*Sold+1.0)*exp(-betabar*pAcutNp1->dEn[ist]);
	// NO! IT GIVES WRONG RESULTS...

	if ( (UseFDM)&&(jst==ist)&&(pAcutN->CheckKept(ist,false)) ){
	  //cout << " Disc-disc : ist=" << ist <<  endl;
	  //cout << " size ChildSt[ist]= " << pAcutN->ChildStates[ist].size();
	  if ( dNEqual(ZNdisc,0.0) )
	    auxMatEl=exp(-betabar*(pAcutN->dEn[ist]))/ZNdisc;
	}
	// end if discarded, discarded

	if (ist==jst) trace+=(2.0*Sold+1.0)*auxMatEl;

	if (pRhoN->IsComplex)
	  pRhoN->MatElCplx.push_back(cMatEl);
	else
	  pRhoN->MatEl.push_back(auxMatEl);

	i1++;
      }
      // end loop in jst
    }
    // end loop in ist
    pRhoN->MatBlockBegEnd.push_back(i1-1);

    time_elapsed=MyTime.elapsed();
    if (debug)
     cout << " ... done in " << time_elapsed << " secs." << endl;

  }
  // end loop in pAcutN blocks

  cout << " ... Rho N  done. Trace(2Si+1) = " << trace << endl;

//   if (pAcutN->Nshell==4){
//     cout << " Rho_N=4: " << endl;
//     //pRhoN->PrintMatBlock(10,10);
//     ThisBl=10;
//     cout << " ibl = " << ThisBl << endl;
//     cout << " pRhoN block structure: " << endl;
//     pRhoN->PrintBlockQNumbers(ThisBl);
//     cout << " Block " << ThisBl << " goes from ist = "
// 	 << pRhoN->GetBlockLimit(ThisBl,0)
// 	 << " to "
// 	 << pRhoN->GetBlockLimit(ThisBl,1)
// 	 << endl;
//     //pRhoN->PrintMatBlock(13,13);
//     ThisBl=13;
//     cout << " ibl = " << ThisBl << endl;
//     cout << " pRhoN block structure: " << endl;
//     pRhoN->PrintBlockQNumbers(ThisBl);
//     cout << " Block " << ThisBl << " goes from ist = "
// 	 << pRhoN->GetBlockLimit(ThisBl,0)
// 	 << " to "
// 	 << pRhoN->GetBlockLimit(ThisBl,1)
// 	 << endl;
//   }

}
// end set RhoN new

/////////////////////////////////////////
/////////////////////////////////////////
/////////////////////////////////////////

void DM_NRG_CalcRhoN_old(CNRGbasisarray* pAcutN,
		     CNRGbasisarray* pAcutNp1,
		     CNRGbasisarray* pAbasisNp1,
		     CNRGmatrix* pRhoN,
		     CNRGmatrix* pRhoNp1){

  // Poor old routine, which does not use uBLAS... (slow old man)

  vector< vector<int> > ChildSt_SameType;
  vector< vector<int> >::iterator arrayit;
  vector<int>::iterator iit;

  // Check time
  boost::timer MyTime;
  double time_elapsed;

  cout << " DM-NRG: Calculating Rho N ..." << endl;

  // Initial set up
  pRhoN->ClearAll();
  pRhoN->SyncNRGarray(*pAcutN);
  pRhoN->UpperTriangular=true;

  // Calculate matrix elements
  int i1=0;
  for (int ibl=0;ibl<pAcutN->NumBlocks();ibl++){
  // Testing in a few blocks
  //for (int ibl=0;ibl<=8;ibl+=8){
    // Rho is block diagonal
    pRhoN->MatBlockMap.push_back(ibl);
    pRhoN->MatBlockMap.push_back(ibl);
    pRhoN->MatBlockBegEnd.push_back(i1);

    int ist0=pAcutN->GetBlockLimit(ibl,0);
    int ist1=pAcutN->GetBlockLimit(ibl,1);

    // testing...

    int BlSize=pAcutN->GetBlockSize(ibl);
    cout << " Setting Block " << ibl << " of " << pAcutN->NumBlocks()-1
	 << "  Size : " << BlSize << " ... ";

    MyTime.restart();
    // Loop in block states (and calculate rhoN(ist,jst))
    for (int ist=ist0;ist<=ist1;ist++){
      for (int jst=ist;jst<=ist1;jst++){

	double auxMatEl=0.0;
	// Debugging
// 	if ( (ist==61)&&(jst==61) ){
// 	  cout << " ist = " << ist << " jst = " << jst << endl;
// 	}

	DM_NRG_SetChildSameType(ist,jst,pAcutN,pAbasisNp1,ChildSt_SameType);

// 	if ( (ist==61)&&(jst==61) ){
// 	  cout << "bl  ist_Np1  jst_Np1 " << endl;
// 	  for (arrayit=ChildSt_SameType.begin();
// 	       arrayit<ChildSt_SameType.end();arrayit++){
// 	    for(iit=(*arrayit).begin();iit<(*arrayit).end();iit++){
// 	      cout << (*iit) << " ";
// 	    }
// 	    cout << endl;
// 	  }
// 	}
	// debug

	// Loop over blocks in ChildSt_SameType (N+1)

	for (arrayit=ChildSt_SameType.begin();
	     arrayit<ChildSt_SameType.end();arrayit++){
	  iit=(*arrayit).begin();
	  int iblbasis_Np1=(*iit);
	  int istbasis_Np1=(*(iit+1));
	  int jstbasis_Np1=(*(iit+2));

// 	  if ( (ist==61)&&(jst==61) ){
// 	    cout << " iblbasis_Np1 = " << iblbasis_Np1
// 		 << " istbasis_Np1 = "<< istbasis_Np1
// 		 << " jstbasis_Np1 = "<< jstbasis_Np1
// 		 << endl;
// 	  }
	  //
	  // Find corresponding block in AcutNp1 (compare QNumbers)
	  //
	  double* qnums=new double [pAbasisNp1->NQNumbers];
	  for (int iqn=0;iqn<pAbasisNp1->NQNumbers;iqn++){
	    qnums[iqn]=pAbasisNp1->GetQNumber(iblbasis_Np1,iqn);
	  }
	  int ibl_Np1=pAcutNp1->GetBlockFromQNumbers(qnums);
// 	  if (ibl_Np1==-1){
// 	    cout << " Block  not found in Acut N+1: ";
// 	    cout << " QNs = " ;
// 	    for (int iqn=0;iqn<pAbasisNp1->NQNumbers;iqn++){
// 	      cout << qnums[iqn] << " ";
// 	    }
// 	    cout << endl;
// 	  }
	  // if block not found
	  delete [] qnums;

// 	  if ( (ist==61)&&(jst==61) ){
// 	    cout << " ibl_Np1 = " << ibl_Np1 << endl;
// 	    if (ibl_Np1>0)
// 	      cout << " states: from " << pAcutNp1->GetBlockLimit(ibl_Np1,0)
// 		   << " to " << pAcutNp1->GetBlockLimit(ibl_Np1,1) << endl;
// 	  }
	  // debug
	  // Loop over states in the block
	  if (ibl_Np1>0){ // Block exists in cut basis (kept)
	    for(int ist_Np1=pAcutNp1->GetBlockLimit(ibl_Np1,0);
		ist_Np1<=pAcutNp1->GetBlockLimit(ibl_Np1,1);
		ist_Np1++){
	      for(int jst_Np1=pAcutNp1->GetBlockLimit(ibl_Np1,0);
		  jst_Np1<=pAcutNp1->GetBlockLimit(ibl_Np1,1);
		  jst_Np1++){
	       double U_wi=pAcutNp1->GetEigVecComponent(ist_Np1,istbasis_Np1);
	       double U_wpj=pAcutNp1->GetEigVecComponent(jst_Np1,jstbasis_Np1);
	       double rho_wwp=pRhoNp1->GetMatEl(ist_Np1,jst_Np1);

	       auxMatEl+=rho_wwp*U_wi*U_wpj;

// 	      if ( (ist==61)&&(jst==61) ){
// 		cout << " ist_Np1 = " << ist_Np1
// 		     << " jst_Np1 = " << jst_Np1
// 		     << endl;
// 		cout << " U_wi= " << U_wi
// 		     << " U_wpj= " << U_wpj
// 		     << " rho_wwp= " << rho_wwp
// 		     << endl;
// 		cout << " auxMatEl = " << auxMatEl << endl;
// 	      }
	      // debug
	      }
	      // end loop in j_states in ibl_Np1
	    }
	    // end loop in i_states in ibl_Np1
	  }
	  // end if block exists in Acut

// 	  if ( (ist==61)&&(jst==61) ){
// 	    cout << " auxMatEl = " << auxMatEl << endl;
// 	  }

	}
	// end loop in ChildSt_SameType

	pRhoN->MatEl.push_back(auxMatEl);
	i1++;
      }
      // loop over jst
    }
    // loop over ist
    pRhoN->MatBlockBegEnd.push_back(i1-1);

    time_elapsed=MyTime.elapsed();
    cout << " ... done in " << time_elapsed << " secs." << endl;

  }
  // end loop in blocks

  cout << " ... Rho N done." << endl;

}
// end subroutine

/////////////////////////////////////////
/////////////////////////////////////////
/////////////////////////////////////////


//  bpp.cpp
//  PhyloAcc
//
//  Created by hzr on 3/8/16.
//  Copyright © 2016 hzr. All rights reserved.
//

#include "bpp.hpp"
#include <armadillo>
#include <sys/types.h>
#include <dirent.h>
#include<queue>

#include <cmath>
#include <cassert>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cdf.h>

#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <ctype.h>

#include "newick.h"
#include "utils.h"
#include "bpp_c.hpp"

using namespace std;
using namespace arma;

// load the phylogenetic tree
void BPP::InitPhyloTree(PhyloTree & tree) //, double indel_pi), double indel, double indel2
{

    submat = tree.subs_rate;
    children    = new int[N][2];
    parent      = new int[N];
    distances   = new double[N];
    thetas   = new double[N];
    heights = vector<double>(N);
    move_br = vector<int>(N);

    for(int s=0; s<N; s++)
    {
        distances[s] = tree.distances[s];
        thetas[s] = tree.thetas[s];
        heights[s] = 0; // root
    }

    for(int i=0; i<N; i++)
    {
        children[i][0] = -1;
        children[i][1] = -1;
        parent[i] = N;
    }

    for(int i=0; i<N; i++)
    {
        int p = -1;
        for(int j=0; j<N; j++)
        {
            if (tree.dag[i][j])
            {
                p++;
                children[i][p] = j;
                parent[j] = i;
            }
        }
    }

    for(int i = N-2; i >= 0; i--)
    {
        heights[i] = heights[parent[i]] - distances[i]; //heights<=0. root=0. Distance: br length. +ve.
    }
}

// try to match the phylogenetic profile and tree
void BPP::MatchProfAndTree(PhyloProf & _prof, PhyloTree & _tree)
{
    // try to match the species
    bool success_match = true;
    int S = _tree.S;
    int S2 = _prof.S;
    vector<int> reorder(S);  //each species in the tree where is in prof
    for(int s1=0; s1<S; s1++)
    {
        bool has_same_species = false;
        string sname1 = _tree.species_names[s1];
        for(int s2=0; s2<S2; s2++)
        {
            string sname2 = _prof.species_names[s2];
            if (sname1 == sname2)
            {
                has_same_species = true;
                reorder[s1] = s2;
                break;
            }
        }
        if (!has_same_species)
        {
            cout << "No matrix species " << _prof.species_names[s1] << " found in tree." << endl;
            success_match = false;
            break;
        }
    }

    if (!success_match) // if cannot match literally
    {
        cout << endl << "The species in phylogenetic profile and tree cannot be matched literally:" << endl;
        cout << "The program will use the default mapping in data:" << endl;
        for(int s=0; s<S; s++)
            cout << "(" << _prof.species_names[s] << "\t=  " << _tree.species_names[s] << ")" << endl;
        cout << endl;
    }
    else                // if successully matched
    {
        cout << "The species in profile and tree match perfectly. Reorder the species in profile matrix by the tree." << endl << endl;
        vector<string> old_X = _prof.X;
        for(int s=0; s<S; s++)
        {
            int reorder_s = reorder[s];
            _prof.X[s] = old_X[reorder_s];
        }
        _prof.species_names = _tree.species_names;
    }

}

void BPP::InitMCMC(int _num_burn, int _num_mcmc, int _num_thin)
{
    // init parameters
    num_burn = _num_burn;

    num_mcmc = _num_mcmc;
    num_thin = _num_thin;

    last_time = time(NULL);

    // init MCMC sampling storage
    Max_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));
    cur_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));

    genetrees = vector<vector<string>>(3, vector<string>(C)); // for max gene tree

    log_liks_null = vector <double>(C,0);
    log_liks_null_L = vector <double>(C,0);
    log_liks_Z = vector<vector <double>>(3,vector <double>(C,0));
    log_liks_sgl = vector <double>(C,0);
    log_liks_resZ = vector <double>(C,0);
    log_liks_sgl_L = vector <double>(C,0);
    log_liks_resZ_L = vector <double>(C,0);
    log_liks_WL = vector< vector <double> > (3, vector <double>(C, 0));
    log_mle = vector<vector<double> > (3, vector<double> (C,0));

    log_liks_curZ = vector <double>(C,0);
    log_liks_propZ = vector <double>(C,0);
    MH_ratio_gain = vector <double>(C,0);
    MH_ratio_loss = vector <double>(C,0);

    cur_crate = vector <double>(C,ratio0);
    cur_nrate = vector <double>(C,ratio1);

    cur_lrate = vector <double>(C,ind_lrate);
    cur_lrate2 = vector <double>(C,ind_lrate2);
    cur_grate = vector <double>(C, ind_grate);

   //Han*: used to save mode of MCMC output, so one element one vector.
    cur_pi = vector<vector<vector<double>>>(3,vector<vector<double>> (C, vector<double>(4,0.25)));

}

void BPP::sample_proposal(int iter, double & lrate_prop, double & grate_prop, ofstream & output)
{
    lrate_prop =gsl_ran_beta(RNG, ind_lrate * vlr, (1 - ind_lrate) *vlr); // let vlr == vgr!!
    grate_prop =gsl_ran_beta(RNG, ind_grate * vgr, (1 - ind_grate - ind_lrate) *vgr); //gsl_ran_gamma(RNG, vgr, ind_grate/vgr);
    grate_prop = grate_prop * (1 - lrate_prop);

    output << iter << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< indel << "\t"<< indel2 << "\t"<<ind_grate<< "\t"<< ind_lrate <<endl;
}

vec BPP::getlogTM(double dist, double rate, mat & lam )
{
    mat tmp_diag  = exp(eigenval* dist * rate);
    mat x = eigenvec;
    x.each_col()%=tmp_diag;

    mat log_cache_TM = log(eigeninv * x) ; //transpose Q
    return(log_multi(log_cache_TM, lam.col(lam.n_cols -1)));
}

mat BPP::getlogTM(double dist, double rate) //Han: get log transition matrix.
{
    if(dist < 1e-8)
    {
        mat x(num_base, num_base);
        x.fill(-INFINITY);
        x.diag().zeros();
        return(x);
    }
    mat tmp_diag  = exp(eigenval* dist * rate);
    mat x = eigenvec;
    x.each_col()%=tmp_diag;

    return(log(eigeninv * x)) ; //transpose Q

}

//Han*: new function. to account for updated Q
//For each element, since eigen value/vec changed
mat BPP::getlogTMc(double dist, double rate, mat& c_eigenvec, mat& c_eigenval, mat& c_eigeninv)
{
    if(dist < 1e-8)
    {
        mat x(num_base, num_base);
        x.fill(-INFINITY);
        x.diag().zeros();
        return(x);
    }
    mat tmp_diag  = exp(c_eigenval* dist * rate);
    mat x = c_eigenvec;
    x.each_col()%=tmp_diag;

    return(log(c_eigeninv * x)) ; //transpose Q //element-wise log
}

mat BPP::getlogTM_len(double dist, mat& c_eigenvec, mat& c_eigenval, mat& c_eigeninv){
    if(dist < 1e-8){
        mat x(num_base, num_base);
        x.fill(-INFINITY);
        x.diag().zeros();
        return(x);
    }
    mat tmp_diag  = exp(c_eigenval* dist);
    mat x = c_eigenvec;
    x.each_col()%=tmp_diag;

    return(log(c_eigeninv * x)) ; //transpose Q
}

double BPP::log_lik(vector< vector<vec> > & lambda, double _indel, double _indel2, int start1, int end1, vector<unsigned int> & v, double p)
{
    // compute loglik
    double result =0;
    mat x(2,2);

    int rr = *subtree.rbegin();
    // 1. sending the lambda msg from leaves bottom up through the network
    for(vector<int>::iterator it = subtree.begin(); it!=subtree.end(); it++) //int s=S; s<N; s++)
    {
        int s = *it;
        if(s<S) continue;
        int* p = children[s];
        for(int it = start1; it < end1; it++)  lambda[v[it]][s].fill(0);

        for(int cc=0;cc<2;cc++)
        {
            int chi = p[cc];
            assert(chi != -1);
            if(distances[chi]>0 )
            {
                double tt = (1 - exp(-(_indel + _indel2) * distances[chi]))/(_indel + _indel2);
                x.at(1,0) = _indel * tt;
                x.at(0,0) = 1 - x.at(1,0);

                x.at(0,1) = _indel2 * tt;
                x.at(1,1) = 1 - x.at(0,1);

                //cout << x;
                x = log(x);

            }
            else{
                x.fill(-INFINITY); //83, root
                x.diag().fill(0);
            }

            #pragma omp parallel for schedule (guided)
            for(int it = start1; it < end1; it++)  lambda[v[it]][s] +=  BPP::log_multi(x,lambda[v[it]][chi]);
        }

    }

    // 2. processing the distribution of root species
    for(int it = start1; it < end1; it++)
    {

        lambda[v[it]][rr][0] += log(1-p); //N-1
        lambda[v[it]][rr][1] += log(p) ;
        result += BPP::log_exp_sum(lambda[v[it]][rr]);
    }

    return(result);
}

void BPP::sample_hyperparam(int iter, vector<int> & ids, ofstream & output) // recompute log_TM, double indel_prop, double indel2_prop,
{
    //indepent MH to sample hyperparam of rates
    double p=1,r = 1; //hyperparam for shape
    double q=0.1,s = 0.1; // hyperparam for scale

    double vna = 100, vnb = 100, vca = 100, vcb = 100;
    double nprior_a_prop =gsl_ran_gamma(RNG, vna, nprior_a/vna);
    double cprior_a_prop =gsl_ran_gamma(RNG, vca, cprior_a/vca);

    double nprior_b_prop =gsl_ran_gamma(RNG, vnb, nprior_b/vnb);
    double cprior_b_prop =gsl_ran_gamma(RNG, vcb, cprior_b/vcb);

    //MH proposal
    double sum_r = 0;
    double log_prod_r = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        sum_r += cur_nrate[c];
        //var_r += pow(*it, 2);
        log_prod_r += log(cur_nrate[c]);
    }

    double M_ratio = (nprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b_prop - (ids.size() + r)*lgamma(nprior_a_prop) - log(nprior_b_prop) * nprior_a_prop * (s + ids.size());
    M_ratio -= (nprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b - (ids.size() + r)*lgamma(nprior_a) - log(nprior_b) * nprior_a * (s + ids.size());

    double H_ratio = log(gsl_ran_gamma_pdf(nprior_a,vna,nprior_a_prop/vna)) - log(gsl_ran_gamma_pdf(nprior_a_prop,vna,nprior_a/vna)) + log(gsl_ran_gamma_pdf(nprior_b,vnb,nprior_b_prop/vnb)) - log(gsl_ran_gamma_pdf(nprior_b_prop,vnb,nprior_b/vnb));

    cout << "nrate_MH_ratio: " << M_ratio <<", " << H_ratio << ", " << nprior_a << ", " << nprior_a_prop << ", " << nprior_b << ", " << nprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        nprior_a = nprior_a_prop;
        nprior_b = nprior_b_prop;
    }

    sum_r = 0;
    log_prod_r = 0;
    //for(vector<double>::iterator it = cur_crate.begin(); it< cur_crate.end(); it++)
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        sum_r += cur_crate[c];
        //var_r += pow(*it, 2);
        log_prod_r += log(cur_crate[c]);
    }

    M_ratio = (cprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b_prop - (ids.size() + r)*lgamma(cprior_a_prop) - log(cprior_b_prop) * cprior_a_prop * (s + ids.size()) - ((cprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b - (ids.size() + r)*lgamma(cprior_a) - log(cprior_b) * cprior_a * (s + ids.size()));

    H_ratio = log(gsl_ran_gamma_pdf(cprior_a,vca,cprior_a_prop/vca)) - log(gsl_ran_gamma_pdf(cprior_a_prop,vca,cprior_a/vca)) + log(gsl_ran_gamma_pdf(cprior_b,vcb,cprior_b_prop/vcb)) - log(gsl_ran_gamma_pdf(cprior_b_prop,vcb,cprior_b/vcb));

    cout << "crate_MH_ratio: " << M_ratio + H_ratio << ", " << cprior_a << ", " << cprior_a_prop <<", " << cprior_b << ", " << cprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        cprior_a = cprior_a_prop;
        cprior_b = cprior_b_prop;
    }

  // sample hyperparameters of lrate and grate, exponential prior for prior_l_a and prior_l_b, prior_g_a and prior_g_b
    double u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_l_a_prop = prior_l_a *  u;

    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_l_b_prop = prior_l_b *  u;
    double log_p = 0, log_pc = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        log_p += log(cur_lrate[c]);
        log_pc += log(1 - cur_lrate[c]);
    }

    M_ratio = (prior_l_a_prop - prior_l_a) * (log_p - 1) + (prior_l_b_prop - prior_l_b) * (log_pc - 1);
    M_ratio += ids.size() * (gsl_sf_lnbeta(prior_l_a, prior_l_b) - gsl_sf_lnbeta(prior_l_a_prop, prior_l_b_prop));

    H_ratio = log(prior_l_a) - log(prior_l_a_prop) + log(prior_l_b) - log(prior_l_b_prop);

    cout << "lrate_MH_ratio: " << M_ratio + H_ratio << ", " << prior_l_a << ", " << prior_l_a_prop <<", " << prior_l_b << ", " << prior_l_b_prop  << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        prior_l_a = prior_l_a_prop;
        prior_l_b = prior_l_b_prop;
    }

    // sample grate
    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_g_a_prop = prior_g_a *  u;

    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_g_b_prop = prior_g_b *  u;
    log_p = 0; log_pc = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        log_p += log(cur_grate[c]);
        log_pc += log(1 - cur_grate[c]);
    }

    M_ratio = (prior_g_a_prop - prior_g_a) * (log_p - 1) + (prior_g_b_prop - prior_g_b) * (log_pc - 1);
    M_ratio += ids.size() * (gsl_sf_lnbeta(prior_g_a, prior_g_b) - gsl_sf_lnbeta(prior_g_a_prop, prior_g_b_prop));

    H_ratio = log(prior_g_a) - log(prior_g_a_prop) + log(prior_g_b) - log(prior_g_b_prop);

    cout << "grate_MH_ratio: " << M_ratio + H_ratio << ", " << prior_g_a << ", " << prior_g_a_prop <<", " << prior_g_b << ", " << prior_g_b_prop  << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        prior_g_a = prior_g_a_prop;
        prior_g_b = prior_g_b_prop;
    }

    output << iter << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< prior_l_a << "\t"<< prior_l_b << "\t"<< prior_g_a << "\t"<< prior_g_b <<endl;
}

void BPP::getUppertree(int root, vector<int>& child, set<int> & visited_init)  // go to the root, include root!
{
    for(vector<int>::iterator it = child.begin(); it!=child.end(); it++)
    {
       int p = *it;
       while(p!=root)
       {
           visited_init.insert(p);
           p = parent[p];
       }
    }
    visited_init.insert(root);
}

void BPP::getSubtree(int root, vector<int> & visited_init)  // traverse from root to children, include root
{

    int j = root;

    //cout << nodes_names[j]<<"\t";

    if(children[j][0]!=-1)
    {
        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], visited_init);
        }
    }
    visited_init.push_back(j);
}

void BPP::getSubtree(int root, set<int>& child, vector<int> & visited_init)  // traverse from root, stop at children, 74 & 64; do include 1-S!
{
    int j = root;

    //cout << nodes_names[j]<<"\t";
    if(child.find(j) != child.end())
    {
        visited_init.push_back(j);
        return;
    }

    if(children[j][0]!=-1)
    {
        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], child, visited_init);
        }
    }
     visited_init.push_back(j);
}

void BPP::Output_init(PhyloProf & prof, string output_path, vector<int> & ids){

        string outpath_elem = output_path+ "_elem_lik.txt";
        ofstream out_lik(outpath_elem.c_str());
        out_lik.precision(8);

        //out_lik<<"No.\tID\tloglik_Null_W\tloglik_Acc_W\tloglik_Full_W\tlogBF1\tlogBF2\tlogPost_Max_M0\tlogPost_Max_M1\tlogPost_Max_M2\n";
        out_lik<<"No.\tID\tloglik_Null_W\tloglik_Acc_W\tloglik_Full_W\tlogBF1\tlogBF2"<<endl;
        for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
        {
            int cc = *it;
            out_lik <<cc << "\t" << prof.element_names[cc] << "\t" << log_liks_WL[0][cc] <<"\t"<< log_liks_WL[2][cc] <<"\t"<< log_liks_WL[1][cc] <<"\t";
            out_lik<<log_liks_WL[2][cc]-log_liks_WL[0][cc]<<"\t"<<log_liks_WL[2][cc]-log_liks_WL[1][cc]<<endl;
            //out_lik <<log_liks_Z[0][cc] << "\t" <<log_liks_Z[2][cc]<<"\t" <<log_liks_Z[1][cc]<<"\t";
            //out_lik<<log_mle[0][cc]<<"\t"<<log_mle[2][cc]<<"\t"<<log_mle[1][cc]<<"\t"<<log_mle[2][cc]-log_mle[0][cc]<<"\t"<<log_mle[2][cc]-log_mle[1][cc];
            //out_lik << endl;
        }

        out_lik.close();

    ofstream out_z;
    for(int r =0;r<3;r++)
    {
      if(r == 2)
      {
          outpath_elem = output_path+"_M" +to_string(1) + "_elem_Z.txt";
      }else if(r == 1)
      {
          outpath_elem = output_path+"_M" +to_string(2) + "_elem_Z.txt";
      }else{
          outpath_elem = output_path+"_M" +to_string(0) + "_elem_Z.txt";
      }

            out_z.open(outpath_elem.c_str());
            out_z<<"No.";
                for(int s =0 ;s<N;s++){  // header: species name
                    out_z<< "\t" << nodes_names[s];
                }
            out_z << "\tgenetree" <<endl;

            for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
            {
                int c = *it;
                out_z<<c;
                for(int s=0; s<N;s++)
                    out_z<<"\t" << Max_Z[r][c][s];
                out_z << "\t" << genetrees[r][c];
                out_z <<endl;
            }
        out_z.close();
    }

    ofstream out_pi;
    for(int r=0; r<3;r++){
        if(r==0){
            outpath_elem=output_path+"_M"+to_string(0)+"_Beta_Post_pi_mode.txt";
        }else if(r==2){
            outpath_elem=output_path+"_M"+to_string(1)+"_Beta_Post_pi_mode.txt";
        }else{
            outpath_elem=output_path+"_M"+to_string(2)+"_Beta_Post_pi_mode.txt";
        }
        out_pi.open(outpath_elem.c_str());

        for(vector<int>:: iterator it = ids.begin(); it !=ids.end(); it++)
        {
            int c=*it;
            out_pi << c;
            for(int b=0; b<4; b++) {//4 or num_base
                out_pi<<"\t"<<cur_pi[r][c][b];
            }
            out_pi <<endl;
        }
        out_pi.close();
    }

}

void BPP::Output_init0(PhyloProf & prof, ofstream& out_lik, vector<int> & ids){

    //for(int cc=0; cc<C;cc++)
    for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
    {
        int cc = *it;
        out_lik <<cc << "\t" << prof.element_names[cc] << "\t"  <<log_liks_sgl[cc]<< "\t"<<log_liks_Z[1][cc];
        out_lik << endl;
    }
}

/*** output simulated sequence ***/
void BPP::Output_simu(PhyloProf & prof, string outpath, int c){
    ofstream output;
    string outpath1 = outpath + ".fasta";
    string outpath2 = outpath + ".bed";
    string outpath3 = outpath +"_Z.txt";
    string outpath4 = outpath +"_pi.txt";
    output.open(outpath1.c_str());
    for(int s = 0; s< S; s++)
    {
        output << ">" << prof.species_names[s] << endl;
        output << prof.X[s].substr(0, element_start[c] + element_size[c])<< endl; //strutils::ToUpperCase(
    }
    output.close();

    output.open(outpath2.c_str());
    for(int i = 0; i < c; i++)
    {
        output << i << "\t" << element_start[i] << "\t" << element_start[i] + element_size[i]<< "\t";
        //Han*: add in output for pi
        output << i << "\t" << 1 << "\t" << cur_crate[i] << "\t" << cur_nrate[i] << "\t" << genetrees[0][i] << endl;
    }
    output.close();

    //Han*: output for Z
    output.open(outpath3.c_str());
    for(int s=0; s<N; s++){
        output<<"\t"<<nodes_names[s];
    }
    output<<endl;
    for(int s=0; s<N; s++){
        output<<"\t"<<cur_Z[0][0][s];
    }
    output<<endl;
    output.close();

    //Han*: output pi
    output.open(outpath4.c_str());
    for(int i=0; i<c; i++){
        output<<i<<"\t"<<cur_pi[0][i][0]<<endl; //only need to output pi_A (double-stranded)
    }
    output.close();
}

    void BPP::getTreeString(int rootN, std::stringstream & buffer)
    {
        if (children[rootN][0] == -1)
        {
            buffer << species_names[rootN] << ":"<< distances[rootN];
        }
        else
        {
            buffer << "(";
            for(int i =0;i <2; i++)
            {
                int child = children[rootN][i];

                getTreeString(child, buffer);
                if(i==0) buffer << ",";
            }

            if(parent[rootN] < N)
            {
                buffer << "):" << distances[rootN];
            }else{
                buffer << ");" ;
            }
        }
    }


/**
 * @file budgeted_maximum_coverage.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-03-18
 */
#ifndef PAAL_BUDGETED_MAXIMUM_COVERAGE_HPP
#define PAAL_BUDGETED_MAXIMUM_COVERAGE_HPP

#include "paal/data_structures/fraction.hpp"
#include "paal/utils/accumulate_functors.hpp"
#include "paal/utils/algorithms/subset_backtrack.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/heap/d_ary_heap.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/combine.hpp>

#include <algorithm>
#include <vector>

namespace paal {
namespace greedy {
namespace detail {

template <typename ElementWeight, typename SetCost> struct set_data_type {
    set_data_type()
        : m_weight_of_uncovered_elements{}, m_cost{}, m_is_processed{ true } {}
    ElementWeight m_weight_of_uncovered_elements; // sum of weight of uncovered
                                                  // elements in backtrack and
                                                  // greedy
    SetCost m_cost;
    bool m_is_processed;
    // set is processed if is selected or is not selected and at least one of
    // the following situations occurs
    // -we have not enough budget to select set
    // -sum of weight of uncovered elements belong to set equal 0
};

template <typename SetReference, typename GetElementsOfSet,
          typename GetWeightOfElement>
using element_weight_t = pure_result_of_t<GetWeightOfElement(
    range_to_elem_t<pure_result_of_t<GetElementsOfSet(SetReference)>>)>;

const int UNCOVERED = -1;
template <class Budget, class SetCost, class SetIdToData, class ElementWeight,
          class SetIdToElements, class ElementIndex, class GetWeightOfElement,
          typename DecreseWeight>
class selector {
    const Budget m_budget;
    SetCost &m_cost_of_solution;
    std::vector<int> m_selected_sets;
    SetIdToData &m_sets_data;
    const ElementWeight &m_weight_of_bests_solution;
    ElementWeight &m_weight_of_covered_elements;
    SetIdToElements m_set_id_to_elements;
    std::vector<int> &m_covered_by;
    std::vector<std::vector<int>> &m_sets_covering_element;
    ElementIndex &m_get_el_index;
    GetWeightOfElement &m_element_to_weight;
    DecreseWeight m_decrese_weight;

    using SetData = range_to_elem_t<SetIdToData>;

  public:
    selector(const Budget budget, SetCost &cost_of_solution,
             SetIdToData &sets_data,
             const ElementWeight &weight_of_bests_solution,
             ElementWeight &weight_of_covered_elements,
             SetIdToElements set_id_to_elements, std::vector<int> &covered_by,
             std::vector<std::vector<int>> &sets_covering_element,
             ElementIndex &get_el_index, GetWeightOfElement &element_to_weight,
             DecreseWeight decrese_weight)
        : m_budget(budget), m_cost_of_solution(cost_of_solution),
          m_sets_data(sets_data),
          m_weight_of_bests_solution(weight_of_bests_solution),
          m_weight_of_covered_elements(weight_of_covered_elements),
          m_set_id_to_elements(set_id_to_elements), m_covered_by(covered_by),
          m_sets_covering_element(sets_covering_element),
          m_get_el_index(get_el_index), m_element_to_weight(element_to_weight),
          m_decrese_weight(decrese_weight) {}

    // we return true if (we select set) or (set is already in solution)
    bool select_set_backtrack(int selected_set_id, bool in_reset = false) {
        if(!can_select(m_sets_data[selected_set_id])) return false;

        select_set(selected_set_id, true);
        if(!in_reset) m_selected_sets.push_back(selected_set_id);
        return true;
    }

    // we return true if we violated budget
    bool select_set_greedy(int selected_set_id) {
        auto &select_set_data = m_sets_data[selected_set_id];

        if(!can_select(select_set_data)) return false;
        m_selected_sets.push_back(selected_set_id);

        if(greedy_prune(select_set_data))  return true;
        select_set(selected_set_id, false);
        return false;
    }

    void deselect_set(int selected_set_id, bool backtrack = true) {
        // we deselect set

        m_cost_of_solution -= m_sets_data[selected_set_id].m_cost;
        for (auto element : m_set_id_to_elements(selected_set_id)) {
            if (covered_by(element) == selected_set_id) {
                m_covered_by[m_get_el_index(element)] = UNCOVERED;
                auto weight_of_element = m_element_to_weight(element);
                cover_element(element, -weight_of_element, backtrack, false);
            }
        }
        m_selected_sets.pop_back();
    }

    void set_unprocessed(
            boost::iterator_range<std::vector<int>::const_iterator> const & set_ids) {
        for (auto set_id : set_ids) {
            m_sets_data[set_id].m_is_processed = false;
        }
    };

    void reset() {
        set_unprocessed(m_selected_sets);
        if (m_selected_sets.size() > 0) {
            boost::for_each(m_selected_sets, [&](int selected_set_id) {
                select_set_backtrack(selected_set_id, true);
            });
        }
    }

    std::size_t size() {
        return m_selected_sets.size();
    }

    void resize(std::size_t size) {
        return m_selected_sets.resize(size);
    }

    template <typename OutputIterator>
    void copy_to(OutputIterator out){
        boost::copy(m_selected_sets, out);
    }
private:

    void select_set(int selected_set_id, bool backtrack) {
        auto &select_set_data = m_sets_data[selected_set_id];

        m_cost_of_solution += select_set_data.m_cost;
        for (auto element : m_set_id_to_elements(selected_set_id)) {
            if (covered_by(element) == UNCOVERED) { /* we do not cover the
                                                        elements being covered*/

                m_covered_by[m_get_el_index(element)] = selected_set_id;
                auto weight_of_element = m_element_to_weight(element);
                cover_element(element, weight_of_element, backtrack);
            }
        }
    }

    /// optimization:
    /// in greedy phase we get sets in decreasing density order, so we can cut
    /// when spend rest of budget with current density product solution, worse
    /// then best found solution.
    bool greedy_prune(const SetData & set_data) {
        return (m_budget - m_cost_of_solution) * set_data.m_weight_of_uncovered_elements <=
                static_cast<Budget>(set_data.m_cost * (m_weight_of_bests_solution - m_weight_of_covered_elements));
    }

    ///this function is ALWAYS called from select_set, thats why we set set_data.m_is_processed!
    bool can_select(SetData & set_data) {
        if (set_data.m_is_processed) return false;
        set_data.m_is_processed = true;
        return static_cast<Budget>(m_cost_of_solution + set_data.m_cost) <= m_budget &&
            set_data.m_weight_of_uncovered_elements!= ElementWeight{};
    }

    template <typename Element>
    void cover_element(Element && el, ElementWeight weight_diff, bool backtrack, bool select = true) {
        m_weight_of_covered_elements += weight_diff;
        for (auto set_id :
                m_sets_covering_element[m_get_el_index(el)]) {
            if (m_sets_data[set_id].m_weight_of_uncovered_elements >
                    weight_diff || backtrack) {
                m_sets_data[set_id].m_weight_of_uncovered_elements -= weight_diff;
                if (!backtrack && !m_sets_data[set_id].m_is_processed) {
                    m_decrese_weight(set_id);
                }
            } else {
                if (select) {
                    m_sets_data[set_id].m_is_processed = true;
                }
            }
        }
    }

    template <typename Element>
    auto covered_by(Element && el) const
        -> decltype(m_covered_by[m_get_el_index(el)]) {

        return m_covered_by[m_get_el_index(el)];
    }
};

template <class Budget, class SetCost, class SetIdToData, class ElementWeight,
         class SetIdToElements, class ElementIndex, class GetWeightOfElement,
         typename DecreseWeight>
auto make_selector(const Budget budget, SetCost &cost_of_solution,
                   SetIdToData &sets_data,
                   const ElementWeight &weight_of_bests_solution,
                   ElementWeight &weight_of_covered_elements,
                   SetIdToElements set_id_to_elements,
                   std::vector<int> &covered_by,
                   std::vector<std::vector<int>> &sets_covering_element,
                   ElementIndex &get_el_index,
                   GetWeightOfElement &element_to_weight,
                   DecreseWeight decrese_weight) {
    return selector<Budget, SetCost, SetIdToData, ElementWeight,
                    SetIdToElements, ElementIndex, GetWeightOfElement,
                    DecreseWeight>(
        budget, cost_of_solution, sets_data,
        weight_of_bests_solution, weight_of_covered_elements,
        set_id_to_elements, covered_by, sets_covering_element, get_el_index,
        element_to_weight, decrese_weight);
}
} //!detail

/**
 * @brief this is solve Set Cover problem
 * and return set cover cost
 * example:
 *  \snippet set_cover_example.cpp Set Cover Example
 *
 * complete example is set_cover_example.cpp
 * @param sets
 * @param set_to_cost
 * @param set_to_elements
 * @param result set iterators of chosen sets
 * @param get_el_index
 * @param budget
 * @param element_to_weight
 * @param initial_set_size
 * @tparam SetRange
 * @tparam GetCostOfSet
 * @tparam GetElementsOfSet
 * @tparam OutputIterator
 * @tparam ElementIndex
 * @tparam Budget
 * @tparam GetWeightOfElement
 */
template <typename SetRange, class GetCostOfSet, class GetElementsOfSet,
          class OutputIterator, class ElementIndex, class Budget,
          class GetWeightOfElement = utils::return_one_functor>
auto budgeted_maximum_coverage(
    SetRange && sets, GetCostOfSet set_to_cost,
    GetElementsOfSet set_to_elements, OutputIterator result,
    ElementIndex get_el_index, Budget budget,
    GetWeightOfElement element_to_weight = GetWeightOfElement(),
    const unsigned int initial_set_size = 3) {

    using set_reference = typename boost::range_reference<SetRange>::type;
    using element_weight = typename detail::element_weight_t<
        set_reference, GetElementsOfSet, GetWeightOfElement>;
    using set_cost = pure_result_of_t<GetCostOfSet(set_reference)>;
    using set_id_to_data = std::vector<detail::set_data_type<element_weight, set_cost>>;

    auto nu_sets = boost::distance(sets);
    set_id_to_data initial_sets_data(nu_sets), sets_data(nu_sets);
    set_cost cost_of_solution{}, cost_of_best_solution{};
    int number_of_elements = 0;

    // we find max index of elements in all sets
    for (auto set_and_data : boost::combine(sets, initial_sets_data)) {
        auto const & set = boost::get<0>(set_and_data);
        auto &cost = boost::get<1>(set_and_data).m_cost;
        cost = set_to_cost(set);
        assert(cost != set_cost{});
        auto const & elements = set_to_elements(set);
        if (!boost::empty(elements)) {
            number_of_elements = std::max(number_of_elements,
                    *max_element_functor(elements, get_el_index) + 1);
        }
    }
    element_weight weight_of_covered_elements{}, weight_of_bests_solution{};
    std::vector<int> best_solution(1);
    std::vector<int> covered_by(
        number_of_elements, detail::UNCOVERED); // index of the first set that covers
                                        // element or -1 if element is uncovered
    std::vector<int> sets_id(nu_sets);
    boost::iota(sets_id, 0);
    std::vector<std::vector<int>> sets_covering_element(number_of_elements);
    auto decreasing_density_order =
        utils::make_functor_to_comparator([&](int x) {
            return data_structures::make_fraction(
                sets_data[x].m_weight_of_uncovered_elements, sets_data[x].m_cost);
        });
    using queue = boost::heap::d_ary_heap<
        int, boost::heap::arity<3>, boost::heap::mutable_<true>,
        boost::heap::compare<decltype(decreasing_density_order)>>;

    queue uncovered_set_queue{ decreasing_density_order };
    std::vector<typename queue::handle_type> set_id_to_handle(nu_sets);

    // we fill sets_covering_element and setToWeightOfElements
    for (auto set : sets | boost::adaptors::indexed()) {
        auto set_id = set.index();
        auto &set_data = initial_sets_data[set_id];

        for (auto &&element : set_to_elements(set.value())) {
            sets_covering_element[get_el_index(element)].push_back(set_id);
            set_data.m_weight_of_uncovered_elements += element_to_weight(element);
        }
        if (initial_set_size ==
            0) { /* we check all one element set. if initial_set_size!= 0 then
                    we will do it anyway */
            set_cost cost_of_set = set_data.m_cost;
            if (set_data.m_weight_of_uncovered_elements >= weight_of_bests_solution &&
                static_cast<Budget>(cost_of_set) <= budget) {
                weight_of_bests_solution = set_data.m_weight_of_uncovered_elements;
                best_solution[0] = set_id;
                cost_of_best_solution = cost_of_set;
            }
        }
    };

    auto sort_sets = [&](std::vector<int>& sets_range) {
        boost::sort(sets_range, utils::make_functor_to_comparator([&](int x) {
                                    return sets_data[x].m_weight_of_uncovered_elements;
                                }));
        return sets_range.end();
    };
    auto selector = detail::make_selector
                (budget, cost_of_solution,sets_data,
                 weight_of_bests_solution,weight_of_covered_elements,
                 [&](int selected_set_id){return set_to_elements(sets[selected_set_id]);},
                 covered_by,sets_covering_element,get_el_index,
                 element_to_weight,
                 [&](int set_id){uncovered_set_queue.decrease(set_id_to_handle[set_id]);});

    boost::copy(initial_sets_data, sets_data.begin());
    sort_sets(sets_id);
    auto solver = make_subset_backtrack(sets_id);

    auto reset = [&]() {
        boost::copy(initial_sets_data, sets_data.begin());
        cost_of_solution = set_cost{};
        selector.set_unprocessed(solver.get_moves());
        boost::fill(covered_by, detail::UNCOVERED);
        weight_of_covered_elements = element_weight{};
        selector.reset();
    };

    auto on_pop = [&](int deselected_set_id) {
        selector.deselect_set(deselected_set_id);
        selector.set_unprocessed(solver.get_moves());
    };

    auto save_best_solution = [&]() {
        // we check that new solution is better than any previous
        // if is we remember them
        // tricky: either better weight, or equal weight and lower cost
        if (std::make_pair(weight_of_covered_elements, cost_of_best_solution) >
            std::make_pair(weight_of_bests_solution, cost_of_solution)) {
            weight_of_bests_solution = weight_of_covered_elements;
            cost_of_best_solution = cost_of_solution;
            best_solution.resize(selector.size());
            selector.copy_to(best_solution.begin());
        }
    };
    auto greedy_phase = [&]() {
        uncovered_set_queue.clear();
        auto moves = solver.get_moves();
        if(boost::empty(moves)) return;

        for (auto set_id : moves) {
            set_id_to_handle[set_id] = uncovered_set_queue.push(set_id);
        }
        /* we select set with best elements to cost ratio, and add it to the
         * result until all elements are covered*/
        int uncovered_set_id;
        do {
            uncovered_set_id = uncovered_set_queue.top();
            uncovered_set_queue.pop();
        } while (!selector.select_set_greedy(uncovered_set_id) &&
                !uncovered_set_queue.empty());
    };

    auto can_push = [&](int candidate) {
        if (!selector.select_set_backtrack(candidate)) {
            return false;
        }
        if (selector.size() == initial_set_size) {
            greedy_phase();
            save_best_solution();
            selector.resize(initial_set_size-1);
            reset();
            return false;
        } else {
            save_best_solution();
            return true;
        }
    };

    reset();
    if (initial_set_size != 0) { /* if initial_set_size == 0 then we do greedy
    algorithm once starts from empty initial Set.
    Otherwise we starts greedy algorithm from all initial set of size equal
    initial_set_size those for which we have enough budget*/
        solver.solve(can_push, on_pop, sort_sets);
    } else {
        greedy_phase();
        save_best_solution();
    }

    for (auto set_id : best_solution) {
        *result = *(sets.begin() + set_id);
        ++result;
    }
    return weight_of_bests_solution;
};
} //!greedy
} //!paal

#endif /* PAAL_BUDGETED_MAXIMUM_COVERAGE_HPP */


// boost\math\distributions\bernoulli.hpp

// Copyright John Maddock 2006.
// Copyright Paul A. Bristow 2007.

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

// http://en.wikipedia.org/wiki/bernoulli_distribution
// http://mathworld.wolfram.com/BernoulliDistribution.html

// bernoulli distribution is the discrete probability distribution of
// the number (k) of successes, in a single Bernoulli trials.
// It is a version of the binomial distribution when n = 1.

// But note that the bernoulli distribution
// (like others including the poisson, binomial & negative binomial)
// is strictly defined as a discrete function: only integral values of k are envisaged.
// However because of the method of calculation using a continuous gamma function,
// it is convenient to treat it as if a continuous function,
// and permit non-integral values of k.
// To enforce the strict mathematical model, users should use floor or ceil functions
// on k outside this function to ensure that k is integral.

#ifndef BOOST_MATH_SPECIAL_BERNOULLI_HPP
#define BOOST_MATH_SPECIAL_BERNOULLI_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.

#include <utility>

namespace boost
{
  namespace math
  {
    namespace bernoulli_detail
    {
      // Common error checking routines for bernoulli distribution functions:
      template <class RealType, class Policy>
      inline bool check_success_fraction(const char* function, const RealType& p, RealType* result, const Policy& /* pol */)
      {
        if(!(boost::math::isfinite)(p) || (p < 0) || (p > 1))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Success fraction argument is %1%, but must be >= 0 and <= 1 !", p, Policy());
          return false;
        }
        return true;
      }
      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& p, RealType* result, const Policy& /* pol */, const std::true_type&)
      {
        return check_success_fraction(function, p, result, Policy());
      }
      template <class RealType, class Policy>
      inline bool check_dist(const char* , const RealType& , RealType* , const Policy& /* pol */, const std::false_type&)
      {
         return true;
      }
      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& p, RealType* result, const Policy& /* pol */)
      {
         return check_dist(function, p, result, Policy(), typename policies::constructor_error_check<Policy>::type());
      }

      template <class RealType, class Policy>
      inline bool check_dist_and_k(const char* function, const RealType& p, RealType k, RealType* result, const Policy& pol)
      {
        if(check_dist(function, p, result, Policy(), typename policies::method_error_check<Policy>::type()) == false)
        {
          return false;
        }
        if(!(boost::math::isfinite)(k) || !((k == 0) || (k == 1)))
        {
          *result = policies::raise_domain_error<RealType>(
            function,
            "Number of successes argument is %1%, but must be 0 or 1 !", k, pol);
          return false;
        }
       return true;
      }
      template <class RealType, class Policy>
      inline bool check_dist_and_prob(const char* function, RealType p, RealType prob, RealType* result, const Policy& /* pol */)
      {
        if((check_dist(function, p, result, Policy(), typename policies::method_error_check<Policy>::type()) && detail::check_probability(function, prob, result, Policy())) == false)
        {
          return false;
        }
        return true;
      }
    } // namespace bernoulli_detail

    template <class RealType = double, class Policy = policies::policy<> >
    class bernoulli_distribution
    {
    public:
      typedef RealType value_type;
      typedef Policy policy_type;

      bernoulli_distribution(RealType p = 0.5) : m_p(p)
      { // Default probability = half suits 'fair' coin tossing
        // where probability of heads == probability of tails.
        RealType result; // of checks.
        bernoulli_detail::check_dist(
           "boost::math::bernoulli_distribution<%1%>::bernoulli_distribution",
          m_p,
          &result, Policy());
      } // bernoulli_distribution constructor.

      RealType success_fraction() const
      { // Probability.
        return m_p;
      }

    private:
      RealType m_p; // success_fraction
    }; // template <class RealType> class bernoulli_distribution

    typedef bernoulli_distribution<double> bernoulli;

    #ifdef __cpp_deduction_guides
    template <class RealType>
    bernoulli_distribution(RealType)->bernoulli_distribution<typename boost::math::tools::promote_args<RealType>::type>;
    #endif

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> range(const bernoulli_distribution<RealType, Policy>& /* dist */)
    { // Range of permissible values for random variable k = {0, 1}.
      using boost::math::tools::max_value;
      return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
    }

    template <class RealType, class Policy>
    inline const std::pair<RealType, RealType> support(const bernoulli_distribution<RealType, Policy>& /* dist */)
    { // Range of supported values for random variable k = {0, 1}.
      // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
      return std::pair<RealType, RealType>(static_cast<RealType>(0), static_cast<RealType>(1));
    }

    template <class RealType, class Policy>
    inline RealType mean(const bernoulli_distribution<RealType, Policy>& dist)
    { // Mean of bernoulli distribution = p (n = 1).
      return dist.success_fraction();
    } // mean

    // Rely on dereived_accessors quantile(half)
    //template <class RealType>
    //inline RealType median(const bernoulli_distribution<RealType, Policy>& dist)
    //{ // Median of bernoulli distribution is not defined.
    //  return tools::domain_error<RealType>(BOOST_CURRENT_FUNCTION, "Median is not implemented, result is %1%!", std::numeric_limits<RealType>::quiet_NaN());
    //} // median

    template <class RealType, class Policy>
    inline RealType variance(const bernoulli_distribution<RealType, Policy>& dist)
    { // Variance of bernoulli distribution =p * q.
      return  dist.success_fraction() * (1 - dist.success_fraction());
    } // variance

    template <class RealType, class Policy>
    RealType pdf(const bernoulli_distribution<RealType, Policy>& dist, const RealType& k)
    { // Probability Density/Mass Function.
      BOOST_FPU_EXCEPTION_GUARD
      // Error check:
      RealType result = 0; // of checks.
      if(false == bernoulli_detail::check_dist_and_k(
        "boost::math::pdf(bernoulli_distribution<%1%>, %1%)",
        dist.success_fraction(), // 0 to 1
        k, // 0 or 1
        &result, Policy()))
      {
        return result;
      }
      // Assume k is integral.
      if (k == 0)
      {
        return 1 - dist.success_fraction(); // 1 - p
      }
      else  // k == 1
      {
        return dist.success_fraction(); // p
      }
    } // pdf

    template <class RealType, class Policy>
    inline RealType cdf(const bernoulli_distribution<RealType, Policy>& dist, const RealType& k)
    { // Cumulative Distribution Function Bernoulli.
      RealType p = dist.success_fraction();
      // Error check:
      RealType result = 0;
      if(false == bernoulli_detail::check_dist_and_k(
        "boost::math::cdf(bernoulli_distribution<%1%>, %1%)",
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      if (k == 0)
      {
        return 1 - p;
      }
      else
      { // k == 1
        return 1;
      }
    } // bernoulli cdf

    template <class RealType, class Policy>
    inline RealType cdf(const complemented2_type<bernoulli_distribution<RealType, Policy>, RealType>& c)
    { // Complemented Cumulative Distribution Function bernoulli.
      RealType const& k = c.param;
      bernoulli_distribution<RealType, Policy> const& dist = c.dist;
      RealType p = dist.success_fraction();
      // Error checks:
      RealType result = 0;
      if(false == bernoulli_detail::check_dist_and_k(
        "boost::math::cdf(bernoulli_distribution<%1%>, %1%)",
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      if (k == 0)
      {
        return p;
      }
      else
      { // k == 1
        return 0;
      }
    } // bernoulli cdf complement

    template <class RealType, class Policy>
    inline RealType quantile(const bernoulli_distribution<RealType, Policy>& dist, const RealType& p)
    { // Quantile or Percent Point Bernoulli function.
      // Return the number of expected successes k either 0 or 1.
      // for a given probability p.

      RealType result = 0; // of error checks:
      if(false == bernoulli_detail::check_dist_and_prob(
        "boost::math::quantile(bernoulli_distribution<%1%>, %1%)",
        dist.success_fraction(),
        p,
        &result, Policy()))
      {
        return result;
      }
      if (p <= (1 - dist.success_fraction()))
      { // p <= pdf(dist, 0) == cdf(dist, 0)
        return 0;
      }
      else
      {
        return 1;
      }
    } // quantile

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<bernoulli_distribution<RealType, Policy>, RealType>& c)
    { // Quantile or Percent Point bernoulli function.
      // Return the number of expected successes k for a given
      // complement of the probability q.
      //
      // Error checks:
      RealType q = c.param;
      const bernoulli_distribution<RealType, Policy>& dist = c.dist;
      RealType result = 0;
      if(false == bernoulli_detail::check_dist_and_prob(
        "boost::math::quantile(bernoulli_distribution<%1%>, %1%)",
        dist.success_fraction(),
        q,
        &result, Policy()))
      {
        return result;
      }

      if (q <= 1 - dist.success_fraction())
      { // // q <= cdf(complement(dist, 0)) == pdf(dist, 0)
        return 1;
      }
      else
      {
        return 0;
      }
    } // quantile complemented.

    template <class RealType, class Policy>
    inline RealType mode(const bernoulli_distribution<RealType, Policy>& dist)
    {
      return static_cast<RealType>((dist.success_fraction() <= 0.5) ? 0 : 1); // p = 0.5 can be 0 or 1
    }

    template <class RealType, class Policy>
    inline RealType skewness(const bernoulli_distribution<RealType, Policy>& dist)
    {
      BOOST_MATH_STD_USING; // Aid ADL for sqrt.
      RealType p = dist.success_fraction();
      return (1 - 2 * p) / sqrt(p * (1 - p));
    }

    template <class RealType, class Policy>
    inline RealType kurtosis_excess(const bernoulli_distribution<RealType, Policy>& dist)
    {
      RealType p = dist.success_fraction();
      // Note Wolfram says this is kurtosis in text, but gamma2 is the kurtosis excess,
      // and Wikipedia also says this is the kurtosis excess formula.
      // return (6 * p * p - 6 * p + 1) / (p * (1 - p));
      // But Wolfram kurtosis article gives this simpler formula for kurtosis excess:
      return 1 / (1 - p) + 1/p -6;
    }

    template <class RealType, class Policy>
    inline RealType kurtosis(const bernoulli_distribution<RealType, Policy>& dist)
    {
      RealType p = dist.success_fraction();
      return 1 / (1 - p) + 1/p -6 + 3;
      // Simpler than:
      // return (6 * p * p - 6 * p + 1) / (p * (1 - p)) + 3;
    }

  } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_SPECIAL_BERNOULLI_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// 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_UNITS_CODATA_ELECTROMAGNETIC_CONSTANTS_HPP
#define BOOST_UNITS_CODATA_ELECTROMAGNETIC_CONSTANTS_HPP

///
/// \file
/// \brief CODATA recommended values of fundamental electromagnetic constants.
/// \details CODATA recommended values of the fundamental physical constants: NIST SP 961
///   CODATA 2006 values as of 2007/03/30
///

#include <boost/units/quantity.hpp>
#include <boost/units/static_constant.hpp>

#include <boost/units/systems/detail/constants.hpp>
#include <boost/units/systems/si/conductance.hpp>
#include <boost/units/systems/si/current.hpp>
#include <boost/units/systems/si/electric_charge.hpp>
#include <boost/units/systems/si/electric_potential.hpp>
#include <boost/units/systems/si/energy.hpp>
#include <boost/units/systems/si/frequency.hpp>
#include <boost/units/systems/si/magnetic_flux.hpp>
#include <boost/units/systems/si/magnetic_flux_density.hpp>
#include <boost/units/systems/si/resistance.hpp>

#include <boost/units/systems/si/codata/typedefs.hpp>

namespace boost {

namespace units {

namespace si {

namespace constants {

namespace codata {

// ELECTROMAGNETIC
/// elementary charge
BOOST_UNITS_PHYSICAL_CONSTANT(e,quantity<electric_charge>,1.602176487e-19*coulombs,4.0e-27*coulombs);
/// elementary charge to Planck constant ratio
BOOST_UNITS_PHYSICAL_CONSTANT(e_over_h,quantity<current_over_energy>,2.417989454e14*amperes/joule,6.0e6*amperes/joule);
/// magnetic flux quantum
BOOST_UNITS_PHYSICAL_CONSTANT(Phi_0,quantity<magnetic_flux>,2.067833667e-15*webers,5.2e-23*webers);
/// conductance quantum
BOOST_UNITS_PHYSICAL_CONSTANT(G_0,quantity<conductance>,7.7480917004e-5*siemens,5.3e-14*siemens);
/// Josephson constant
BOOST_UNITS_PHYSICAL_CONSTANT(K_J,quantity<frequency_over_electric_potential>,483597.891e9*hertz/volt,1.2e7*hertz/volt);
/// von Klitzing constant
BOOST_UNITS_PHYSICAL_CONSTANT(R_K,quantity<resistance>,25812.807557*ohms,1.77e-5*ohms);
/// Bohr magneton
BOOST_UNITS_PHYSICAL_CONSTANT(mu_B,quantity<energy_over_magnetic_flux_density>,927.400915e-26*joules/tesla,2.3e-31*joules/tesla);
/// nuclear magneton
BOOST_UNITS_PHYSICAL_CONSTANT(mu_N,quantity<energy_over_magnetic_flux_density>,5.05078324e-27*joules/tesla,1.3e-34*joules/tesla);

} // namespace codata

} // namespace constants

} // namespace si

} // namespace units

} // namespace boost

#endif // BOOST_UNITS_CODATA_ELECTROMAGNETIC_CONSTANTS_HPP