// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2016-2018 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_BOX_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_BOX_BOX_HPP

#include <boost/config.hpp>
#include <boost/concept_check.hpp>
#include <boost/mpl/if.hpp>
#include <boost/type_traits/is_void.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/concepts/distance_concept.hpp>
#include <boost/geometry/strategies/spherical/distance_cross_track.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/algorithms/detail/assign_box_corners.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace distance
{

namespace details
{

template <typename ReturnType>
class cross_track_box_box_generic
{
public :

    template
    <
            typename Box1,
            typename Box2,
            typename Strategy
    >
    ReturnType static inline apply (Box1 const& box1,
                                    Box2 const& box2,
                                    Strategy ps_strategy)
    {

        // this method assumes that the coordinates of the point and
        // the box are normalized

        typedef typename point_type<Box1>::type box_point_type1;
        typedef typename point_type<Box2>::type box_point_type2;

        box_point_type1 bottom_left1, bottom_right1, top_left1, top_right1;
        geometry::detail::assign_box_corners(box1,
                                             bottom_left1, bottom_right1,
                                             top_left1, top_right1);

        box_point_type2 bottom_left2, bottom_right2, top_left2, top_right2;
        geometry::detail::assign_box_corners(box2,
                                             bottom_left2, bottom_right2,
                                             top_left2, top_right2);

        ReturnType lon_min1 = geometry::get_as_radian<0>(bottom_left1);
        ReturnType const lat_min1 = geometry::get_as_radian<1>(bottom_left1);
        ReturnType lon_max1 = geometry::get_as_radian<0>(top_right1);
        ReturnType const lat_max1 = geometry::get_as_radian<1>(top_right1);

        ReturnType lon_min2 = geometry::get_as_radian<0>(bottom_left2);
        ReturnType const lat_min2 = geometry::get_as_radian<1>(bottom_left2);
        ReturnType lon_max2 = geometry::get_as_radian<0>(top_right2);
        ReturnType const lat_max2 = geometry::get_as_radian<1>(top_right2);

        ReturnType const two_pi = math::two_pi<ReturnType>();

        // Test which sides of the boxes are closer and if boxes cross
        // antimeridian
        bool right_wrap;

        if (lon_min2 > 0 && lon_max2 < 0) // box2 crosses antimeridian
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(box2 crosses antimeridian)";
#endif
            right_wrap = lon_min2 - lon_max1 < lon_min1 - lon_max2;
            lon_max2 += two_pi;
            if (lon_min1 > 0 && lon_max1 < 0) // both boxes crosses antimeridian
            {
                lon_max1 += two_pi;
            }
        }
        else if (lon_min1 > 0 && lon_max1 < 0) // only box1 crosses antimeridian
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(box1 crosses antimeridian)";
#endif
            return apply(box2, box1, ps_strategy);
        }
        else
        {
            right_wrap = lon_max1 <= lon_min2
                         ? lon_min2 - lon_max1 < two_pi - (lon_max2 - lon_min1)
                         : lon_min1 - lon_max2 > two_pi - (lon_max1 - lon_min2);

        }

        // Check1: if box2 crosses the band defined by the
        // minimum and maximum longitude of box1; if yes, determine
        // if the box2 is above, below or intersects/is inside box1 and compute
        // the distance (easy in this case)

        bool lon_min12 = lon_min1 <= lon_min2;
        bool right = lon_max1 <= lon_min2;
        bool left = lon_min1 >= lon_max2;
        bool lon_max12 = lon_max1 <= lon_max2;

        if ((lon_min12 && !right)
                || (!left && !lon_max12)
                || (!lon_min12 && lon_max12))
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(up-down)\n";
#endif
            if (lat_min1 > lat_max2)
            {
                return geometry::strategy::distance::services::result_from_distance
                    <
                        Strategy, box_point_type1, box_point_type2
                    >::apply(ps_strategy, ps_strategy.get_distance_strategy()
                               .meridian(lat_min1, lat_max2));
            }
            else if (lat_max1 < lat_min2)
            {
                return geometry::strategy::distance::services::result_from_distance
                    <
                        Strategy, box_point_type1, box_point_type2
                    >::apply(ps_strategy, ps_strategy.get_distance_strategy().
                             meridian(lat_min2, lat_max1));
            }
            else
            {
                //BOOST_GEOMETRY_ASSERT(plat >= lat_min && plat <= lat_max);
                return ReturnType(0);
            }
        }

        // Check2: if box2 is right/left of box1
        // the max lat of box2 should be less than the max lat of box1
        bool bottom_max;

        ReturnType top_common = (std::min)(lat_max1, lat_max2);
        ReturnType bottom_common = (std::max)(lat_min1, lat_min2);

        // true if the closest points are on northern hemisphere
        bool north_shortest = math::abs(top_common) > math::abs(bottom_common)
                || lat_max1 <= lat_min2
                || lat_min1 >= lat_max2;

        if (north_shortest)
        {
            bottom_max = lat_max1 >= lat_max2;
        }
        else
        {
            bottom_max = lat_min1 <= lat_min2;
        }

#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
        std::cout << "(diagonal)";
#endif
        if (bottom_max && !right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(bottom left)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_right2, top_left1, bottom_left1);
            }
            return ps_strategy.apply(bottom_right2, top_left1, bottom_left1);
        }
        if (bottom_max && right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(bottom right)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_left2, top_right1, bottom_right1);
            }
            return ps_strategy.apply(bottom_left2, top_right1, bottom_right1);
        }
        if (!bottom_max && !right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(top left)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_left1, top_right2, bottom_right2);
            }
            return ps_strategy.apply(bottom_left1, top_right2, bottom_right2);
        }
        if (!bottom_max && right_wrap)
        {
#ifdef BOOST_GEOMETRY_DEBUG_CROSS_TRACK_BOX_BOX
            std::cout << "(top right)";
#endif
            if (north_shortest)
            {
                return ps_strategy.apply(top_right1, top_left2, bottom_left2);
            }
            return ps_strategy.apply(bottom_right1, top_left2, bottom_left2);
        }
        return ReturnType(0);
    }
};

} //namespace details

/*!
\brief Strategy functor for distance box to box calculation
\ingroup strategies
\details Class which calculates the distance of a box to a box, for
boxes on a sphere or globe
\tparam CalculationType \tparam_calculation
\tparam Strategy underlying point-segment distance strategy, defaults
to cross track
\qbk{
[heading See also]
[link geometry.reference.algorithms.distance.distance_3_with_strategy distance (with strategy)]
}
*/
template
<
    typename CalculationType = void,
    typename Strategy = cross_track<CalculationType>
>
class cross_track_box_box
{
public:
    template <typename Box1, typename Box2>
    struct return_type
        : services::return_type<Strategy,
                                typename point_type<Box1>::type,
                                typename point_type<Box2>::type>
    {};

    typedef typename Strategy::radius_type radius_type;

    inline cross_track_box_box()
    {}

    explicit inline cross_track_box_box(typename Strategy::radius_type const& r)
        : m_ps_strategy(r)
    {}

    inline cross_track_box_box(Strategy const& s)
        : m_ps_strategy(s)
    {}

    // It might be useful in the future
    // to overload constructor with strategy info.
    // crosstrack(...) {}

    template <typename Box1, typename Box2>
    inline typename return_type<Box1, Box2>::type
    apply(Box1 const& box1, Box2 const& box2) const
    {
#if !defined(BOOST_MSVC)
        BOOST_CONCEPT_ASSERT
            (
                (concepts::PointSegmentDistanceStrategy
                    <
                        Strategy,
                        typename point_type<Box1>::type,
                        typename point_type<Box2>::type
                    >)
            );
#endif
        typedef typename return_type<Box1, Box2>::type return_type;
        return details::cross_track_box_box_generic
                                <return_type>::apply(box1, box2, m_ps_strategy);
    }

    inline typename Strategy::radius_type radius() const
    {
        return m_ps_strategy.radius();
    }

private:
    Strategy m_ps_strategy;
};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS
namespace services
{

template <typename CalculationType, typename Strategy>
struct tag<cross_track_box_box<CalculationType, Strategy> >
{
    typedef strategy_tag_distance_box_box type;
};

template <typename CalculationType, typename Strategy, typename Box1, typename Box2>
struct return_type<cross_track_box_box<CalculationType, Strategy>, Box1, Box2>
    : cross_track_box_box
        <
            CalculationType, Strategy
        >::template return_type<Box1, Box2>
{};

template <typename CalculationType, typename Strategy>
struct comparable_type<cross_track_box_box<CalculationType, Strategy> >
{
    typedef cross_track_box_box
        <
            CalculationType, typename comparable_type<Strategy>::type
        > type;
};

template <typename CalculationType, typename Strategy>
struct get_comparable<cross_track_box_box<CalculationType, Strategy> >
{
    typedef cross_track_box_box<CalculationType, Strategy> this_strategy;
    typedef typename comparable_type<this_strategy>::type comparable_type;

public:
    static inline comparable_type apply(this_strategy const& strategy)
    {
        return comparable_type(strategy.radius());
    }
};

template <typename CalculationType, typename Strategy, typename Box1, typename Box2>
struct result_from_distance
    <
        cross_track_box_box<CalculationType, Strategy>, Box1, Box2
    >
{
private:
    typedef cross_track_box_box<CalculationType, Strategy> this_strategy;

    typedef typename this_strategy::template return_type
        <
            Box1, Box2
        >::type return_type;

public:
    template <typename T>
    static inline return_type apply(this_strategy const& strategy,
                                    T const& distance)
    {
        Strategy s(strategy.radius());

        return result_from_distance
            <
                Strategy,
                typename point_type<Box1>::type,
                typename point_type<Box2>::type
            >::apply(s, distance);
    }
};

// define cross_track_box_box<default_point_segment_strategy> as
// default box-box strategy for the spherical equatorial coordinate system
template <typename Box1, typename Box2, typename Strategy>
struct default_strategy
    <
        box_tag, box_tag, Box1, Box2,
        spherical_equatorial_tag, spherical_equatorial_tag,
        Strategy
    >
{
    typedef cross_track_box_box
        <
            void,
            typename boost::mpl::if_
                <
                    boost::is_void<Strategy>,
                    typename default_strategy
                        <
                            point_tag, segment_tag,
                            typename point_type<Box1>::type, typename point_type<Box2>::type,
                            spherical_equatorial_tag, spherical_equatorial_tag
                        >::type,
                    Strategy
                >::type
        > type;
};

} // namespace services
#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}} // namespace strategy::distance

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_DISTANCE_CROSS_TRACK_BOX_BOX_HPP


#ifndef BOOST_ADAPTBX_GRAPH_MAXIMUM_CLIQUE_RASCAL_H
#define BOOST_ADAPTBX_GRAPH_MAXIMUM_CLIQUE_RASCAL_H

#include <boost_adaptbx/graph/vertex_map.hpp>

#include <boost/graph/sequential_vertex_coloring.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>

#include <boost/tuple/tuple.hpp>
#include <boost/shared_ptr.hpp>

#include <vector>
#include <set>
#include <stack>
#include <algorithm>
#include <iterator>

namespace boost_adaptbx
{
namespace graph
{

template< typename VertexDescriptor, typename SizeType >
class rascal_state
{
public:
  typedef VertexDescriptor vertex_descriptor_type;
  typedef SizeType size_type;
  typedef std::set< vertex_descriptor_type > vertex_group_type;
  typedef std::vector< vertex_group_type > vertex_partition_type;

  typedef typename vertex_partition_type::const_iterator const_partition_iterator;

private:
  vertex_partition_type m_partition;
  size_type m_upper_bound_kwp;

public:
  rascal_state()
    : m_upper_bound_kwp( 0 )
  {};

  rascal_state(size_type const& upper_bound_kwp)
    : m_partition( upper_bound_kwp ), m_upper_bound_kwp( upper_bound_kwp )
  {};

  rascal_state(vertex_partition_type const& partition, size_type const& upper_bound_kwp)
    : m_partition( partition ), m_upper_bound_kwp( upper_bound_kwp )
  {};

  size_type& upper_bound_kwp()
  {
    return m_upper_bound_kwp;
  }

  size_type const& upper_bound_kwp() const
  {
    return m_upper_bound_kwp;
  }

  size_type upper_bound_ka() const
  {
    return m_partition.size();
  }

  size_type upper_bound() const
  {
    return std::min( upper_bound_kwp(), upper_bound_ka() );
  }

  vertex_partition_type& partition()
  {
    return m_partition;
  }

  vertex_partition_type const& partition() const
  {
    return m_partition;
  }
};

struct size_sort_predicate
{
  template< typename Container >
  bool operator ()(Container const& left, Container const& right) const
  {
    return left.size() > right.size();
  }
};

struct initial_partition_by_vertex_coloring
{
  template< typename VertexListGraph >
  rascal_state<
    typename boost::graph_traits< VertexListGraph >::vertex_descriptor,
    typename boost::graph_traits< VertexListGraph >::vertices_size_type
    >
  operator ()(VertexListGraph const& g) const
  {
    typedef boost::graph_traits< VertexListGraph > graph_traits;
    typedef typename graph_traits::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits::vertex_iterator vertex_iterator;
    typedef typename graph_traits::vertices_size_type vertices_size_type;

    typedef vertex_map::generic_vertex_map< VertexListGraph, vertices_size_type >
      color_map_type;
    typedef typename color_map_type::property_map_type color_property_map_type;

    typedef rascal_state< vertex_descriptor, vertices_size_type > state_type;
    typedef typename state_type::vertex_group_type vertex_group_type;
    typedef typename state_type::vertex_partition_type vertex_partition_type;

    color_map_type color_map( g );
    color_property_map_type color_property_map( color_map.get() );
    vertices_size_type color_count = boost::sequential_vertex_coloring(
      g,
      color_property_map
      );

    state_type result( color_count );

    vertex_iterator v, vend;
    vertex_partition_type& partition = result.partition();

    for( boost::tie( v, vend ) = boost::vertices( g ); v != vend; ++v )
    {
      vertex_descriptor const& vertex = *v;
      partition[ boost::get( color_property_map, vertex ) ].insert( vertex );
    }

    std::stable_sort( partition.begin(), partition.end(), size_sort_predicate() );
    return result;
  }
};

template< typename Descriptor >
class partial_graph_selection_predicate
{
public:
  typedef std::set< Descriptor > descriptor_set_type;
  typedef boost::shared_ptr< descriptor_set_type > descriptor_set_ptr_type;

private:
  descriptor_set_ptr_type m_descriptor_set_ptr;

public:
  partial_graph_selection_predicate()
    : m_descriptor_set_ptr( new descriptor_set_type() )
  {};

  partial_graph_selection_predicate(descriptor_set_ptr_type const& ptr)
    : m_descriptor_set_ptr( ptr )
  {};

  template< typename InputIterator >
  partial_graph_selection_predicate(InputIterator begin, InputIterator end)
    : m_descriptor_set_ptr( new descriptor_set_type( begin, end ) )
  {}

  bool operator ()(Descriptor const& desc) const
  {
    return m_descriptor_set_ptr->find( desc ) != m_descriptor_set_ptr->end();
  }

  descriptor_set_type& selection() const
  {
    return *m_descriptor_set_ptr;
  }
};

template< typename Graph1, typename Graph2, typename InputIterator >
void
selected_subgraph(Graph1 const& graph, Graph2& subgraph, InputIterator begin, InputIterator end)
{
  typedef Graph1 graph_type;
  typedef boost::graph_traits< graph_type > graph_traits;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::vertex_iterator vertex_iterator;
  typedef typename graph_traits::edge_descriptor edge_descriptor;
  typedef typename graph_traits::out_edge_iterator out_edge_iterator;
  typedef typename graph_traits::vertices_size_type vertices_size_type;

  typedef partial_graph_selection_predicate< vertex_descriptor > vertex_predicate_type;
  typedef typename vertex_predicate_type::descriptor_set_type vertex_descriptor_set_type;
  typedef partial_graph_selection_predicate< edge_descriptor > edge_predicate_type;
  typedef typename edge_predicate_type::descriptor_set_type edge_descriptor_set_type;

  typedef boost::filtered_graph< graph_type, edge_predicate_type, vertex_predicate_type >
    filtered_graph_type;
  typedef vertex_map::index_map< filtered_graph_type > index_map_type;

  vertex_predicate_type keep_vertices_pred( begin, end );
  vertex_descriptor_set_type const& keep_vertices_set = keep_vertices_pred.selection();

  edge_predicate_type keep_edges_pred;
  edge_descriptor_set_type& keep_edges_set = keep_edges_pred.selection();

  for (
    typename vertex_descriptor_set_type::const_iterator it = keep_vertices_set.begin();
    it != keep_vertices_set.end();
    ++it
    )
  {
    out_edge_iterator e, eend;

    for( boost::tie( e, eend ) = boost::out_edges( *it, graph ); e != eend; ++e )
    {
      if ( keep_vertices_set.find( boost::target( *e, graph ) ) != keep_vertices_set.end() )
      {
        keep_edges_set.insert( *e );
      }
    }
  }

  filtered_graph_type filtgraph(
    graph,
    keep_edges_pred,
    keep_vertices_pred
    );

  index_map_type index_map( filtgraph );

  boost::copy_graph(
    filtgraph,
    subgraph,
    boost::vertex_index_map( index_map.get() )
    );
}

struct upper_bound_by_chromatic_number
{
  template< typename VertexListGraph, typename Partition >
  typename boost::graph_traits< VertexListGraph >::vertices_size_type
  operator ()(VertexListGraph const& graph, Partition const& partition) const
  {
    typedef VertexListGraph graph_type;
    typedef boost::graph_traits< graph_type > graph_traits;
    typedef typename graph_traits::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits::vertices_size_type vertices_size_type;

    typedef vertex_map::generic_vertex_map< VertexListGraph, vertices_size_type >
      color_map_type;

    std::vector< vertex_descriptor > vertices;

    for (
      typename Partition::const_iterator it = partition.begin();
      it != partition.end();
      ++it
      )
    {
      std::copy( it->begin(), it->end(), std::back_inserter( vertices ) );
    }

    graph_type subgraph;
    selected_subgraph( graph, subgraph, vertices.begin(), vertices.end() );

    color_map_type color_map( graph );
    return boost::sequential_vertex_coloring( subgraph, color_map.get() );
  }

};

struct empty_size_predicate
{
  template< typename Container >
  bool operator ()(Container const& con) const
  {
    return con.empty();
  }
};

template< typename VertexListGraph, typename Partition, typename SizeType >
void
move_vertices_from_excess_partitions_if_possible(
  VertexListGraph const& graph,
  Partition& partition,
  SizeType lower_bound
  )
{
  typedef VertexListGraph graph_type;
  typedef boost::graph_traits< graph_type > graph_traits;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::vertex_iterator vertex_iterator;
  typedef typename graph_traits::edge_descriptor edge_descriptor;
  typedef typename graph_traits::adjacency_iterator adjacency_iterator;
  typedef typename graph_traits::vertices_size_type vertices_size_type;
  typedef typename Partition::value_type vertex_group_type;
  typedef typename Partition::iterator partition_iterator;
  typedef typename vertex_group_type::iterator vertex_group_iterator;

  typedef std::set< vertex_descriptor > vertex_descriptor_set;

  partition_iterator divisor = partition.begin() + std::min( lower_bound, partition.size() );

  for ( partition_iterator pit_e = divisor; pit_e != partition.end(); ++pit_e )
  {
    vertex_group_iterator next = pit_e->begin();

    for ( vertex_group_iterator vgit = next; vgit != pit_e->end(); vgit = next )
    {
      ++next;
      adjacency_iterator v, vend;
      boost::tie( v, vend ) = boost::adjacent_vertices( *vgit, graph );
      vertex_descriptor_set neighbours( v, vend );

      for ( partition_iterator pit_m = partition.begin(); pit_m != divisor; ++pit_m )
      {
        vertex_descriptor_set intersect;
        std::set_intersection(
          neighbours.begin(),
          neighbours.end(),
          pit_m->begin(),
          pit_m->end(),
          std::inserter( intersect, intersect.end() )
          );

        if ( intersect.empty() )
        {
          pit_m->insert( *vgit );
          pit_e->erase( vgit );
          break;
        }
      }
    }
  }

  partition.erase(
    std::remove_if( divisor, partition.end(), empty_size_predicate() ),
    partition.end()
    );
  std::stable_sort( partition.begin(), partition.end(), size_sort_predicate() );
}

template<
  typename AdjacencyGraph,
  typename InitialPartition,
  typename UpperBound,
  typename UserCallback
  >
void
maximum_clique_rascal(
  AdjacencyGraph const& graph,
  InitialPartition const& initial_partition,
  UpperBound const& upper_bound,
  UserCallback callback,
  typename boost::graph_traits< AdjacencyGraph >::vertices_size_type lower_bound = 1
  )
{
  typedef boost::graph_traits< AdjacencyGraph > graph_traits;
  typedef typename graph_traits::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits::vertices_size_type vertices_size_type;
  typedef typename graph_traits::adjacency_iterator adjacency_iterator;
  typedef rascal_state< vertex_descriptor, vertices_size_type > state_type;
  typedef typename state_type::vertex_group_type vertex_group_type;

  vertices_size_type max_clique_size = lower_bound + 1;
  std::vector< vertex_descriptor > branch_points;
  std::stack< state_type > states;
  states.push( initial_partition( graph ) );
  branch_points.push_back( graph_traits::null_vertex() );

  while ( true )
  {
    state_type& state = states.top();

    if (
      !state.partition().empty()
      && ( max_clique_size <= branch_points.size() + state.upper_bound() )
      )
    {
      typename vertex_group_type::iterator selit = state.partition().back().begin();
      vertex_descriptor selected = *selit;
      state.partition().back().erase( selit );
      branch_points.push_back( selected );
      max_clique_size = std::max( max_clique_size, branch_points.size() );

      adjacency_iterator v, vend;
      boost::tie( v, vend ) = boost::adjacent_vertices( selected, graph );
      std::set< vertex_descriptor > adjacents( v, vend );
      states.push( state_type() );
      state_type& next = states.top();

      for (
        typename state_type::const_partition_iterator pit = state.partition().begin();
        pit != state.partition().end() - 1;
        ++pit
        )
      {
        vertex_group_type igroup;
        std::set_intersection(
          adjacents.begin(),
          adjacents.end(),
          pit->begin(),
          pit->end(),
          std::inserter( igroup, igroup.end() )
          );

        if ( ! igroup.empty() )
        {
          next.partition().push_back( igroup );
        }
      }

      if ( state.partition().back().empty() )
      {
        state.partition().pop_back();
      }

      std::stable_sort(
        next.partition().begin(),
        next.partition().end(),
        size_sort_predicate()
        );
      move_vertices_from_excess_partitions_if_possible(
        graph,
        next.partition(),
        max_clique_size - branch_points.size()
        );
      next.upper_bound_kwp() = upper_bound( graph, next.partition() );
    }

    else
    {
      if ( max_clique_size <= branch_points.size() )
      {
        callback( branch_points.begin() + 1, branch_points.end() );
      }

      do
      {
        states.pop();
        branch_points.pop_back();
      }
      while (
        !states.empty()
        && !( max_clique_size <= branch_points.size() + states.top().upper_bound() )
        );

      if ( !states.empty() )
      {
        state_type& top = states.top();
        top.upper_bound_kwp() =  upper_bound( graph, top.partition() );
      }

      else
      {
        break;
      }
    }
  }
}

} // namespace graph
} // namespace boost_adaptbx

#endif // BOOST_ADAPTBX_GRAPH_MAXIMUM_CLIQUE_RASCAL_H


/**

\file
\author Datta Ramadasan
//==============================================================================
//         Copyright 2015 INSTITUT PASCAL UMR 6602 CNRS/Univ. Clermont II
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================

*/

#ifndef __OPTIMISATION2_BA_PCG_HPP__
#define __OPTIMISATION2_BA_PCG_HPP__

#include <libv/lma/time/tictoc.hpp>
#include <libv/lma/ttt/traits/wrap.hpp>
#include <libv/lma/ttt/traits/naming.hpp>
#include <libv/lma/lm/omp/omp.hpp>
#include <boost/fusion/include/as_map.hpp>
#include <boost/fusion/include/fold.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/mpl/for_each.hpp>
#include <boost/mpl/transform.hpp>

#include <iomanip>
#include <exception>

#include "make_type.hpp"
#include "utils.hpp"

namespace lma
{
  template<class T> bool is_zero_or_infinite(T x) { return ((x == 0.0) || (std::isinf(x))); }

  template<class S> struct AssignSameDiagInv
  {
    const S& s;
    AssignSameDiagInv(const S& s_):s(s_){}
    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Pair<Key,Key>,Cont>& obj) const
    {
      obj.second.set_diag_inv(boost::fusion::at_key<Pair<Key,Key>>(s));
    }

    template<class T> void operator()(T&) const {}
  };

  template<class D, class S> void assign_same_diag_inv(D& d, const S& s)
  {
    boost::fusion::for_each(d,AssignSameDiagInv<S>(s));
  }

  struct Minus
  {
    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& obj) const
    { this->operator()(obj.second); }

    template<class T> void operator()(T& obj) const { obj.minus(); }
  };

  template<class A> struct Dot
  {
    typedef double result_type;
    const A& a;

    Dot(const A& a_):a(a_){}

    template<class Key,class Cont, template<class,class> class Pair> result_type operator()(const result_type& prev, const Pair<Key,Cont>& pair) const
    { return this->operator()<Cont>(prev,pair.second,boost::fusion::at_key<Key>(a)); }

    template<class Cont1, class Cont2> result_type operator()(const result_type& result, const Cont1& cont, const Cont2& b) const
    {
      return result + cont.compute_dot(b);
    }
  };

  template<class A> Dot<A> make_dot(const A& a) { return Dot<A>(a); }

  template<class P, class A, class B> struct ProdDiag21
  {
    P& p;
    const A& a;
    const B& b;
    ProdDiag21(P& p_, const A& a_, const B& b_):p(p_),a(a_),b(b_){}

    template<class Key> void operator()(ttt::wrap<Key>)
    {

      auto& residu = bf::at_key<Key>(p);
      const auto& refa = bf::at_key<bf::pair<Key,Key>>(a);
//       clement(a);
//       std::cout << ttt::name<Key>() << std::endl;
//       std::cout << ttt::name<A>() << std::endl;
      const auto& refb = bf::at_key<Key>(b);
      residu.resize(refb.size());
//       #pragma omp parallel for if(use_omp())
      for(auto i = residu.first() ; i < residu.size() ; ++i)
        residu(i) = refa(i,0) * refb(i);
//       prod_trig_sup(residu,refa,refb);
    }
  };

  template<class Scalar, class V> struct DecScalarProdV
  {
    const Scalar scalar;
    const V& v;

    DecScalarProdV(const Scalar& scalar_, const V& v_):scalar(scalar_),v(v_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref = boost::fusion::at_key<Key>(v);
      result.resize(ref.size());
//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) -= scalar * ref(i);
    }
  };

  template<class Scalar, class V> struct IncScalarProdV
  {
    const Scalar scalar;
    const V& v;

    IncScalarProdV(const Scalar& scalar_, const V& v_):scalar(scalar_),v(v_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref = boost::fusion::at_key<Key>(v);
      result.resize(ref.size());
//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) += scalar * ref(i);
    }
  };

  //     p = -y + beta * p;
  template<class A, class B, class C> struct MinusPlusScalarProdV
  {
    const A& a;
    const B b;
    const C& c;
    MinusPlusScalarProdV(const A& a_, const B& b_, const C& c_):a(a_),b(b_),c(c_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref1 = boost::fusion::at_key<Key>(a);
      const auto& ref2 = boost::fusion::at_key<Key>(c);

//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) =  - ref1(i) + b * ref2(i);
    }
  };

  template<class A, class B, class C> struct PlusScalarProdV
  {
    const A& a;
    const B b;
    const C& c;
    PlusScalarProdV(const A& a_, const B& b_, const C& c_):a(a_),b(b_),c(c_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref1 = boost::fusion::at_key<Key>(a);
      const auto& ref2 = boost::fusion::at_key<Key>(c);

//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) =  ref1(i) + b * ref2(i);
    }
  };

  template<class A, class B> struct AssignSame2
  {
    A& a;
    const B& b;
    AssignSame2(A& a_, const B& b_):a(a_),b(b_){}

    template<class Key> void operator()(ttt::wrap<Key>)
    {
      auto& refa = boost::fusion::at_key<Key>(a);
      const auto& refb = boost::fusion::at_key<Key>(b);

      if (refa.size() != refb.size())
      {
        std::cout << " DIFF ! " << ttt::name<Key>() << std::endl;
        std::cout << refa.name() << std::endl;
        std::cout << refb.name() << std::endl;
        std::cout << refa.size() << " , " << refb.size() << std::endl;
        getchar();
      }
      refa = refb;

      assert(!refa.is_invalid());

    }
  };

  template<class B, class Z> struct BMINUSZ
  {
    const B& b;
    const Z& z;
    BMINUSZ(const B& b_, const Z& z_):b(b_),z(z_){}

    template<class Key, class Cont, template<class,class> class Pair> void operator()(Pair<Key,Cont>& pair) const
    {
      auto& result = pair.second;
      const auto& ref1 = boost::fusion::at_key<Key>(b);
      const auto& ref2 = boost::fusion::at_key<Key>(z);

//       #pragma omp parallel for if(use_omp())
      for(auto i = result.first() ; i < result.size() ; ++i)
        result(i) =  ref1(i) - ref2(i);
    }
  };

  template<class Scalar, class V> IncScalarProdV<Scalar,V> incscalarprodv(const Scalar& a, const V& v) { return IncScalarProdV<Scalar,V>(a,v); }

  template<class Scalar, class V> DecScalarProdV<Scalar,V> decscalarprodv(const Scalar& a, const V& v) { return DecScalarProdV<Scalar,V>(a,v); }

  template<class A, class B, class C> MinusPlusScalarProdV<A,B,C> minus_plus_scalar_prod_v(const A& a, const B& b, const C& c){ return MinusPlusScalarProdV<A,B,C>(a,b,c); }

  template<class A, class B, class C> PlusScalarProdV<A,B,C> plus_scalar_prod_v(const A& a, const B& b, const C& c){ return PlusScalarProdV<A,B,C>(a,b,c); }

  template<class A, class B> AssignSame2<A,B> assign_same2(A& a, const B& b){ return AssignSame2<A,B>(a,b); }

  template<class B, class Z> BMINUSZ<B,Z> b_minus_z(const B& b, const Z& z) { return BMINUSZ<B,Z>(b,z); }
  struct SquaredNorm
  {
    typedef double result_type;
    template<class Key, class Cont, template<class,class> class Pair> result_type operator()(const result_type& prev, const Pair<Key,Cont>& obj) const
    {
//       std::cout << " Norm : " << prev << " " << obj.second.squaredNorm() << std::endl;
      return prev + obj.second.squaredNorm();
    }
  };

  template<class Tuple> double norm(const Tuple& t)
  {
    return std::sqrt(bf::fold(t,0.0,SquaredNorm()));
  }

  template<class Tuple> double dot(const Tuple& t)
  {
    return bf::fold(t,0.0,make_dot(t));
  }

  struct PcgConfig
  {
    double seuil;
    size_t max_iteration;
    size_t frequence;
    PcgConfig(double s = 0.9999, size_t m = 100, size_t f = -1):seuil(s),max_iteration(m),frequence(f){}
  };

  struct PCG : PcgConfig
  {
    PCG(PcgConfig config):PcgConfig(config) {}

    template<class Tag, class Container, class Delta>
    void operator()(const Container& cont, Delta& delta, const Tag&)
    {
      //Pour la notation et la méthode cf Méthode numérique appliquées, A. Gourdin, M. Boumahrat : page 258,259
      static const bool disp = false;

      typedef typename Tag::second_type Float;
      typedef typename Container::OptimizeKeys ListeParametre;
      typedef typename mpl::transform<
				      ListeParametre,
				      MakeTupleTable<mpl::_1,mpl::_1,Tag>
				     >::type ListeDiag;
      typedef typename mpl::transform<ListeParametre, VectorToPairStruct<mpl::_1,Tag> >::type ListeVector;
      typedef typename br::as_map<ListeVector>::type TupleResidu;
      typedef typename br::as_map<ListeDiag>::type TupleDiag;

      TupleDiag C;
      TupleResidu r,S,p,x;

      Float r0y0 = 0,r1y1 = 0;
      Float residualNorm2=std::numeric_limits<Float>::max(),residualNorm2Initial = 0;

      //r0 = b
      r = cont.B();// - mat * x;

      residualNorm2Initial = norm(r);//bf::fold(residual,0.0,dot(residual));
//       std::cout << " Initial norm : " << residualNorm2Initial << std::endl;

      if (residualNorm2Initial==0) return;

//       cont.get_preconditionner(C);

      assign_same_diag_inv(C,cont.A());// return le preconditionner inversé

      //p = C * r
//       bf::for_each(p,prod_diag_21(C,r));
      mpl::for_each<ListeParametre,ttt::wrap<mpl::_1>>(ProdDiag21<TupleResidu,TupleDiag,TupleResidu>(p,C,r));

      // S = p
      S = p;

      size_t max_element = nb_element(r);
      max_iteration = std::min(max_iteration,max_element);

      //if (cpt > max_element) std::cerr << __FILE__ << " : " << __LINE__ <<  " cpt > max_element : " << cpt << " > " << max_element << std::endl;

//       std::cout << " S \n " << to_vect(S).transpose() << std::endl;
      // r0y0 = r * S
      r0y0 = bf::fold(r,0.0,make_dot(S));
//       std::cout << " r0y0 " << r0y0 << std::endl;

//       std::cout << " Diff r0y0" << std::abs(to_vect(r).dot(to_vect(S)) - r0y0) <<  std::endl;
//       r0y0 = to_vect(r).dot(to_vect(S));
//       assert(  to_vect(r).dot(to_vect(S)) == r0y0 );
      if (r0y0==0) {std::cerr << " r0y0 = " << r0y0 << std::endl;return;}

      std::size_t it = 0;
// /*
      for( ; it < max_iteration ; ++it)
      {
        TupleResidu Ap;
        // AP = s * p
        // on fait le calcul ailleurs pour gérer les deux cas : S implicite ou explicite
        cont.prodAP(Ap,p);
// 	mpl::for_each<typename Container::TypesA,ttt::wrap<mpl::_1>>(prod_ap_p(Ap,cont.A(),p));

        // denom = p * Ap
        Float denom = bf::fold(p,0.0,make_dot(Ap));

//         std::cout << " Diff denom " << std::abs(to_vect(p).dot(to_vect(Ap)) - denom) <<  std::endl;
//         denom = to_vect(p).dot(to_vect(Ap));

        if (std::isinf(denom)) throw INF_ERROR("PCG denom");

        if (std::isnan(denom)) { std::cerr << color.bold() << color.red() <<  " PCG : p.dot(Ap) == NAN " << color.reset() << std::endl; throw NAN_ERROR("PCG: isnan(p*Ap)"); }
        if (denom==0) { std::cerr << color.bold() << color.red() <<  " PCG : p.dot(Ap) == 0 " << color.reset() << std::endl; throw NAN_ERROR("PCG: p*Ap==0"); }

//         std::cout << " numerateur   " << r0y0 << std::endl;
//         std::cout << " denominateur " << denom << std::endl;

        const Float alpha = r0y0 / denom;

        if (std::isinf(denom)) throw INF_ERROR("PCG alpha");

        if (it !=0 && it%frequence==0)
        {
//         si r dérive, on peut utiliser (mais c'est plus lent!) :
//         r = b - Ax
          TupleResidu Ax;
          cont.prodAP(Ax,x);
          bf::for_each(r,b_minus_z(cont.B(),Ax));
        }
        else
        {
        // r -= alpha * Ap
          bf::for_each(r,decscalarprodv(alpha,Ap));
        }

        residualNorm2 = norm(r);
//         std::cout << alpha << " ";
        // x += alpha * p
//         std::cout << to_vect(x) << std::endl << std::endl;
//         std::cout << to_vect(p) << std::endl;
//         auto x2 = (to_vect(x)  + alpha * to_vect(p)).eval();
        bf::for_each(x,incscalarprodv(alpha,p));

//         std::cout << (to_vect(x) - x2).transpose() << std::endl;
//         std::cout << " residu : " << residualNorm2 << std::endl;

        if (std::abs(residualNorm2-residualNorm2Initial)/residualNorm2Initial > seuil && residualNorm2<residualNorm2Initial)
        {
//           std::cout << " break condition " << std::endl;
          break;
        }

        // S = invM * r
// //         bf::for_each(S,prod_diag_21(C,r));
	  mpl::for_each<ListeParametre,ttt::wrap<mpl::_1>>(ProdDiag21<TupleResidu,TupleDiag,TupleResidu>(S,C,r));

        // r1y1 = r.dot(S);
        r1y1 = bf::fold(r,0.0,make_dot(S));

//         std::cout << " Diff r1y1 " << std::abs(to_vect(r).dot(to_vect(S)) - r1y1) <<  std::endl;
//         r1y1 = to_vect(r).dot(to_vect(S));
        //if (r1y1>r0y0) break;
        if(is_zero_or_infinite(r1y1)) {std::cout << " r1y1 is shitted ..." << std::endl;break;}//throw ZeroOrInfiniteError("PCG r1y1");

//         if(disp) std::cout << color.magenta() << " r1y1 = " << r1y1 << color.reset() << std::endl;

        const Float beta = r1y1 / r0y0;

        if(disp) std::cout << color.magenta() << " beta = " << beta << color.reset() << std::endl;

        if (is_zero_or_infinite(beta)) ZeroOrInfiniteError("PCG beta");
        // p = S + beta * p;
        bf::for_each(p,plus_scalar_prod_v(S,beta,p));

        r0y0 = r1y1;

//         std::cout << " critère : " << residualNorm2 << " / " << residualNorm2Initial << " = " << residualNorm2 / residualNorm2Initial << std::endl;
//         if (residualNorm2 / residualNorm2Initial < 0.1) break;
//         std::cout << " residual[" << it << "]: " << r1y1 << std::endl;
      }

//      std::cout << " nb iteration = " << it << " / " << max_iteration << std::endl;
//       std::cout <<  "final residual : " << residualNorm2 << std::endl;
//     std::cout << " FINAL [" << it << "]: " << r1y1 << std::endl;
      mpl::for_each<ListeParametre,ttt::wrap<mpl::_1>>(assign_same2(delta,x));
//       std::cout << " my pcg     :  " << to_matv(x).transpose() << std::endl;

    }
  };
}// eon

namespace ttt
{
  template<> struct Name<lma::PCG> { static std::string name(){ return "SSparsePCG"; } };
}

/*
void solve(const MatD& S, VecD& delta, const VecD& E, const std::size_t& nb_camera)
{
  const std::size_t size = nb_camera * C;

  double seuil = 1e-16;   //à choisir entre 1e-8 et 1e-16 (influe sur le nombre d'itérations du PCG et sa précision)

  double residual, residual_first;

  VecD r = -E;
  VecD x = VecD::Zero(size);
  residual_first = r.dot(r);

  VectorCC invMv(nb_camera);
  for(std::size_t i = 0 ; i < nb_camera ; ++i)
    invMv[i] = S.inverse();

  VecD y = VecD::Zero(size);

  for(std::size_t i = 0 ; i < nb_camera ; ++i)
    BlockC1::view(y,i*C,0) = invMv[i] * BlockC1::view(r,i*C,0);

  VecD p = - y;
  double r0y0 = r.dot(y), r1y1;

  for(std::size_t it = 0 ; it < size ; ++it)
  {
    VecD Ap = S * p;

    double temp = p.dot(Ap) ;

    if (std::isnan(temp)) { V_TXT(" PCG : p.dot(Ap) == NAN "); break; }

    const double alpha = r0y0 / temp;

    x += alpha * p;
    r += alpha * Ap;
    residual = r.dot(r);

    if ( residual < seuil * residual_first ) {break;}

    for(std::size_t i = 0 ; i < nb_camera ; ++i)
      BlockC1::view(y,i*C,0) = invMv[i] * BlockC1::view(r,i*C,0);

    r1y1 = r.dot(y);

    const double beta = r1y1 / r0y0;
    p = -y + beta * p;

    r0y0 = r1y1;
  }
  delta = x;
}
*/

#endif


#include <boost/test/unit_test.hpp>
#include <boost/format.hpp>

#include <sstream>

#include <madym/tests/mdm_test_utils.h>

#include <madym/mdm_AIF.h>
#include <madym/dce_models/mdm_DCEModelGenerator.h>
#include <madym/run/mdm_RunTools_madym_DCE.h>
#include <madym/image_io/mdm_ImageIO.h>
#include <madym/mdm_Image3D.h>
#include <madym/mdm_ParamSummaryStats.h>
#include <cmath>
#include <mdm_version.h>

namespace fs = boost::filesystem;

void check_output(
	const std::string& Ct_output_dir,
	const std::vector<double> &trueParams,
	const std::vector<double> IAUCTimes,
	const std::vector<double> IAUCVals,
  mdm_ImageIO::ImageFormat imageFormat)
{
  double tol = 1.0;

  //Even with empty model, these should be created
  mdm_Image3D model_fit = mdm_ImageIO::readImage3D(imageFormat, Ct_output_dir + "residuals", false);
  mdm_Image3D error_codes = mdm_ImageIO::readImage3D(imageFormat, Ct_output_dir + "error_tracker", false);
  mdm_Image3D enhancing = mdm_ImageIO::readImage3D(imageFormat, Ct_output_dir + "enhVox", false);

  //Check error codes and enhancing
  BOOST_TEST_MESSAGE("No error code");
  BOOST_CHECK(!error_codes.voxel(0));
  BOOST_TEST_MESSAGE("Enhancing");
  BOOST_CHECK(enhancing.voxel(0));

  if (!trueParams.empty())
  {
    //Load in the parameter img vols and extract the single voxel from each
    mdm_Image3D ktrans_fit = mdm_ImageIO::readImage3D(imageFormat, Ct_output_dir + "Ktrans", false);
    mdm_Image3D ve_fit = mdm_ImageIO::readImage3D(imageFormat, Ct_output_dir + "v_e", false);
    mdm_Image3D vp_fit = mdm_ImageIO::readImage3D(imageFormat, Ct_output_dir + "v_p", false);
    mdm_Image3D tau_fit = mdm_ImageIO::readImage3D(imageFormat, Ct_output_dir + "tau_a", false);

    //Check the model parameters have fitted correctly
    BOOST_TEST_MESSAGE(boost::format("Fitted ktrans (%1.2f, %2.2f)")
      % ktrans_fit.voxel(0) % trueParams[0]);
    BOOST_CHECK_CLOSE(ktrans_fit.voxel(0), trueParams[0], tol);
    BOOST_TEST_MESSAGE(boost::format("Fitted Ve (%1.2f, %2.2f)")
      % ve_fit.voxel(0) % trueParams[1]);
    BOOST_CHECK_CLOSE(ve_fit.voxel(0), trueParams[1], tol);
    BOOST_TEST_MESSAGE(boost::format("Fitted Vp (%1.2f, %2.2f)")
      % vp_fit.voxel(0) % trueParams[2]);
    BOOST_CHECK_CLOSE(vp_fit.voxel(0), trueParams[2], tol);
    BOOST_TEST_MESSAGE(boost::format("Fitted tau (%1.2f, %2.2f)")
      % tau_fit.voxel(0) % trueParams[3]);
    BOOST_CHECK_CLOSE(tau_fit.voxel(0), trueParams[3], tol);

    //Check model fit, error codes and enhancing
    BOOST_TEST_MESSAGE(boost::format("Model residuals = %1%")
      % model_fit.voxel(0));

    //Read stats file
    mdm_ParamSummaryStats stats;
    stats.openStatsFile(Ct_output_dir + "ROI_summary_stats.csv");
    for (int param = 0; param < 4; param++)
    {
      stats.readStats();
      BOOST_CHECK_CLOSE(stats.stats().mean_, trueParams[param], tol);
      BOOST_CHECK_EQUAL(stats.stats().stddev_, 0);
      BOOST_CHECK_EQUAL(stats.stats().validVoxels_, 1);
    }

    stats.closeStatsFile();
  }

	//Check IAUC
	for (auto i = 0; i < IAUCTimes.size(); i++)
	{
		std::string iauc_name = Ct_output_dir + "IAUC" + std::to_string((int)IAUCTimes[i]);
		mdm_Image3D iauc = mdm_ImageIO::readImage3D(imageFormat, iauc_name, false);
		BOOST_TEST_MESSAGE("Fitted IAUC" + std::to_string((int)IAUCTimes[i]));
		BOOST_CHECK_CLOSE(iauc.voxel(0), IAUCVals[i], tol);
	}

	//Tidy up
	fs::remove_all(Ct_output_dir);
}

BOOST_AUTO_TEST_SUITE(test_mdm_tools)

BOOST_AUTO_TEST_CASE(test_madym_DCE) {
	BOOST_TEST_MESSAGE("======= Testing tool: madym_DCE =======");
	//Need to generate a dataset of analyze images. To do this, load in calibration
	//data

	//Read in dyn times
	int nTimes;
	std::string timesFileName(mdm_test_utils::calibration_dir() + "dyn_times.dat");
	std::ifstream timesFileStream(timesFileName, std::ios::in | std::ios::binary);
	timesFileStream.read(reinterpret_cast<char*>(&nTimes), sizeof(int));
	std::vector<double> dynTimes(nTimes);
	for (double &t : dynTimes)
		timesFileStream.read(reinterpret_cast<char*>(&t), sizeof(double));
	timesFileStream.close();

	//Read in AIF parameters
	int injectionImage;
	double hct;
	double dose;

	std::string aifFileName(mdm_test_utils::calibration_dir() + "aif.dat");
	std::ifstream aifFileStream(aifFileName, std::ios::in | std::ios::binary);
	aifFileStream.read(reinterpret_cast<char*>(&injectionImage), sizeof(int));
	aifFileStream.read(reinterpret_cast<char*>(&hct), sizeof(double));
	aifFileStream.read(reinterpret_cast<char*>(&dose), sizeof(double));
	aifFileStream.close();

	//Read (noisy) ETM times series from calibration data
	int nParams;
	std::vector<double> Ct(nTimes);
	std::string modelFileName = mdm_test_utils::calibration_dir() + "ETM_noise.dat";

	std::ifstream modelFileStream(modelFileName, std::ios::in | std::ios::binary);
	modelFileStream.read(reinterpret_cast<char*>(&nParams), sizeof(int));

	std::vector<double> trueParams(nParams);
	for (double &p : trueParams)
		modelFileStream.read(reinterpret_cast<char*>(&p), sizeof(double));
	for (double &c : Ct)
		modelFileStream.read(reinterpret_cast<char*>(&c), sizeof(double));
	modelFileStream.close();

	//Read IAUC values
	int nIAUC;
	std::string iaucFileName = mdm_test_utils::calibration_dir() + "ETM_IAUC.dat";
	std::ifstream iaucFileStream(iaucFileName, std::ios::in | std::ios::binary);
	iaucFileStream.read(reinterpret_cast<char*>(&nIAUC), sizeof(int));
	std::vector<double> IAUCTimes(nIAUC);
	std::vector<double> IAUCVals(nIAUC);
	for (double &it : IAUCTimes)
		iaucFileStream.read(reinterpret_cast<char*>(&it), sizeof(double));

	for (double &iv : IAUCVals)
		iaucFileStream.read(reinterpret_cast<char*>(&iv), sizeof(double));
	iaucFileStream.close();

	std::string IAUC_str = std::to_string((int)IAUCTimes[0]);
	for (int i = 1; i < nIAUC; i++)
		IAUC_str += ("," + std::to_string((int)IAUCTimes[i]));

	//Create a temporary directory where we'll run these tests, which we can then cleanup
	//easily at the end
	std::string test_dir = mdm_test_utils::temp_dir();
	std::string dyn_dir = test_dir +"/dynamics/";
	fs::create_directories(dyn_dir);

	for (int i_t = 0; i_t < nTimes; i_t++)
	{
		//Write out 1x1 concentration maps and xtr files
		std::string Ct_name = dyn_dir + "Ct_" + (boost::format("%02u") % (i_t + 1)).str();

		mdm_Image3D Ct_img;
		Ct_img.setDimensions(1, 1, 1);
		Ct_img.setVoxelDims(1, 1, 1);
		Ct_img.setTimeStampFromMins(dynTimes[i_t]);
		Ct_img.setVoxel(0, Ct[i_t]);

		mdm_AnalyzeFormat::writeImage3D(Ct_name, Ct_img,
			mdm_ImageDatatypes::DT_FLOAT, mdm_XtrFormat::NEW_XTR, false);

		if (!i_t)
			BOOST_TEST_MESSAGE("Saved 1st dynamic image " << Ct_name);
	}

	//Run 2 types of tests:
	// 1) Using a run tools object, this runs the complete pipeline but doesn't involve a system call
	// 2) Calling system to run a command line call

	//-------------------------------------------------------------------------------
	// 1) Using a run tools object
	//-------------------------------------------------------------------------------
	{
		std::string Ct_output_dir = test_dir + "/mdm_analysis_Ct1/";
    mdm_RunTools_madym_DCE madym_exe;
		auto &madym_options = madym_exe.options();

		madym_options.model.set("ETM");
		madym_options.outputDir.set(Ct_output_dir);
		madym_options.dynDir.set(dyn_dir);
		madym_options.dynName.set("Ct_");
		madym_options.sequenceFormat.set("%02u");
		madym_options.nDyns.set(nTimes);
		madym_options.injectionImage.set(injectionImage);
		madym_options.dose.set(dose);
		madym_options.hct.set(hct);
		madym_options.IAUCTimes.set(IAUCTimes);
		madym_options.inputCt.set(true);
    madym_options.imageWriteFormat.set("ANALYZE");
		madym_options.overwrite.set(true);
    madym_options.noAudit.set(true);

		madym_exe.parseInputs("test_madym_DCE");
		int result = madym_exe.run_catch();

		BOOST_CHECK_MESSAGE(!result, "Running madym_DCE failed");
		check_output(Ct_output_dir,
			trueParams,
			IAUCTimes,
			IAUCVals,
      mdm_ImageIO::ImageFormat::ANALYZE
    );
	}

	//-------------------------------------------------------------------------------
	// 2) From the command line
	//-------------------------------------------------------------------------------
	{
		std::string Ct_output_dir = test_dir + "/mdm_analysis_Ct2/";
		std::stringstream cmd;
		cmd << mdm_test_utils::tools_exe_dir() << "madym_DCE"
			<< " -m ETM"
			<< " -o " << Ct_output_dir
			<< " --dyn " << dyn_dir << "Ct_"
			<< " --sequence_format " << "%02u"
			<< " -n " << nTimes
			<< " -i " << injectionImage
			<< " -D " << dose
			<< " -H " << hct
			<< " -I " << IAUC_str
			<< " --Ct"
      << " --img_fmt_w " << "ANALYZE"
      << " --overwrite"
      << " --no_audit";

		BOOST_TEST_MESSAGE("Command to run: " + cmd.str());

		int error;
		try
		{
			error = std::system(cmd.str().c_str());
		}
		catch (...)
		{
			BOOST_CHECK_MESSAGE(false, "Running madym_DCE failed");
			return;
		}

		BOOST_CHECK_MESSAGE(!error, "Error returned from madym_DCE tool");
		check_output(Ct_output_dir,
			trueParams,
			IAUCTimes,
			IAUCVals,
      mdm_ImageIO::ImageFormat::ANALYZE);
	}

	//-------------------------------------------------------------------------------
	// 3) Using a run tools object with empty IAUC values
	//-------------------------------------------------------------------------------
	{
		std::string Ct_output_dir = test_dir + "/mdm_analysis_Ct3/";
    mdm_RunTools_madym_DCE madym_exe;
    auto &madym_options = madym_exe.options();
		madym_options.model.set("ETM");
		madym_options.outputDir.set(Ct_output_dir);
		madym_options.dynDir.set(dyn_dir);
		madym_options.sequenceFormat.set("%02u");
		madym_options.dynName.set("Ct_");
		madym_options.nDyns.set(nTimes);
		madym_options.injectionImage.set(injectionImage);
		madym_options.dose.set(dose);
		madym_options.hct.set(hct);
		madym_options.IAUCTimes.set({});
		madym_options.inputCt.set(true);
    madym_options.imageWriteFormat.set("ANALYZE");
    madym_options.overwrite.set(true);
    madym_options.noAudit.set(true);

		madym_exe.parseInputs("test_madym_DCE_noI");
		int result = madym_exe.run_catch();

		BOOST_CHECK_MESSAGE(!result, "Running madym_DCE failed");
		check_output(Ct_output_dir,
			trueParams,
			{},
			{},
      mdm_ImageIO::ImageFormat::ANALYZE);
	}

  //-------------------------------------------------------------------------------
  // 4) Using a run tools object with empty model
  //-------------------------------------------------------------------------------
  {
    std::string Ct_output_dir = test_dir + "/mdm_analysis_Ct4/";
    mdm_RunTools_madym_DCE madym_exe;
    auto &madym_options = madym_exe.options();
    madym_options.model.set("NONE");
    madym_options.outputDir.set(Ct_output_dir);
    madym_options.dynDir.set(dyn_dir);
    madym_options.sequenceFormat.set("%02u");
    madym_options.dynName.set("Ct_");
    madym_options.nDyns.set(nTimes);
    madym_options.injectionImage.set(injectionImage);
    madym_options.dose.set(dose);
    madym_options.hct.set(hct);
    madym_options.IAUCTimes.set(IAUCTimes);
    madym_options.inputCt.set(true);
    madym_options.imageWriteFormat.set("ANALYZE");
    madym_options.overwrite.set(true);
    madym_options.noAudit.set(true);

    madym_exe.parseInputs("test_madym_DCE_noM");
    int result = madym_exe.run_catch();

    BOOST_CHECK_MESSAGE(!result, "Running madym_DCE failed");
    check_output(Ct_output_dir,
      {},
      IAUCTimes,
      IAUCVals,
      mdm_ImageIO::ImageFormat::ANALYZE);
  }

  //-------------------------------------------------------------------------------
  // 5) Using NIFTI as image format
  //-------------------------------------------------------------------------------
  {
    std::string Ct_output_dir = test_dir + "/mdm_analysis_Ct1/";
    mdm_RunTools_madym_DCE madym_exe;
    auto &madym_options = madym_exe.options();

    madym_options.model.set("ETM");
    madym_options.outputDir.set(Ct_output_dir);
    madym_options.dynDir.set(dyn_dir);
    madym_options.dynName.set("Ct_");
    madym_options.sequenceFormat.set("%02u");
    madym_options.nDyns.set(nTimes);
    madym_options.injectionImage.set(injectionImage);
    madym_options.dose.set(dose);
    madym_options.hct.set(hct);
    madym_options.IAUCTimes.set(IAUCTimes);
    madym_options.inputCt.set(true);
    madym_options.imageWriteFormat.set("NIFTI");
    madym_options.overwrite.set(true);
    madym_options.noAudit.set(true);

    madym_exe.parseInputs("test_madym_DCE");
    int result = madym_exe.run_catch();

    BOOST_CHECK_MESSAGE(!result, "Running madym_DCE failed");
    check_output(Ct_output_dir,
      trueParams,
      IAUCTimes,
      IAUCVals,
      mdm_ImageIO::ImageFormat::NIFTI
    );
  }

  //-------------------------------------------------------------------------------
  // 6) Fit once, then reload params and call no opt
  //-------------------------------------------------------------------------------
  {
    std::string Ct_output_dir = test_dir + "/mdm_analysis_Ct1/";
    {
      mdm_RunTools_madym_DCE madym_exe;
      auto &madym_options = madym_exe.options();

      madym_options.model.set("ETM");
      madym_options.outputDir.set(Ct_output_dir);
      madym_options.dynDir.set(dyn_dir);
      madym_options.dynName.set("Ct_");
      madym_options.sequenceFormat.set("%02u");
      madym_options.nDyns.set(nTimes);
      madym_options.injectionImage.set(injectionImage);
      madym_options.dose.set(dose);
      madym_options.hct.set(hct);
      madym_options.IAUCTimes.set(IAUCTimes);
      madym_options.inputCt.set(true);
      madym_options.imageWriteFormat.set("ANALYZE");
      madym_options.overwrite.set(true);
      madym_options.noAudit.set(true);

      madym_exe.parseInputs("test_madym_DCE");
      int result = madym_exe.run_catch();

      BOOST_CHECK_MESSAGE(!result, "Running madym_DCE failed");
    }
    {
      mdm_RunTools_madym_DCE madym_exe;
      auto &madym_options = madym_exe.options();

      madym_options.model.set("ETM");
      madym_options.outputDir.set(Ct_output_dir);
      madym_options.dynDir.set(dyn_dir);
      madym_options.dynName.set("Ct_");
      madym_options.sequenceFormat.set("%02u");
      madym_options.nDyns.set(nTimes);
      madym_options.injectionImage.set(injectionImage);
      madym_options.dose.set(dose);
      madym_options.hct.set(hct);
      madym_options.IAUCTimes.set(IAUCTimes);
      madym_options.inputCt.set(true);
      madym_options.imageWriteFormat.set("ANALYZE");
      madym_options.initMapsDir.set(Ct_output_dir);
      madym_options.noOptimise.set(true);
      madym_options.overwrite.set(true);
      madym_options.noAudit.set(true);

      madym_exe.parseInputs("test_madym_DCE");
      int result = madym_exe.run_catch();

      BOOST_CHECK_MESSAGE(!result, "Running madym_DCE from init params map failed");
    }

    check_output(Ct_output_dir,
      trueParams,
      IAUCTimes,
      IAUCVals,
      mdm_ImageIO::ImageFormat::ANALYZE
    );
  }
  //-------------------------------------------------------------------------------
  // 6) Check voxel size checks works as expected
  //-------------------------------------------------------------------------------
  {
    //
    mdm_Image3D ROI;
    ROI.setDimensions(1, 1, 1);
    ROI.setVoxelDims(1, 1, 2);
    ROI.setVoxel(0, 1);

    std::string ROI_name = dyn_dir + "ROI";
    mdm_AnalyzeFormat::writeImage3D(ROI_name, ROI,
      mdm_ImageDatatypes::DT_FLOAT, mdm_XtrFormat::NO_XTR, false);

    std::string Ct_output_dir = test_dir + "/mdm_analysis_Ct1/";

    {
      mdm_RunTools_madym_DCE madym_exe;
      auto &madym_options = madym_exe.options();

      madym_options.model.set("ETM");
      madym_options.outputDir.set(Ct_output_dir);
      madym_options.dynDir.set(dyn_dir);
      madym_options.dynName.set("Ct_");
      madym_options.sequenceFormat.set("%02u");
      madym_options.nDyns.set(nTimes);
      madym_options.roiName.set(ROI_name);
      madym_options.injectionImage.set(injectionImage);
      madym_options.dose.set(dose);
      madym_options.hct.set(hct);
      madym_options.IAUCTimes.set(IAUCTimes);
      madym_options.inputCt.set(true);
      madym_options.imageWriteFormat.set("ANALYZE");
      madym_options.overwrite.set(true);
      madym_options.noAudit.set(true);

      madym_exe.parseInputs("test_madym_DCE");
      int result = madym_exe.run_catch();
      BOOST_CHECK_EQUAL(result, 1.0);

    }
    {
      mdm_RunTools_madym_DCE madym_exe;
      auto &madym_options = madym_exe.options();

      madym_options.model.set("ETM");
      madym_options.outputDir.set(Ct_output_dir);
      madym_options.dynDir.set(dyn_dir);
      madym_options.dynName.set("Ct_");
      madym_options.sequenceFormat.set("%02u");
      madym_options.nDyns.set(nTimes);
      madym_options.roiName.set(ROI_name);
      madym_options.injectionImage.set(injectionImage);
      madym_options.dose.set(dose);
      madym_options.hct.set(hct);
      madym_options.IAUCTimes.set(IAUCTimes);
      madym_options.inputCt.set(true);
      madym_options.imageWriteFormat.set("ANALYZE");
      madym_options.voxelSizeWarnOnly.set(true);
      madym_options.overwrite.set(true);
      madym_options.noAudit.set(true);

      madym_exe.parseInputs("test_madym_DCE");
      int result = madym_exe.run_catch();

      BOOST_CHECK_MESSAGE(!result, "Running madym_DCE failed");
    }

    check_output(Ct_output_dir,
      trueParams,
      IAUCTimes,
      IAUCVals,
      mdm_ImageIO::ImageFormat::ANALYZE
    );
  }

	//---------------------------------------------------------------------------
	//Tidy up
	fs::remove_all(dyn_dir);
}

BOOST_AUTO_TEST_SUITE_END() //


#define BOOST_TEST_MODULE "test_position_restraint_interaction"

#ifdef BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif

#include <test/util/utility.hpp>

#include <mjolnir/core/BoundaryCondition.hpp>
#include <mjolnir/core/SimulatorTraits.hpp>
#include <mjolnir/forcefield/external/PositionRestraintInteraction.hpp>
#include <mjolnir/forcefield/local/HarmonicPotential.hpp>
#include <mjolnir/util/make_unique.hpp>
#include <random>

BOOST_AUTO_TEST_CASE(PositionRestraint_numerical_differentiation)
{
    namespace test = mjolnir::test;

    mjolnir::LoggerManager::set_default_logger("test_position_restraint_interaction.log");

    using traits_type      = mjolnir::SimulatorTraits<double, mjolnir::UnlimitedBoundary>;
    using real_type        = traits_type::real_type;
    using coordinate_type  = traits_type::coordinate_type;
    using boundary_type    = traits_type::boundary_type;
    using system_type      = mjolnir::System<traits_type>;
    using potential_type   = mjolnir::HarmonicPotential<real_type>;
    using interaction_type = mjolnir::PositionRestraintInteraction<traits_type, potential_type>;

    const potential_type pot1{/*k =*/1.0, /*r0 =*/ 0.0};
    const potential_type pot2{/*k =*/1.0, /*r0 =*/10.0};

    interaction_type interaction(
        std::vector<std::tuple<std::size_t, coordinate_type, potential_type>>{
            std::make_tuple(0, coordinate_type{0.0,0.0,0.0}, pot1),
            std::make_tuple(1, coordinate_type{0.0,0.0,0.0}, pot2)
        });

    std::mt19937 rng(123456789);

    for(std::size_t trial = 0; trial < 1000; ++trial)
    {
        system_type sys(2, boundary_type{});
        test::clear_everything(sys);

        sys.position(0) = coordinate_type( 0.0,  0.0,  0.0);
        sys.position(1) = coordinate_type(10.0,  0.0,  0.0);

        interaction.initialize(sys);

        test::apply_random_perturbation(sys, rng, 0.1);

        constexpr real_type tol = 1e-3;
        constexpr real_type dr  = 1e-4;
        test::check_force(sys, interaction, tol, dr, false);
        test::check_force_and_energy(sys, interaction, tol);
    }
}


#include <iostream>                    // std::flush
#include <fstream>                     // std::ofstream
#include <string>                      // std::string
#include <tuple>                       // std::tuple
#include <vector>                      // std::vector
// For External Library
#include <torch/torch.h>               // torch
#include <boost/program_options.hpp>   // boost::program_options
// For Original Header
#include "loss.hpp"                    // Loss
#include "networks.hpp"                // YOLOv2
#include "dataloader.hpp"              // DataLoader::ImageFolderBBWithPaths
#include "visualizer.hpp"              // visualizer::graph

// Define Namespace
namespace po = boost::program_options;

// -------------------
// Validation Function
// -------------------
void valid(po::variables_map &vm, DataLoader::ImageFolderBBWithPaths &valid_dataloader, torch::Device &device, Loss &criterion, YOLOv2 &model, const std::vector<std::string> class_names, const size_t epoch, std::vector<visualizer::graph> &writer){

    // (0) Initialization and Declaration
    size_t iteration;
    float ave_loss_coord_xy, total_loss_coord_xy;
    float ave_loss_coord_wh, total_loss_coord_wh;
    float ave_loss_obj, total_loss_obj;
    float ave_loss_noobj, total_loss_noobj;
    float ave_loss_class, total_loss_class;
    float ave_loss_all;
    std::ofstream ofs;
    std::tuple<torch::Tensor, std::vector<std::tuple<torch::Tensor, torch::Tensor>>, std::vector<std::string>, std::vector<std::string>> mini_batch;
    torch::Tensor image, output;
    torch::Tensor loss_coord_xy, loss_coord_wh, loss_obj, loss_noobj, loss_class;
    std::vector<std::tuple<torch::Tensor, torch::Tensor>> label;
    std::tuple<torch::Tensor, torch::Tensor, torch::Tensor, torch::Tensor, torch::Tensor> losses;

    // (1) Tensor Forward per Mini Batch
    torch::NoGradGuard no_grad;
    model->eval();
    iteration = 0;
    total_loss_coord_xy = 0.0; total_loss_coord_wh = 0.0; total_loss_obj = 0.0; total_loss_noobj = 0.0; total_loss_class = 0.0;
    while (valid_dataloader(mini_batch)){

        image = std::get<0>(mini_batch).to(device);  // {N,C,H,W} (images)
        label = std::get<1>(mini_batch);  // {N, ({BB_n}, {BB_n,4}) } (annotations)

        output = model->forward(image);  // {N,C,H,W} ===> {N,G,G,FF}
        losses = criterion(output, label);

        loss_coord_xy = std::get<0>(losses) * vm["Lambda_coord"].as<float>();
        loss_coord_wh = std::get<1>(losses) * vm["Lambda_coord"].as<float>();
        loss_obj = std::get<2>(losses) * vm["Lambda_object"].as<float>();
        loss_noobj = std::get<3>(losses) * vm["Lambda_noobject"].as<float>();
        loss_class = std::get<4>(losses) * vm["Lambda_class"].as<float>();

        total_loss_coord_xy += loss_coord_xy.item<float>();
        total_loss_coord_wh += loss_coord_wh.item<float>();
        total_loss_obj += loss_obj.item<float>();
        total_loss_noobj += loss_noobj.item<float>();
        total_loss_class += loss_class.item<float>();

        iteration++;

    }

    // (2) Calculate Average Loss
    ave_loss_coord_xy = total_loss_coord_xy / (float)iteration;
    ave_loss_coord_wh = total_loss_coord_wh / (float)iteration;
    ave_loss_obj = total_loss_obj / (float)iteration;
    ave_loss_noobj = total_loss_noobj / (float)iteration;
    ave_loss_class = total_loss_class / (float)iteration;
    ave_loss_all = ave_loss_coord_xy + ave_loss_coord_wh + ave_loss_obj + ave_loss_noobj + ave_loss_class;

    // (3.1) Record Loss (Log)
    ofs.open("checkpoints/" + vm["dataset"].as<std::string>() + "/log/valid.txt", std::ios::app);
    ofs << "epoch:" << epoch << '/' << vm["epochs"].as<size_t>() << ' ' << std::flush;
    ofs << "coord_xy:" << ave_loss_coord_xy << ' ' << std::flush;
    ofs << "coord_wh:" << ave_loss_coord_wh << ' ' << std::flush;
    ofs << "conf_o:" << ave_loss_obj << ' ' << std::flush;
    ofs << "conf_x:" << ave_loss_noobj << ' ' << std::flush;
    ofs << "class:" << ave_loss_class << std::endl;
    ofs.close();

    // (3.2) Record Loss (Graph)
    writer.at(0).plot(/*base=*/epoch, /*value=*/{ave_loss_all});
    writer.at(1).plot(/*base=*/epoch, /*value=*/{ave_loss_coord_xy});
    writer.at(2).plot(/*base=*/epoch, /*value=*/{ave_loss_coord_wh});
    writer.at(3).plot(/*base=*/epoch, /*value=*/{ave_loss_obj});
    writer.at(4).plot(/*base=*/epoch, /*value=*/{ave_loss_noobj});
    writer.at(5).plot(/*base=*/epoch, /*value=*/{ave_loss_class});

    // End Processing
    return;

}


#include <iostream>
#include <string>
#include <vector>
#include <map>
#include <algorithm>
#include <stdexcept>
#include <list>
#include <set>
#include <fstream>
#include <iomanip>

#include <boost/program_options.hpp>
#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/regex.hpp>

typedef unsigned long long ullong;

using namespace std;
/*
first_indices = {
    'AACGTCAA':0,#	85457155
    'GCATCTCC':1,#	84432940
    'CTGCGATG':2,#	70279153
    'TGTAAGGT':3,#	54412965

    'GTCCTAAC':4,#	83802878
    'CGGTCCCA':5,#	78867120
    'ACAGAGGT':6,#	73400348
     NCAGAGGT
    'TATAGTTG':7}#	67410698
*/

namespace {
  string convert_from_code(ullong code, int length) {
    char *buf = new char[length + 1];
    buf[length] = '\0';
    for (int i = 0; i < length; i++) {
      ullong n = (code >> ((length - 1 - i) * 2)) & 0x03;
      switch (n) {
      case 0:
	buf[i] = 'A'; break;
      case 1:
	buf[i] = 'C'; break;
      case 2:
	buf[i] = 'G'; break;
      default:
	buf[i] = 'T'; break;
      }
    }
    string seq = buf;
    delete[] buf;
    return seq;
  }
  ullong convert_to_code(const string& line, int length=0) {
    const char* ptr = line.c_str();
    if (length == 0) {
      length = line.size();
    }
    ullong code = 0;
    for (int i = 0; i < (int)line.size(); i++) {
      char c = ptr[i];
      ullong c_ = 0;
      switch (c) {
      case 'A': c_ = 0; break;
      case 'C': c_ = 1; break;
      case 'G': c_ = 2; break;
      case 'T': c_ = 3; break;
      default :
	c_ = 0; break;
      }
      code = (code << 2) | c_;
    }
    return code;
  }

  pair<ullong,ullong> encode_sequence(const string& seq, int N=0) {
    ullong code = 0;
    ullong mask = 0;
    const char* ptr = seq.c_str();// + code.size() - 1;
    if (N == 0) {
      N = seq.size();
    }
    for (int i = 0; i < N; i++) {
      char c = ptr[i];
      int shift = (N - 1 - i) << 1;
      if (c == 'N') {
	mask |= (3 << shift);
      } else if (c == 'C') {
	code |= (1 << shift);
      } else if (c == 'G') {
	code |= (2 << shift);
      } else if (c == 'T') {
	code |= (3 << shift);
      }
    }
    return make_pair(code, mask);
  }

  vector<ullong> encode_indexes(const vector<string>& seqs, int length=0) {
    vector<ullong> codes;
    //cout << "encode requested " << seqs.size() << ", " << length << endl;
    for (auto it = seqs.begin(); it != seqs.end(); it++) {
      //cout << length << " " << *it << " " << it->size() << endl;
      if (length > 0 && it->size() != length) {
	//cout << "SKIP " << *it << " " << length << " " << it->size() << endl;
	continue;
      }
      pair<ullong,ullong> cm = encode_sequence(*it, length);
      if (cm.second == 0) {
	codes.push_back(cm.first);
      // } else {
      // 	cout << "N included " << *it << endl;
	continue;
      }
    }
    cout << codes.size() << " codes generated\n";
    return codes;
  }
}

namespace {
  map<string,ullong> reduce_cache_size(const map<string,ullong>& cache, ullong capacity) {
    if (capacity > cache.size()) {
      return cache;
    }
    map<string,ullong> reduced;
    ullong* buf = new ullong[cache.size()];
    size_t i = 0;
    for (auto it = cache.begin(); it != cache.end(); it++) {
      buf[i++] = it->second;
    }
    sort(buf, buf + cache.size());
    ullong threshold = buf[capacity];
    //cerr << cache.size() << " => " << threshold << "      ";
    delete[] buf;
    for (auto it = cache.begin(); it != cache.end(); it++) {
      if (it->second > threshold) {
	reduced[it->first] = it->second;
      }
    }

    //cerr << cache.size() << " => " << reduced.size() << "         \r";
    return reduced;
  }

  void display_frequency_stat(const map<string, ullong>& cache, ullong size, ostream& out) {
    out << "display frequency\n";
    vector<string> codes;
    for (auto it = cache.begin(); it != cache.end(); it++) {
      codes.push_back(it->first);
    }
    sort(codes.begin(), codes.end(), [&](const string& a, const string& b){return cache.find(a)->second > cache.find(b)->second;});
    ullong end = size < cache.size() ? size : cache.size();
    //out << end << ", " << codes.size() << endl;
    for (size_t i = 0; i < end; i++) {
      out << codes[i] << "\t" << cache.find(codes[i])->second << "\n";
    }
  }

  pair<map<string,ullong>,map<string,ullong>> load_index_sets(const string& filename_index, int well_barcode_size=8, int cell_barcode_size=16, ullong well_index_capacity=200,ullong cell_index_capacity=16000, ullong lap=1000000, bool verbose=false) throw (exception) {
    namespace io = boost::iostreams;

    map<string,ullong> well_indexes;
    map<string,ullong> cell_indexes;
    // count_freq
    io::filtering_istream fistr;// io::filtering_istream();
    if (filename_index.rfind(".gz") == filename_index.size() - 3) {
      fistr.push(io::gzip_decompressor());
      //cerr << ".gz file\n";
    }
    fistr.push(io::file_source(filename_index));
    fistr.set_auto_close(true);
    list<string> items;
    ullong num_line = 0;
    while (true) {
      string line;
      if (!getline(fistr, line)) {
	break;
      }
      //cout << num_line << " " << line << endl;
      size_t slot = num_line & 3;
      num_line ++;
      if (slot == 0) { // title
	size_t pos = line.rfind(':');
	if (pos != string::npos) {
	  string wellcode = line.substr(pos + 1, well_barcode_size);
	  auto it = well_indexes.find(wellcode);
	  if (it == well_indexes.end()) {
	    //cerr << (well_indexes.size() + 1) << " " << wellcode << " new well index ";
	    if (well_indexes.size() > (well_index_capacity * 4)) {
	      well_indexes = reduce_cache_size(well_indexes, well_index_capacity);
	    }
	    //cout << wellcode << " new well index\n";
	    well_indexes[wellcode] = 1;
	    //cout << well_indexes.size() << endl;
	  } else {
	    //cout << wellcode << " count up\n";
	    it->second ++;
	  }
	}
      } else if (slot == 1) {
	const string& code = line.substr(0, cell_barcode_size);
	//cout << code << " " << cell_barcode_size << " " << line << endl;
	auto it = cell_indexes.find(code);
	if (it == cell_indexes.end()) {
	  //cout << (cell_indexes.size() + 1) << " " << code << " new cell index\n";
	  if (cell_indexes.size() > (cell_index_capacity * 4) ) {
	    //cerr << cell_indexes.size() << " ? " << cell_index_capacity << " ? " << (cell_indexes.size() > (cell_index_capacity * 4)) << endl;
	    cell_indexes = reduce_cache_size(cell_indexes, cell_index_capacity);
	  }
	  cell_indexes[code] = 1;
	} else {
	  it->second++;
	}
      } else if (slot == 3) {
	if (lap > 0 && ((num_line >> 2) % lap) == 0) {
	  cerr << "#Well index : " << well_indexes.size() << " in " << (num_line / 4) << " reads\n";
	  display_frequency_stat(well_indexes, 8, cerr);
	  cerr << "#Cell index : " << cell_indexes.size() << "\n";
	  display_frequency_stat(cell_indexes, 10, cerr);
	  //cerr << "EXIT\n";
	}
      }
    }

    io::close(fistr);
    //fistr.close();
    if (verbose) {
      display_frequency_stat(well_indexes, 20, cerr);
      display_frequency_stat(cell_indexes, 20, cerr);
    }
    well_indexes = reduce_cache_size(well_indexes, well_index_capacity);
    cell_indexes = reduce_cache_size(cell_indexes, cell_index_capacity);
    return make_pair(well_indexes, cell_indexes);
  }
  vector<string> obtain_reduced_indexes(const map<string,ullong>& indexes, int num) {
    vector<string> seqs;
    for (auto it = indexes.begin(); it != indexes.end(); it++) {
      if (it->second > num) {
	seqs.push_back(it->first);
      }
    }
    return seqs;
  }

  string decode_index(int length, ullong code) {//, int offset=0) {
    char* buf = new char[length + 1];
    //buf[length] = buf[length + 1] = '\0';
    int index = 0;
    for (int i = 0; i < length; i++) {
      ullong c = (code >> ((length - 1 - i) << 1)) & 0x03;
      char base = 'A';
      if (c == 1) {
	base = 'C';
      } else if (c == 2) {
	base = 'G';
      } else if (c == 3) {
	base = 'T';
      }
      buf[index++] = base;
      // if (offset > 0 && offset == i) {
      // 	  buf[index++] = '_';
      // }
    }
    buf[index] = '\0';
    string name(buf);
    delete[] buf;
    return name;
  }

#define HAVE_X64INTRIN 1
#ifdef HAVE_X64INTRIN
#include <x86intrin.h>

  inline uint64_t popcount(uint64_t n) {
    uint64_t res;
    __asm__( "popcntq %1, %0" : "=r"(res) : "r"(n));
    return res;
  }
#else
#error cannot use popcount
#endif
  void generate_renamed_fastq(const string& filename_index,
			      int well_code_length, const vector<ullong>& well_codes,
			      int cell_code_length, const vector<ullong>& cell_codes,
			      const string& filename_read, int mismatches, boost::iostreams::filtering_ostream& ostr) throw (exception) {

    namespace io = boost::iostreams;
    io::filtering_istream fistr_index;// io::filtering_istream();
    if (filename_index.rfind(".gz") == filename_index.size() - 3) {
      fistr_index.push(io::gzip_decompressor());
    }
    fistr_index.push(io::file_source(filename_index));
    fistr_index.set_auto_close(true);
    io::filtering_istream fistr_read;// io::filtering_istream();
    if (filename_read.rfind(".gz") == filename_index.size() - 3) {
      fistr_read.push(io::gzip_decompressor());
    }
    fistr_read.push(io::file_source(filename_read));
    fistr_read.set_auto_close(true);

    size_t num_lines = 0;
    bool resolved = false;
    //map<ullong,vector<pair<string,string>> seqs;
    //map<ullong>,
    string seq;
    ullong code;
    map<ullong,ullong> counts;
    string umi;
    ullong wellcode = 0;
    ullong cellcode = 0;
    //pair<ullong,ullong> well_code_wm; // with mask
    //pair<ullong,ullong> cell_code_wm; // with mask
    //string qual;
    while (true) {
      string li, lr;
      if (!getline(fistr_index, li) || !getline(fistr_read, lr)) {
	break;
      }
      //cout << num_lines << " " << li << endl;
      size_t slot = num_lines & 3;
      if (slot == 0) {
	wellcode = cellcode = 0;
	size_t pos = li.rfind(':');
	if (pos == string::npos || li.size() - pos < well_code_length) {
	  resolved = false;
	} else {
	  pair<ullong,ullong> well_code_wm = encode_sequence(li.substr(pos + 1, well_code_length));
	  int num_N = popcount(well_code_wm.second) >> 1;
	  //cout << li.substr(pos + 1, well_code_length) << " : " << well_code_wm.second << " => " << num_N << endl;
	  if (num_N > mismatches) {
	    resolved = false;
	  } else {
	    ullong wc = well_code_wm.first;
	    ullong mask = 0xffffffffffffffff ^ well_code_wm.second;
	    for (auto it = well_codes.begin(); it != well_codes.end(); it++) {
	      ullong c = *it;
	      ullong a = (c ^ wc) & mask;
	      ullong mm = (a & 0x5555555555555555) | ((a & 0xaaaaaaaaaaaaaaaa) >> 1);
	      int num_mm = popcount(mm) + num_N;
	      //cout << li.substr(pos + 1, well_code_length) << " " << hex << *it << " " << num_mm << endl;
	      if (num_mm <= mismatches) {
		resolved = true;
		wellcode = c;//code = c << (cell_code_length << 1);
		break;
	      }
	    }
	  }
	}
      } else if (slot == 3) {
	if (cellcode != 0) {
	  //if (resolved) {
	  ostr << lr << "\n";
	}
	// cell code
      } else if (slot == 1) {
	//cout << resolved << " " << li.size() << " " << cell_code_length << endl;
	if (resolved && li.size() >= cell_code_length) {
	  pair<ullong,ullong> cell_code_wm = encode_sequence(li.substr(0, cell_code_length));
	  int num_N = popcount(cell_code_wm.second) >> 1;
	  //cout << num_N << " " << mismatches << endl;
	  if (num_N <= mismatches) {
	    ullong cc = cell_code_wm.first;
	    ullong mask = 0xffffffffffffffff ^ cell_code_wm.second;
	    umi = li.substr(cell_code_length, li.size() - cell_code_length);
	    resolved = false;
	    //cout << hex << cc << " " << mask << " " << cell_codes.size() << endl;
	    for (auto it = cell_codes.begin(); it != cell_codes.end(); it++) {
	      ullong c = *it;
	      ullong a = (c ^ cc) & mask;
	      ullong mm = (a & 0x5555555555555555) | ((a & 0xaaaaaaaaaaaaaaaa) >> 1);
	      int num_mm = popcount(mm) + num_N;
	      if (num_mm <= mismatches) {
		//cout << "OK " << num_mm << " " << decode_index(cell_code_length, c) << " " << decode_index(cell_code_length, cc) << ":" << li.substr(0, 16) << " " << umi << endl;
		//		cout << "OK\n";
		//code |= c;
		//cout << hex << code << endl;
		cellcode = c;
		resolved = true;
		break;
	      }
	    }
	  }
	  if (cellcode != 0) {//resolved) {
	    ullong code = (ullong)((ullong)wellcode << (ullong)(cell_code_length << (ullong)1)) | (ullong)cellcode;
	    auto it = counts.find(code);
	    int n;
	    if (it == counts.end()) {
	      n = 1;
	      counts.insert(make_pair(code, 1));
	    } else {
	      n = (it->second++);
	    }
	    //cout << hex << code << "=>" << decode_index(well_code_length + cell_code_length, code) << ", UMI:" << umi << dec << endl;
	    //cout << "@" << decode_index(well_code_length, wellcode) << "_" << decode_index(cell_code_length, cellcode) << ":" << n << ":" << umi << endl;
	    //cout << "@" << decode_index(8, code >> 32) << "_" << decode_index(16, cell_code_length & 0xffffffff) << ":" << n << ":" << umi << endl;
	    //cout << "@" << decode_index(cell_code_length + well_code_length, code, well_code_length) << ":" << n << ":" << umi << "\n" << lr << "\n+\n";
	    //ostr << "@" << decode_index(cell_code_length + well_code_length, code) << ":" << n << ":" << umi << "\n" << lr << "\n+\n";
	    ostr << "@" << decode_index(well_code_length, wellcode) << "_" << decode_index(cell_code_length, cellcode) << ":" << n << ":" << umi << "\n" << lr << "\n+\n";
	  } else {
	    code = 0;
	  }
	}
      }
      num_lines++;
    }

    io::close(ostr);
    //ostr.sync();
    fistr_index.sync();
    fistr_read.sync();
  }
}

int main(int argc, char** argv) {
  namespace io = boost::iostreams;
  namespace po = boost::program_options;
  po::options_description opt("options");
  //string filename_sam;
  string filename_index;
  string filename_out;
  string filename_read;
  map<string,int> wellcode;
  //map<string,int> wellcode_strict;
  ullong range_max = 0;
  ullong range_min = 0;
  int lap;
  //int shift = 60;
  int well_barcode_size = 8;
  int cell_barcode_size = 16;
  bool verbose = false;
  float threshold_well_composition = 0.05f;
  int minimum_reads_per_cell = 500;
  int mismatches = 1;
  opt.add_options()
    ("output,o", po::value<string>(&filename_out)->default_value(""), "output file")
    ("R1,1", po::value<string>(&filename_index), "fastq.gz")
    ("R2,2", po::value<string>(&filename_read), "fastq.gz")
    ("range_max", po::value<ullong>(&range_max)->default_value(0), "Index range")
    ("range_min", po::value<ullong>(&range_min)->default_value(0), "Index range")
    ("barcode_size,b", po::value<int>(&cell_barcode_size)->default_value(16), "Barcode length (16)")
    ("lap,L", po::value<int>(&lap)->default_value(0), "log period")
    ("minimum_reads,m", po::value<int>(&minimum_reads_per_cell)->default_value(500), "Minimum reads per cell (default 500)")
    ("mismatches,k", po::value<int>(&mismatches)->default_value(1), "Mismatches (default 1)")
    ("verbose,V", "verbose mode")
  ("help,h", "display help");
  //("samfile,s", po::value<string>(&filename_sam), "sam file")

  ullong well_index_capacity = 100;
  ullong cell_index_capacity = 0x1fff;

  // if (true) {
  //   for (int i = 0; i < 100; i++) {
  //     cout << i << " " << popcount(i) << endl;
  //   }
  //   exit(0);
  // }

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, opt), vm);
  } catch (exception& e) {
    cerr << opt << endl;
    cerr << e.what() << endl;
    return -1;
  }
  po::notify(vm);
  verbose = vm.count("verbose");
  if (verbose && lap < 1000) {
    lap = 1000;
  }
  lap *= 1000;
  // {
  //   map<ullong,ullong> test;
  //   vector<string> a = {"CAAAAAAA", "AAAAAAAT", "GGGGGGGG", "NNTTTTTT", "NNNNNNNN"};
  //     //char*[] a = {
  //   for (auto b : a) {
  //     auto test = encode_sequence(b);
  //     cout << hex << b << " " << test.first << " " << test.second << endl;
  //   }
  //   exit(0);
  // }

  //cout << opt << endl;
  //cout << vm << endl;
  if (vm.count("help")) {
    cout << opt << endl;
  } else {
    try {
      pair<map<string,ullong>,map<string,ullong>> index_sets = load_index_sets(filename_index, well_barcode_size, cell_barcode_size, well_index_capacity, cell_index_capacity, lap, verbose);
      size_t num_total = 0;
      for (auto it = index_sets.first.begin(); it != index_sets.first.end(); it++) {
	num_total += it->second;
      }

      vector<string> well_indexes = obtain_reduced_indexes(index_sets.first, int(num_total * threshold_well_composition));
      vector<string> cell_indexes = obtain_reduced_indexes(index_sets.second, minimum_reads_per_cell);
      //cout << well_indexes.size() << " " << cell_indexes.size() << endl;
      for (int i = 0; i < 1; i++) {
	cout << well_indexes[i] << " " << cell_indexes[i] << endl;
      }

      //cout << "WELL " << well_indexes.size() << "\n";
      vector<ullong> well_codes = encode_indexes(well_indexes, well_barcode_size);
      //cout << "CELL " << cell_indexes.size() << "\n";
      vector<ullong> cell_codes = encode_indexes(cell_indexes, cell_barcode_size);

      // cout << well_codes.size() << endl;
      // cout << cell_codes.size() << endl;
      // cout << minimum_reads_per_cell << endl;
      // exit(0);

      io::filtering_ostream fostr;
      fostr.set_auto_close(true);
      ostream* ost_ = &cout;
      if (filename_out != "") {
	ost_ = new ofstream(filename_out.c_str());
	if (filename_out.rfind(".gz") == filename_out.size() - 3) {
	  fostr.push(io::gzip_compressor());
	}
      }
      fostr.push(*ost_);//filename_out);
      generate_renamed_fastq(filename_index, well_barcode_size, well_codes, cell_barcode_size, cell_codes,
			     filename_read,
			     mismatches, fostr);

      if (filename_out != "") {
	reinterpret_cast<ofstream*>(ost_)->close();
	delete ost_;
      }

      // if (filename_out.rfind(".gz") == filename_out.size() - 3) {
      // 	fostr.push(io::gzip_compressor());
      // }

//ostream* ost = &cout;
//      ostream& ost = cout;
//       io::filtering_ostream fostr;
// //      ofstream* fstr = NULL;
//       if (filename_out.size() > 0) {
// 	fostr.push(filename_out);
// 	//fstr = ofstream(filename_out);
// 	if (filename_out.rfind(".gz") == filename_out.size() - 3) {
// 	  fostr.push(io::gzip_compressor());
// 	}
// 	//fstr = ofstream(filename_out);
// 	ost = &fostr;
//       }
//      fostr.push(ost);
      //generate_renamed_fastq(filename_index, filename_read, well_codes, cell_codes, mismatches, fostr);

      // vector<string> cell_index = obtain_reduced_indexes(index_sets.second, minimum_reads_per_cell);
      // vector<ullong> well_codes = encode_indexes(well_index, well_barcode_size);
      // vector<ullong> cell_codes = encode_indexes(cell_index, barcode_size);

      //process_fastq_files(filename_index, filename_read, filename_out

      //pair<vector<ullong>,vector<ullong>> available_index_codes = obtain_reduced_index_sets(filename_index, barcode_size, well_index_capacity, cell_index_capacity, verbose);

      //vector<ullong> available_well_index;
    } catch (exception& e) {
      cerr << e.what() << endl;
    }
  }
  return 0;
}