// This file is part of snark, a generic and flexible library for robotics research
// Copyright (c) 2011 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 <boost/geometry.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/foreach.hpp>
#include <Eigen/Core>
#include "region_properties.h"
#include <comma/base/exception.h>

#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS( cs::cartesian )

namespace snark{ namespace imaging {

/// compute the area of a polygon and its convex hull
/// @param points polygon points
/// @param area area of the polygon
/// @param convexArea area of the convex hull
void compute_area( const std::vector< cv::Point >& points, double& area, double& convexArea )
{
    boost::geometry::model::polygon< boost::tuple< double, double > > polygon;
    for( unsigned int i = 0; i < points.size(); i++ )
    {
        boost::geometry::append( polygon, boost::make_tuple( points[i].x, points[i].y ) );
    }
    boost::geometry::append( polygon, boost::make_tuple( points[0].x, points[0].y ) ); // close polygon

    area = boost::geometry::area( polygon );

    boost::geometry::model::polygon< boost::tuple<double, double> > hull;
    boost::geometry::convex_hull( polygon, hull );

    convexArea = boost::geometry::area( hull );
}

/// constructor
/// @param image input image, is considered as a binary image ( all non-zero pixels are 1 )
region_properties::region_properties ( const cv::Mat& image, double minArea ):
    m_minArea( minArea )
{
    cv::Mat binary;
    if( image.channels() == 3 )
    {
        cv::cvtColor( image, binary, CV_RGB2GRAY );
    }
    else if( image.channels() == 1 )
    {
        binary = image;
    }
    else
    {
        COMMA_THROW( comma::exception, "incorrect number of channels, should be 1 or 3, not " << image.channels() );
    }
//     cv::Mat closed;
//     cv::morphologyEx( binary, closed, cv::MORPH_CLOSE, cv::Mat::ones( 3, 3, CV_8U) );
    std::vector< std::vector<cv::Point> > contours;
    cv::findContours( binary, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE );

    for( unsigned int i = 0; i < contours.size(); i++ )
    {
        binary = cv::Scalar(0);
        cv::drawContours( binary, contours, i, cv::Scalar(0xFF), CV_FILLED );
        cv::Rect rect = cv::boundingRect( cv::Mat( contours[i]) );
        cv::Moments moments = cv::moments( binary( rect ), true );
        double x = moments.m10/moments.m00;
        double y = moments.m01/moments.m00;
        double area = moments.m00; // cv::countNonZero( binary( rect ) )
        if( area > m_minArea )
        {
            // see wikipedia, image moments
            double diff = moments.nu20 - moments.nu02;
            double a = 0.5 * ( moments.nu20 + moments.nu02 );
            double b = 0.5 * std::sqrt( 4 * moments.nu11 * moments.nu11 + diff * diff );
            double minEigenValue = a - b;
            double maxEigenValue = a + b;
    //         std::cerr << " min " << minEigenValue << " max " << maxEigenValue << std::endl;
            double theta = 0.5 * std::atan2( 2 * moments.nu11, diff );
            double eccentricity = 1;
            if( std::fabs( maxEigenValue ) > 1e-15 )
            {
                eccentricity = std::sqrt( 1 - minEigenValue / maxEigenValue );
            }

            double polygonArea;
            double convexArea;
            compute_area( contours[i], polygonArea, convexArea );
    //         std::cerr << " area " << area << " polygon " << polygonArea << " convex " << convexArea << std::endl;

            blob blob;
            blob.majorAxis = 2 * std::sqrt( moments.m00 * maxEigenValue );
            blob.minorAxis = 2 * std::sqrt( moments.m00 * minEigenValue );
            blob.orientation = theta;
            blob.centroid = cv::Point( x + rect.x, y + rect.y );
            blob.area = area;
            blob.eccentricity = eccentricity;
            blob.solidity = 0;
            if( std::fabs( convexArea ) > 1e-15 )
            {
                blob.solidity = polygonArea / convexArea;
            }
            m_blobs.push_back( blob );
        }
    }
}

/// draw debug information on the image
void region_properties::show( cv::Mat& image, bool text )
{
    for( unsigned int i = 0; i < m_blobs.size(); i++ )
    {
        cv::Point centroid = m_blobs[i].centroid;
        cv::circle( image, centroid, 3, cv::Scalar( 0, 0, 255 ), 2 );
        std::stringstream s;
        s << i;
        if( text )
        {
            cv::putText( image, s.str(), centroid + cv::Point( 2, 2 ), cv::FONT_HERSHEY_PLAIN ,1, cv::Scalar( 0, 0, 255 ) );
        }
        cv::ellipse( image, centroid, cv::Size( m_blobs[i].majorAxis, m_blobs[i].minorAxis ), m_blobs[i].orientation * 180.0 / M_PI, 0, 360, cv::Scalar( 0, 255, 0 ) );
//         std::cerr << i << ": area " << m_blobs[i].area << " eccentricity " << m_blobs[i].eccentricity << " solidity " << m_blobs[i].solidity << std::endl;
    }
}

} }


/*
Copyright 2009-2019 Nicolas Colombe

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

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

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

#include <boost/polygon/polygon.hpp>

namespace boost
{
  namespace polygon
  {

    template <typename Real>
    struct geometry_concept<eXl::Polygon<Real> >{ typedef polygon_with_holes_concept type; };

    template <typename Real>
    struct geometry_concept<eXl::Vector<eXl::Vector2<Real> > >{ typedef polygon_concept type; };

    template <typename Real>
    struct polygon_90_traits < eXl::Polygon<Real>, boost::polygon::gtl_no > {};

    template <typename Real>
    struct polygon_90_traits < eXl::Vector<eXl::Vector2<Real> >, boost::polygon::gtl_no > {};

    template <typename Real>
    struct polygon_traits_general<eXl::Polygon<Real> > {
      typedef Real coordinate_type;
      typedef typename eXl::Vector<eXl::Vector2<Real> >::const_iterator iterator_type;
      typedef eXl::Vector2<Real> point_type;

      static inline iterator_type begin_points(eXl::Polygon<Real> const& t) {
          return t.Border().begin();
      }
      static inline iterator_type end_points(eXl::Polygon<Real> const& t) {
          return t.Border().end();
      }

      // Get the number of sides of the polygon
      static inline std::size_t size(eXl::Polygon<Real> const& t) {
        return t.Border().size();
      }

      // Get the winding direction of the polygon
      static inline winding_direction winding(eXl::Polygon<Real> const& t) {
        return unknown_winding;
      }
    };

    template <typename Real>
    struct polygon_mutable_traits<eXl::Polygon<Real> > {

      template <typename iT>
      static inline eXl::Polygon<Real>& set_points(eXl::Polygon<Real> & t,
                                         iT input_begin, iT input_end) {

        eXl::Vector<eXl::Vector2<Real> > tempStor;
        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterBegin(input_begin,input_end);
        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterEnd(input_end,input_end);

        //tempStor.assign(input_begin,input_end);
        for (; input_begin != input_end; ++input_begin)
        {
          eXl::Vector2<Real> temp(boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::HORIZONTAL),
                                  boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::VERTICAL)) ;
          tempStor.push_back(temp);
        }

        t = eXl::Polygon<Real>();
        t.m_Ext.swap(tempStor);
        t.ForceClockwise();
        t.UpdateAABB();
        return t;
      }

    };

    template <typename Real>
    struct polygon_traits_general<eXl::Vector<eXl::Vector2<Real> > > {
      typedef Real coordinate_type;
      typedef typename eXl::Vector<eXl::Vector2<Real> >::const_iterator iterator_type;
      typedef eXl::Vector2<Real> point_type;

      static inline iterator_type begin_points(typename eXl::Polygon<Real>::PtList const& t) {
          return t.begin();
      }
      static inline iterator_type end_points(typename eXl::Polygon<Real>::PtList const& t) {
          return t.end();
      }

      // Get the number of sides of the polygon
      static inline std::size_t size(typename eXl::Polygon<Real>::PtList const& t) {
        return t.size();
      }

      // Get the winding direction of the polygon
      static inline winding_direction winding(typename eXl::Polygon<Real>::PtList const& t) {
        return unknown_winding;
      }
    };

    template <typename Real>
    struct polygon_mutable_traits<eXl::Vector<eXl::Vector2<Real> > > {

      template <typename iT>
      static inline typename eXl::Polygon<Real>::PtList& set_points(typename eXl::Polygon<Real>::PtList & t,
                                         iT input_begin, iT input_end) {

        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterBegin(input_begin,input_end);
        //iterator_compact_to_points<iT,eXl::Vector2<Real> > iterEnd(input_end,input_end);
        //t.assign(input_begin,input_end);
        t.clear();
        for (; input_begin != input_end; ++input_begin)
        {
          eXl::Vector2<Real> temp(boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::HORIZONTAL),
                                  boost::polygon::point_traits<typename iT::value_type>::get(*input_begin, boost::polygon::VERTICAL)) ;
          t.push_back(temp);
        }
        return t;
      }

    };

    template <typename Real,typename enable>
    struct polygon_with_holes_traits<eXl::Polygon<Real>,enable> {
         typedef typename eXl::Polygon<Real>::PtLists::const_iterator iterator_holes_type;
         typedef typename eXl::Polygon<Real>::PtList hole_type;
         static inline iterator_holes_type begin_holes(const eXl::Polygon<Real>& t) {
              return t.Holes().begin();
         }
         static inline iterator_holes_type end_holes(const eXl::Polygon<Real>& t) {
              return t.Holes().end();
         }
         static inline Real size_holes(const eXl::Polygon<Real>& t) {
              return t.Holes().size();
         }
    };

    template <typename Real, typename enable>
    struct polygon_with_holes_mutable_traits<eXl::Polygon<Real>,enable> {
         template <typename iT>
         static inline eXl::Polygon<Real>& set_holes(eXl::Polygon<Real>& t, iT inputBegin, iT inputEnd) {

              for(;inputBegin != inputEnd;++inputBegin)
              {
                typename eXl::Polygon<Real>::PtList hole;
                boost::polygon::assign(hole,*inputBegin);
                t.HolesRW().push_back(typename eXl::Polygon<Real>::PtList());
                t.HolesRW().back().swap(hole);
              }
              t.ForceCClockwiseHoles();
              return t;
         }
    };

    template <typename T>
    struct is_polygon_set_type<eXl::Vector<T> > {
      typedef typename gtl_or<
        typename is_polygonal_concept<typename geometry_concept<eXl::Vector<T> >::type>::type,
        typename is_polygonal_concept<typename geometry_concept<typename eXl::Vector<T>::value_type>::type>::type>::type type;
    };

    template <typename T>
    struct is_mutable_polygon_set_type<eXl::Vector<T> > {
      typedef typename gtl_or<
        typename gtl_same_type<polygon_set_concept, typename geometry_concept<eXl::Vector<T> >::type>::type,
        typename is_polygonal_concept<typename geometry_concept<typename eXl::Vector<T>::value_type>::type>::type>::type type;
    };

    template <typename T>
    struct polygon_set_mutable_traits<eXl::Vector<T> > {
      template <typename input_iterator_type>
      static inline void set(eXl::Vector<T>& polygon_set, input_iterator_type input_begin, input_iterator_type input_end) {
        polygon_set.clear();
        size_t num_ele = std::distance(input_begin, input_end);
        polygon_set.reserve(num_ele);
        polygon_set_data<typename polygon_set_traits<std::list<T> >::coordinate_type> ps;
        ps.reserve(num_ele);
        ps.insert(input_begin, input_end);
        ps.get(polygon_set);
      }
    };

  }
}

namespace eXl
{
  class Serializer;

  template <typename PolygonType>
  Err Stream_T(PolygonType& iPoly, Serializer iStreamer);

  template <typename Real>
  struct PreciseVector
  {
    typedef eXl::Vector2<typename eXl::PreciseType<Real>::type > type;
  };

  template <typename Real>
  inline typename PreciseVector<Real>::type ToPrecise(Vector2<Real> const& iVec)
  {
    return typename PreciseVector<Real>::type(Math<Real>::ToPrecise(iVec.X()), Math<Real>::ToPrecise(iVec.Y()));
  }

  template <typename Real>
  inline bool VectorNotNull(Vector2<Real> const& iVec)
  {
    return iVec.Length() > Math<Real>::ZERO_TOLERANCE;
  }

  template <>
  inline bool VectorNotNull<int>(Vector2i const& iVec)
  {
    return iVec != Vector2i::ZERO;
  }

  template <typename Real>
  Polygon<Real>::Polygon(){}

  template <typename Real>
  Polygon<Real>::~Polygon(){}

  template <typename Real>
  void Polygon<Real>::Translate(Vector2<Real> const& iTrans)
  {
    for (unsigned int i = 0; i < m_Ext.size(); ++i)
    {
      m_Ext[i] += iTrans;
    }

    for (unsigned int i = 0; i < m_Holes.size(); ++i)
    {
      for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
      {
        m_Holes[i][j] += iTrans;
      }
    }
    //UpdateAABB();
    m_AABB.m_Data[0] += iTrans;
    m_AABB.m_Data[1] += iTrans;
  }

  template <typename Real>
  void Polygon<Real>::Scale(Real iNum, Real iDenom)
  {
    //Polygone valide si edge confondues ??
    for (unsigned int i = 0; i < m_Ext.size(); ++i)
    {
      m_Ext[i] = (m_Ext[i] * iNum) / iDenom;
    }

    for (unsigned int i = 0; i < m_Holes.size(); ++i)
    {
      for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
      {
        m_Holes[i][j] = (m_Holes[i][j] * iNum) / iDenom;
      }
    }
    UpdateAABB();
  }

  template <typename Real>
  void Polygon<Real>::Rotate(typename eXl::PreciseType<Real>::type iAngle)
  {
    if(Math<typename eXl::PreciseType<Real>::type>::Abs(iAngle) > Math<typename eXl::PreciseType<Real>::type>::ZERO_TOLERANCE)
    {
      auto xAxis = Vector2<typename eXl::PreciseType<Real>::type>(Math<typename eXl::PreciseType<Real>::type>::Cos(iAngle), Math<typename eXl::PreciseType<Real>::type>::Sin(iAngle));
      auto yAxis = Vector2<typename eXl::PreciseType<Real>::type>(-xAxis.Y(), xAxis.X());

      for (unsigned int i = 0; i < m_Ext.size(); ++i)
      {
        auto vec = xAxis * m_Ext[i].X() + yAxis * m_Ext[i].Y();
        m_Ext[i] = Vector2<Real>(Math<Real>::Round(vec.X()), Math<Real>::Round(vec.Y()));
      }

      for (unsigned int i = 0; i < m_Holes.size(); ++i)
      {
        for (unsigned int j = 0; j < m_Holes[i].size(); ++j)
        {
          auto vec = xAxis * m_Holes[i][j].X() + yAxis * m_Holes[i][j].Y();
          m_Holes[i][j] = Vector2<Real>(Math<Real>::Round(vec.X()), Math<Real>::Round(vec.Y()));
        }
      }

      UpdateAABB();
    }
  }

  template <typename Real>
  Polygon<Real>::Polygon(Vector<Vector2<Real> > const& iPoints)
  {
    if(iPoints.size() > 2)
    {
      m_Ext = iPoints;
      if(m_Ext.front() != m_Ext.back())
        m_Ext.push_back(m_Ext.front());
      ForceClockwise();
    }
    UpdateAABB();
  }

  template <typename Real>
  void Polygon<Real>::UpdateAABB()
  {
    boost::polygon::extents(m_AABB, *this);
  }

  template <typename Real>
  Polygon<Real>::Polygon(AABB2D<Real> const& iBox)
  {
    m_Ext.clear();

    m_Ext.push_back(iBox.m_Data[0]);
    m_Ext.push_back(Vector2<Real>(iBox.m_Data[0].X(), iBox.m_Data[1].Y()));
    m_Ext.push_back(iBox.m_Data[1]);
    m_Ext.push_back(Vector2<Real>(iBox.m_Data[1].X(), iBox.m_Data[0].Y()));
    m_Ext.push_back(iBox.m_Data[0]);

    ForceClockwise();

    m_AABB = iBox;
  }

  template <typename Real>
  void Polygon<Real>::Swap(Polygon& iOther)
  {
    AABB2D<Real> temp = iOther.m_AABB;
    iOther.m_Ext.swap(m_Ext);
    iOther.m_Holes.swap(m_Holes);
    iOther.m_AABB = m_AABB;
    m_AABB = temp;
  }

  template <typename Real>
  Real Polygon<Real>::Perimeter()const
  {
    return boost::polygon::perimeter(*this);
  }

  template <typename Real>
  Real Polygon<Real>::Area() const
  {
    return boost::polygon::area(*this);
  }

  template <typename Real>
  void Polygon<Real>::ForceClockwise()
  {
    if(boost::polygon::winding(*this) != boost::polygon::CLOCKWISE)
    {
      std::reverse(m_Ext.begin(),m_Ext.end());
    }
  }

  template <typename Real>
  void Polygon<Real>::ForceCClockwiseHoles()
  {
    for(unsigned int i = 0;i<m_Holes.size();++i)
    {
      if(boost::polygon::winding(m_Holes[i]) != boost::polygon::COUNTERCLOCKWISE)
      {
        std::reverse(m_Holes[i].begin(),m_Holes[i].end());
      }
    }
  }

  template <typename Real>
  void Polygon<Real>::InternalSwap(Polygon &iOther)const
  {
    m_Ext.swap(iOther.m_Ext);
    m_Holes.swap(iOther.m_Holes);
  }

  template <typename Real>
  void Polygon<Real>::Clear()
  {
    m_Ext.clear();
    m_Holes.clear();
  }

  template <typename Real>
  bool Polygon<Real>::Empty() const
  {
    return m_Ext.empty();
  }

  template <typename Real>
  bool Polygon<Real>::ContainsPoint(Vector2<Real> const& iPoint) const
  {
    boost::polygon::point_data<Real> point(iPoint.X(), iPoint.Y());
    return boost::polygon::contains(*this, point);
  }

  //template <typename Real>
  //void Polygon<Real>::GetBoxes(Vector<AABB2D<Real> >& oBoxes)const
  //{
  //  Vector<Polygon> meSet;
  //  meSet.push_back(Polygon());
  //  InternalSwap(meSet[0]);
  //
  //  boost::polygon::get_rectangles(oBoxes,meSet);
  //
  //  InternalSwap(meSet[0]);
  //}

  template <typename Real>
  void Polygon<Real>::Merge(Vector<Polygon>& ioPoly)
  {
    Vector<Polygon> res;
    if(ioPoly.size() > 0)
    {
      res.push_back(ioPoly.front());
      for(unsigned int i = 1 ; i<ioPoly.size();++i)
      {
        boost::polygon::operators::operator|=(res,ioPoly[i]);
      }
    }
    ioPoly.swap(res);
  }

  template <typename Real>
  void Polygon<Real>::Union(Polygon const& iOther, Polygon& oPoly)const
  {
    oPoly.Clear();

    Vector<Polygon> meSet;
    Vector<Polygon> otherSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(Polygon());
    iOther.InternalSwap(otherSet[0]);

    Vector<Polygon> result;
    boost::polygon::assign(result,boost::polygon::operators::operator+(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);
    if (result.size() == 1)
    {
      oPoly.Swap(result[0]);
    }
  }

  template <typename Real>
  void Polygon<Real>::Difference(Polygon const& iOther, Vector<Polygon>& oPoly)const
  {
    oPoly.clear();

    Vector<Polygon> meSet;
    Vector<Polygon> otherSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(Polygon());
    iOther.InternalSwap(otherSet[0]);

    boost::polygon::assign(oPoly,boost::polygon::operators::operator-(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);
  }

  template <typename Real>
  void Polygon<Real>::Intersection(Polygon const& iOther, Vector<Polygon>& oPoly)const
  {
    Vector<Polygon> meSet;
    Vector<Polygon> otherSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    otherSet.push_back(Polygon());
    iOther.InternalSwap(otherSet[0]);

    boost::polygon::assign(oPoly,boost::polygon::operators::operator&(meSet,otherSet));

    InternalSwap(meSet[0]);
    iOther.InternalSwap(otherSet[0]);

  }

  template <typename Real>
  Vector<Polygon<Real> > Polygon<Real>::GetTrapezoids() const
  {
    Vector< Polygon<Real> > result;
    Vector<Polygon> meSet;

    meSet.push_back(Polygon());
    InternalSwap(meSet[0]);

    boost::polygon::get_trapezoids(result, meSet, boost::polygon::VERTICAL);

    InternalSwap(meSet[0]);

    return result;
  }

  template <typename Real>
  void Polygon<Real>::CutConvex(Vector2<Real> const& iOrig, Vector2<Real> const& iDir, Polygon& oLeftPoly, Polygon& oRightPoly) const
  {
    oLeftPoly.Clear();
    oRightPoly.Clear();

    auto lowPt = ToPrecise(m_AABB.m_Data[0]);
    auto highPt = ToPrecise(m_AABB.m_Data[1]);

    typename PreciseVector<Real>::type boxSeg[5] =
    {lowPt,  typename PreciseVector<Real>::type(lowPt.X(), highPt.Y()),
     highPt, typename PreciseVector<Real>::type(highPt.X(), lowPt.Y()),
     lowPt
    };

    unsigned int curPoint = 0;
    Vector2<Real> cutPoints[2];

    auto dOrig = ToPrecise(iOrig);
    auto dDir  = ToPrecise(iDir);

    for (unsigned int i = 0; i < 4 && curPoint < 2; ++i)
    {
      typename PreciseVector<Real>::type res;
      unsigned int result = Segment<typename PreciseType<Real>::type>::Intersect(dOrig, dOrig + dDir, boxSeg[i], boxSeg[i + 1], res);
      unsigned int mask = Segment<typename PreciseType<Real>::type>::PointFound | Segment<typename PreciseType<Real>::type>::PointOnSegment2;
      if((result & mask) == mask)
      {
        Vector2<Real> potPoint(Math<Real>::Round(res.X()), Math<Real>::Round(res.Y()));
        if (curPoint == 0 || VectorNotNull(cutPoints[0] - potPoint))
        {
          //Snap to bbox seg if integer.
          cutPoints[curPoint] = potPoint;
          ++curPoint;
        }
      }
    }

    if (curPoint == 2)
    {
      Vector2<Real> cutVect = cutPoints[0] - cutPoints[1];
      if (VectorNotNull(cutVect))
      {
        int origPoint = cutVect.Dot(iDir) > 0 ? 0 : 1;

        typename Segment<Real>::SortByAngle sortMeth(cutPoints[origPoint]);
        std::set<Vector2<Real>, typename Segment<Real>::SortByAngle > sortedPoints(sortMeth);
        sortedPoints.insert(cutPoints[1 - origPoint]);
        sortedPoints.insert(m_AABB.m_Data[0]);
        sortedPoints.insert(m_AABB.m_Data[1]);
        sortedPoints.insert(Vector2<Real>(m_AABB.m_Data[0].X(), m_AABB.m_Data[1].Y()));
        sortedPoints.insert(Vector2<Real>(m_AABB.m_Data[1].X(), m_AABB.m_Data[0].Y()));

        Vector<Vector2<Real> > cutPoly;

        cutPoly.push_back(cutPoints[origPoint]);

        auto iter = sortedPoints.begin();
        while (*iter != cutPoints[1 - origPoint] && iter != sortedPoints.end())
        {
          cutPoly.push_back(*iter);
          ++iter;
        }
        //eXl_ASSERT(iter != sortedPoints.end(), "Error");
        //eXl_ASSERT(cutPoly.size() > 3, "Error");
        if (iter != sortedPoints.end() && cutPoly.size() > 0)
        {
          cutPoly.push_back(*iter);
          Vector<Polygon<Real> > oPoly1;
          Polygon<Real> leftPoly(cutPoly);
          Intersection(leftPoly, oPoly1);
          //eXl_ASSERT(oPoly1.size() == 0 || oPoly1.size() == 1, "Error");
          if (oPoly1.size() == 1)
          {
            Vector<Polygon<Real> > oPoly2;
            Difference(oPoly1[0], oPoly2);
            //eXl_ASSERT(oPoly2.size() == 0 || oPoly2.size() == 1, "Error");
            if (oPoly2.size() < 2)
            {
              if (oPoly2.size() == 1)
              {
                oRightPoly.Swap(oPoly2[0]);
              }
              oLeftPoly.Swap(oPoly1[0]);
            }
          }
          else if (oPoly1.size() == 0)
          {
            oRightPoly = *this;
          }
        }
      }
    }
  }

  template <typename Real>
  bool Polygon<Real>::IsConvex() const
  {
    if(!m_Holes.empty())
      return false;

    if(m_Ext.size() <= 2)
      return true;

    Vector2<Real> initPoint = m_Ext[0];
    Vector2<Real> lastPoint1 = initPoint;
    Vector2<Real> lastPoint2 = m_Ext[1];
    Vector2<Real> curPoint = m_Ext[2];

    Real initSign = Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint);
    initSign = initSign > 0 ? 1 : -1;
    lastPoint1 = lastPoint2;
    lastPoint2 = curPoint;
    //+1 to loop on the last segment.
    for (unsigned int i = 3; i < m_Ext.size() + 1; ++i)
    {
      if (m_Ext.size() == i)
      {
        curPoint = m_Ext[0];
        if(Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint) * initSign < 0)
          return false;
      }
      else
      {
        curPoint = m_Ext[i];
        if (curPoint == initPoint)
        {
          if(Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint) * initSign < 0)
            return false;
          break;
        }
        if(Segment<Real>::IsLeft(lastPoint1, lastPoint2, curPoint) * initSign < 0)
          return false;

      }
      lastPoint1 = lastPoint2;
      lastPoint2 = curPoint;
    }
    return true;
  }

  template <class Real>
  struct SortPoints
  {
    bool operator()(Vector2<Real> const& iPt1, Vector2<Real> const& iPt2)
    {
      if(iPt1.X() == iPt2.X())
        return iPt1.Y() < iPt2.Y();
      return iPt1.X() < iPt2.X();
    }
  };

  template <class Real>
  void Polygon<Real>::ConvexHull(Polygon<Real>& oHull) const
  {
    return ConvexHull(m_Ext, oHull);
  }

  template <class Real>
  void Polygon<Real>::ConvexHull(Vector<Vector2<Real> >const& iPoints, Polygon<Real>& oHull)
  {
    oHull.Clear();
    if(!iPoints.empty())
    {
      Vector<Vector2<Real> > sorted = iPoints;

      std::sort(sorted.begin(), sorted.end(), SortPoints<Real>());

      int n = sorted.size();
      int k = 0;
	    Vector<Vector2<Real> > H(2*n);

	    for (int i = 0; i < n; ++i)
      {
		    while (k >= 2 && Segment<Real>::IsLeft(H[k-2], H[k-1], sorted[i]) <= 0)
          k--;

		    H[k++] = sorted[i];
	    }

	    // Build upper hull
	    for (int i = n-2, t = k+1; i >= 0; i--)
      {
		    while (k >= t && Segment<Real>::IsLeft(H[k-2], H[k-1], sorted[i]) <= 0)
          k--;

		    H[k++] = sorted[i];
	    }

	    H.resize(k);

      oHull = Polygon<Real>(H);
    }
  }

  template <class Real>
  void Polygon<Real>::RemoveUselessPoints()
  {
    _RemoveUselessPoints(m_Ext);
    for(unsigned int i = 0; i<m_Holes.size(); ++i)
    {
      _RemoveUselessPoints(m_Holes[i]);
    }
  }

  template <class Real>
  void Polygon<Real>::_RemoveUselessPoints(Vector<Vector2<Real> >& ioPoints)
  {
    Vector2<Real> prevPt1 = ioPoints.back();
    Vector2<Real> prevPt2 = ioPoints.back();
    for (unsigned int i = 0; i<ioPoints.size(); ++i)
    {
      Vector2<Real> curPt =  ioPoints[i];
      if (curPt != prevPt1 && prevPt1 != prevPt2)
      {
        typename PreciseVector<Real>::type dir1 = ToPrecise<Real>(prevPt1 - prevPt2);
        typename PreciseVector<Real>::type dir2 = ToPrecise<Real>(curPt - prevPt2);
        typename PreciseType<Real>::type len1 = dir1.Normalize();
        typename PreciseType<Real>::type len2 = dir2.Normalize();
        if (dir1.Dot(dir2) > (1 - eXl::Math<typename PreciseType<Real>::type>::EPSILON) && len1 < len2)
        {
          ioPoints[i - 1] = curPt;
          ioPoints.erase(ioPoints.begin() + i);
          prevPt1 = curPt;
          --i;
          continue;
        }
      }
      prevPt2 = prevPt1;
      prevPt1 = curPt;
    }
  }

  template <typename Real>
  void Polygon<Real>::Shrink(Real iFactor,Vector<Polygon>& oOut)const
  {
    Vector<Polygon> meSet;
    meSet.push_back(*this);

    boost::polygon::shrink(meSet,iFactor);

    if(meSet.size()>0)
      meSet.swap(oOut);
    else
      oOut.clear();
  }

  template <typename Real>
  void Polygon<Real>::Bloat(Real iFactor,Polygon& oOut)const
  {
    Vector<Polygon> meSet;
    meSet.push_back(*this);

    boost::polygon::bloat(meSet,iFactor);

    if(meSet.size()>0)
      meSet[0].Swap(oOut);
    else
      oOut.Clear();
  }

  template <typename Real>
  void Polygon<Real>::RemoveTiny(Real iFactor,Vector<Polygon>& oOut,Vector<Polygon>& oRemoved)const
  {
    oOut.clear();
    oRemoved.clear();
    Vector<Polygon> meSet;
    meSet.push_back(*this);

    /*meSet = */boost::polygon::shrink(meSet,iFactor);

    /*meSet = */boost::polygon::bloat(meSet,iFactor);

    //meSet = boost::polygon::keep(meSet,0,ULLONG_MAX,iFactor,ULLONG_MAX,iFactor,ULLONG_MAX);

    if(meSet.empty())
    {
      oRemoved.push_back(*this);
    }
    else
    {

      Vector<Polygon> good;
      good.push_back(Polygon());
      InternalSwap(good.back());

      boost::polygon::assign(oRemoved,boost::polygon::operators::operator-(good,meSet));
      InternalSwap(good.back());

      //meSet.back().Swap(*this);
      oOut.swap(meSet);
    }
  }
}

#include <core/stream/serializer.hpp>

namespace eXl
{

  template <typename PolygonType>
  Err Stream_T(PolygonType& iPoly, Serializer iStreamer)
  {
    iStreamer.BeginStruct();
    iStreamer.PushKey("Border");
    iStreamer &= iPoly.Border();
    iStreamer.PopKey();
    iStreamer.PushKey("Holes");
    iStreamer &= iPoly.Holes();
    iStreamer.PopKey();
    iStreamer.EndStruct();

    RETURN_SUCCESS;
  }

  template <typename Real>
  Err Polygon<Real>::Stream(Streamer& iStreamer) const
  {
    Serializer serializer(iStreamer);
    return Stream_T(*this, serializer);
  }

  template <typename Real>
  Err Polygon<Real>::Unstream(Unstreamer& iStreamer)
  {
    Serializer serializer(iStreamer);
    return Stream_T(*this, serializer);
  }
}


/*
 * Lumi3DReWeighting.cpp
 *
 *  Created on: 5 Dec 2011
 *      Author: kreczko
 */

#include "../interface/Lumi3DReWeighting.h"

#include "TRandom1.h"
#include "TRandom2.h"
#include "TRandom3.h"
#include "TStopwatch.h"
#include "TH1.h"
#include "TH3.h"
#include "TFile.h"
#include <string>
#include <algorithm>
#include <boost/shared_ptr.hpp>
#include <boost/scoped_ptr.hpp>
#include <iostream>

namespace BAT {

Lumi3DReWeighting::Lumi3DReWeighting(std::string generatedFile, std::string dataFile,
		std::string GenHistName = "pileup", std::string DataHistName = "pileup") :
		generatedFileName_(generatedFile), //
		dataFileName_(dataFile), //
		GenHistName_(GenHistName), //
		DataHistName_(DataHistName) {
	generatedFile_ = boost::shared_ptr<TFile>(new TFile(generatedFileName_.c_str())); //MC distribution
	dataFile_ = boost::shared_ptr<TFile>(new TFile(dataFileName_.c_str())); //Data distribution

	boost::scoped_ptr<TH1> Data_temp((static_cast<TH1*>(dataFile_->Get(DataHistName_.c_str())->Clone())));

	boost::scoped_ptr<TH1> MC_temp((static_cast<TH1*>(generatedFile_->Get(GenHistName_.c_str())->Clone())));

	MC_distr_ = boost::shared_ptr<TH1>((static_cast<TH1*>(generatedFile_->Get(GenHistName_.c_str())->Clone())));
	Data_distr_ = boost::shared_ptr<TH1>((static_cast<TH1*>(dataFile_->Get(DataHistName_.c_str())->Clone())));

	// MC * data/MC = data, so the weights are data/MC:

	// normalize both histograms first

	Data_distr_->Scale(1.0 / Data_distr_->Integral());
	MC_distr_->Scale(1.0 / MC_distr_->Integral());

}

Lumi3DReWeighting::Lumi3DReWeighting(std::vector<float> MC_distr, std::vector<float> Lumi_distr) {
	// no histograms for input: use vectors

	// now, make histograms out of them:

	Int_t NMCBins = MC_distr.size();

	MC_distr_ = boost::shared_ptr<TH1>(new TH1F("MC_distr", "MC dist", NMCBins, 0., float(NMCBins)));

	Int_t NDBins = Lumi_distr.size();

	Data_distr_ = boost::shared_ptr<TH1>(new TH1F("Data_distr", "Data dist", NDBins, 0., float(NDBins)));

	for (int ibin = 1; ibin < NMCBins + 1; ++ibin) {
		MC_distr_->SetBinContent(ibin, MC_distr[ibin - 1]);
	}

	for (int ibin = 1; ibin < NDBins + 1; ++ibin) {
		Data_distr_->SetBinContent(ibin, Lumi_distr[ibin - 1]);
	}

	// check integrals, make sure things are normalized

	float deltaH = Data_distr_->Integral();
	if (fabs(1.0 - deltaH) > 0.001) { //*OOPS*...
		Data_distr_->Scale(1.0 / Data_distr_->Integral());
	}
	float deltaMC = MC_distr_->Integral();
	if (fabs(1.0 - deltaMC) > 0.001) {
		MC_distr_->Scale(1.0 / MC_distr_->Integral());
	}

}

double Lumi3DReWeighting::weight3D(int pv1, int pv2, int pv3) {

	using std::min;

	int npm1 = min(pv1, NVERTEX - 1);
	int np0 = min(pv2, NVERTEX - 1);
	int npp1 = min(pv3, NVERTEX - 1);

	return Weight3D_[npm1][np0][npp1];

}

/**
 * @PARAM ScaleFactor: Scale factor is used to shift target distribution (i.e. luminosity scale)  1. = no shift
 */
void Lumi3DReWeighting::weight3D_init(float ScaleFactor) {

	//create histogram to write output weights, save pain of generating them again...

	boost::scoped_ptr<TH3D> WHist(
			new TH3D("WHist", "3D weights", NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5));
	boost::scoped_ptr<TH3D> DHist(
			new TH3D("DHist", "3D weights", NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5));
	boost::scoped_ptr<TH3D> MHist(
			new TH3D("MHist", "3D weights", NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5, NVERTEX, -.5, 49.5));

	using std::min;

	if (MC_distr_->GetEntries() == 0) {
		std::cout << " MC and Data distributions are not initialised! You must call the Lumi3DReWeighting constructor. "
				<< std::endl;
	}

	// arrays for storing number of interactions

	double MC_ints[NVERTEX][NVERTEX][NVERTEX];
	double Data_ints[NVERTEX][NVERTEX][NVERTEX];

	for (int i = 0; i < NVERTEX; i++) {
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				MC_ints[i][j][k] = 0.;
				Data_ints[i][j][k] = 0.;
			}
		}
	}

	double factorial[NVERTEX];
	double PowerSer[NVERTEX];
	double base = 1.;

	factorial[0] = 1.;
	PowerSer[0] = 1.;

	for (int i = 1; i < NVERTEX + 1; ++i) {
		base = base * float(i);
		factorial[i] = base;
	}

	double x;
	double xweight;
	double probi, probj, probk;
	double Expval, mean;
	int xi;

	// Get entries for Data, MC, fill arrays:

	int NMCbin = MC_distr_->GetNbinsX();

	for (int jbin = 1; jbin < NMCbin + 1; jbin++) {
		x = MC_distr_->GetBinCenter(jbin);
		xweight = MC_distr_->GetBinContent(jbin); //use as weight for matrix

		//for Summer 11, we have this int feature:
		xi = int(x);

		// Generate Poisson distribution for each value of the mean

		mean = double(xi);

		if (mean < 0.) {
//      throw cms::Exception("BadInputValue") << " Your histogram generates MC luminosity values less than zero!"
//						<< " Please Check.  Terminating." << std::endl;
		}

		if (mean == 0.) {
			Expval = 1.;
		} else {
			Expval = exp(-1. * mean);
		}

		base = 1.;

		for (int i = 1; i < NVERTEX; ++i) {
			base = base * mean;
			PowerSer[i] = base; // PowerSer is mean^i
		}

		// compute poisson probability for each Nvtx in weight matrix

		for (int i = 0; i < NVERTEX; i++) {
			probi = PowerSer[i] / factorial[i] * Expval;
			for (int j = 0; j < NVERTEX; j++) {
				probj = PowerSer[j] / factorial[j] * Expval;
				for (int k = 0; k < NVERTEX; k++) {
					probk = PowerSer[k] / factorial[k] * Expval;
					// joint probability is product of event weights multiplied by weight of input distribution bin
					MC_ints[i][j][k] = MC_ints[i][j][k] + probi * probj * probk * xweight;
				}
			}
		}

	}

	int NDatabin = Data_distr_->GetNbinsX();

	for (int jbin = 1; jbin < NDatabin + 1; jbin++) {
		mean = (Data_distr_->GetBinCenter(jbin)) * ScaleFactor;
		xweight = Data_distr_->GetBinContent(jbin);

		// Generate poisson distribution for each value of the mean

		if (mean < 0.) {
//      throw cms::Exception("BadInputValue") << " Your histogram generates Data luminosity values less than zero!"
//						<< " Please Check.  Terminating." << std::endl;
		}

		if (mean == 0.) {
			Expval = 1.;
		} else {
			Expval = exp(-1. * mean);
		}

		base = 1.;

		for (int i = 1; i < NVERTEX; ++i) {
			base = base * mean;
			PowerSer[i] = base;
		}

		// compute poisson probability for each Nvtx in weight matrix

		for (int i = 0; i < NVERTEX; i++) {
			probi = PowerSer[i] / factorial[i] * Expval;
			for (int j = 0; j < NVERTEX; j++) {
				probj = PowerSer[j] / factorial[j] * Expval;
				for (int k = 0; k < NVERTEX; k++) {
					probk = PowerSer[k] / factorial[k] * Expval;
					// joint probability is product of event weights multiplied by weight of input distribution bin
					Data_ints[i][j][k] = Data_ints[i][j][k] + probi * probj * probk * xweight;
				}
			}
		}

	}

	for (int i = 0; i < NVERTEX; i++) {
		//if(i<5) std::cout << "i = " << i << std::endl;
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				if ((MC_ints[i][j][k]) > 0.) {
					Weight3D_[i][j][k] = Data_ints[i][j][k] / MC_ints[i][j][k];
				} else {
					Weight3D_[i][j][k] = 0.;
				}
				WHist->SetBinContent(i + 1, j + 1, k + 1, Weight3D_[i][j][k]);
				DHist->SetBinContent(i + 1, j + 1, k + 1, Data_ints[i][j][k]);
				MHist->SetBinContent(i + 1, j + 1, k + 1, MC_ints[i][j][k]);
				//	if(i<5 && j<5 && k<5) std::cout << Weight3D_[i][j][k] << " " ;
			}
			//      if(i<5 && j<5) std::cout << std::endl;
		}
	}

	std::cout << " 3D Weight Matrix initialized! " << std::endl;
	std::cout << " Writing weights to file Weight3D.root for re-use...  " << std::endl;

	boost::scoped_ptr<TFile> outfile(new TFile("Weight3D.root", "RECREATE"));
	outfile->cd();
	WHist->Write();
	MHist->Write();
	DHist->Write();
	outfile->Write();
	outfile->Close();
//	outfile->Delete();

	return;

}

void Lumi3DReWeighting::weight3D_init(std::string WeightFileName) {

	boost::scoped_ptr<TFile> infile(new TFile(WeightFileName.c_str()));
	boost::scoped_ptr<TH3D> WHist((TH3D*) infile->Get("WHist"));

	// Check if the histogram exists
	if (!WHist) {
//    throw cms::Exception("HistogramNotFound") << " Could not find the histogram WHist in the file "
//					      << "in the file " << WeightFileName << "." << std::endl;
	}

	for (int i = 0; i < NVERTEX; i++) {
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				Weight3D_[i][j][k] = WHist->GetBinContent(i + 1, j + 1, k + 1);
			}
		}
	}

	std::cout << " 3D Weight Matrix initialized! " << std::endl;

	return;

}

void Lumi3DReWeighting::weight3D_init(std::string MCWeightFileName, std::string DataWeightFileName) {

	boost::scoped_ptr<TFile> infileMC(new TFile(MCWeightFileName.c_str()));
	boost::scoped_ptr<TH3D> MHist((TH3D*) infileMC->Get("MHist"));

	// Check if the histogram exists
	if (!MHist) {
//    throw cms::Exception("HistogramNotFound") << " Could not find the histogram MHist in the file "
//					      << "in the file " << MCWeightFileName << "." << std::endl;
	}

	boost::scoped_ptr<TFile> infileD(new TFile(DataWeightFileName.c_str()));
	boost::scoped_ptr<TH3D> DHist((TH3D*) infileD->Get("DHist"));

	// Check if the histogram exists
	if (!DHist) {
//    throw cms::Exception("HistogramNotFound") << " Could not find the histogram DHist in the file "
//					      << "in the file " << DataWeightFileName << "." << std::endl;
	}

	for (int i = 0; i < NVERTEX; i++) {
		for (int j = 0; j < NVERTEX; j++) {
			for (int k = 0; k < NVERTEX; k++) {
				Weight3D_[i][j][k] = DHist->GetBinContent(i + 1, j + 1, k + 1)
						/ MHist->GetBinContent(i + 1, j + 1, k + 1);
			}
		}
	}

	std::cout << " 3D Weight Matrix initialized! " << std::endl;

	return;

}

} /* namespace BAT */


#include <iostream>
#include <string>

#include <boost/algorithm/string/split.hpp>

#include <opts/opts.h>
#include <dlog/log.h>
#include <dlog/stats.h>

#include <diy/decomposition.hpp>
#include <diy/reduce-operations.hpp>
#include <diy/io/block.hpp>

#include <reeber/format.h>

#include "reader-interfaces.h"

#include "merge-tree-block.h"
#include "persistent-integral-block.h"

typedef diy::RegularDecomposer<diy::DiscreteBounds>                 Decomposer;
typedef MergeTreeBlock::Vertex                                      Vertex;
typedef MergeTreeBlock::Value                                       Value;

bool vv_cmp(const MergeTreeNode::ValueVertex& a, const MergeTreeNode::ValueVertex& b)
{
    return a.second < b.second;
}

class TreeTracer
{
    private:
        typedef std::map<MergeTreeNode::Vertex, MinIntegral>        MinIntegralMap;
        typedef MergeTreeBlock::OffsetGrid                          OffsetGrid;
        typedef std::vector<OffsetGrid*>                            OffsetGridVector;

    public:
                    TreeTracer(const Decomposer& decomposer_, diy::Master& pi_master_,
                               Real m_, Real t_, Real e_,
                               std::vector<std::string> avg_fn_list_,
                               std::string density_fn_,
                               bool density_weighted_):
                         decomposer(decomposer_), pi_master(pi_master_),
                         m(m_), t(t_), e(e_),
                         density_reader(0), density_weighted(density_weighted_)
        {
            diy::mpi::communicator& world = pi_master_.communicator();

            for(const std::string& fn : avg_fn_list_)
                avg_var_readers.push_back(Reader::create(fn, world));

            if (!density_fn_.empty())
                density_reader = Reader::create(density_fn_, world);
        }

                    ~TreeTracer()
        {
            for(Reader* reader : avg_var_readers)
                delete reader;
            delete density_reader;
        }

        void        operator()(void* b, const diy::ReduceProxy& rp) const
        {
            MergeTreeBlock &block = *static_cast<MergeTreeBlock*>(b);

            MinIntegralMap mi_map;

            if (rp.in_link().size() == 0)
            {
                OffsetGridVector add_data;
                for(Reader* reader : avg_var_readers)
                    add_data.push_back(reader->read(block.core));
                MergeTreeBlock::OffsetGrid *density_data = density_reader ? density_reader->read(block.core) : 0;

                reeber::traverse_persistence(block.mt, Integrator(m,t,e,density_weighted, &mi_map, &block, &add_data, density_data));

                for(const MinIntegral& mi : mi_map | reeber::range::map_values)
                {
                    if (block.mt.cmp(m, mi.min_val))
                        continue;

                    if (mi.integral == 0)                                            // this block doesn't contribute anything to this extremum
                        continue;

                    int dest_gid = decomposer.point_to_gid(block.global.position(mi.min_vtx));
                    diy::BlockID dest = rp.out_link().target(dest_gid);              // out_link targets are ordered as gids
                    assert(dest.gid == dest_gid);
                    rp.enqueue(dest, mi);
                }

                for(MergeTreeBlock::OffsetGrid* og : add_data)
                    delete og;
                delete density_data;
            }
            else
            {
                for (int i = 0; i < rp.in_link().size(); ++i)
                {
                    int gid = rp.in_link().target(i).gid;
                    assert(gid == i);

                    while (rp.incoming(gid))
                    {
                        MinIntegral mi;
                        rp.dequeue(gid, mi);
                        if (mi_map.find(mi.min_vtx) == mi_map.end())
                            mi_map[mi.min_vtx] = mi;
                        else
                            mi_map[mi.min_vtx].combine(mi);
                    }
                }

                PersistentIntegralBlock *pi_block = new PersistentIntegralBlock(block);
                for (MinIntegralMap::const_iterator it = mi_map.begin(); it != mi_map.end(); ++it)
                    pi_block->add_integral(it->second);
                pi_master.add(rp.gid(), pi_block, new diy::Link);
            }
        }

        struct Integrator
        {
            Real                m,t,e;
            bool                density_weighted;
            MinIntegralMap*     mi_map_;
            MergeTreeBlock*     b;
            OffsetGridVector*   add_data_;
            OffsetGrid*         density_data;

                        Integrator(Real m_, Real t_, Real e_, bool density_weighted_,
                                   MinIntegralMap* mi_map, MergeTreeBlock* b_, OffsetGridVector* add_data, OffsetGrid* density_data_):
                            m(m_), t(t_), e(e_), density_weighted(density_weighted_),
                            mi_map_(mi_map), b(b_), add_data_(add_data), density_data(density_data_)    {}

            void        operator()(Neighbor from, Neighbor through, Neighbor to) const
            {
                MinIntegralMap& mi_map = *mi_map_;

                // contribution of the edge from -- from->parent to the integral of from
                mi_map[from->vertex].combine(integrate(from));

                // figure out the contribution of the edge through -- through->parent to the integral of to
                bool record_through = through->children.size() == 2;
                if (!record_through && from != to)    // if we have more than 2 children record through when processing the highest one (lexicographically)
                {
                    Neighbor n = 0;
                    for (size_t i = 0; i < through->children.size(); ++i)
                    {
                        Neighbor nc = through->children[i];
                        if (static_cast<Neighbor>(nc->aux) != to && nc > n)
                            n = nc;
                    }

                    if (n == 0)
                        assert(from == to);
                    else if (static_cast<Neighbor>(n->aux) == from)
                        record_through = true;
                }

                if (record_through && from != to)
                    mi_map[to->vertex].combine(integrate(through));

                // if we are not looking at the global pair, add the entire from-integral (now correct) to the integral of to
                if (from != to && !b->mt.cmp(t, through->value))
                    mi_map[to->vertex].combine(mi_map[from->vertex]);

                // from-through pair is not persistent enough, erase
                if (from->value/through->value < e)     // we should make this more generic somehow
                    mi_map.erase(from->vertex);
                else
                {
                    // fix the integral id (initialized to zero up until now)
                    MinIntegral mi(from);
                    mi.combine(mi_map[from->vertex]);
                    mi_map[from->vertex] = mi;
                }
            }

            void        integrate(MinIntegral& mi, Value val, MergeTree::Vertex vrt) const
            {
                OffsetGridVector& add_data = *add_data_;

                if (b->core.contains(vrt) && !b->mt.cmp(t, val))
                {
                    mi.integral += val * b->cell_size[0] * b->cell_size[1] * b->cell_size[2];
                    ++mi.n_cells;
                    for (size_t i = 0; i < add_data.size(); ++i)
                    {
                        Real new_val = (*add_data[i])(vrt);
                        if (density_data)
                            new_val /= (*density_data)(vrt);
                        if (density_weighted)
                            new_val *= val * b->cell_size[0] * b->cell_size[1] * b->cell_size[2];
                        mi.add_sums[i] += new_val;
                    }
                    mi.push_back(MergeTreeNode::ValueVertex(val, vrt));
                }
            }

            MinIntegral integrate(Neighbor n) const
            {
                MinIntegral mi;

                integrate(mi, n->value, n->vertex);

                for(const MergeTree::Node::ValueVertex& x : n->vertices)
                    integrate(mi, x.first, x.second);

                return mi;
            }
        };

    private:
        const Decomposer&    decomposer;
        diy::Master&         pi_master;
        Real                 m;
        Real                 t;
        Real                 e;
        std::vector<Reader*> avg_var_readers;
        Reader*              density_reader;
        bool                 density_weighted;
};

struct OutputIntegrals
{
                    OutputIntegrals(std::string outfn_, bool density_weighted_, bool verbose_):
                        outfn(outfn_), density_weighted(density_weighted_), verbose(verbose_)   {}

       void         operator()(PersistentIntegralBlock* b, const diy::Master::ProxyWithLink& cp) const
       {
           PersistentIntegralBlock&  block = *b;

           std::string   dgm_fn = fmt::format("{}-b{}.comp", outfn, block.gid);
           std::ofstream ofs(dgm_fn.c_str());

           for(MinIntegral &mi : block.persistent_integrals)
           {
               Vertex v = block.global.position(mi.min_vtx);
               ofs << v[0] * block.cell_size[0] << " " << v[1] * block.cell_size[1] << " " << v[2] * block.cell_size[2] << " ";
               if (verbose)
                   ofs << v[0] << "x" << v[1] << "x" << v[2] << " (" << mi.min_vtx << ") ";
               ofs <<  mi.integral;
               if (verbose)
                   ofs << " " << mi.n_cells;
               for(Real sum : mi.add_sums)
                   ofs << " " << sum / (density_weighted ? mi.integral : mi.n_cells);
               ofs << std::endl;
#ifdef REEBER_PERSISTENT_INTEGRAL_TRACE_VTCS
               std::sort(mi.vertices.begin(), mi.vertices.end(), vv_cmp);
               for (std::vector< MergeTreeNode::ValueVertex >::const_iterator it = mi.vertices.begin(); it != mi.vertices.end(); ++it)
                   ofs << "   " << it->second << " (" << block.global.position(it->second) <<  ")" << std::endl;
#if 1
               // Consistency check for debugging
               for (size_t i = 0; i < mi.vertices.size() - 1; ++i)
                   if (mi.vertices[i].second == mi.vertices[i+1].second)
                       LOG_SEV(fatal) << "Duplicate vertex " << mi.vertices[i].second << " in component " << mi.min_vtx;
#endif
#endif
           }
       }

    private:
        std::string outfn;
        bool        density_weighted;
        bool        verbose;
};

int main(int argc, char** argv)
{
    diy::mpi::environment   env(argc, argv);
    diy::mpi::communicator  world;
#ifdef REEBER_USE_BOXLIB_READER
    reeber::io::BoxLib::environment boxlib_env(argc, argv, world);
#endif

    using namespace opts;

    std::string prefix      = "./DIY.XXXXXX";
    int         in_memory   = -1;
    int         threads     = 1;
    int         k           = 2;
    Real        m           = 200;
    Real        t           = 82;
    Real        e           = m - t;

    std::string profile_path;
    std::string log_level = "info";
    std::string avg_fn_str = "";
    std::string density_fn = "";

    Options ops(argc, argv);
    ops
        >> Option('m', "memory",    in_memory,    "maximum blocks to store in memory")
        >> Option('j', "jobs",      threads,      "threads to use during the computation")
        >> Option('k', "k",         k,            "use k-ary swap")
        >> Option('s', "storage",   prefix,       "storage prefix")
        >> Option('p', "profile",   profile_path, "path to keep the execution profile")
        >> Option('l', "log",       log_level,    "log level")
        >> Option('x', "max",       m,            "maximum threshold")
        >> Option('i', "iso",       t,            "isofind threshold")
        >> Option('e', "epsilon",   e,            "persistence threshold")
        >> Option('f', "mean",      avg_fn_str,   "list of additionals files/variables to average separated by ','")
        >> Option('q', "quotient",  density_fn,   "divide by density in file")
    ;
    bool absolute         = ops >> Present('a', "absolute", "use absolute values for thresholds (instead of multiples of mean)");
    bool verbose          = ops >> Present('v', "verbose",  "verbose output: logical coordiantes and number of cells");
    bool density_weighted = ops >> Present('w', "weight",   "compute density-weighted averages");
    bool split            = ops >> Present(     "split",    "use split IO");

    std::string infn, outfn;
    if (  ops >> Present('h', "help", "show help message") ||
        !(ops >> PosOption(infn) >> PosOption(outfn)))
    {
        if (world.rank() == 0)
        {
            fmt::print("Usage: {} IN.lgt OUT.pi\n{}", argv[0], ops);
        }
        return 1;
    }

    std::vector<std::string> avg_fn_list;
    if (!avg_fn_str.empty())
        boost::split(avg_fn_list, avg_fn_str, std::bind1st(std::equal_to<char>(), ','));

    dlog::add_stream(std::cerr, dlog::severity(log_level))
        << dlog::stamp() << dlog::aux_reporter(world.rank()) << dlog::color_pre() << dlog::level() << dlog::color_post() >> dlog::flush();

    std::ofstream   profile_stream;
    if (profile_path == "-")
        dlog::prof.add_stream(std::cerr);
    else if (!profile_path.empty())
    {
        std::string profile_fn = fmt::format("{}-r{}.prf", profile_path, world.rank());
        profile_stream.open(profile_fn.c_str());
        dlog::prof.add_stream(profile_stream);
    }

    world.barrier();
    dlog::Timer timer;
    LOG_SEV_IF(world.rank() == 0, info) << "Starting computation";

    diy::FileStorage            storage(prefix);

    diy::Master                 mt_master(world,
                                          threads,
                                          in_memory,
                                          &MergeTreeBlock::create,
                                          &MergeTreeBlock::destroy,
                                          &storage,
                                          &MergeTreeBlock::save,
                                          &MergeTreeBlock::load);

    diy::Master                 pi_master(world,
                                          threads,
                                          in_memory);

    diy::ContiguousAssigner     assigner(world.size(), 0);

    // load the trees
    LOG_SEV_IF(world.rank() == 0, debug) << "Reading blocks from " << infn;
    if (!split)
        diy::io::read_blocks(infn, world, assigner, mt_master);
    else
        diy::io::split::read_blocks(infn, world, assigner, mt_master);
    LOG_SEV_IF(world.rank() == 0, info) << "Blocks read: " << mt_master.size();

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to read data:                    " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    // get the domain bounds from any block that's in memory (they are all the same) and set up a decomposer
    MergeTreeBlock::Box global = static_cast<MergeTreeBlock*>(((const diy::Master&) mt_master).block(mt_master.loaded_block()))->global;
    diy::DiscreteBounds domain {3};
    for (unsigned i = 0; i < 3; ++i)
    {
        domain.min[i] = global.from()[i];
        domain.max[i] = global.to()[i];
    }
    diy::RegularDecomposer<diy::DiscreteBounds>     decomposer(3, domain, assigner.nblocks(), Decomposer::BoolVector(3, true));

    // Compute average
    if (!absolute)
    {
        mt_master.foreach(&MergeTreeBlock::compute_average);
        mt_master.exchange();

        const diy::Master::ProxyWithLink& proxy = mt_master.proxy(mt_master.loaded_block());
        double mean = proxy.get<double>() / proxy.get<size_t>();
        m *= mean;
        t *= mean;

        LOG_SEV_IF(world.rank() == 0, info) << "Average value is " << mean << ". Using isofind threshold of " << t << " and maximum threshold of " << m;
    }

    // Compute and combine persistent integrals
    diy::all_to_all(mt_master, assigner, TreeTracer(decomposer, pi_master, m, t, e, avg_fn_list, density_fn, density_weighted), k);

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to compute persistent integrals: " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    // Save persistent integrals to file
    pi_master.foreach(OutputIntegrals(outfn, density_weighted, verbose));

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to output persistent integrals:  " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    dlog::prof.flush();     // TODO: this is necessary because the profile file will close before
                            //       the global dlog::prof goes out of scope and flushes the events.
                            //       Need to eventually fix this.
    dlog::stats.flush();
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/rosenbrock4.hpp

 [begin_description]
 Implementation of the Rosenbrock 4 method for solving stiff ODEs. Note, that a
 controller and a dense-output stepper exist for this method,
 [end_description]

 Copyright 2009-2011 Karsten Ahnert
 Copyright 2009-2011 Mario Mulansky

 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_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

#include <boost/numeric/odeint/stepper/stepper_categories.hpp>

#include <boost/numeric/odeint/util/ublas_wrapper.hpp>
#include <boost/numeric/odeint/util/is_resizeable.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/lu.hpp>

namespace boost {
namespace numeric {
namespace odeint {

/*
 * ToDo:
 *
 * 2. Interfacing for odeint, check if controlled_error_stepper can be used
 * 3. dense output
 */

template< class Value >
struct default_rosenbrock_coefficients
{
    typedef Value value_type;
    typedef unsigned short order_type;

    default_rosenbrock_coefficients( void )
    : gamma ( static_cast< value_type >( 0.25 ) ) ,
      d1 ( static_cast< value_type >( 0.25 ) ) ,
      d2 ( static_cast< value_type >( -0.1043 ) ) ,
      d3 ( static_cast< value_type >( 0.1035 ) ) ,
      d4 ( static_cast< value_type >( 0.3620000000000023e-01 ) ) ,
      c2 ( static_cast< value_type >( 0.386 ) ) ,
      c3 ( static_cast< value_type >( 0.21 ) ) ,
      c4 ( static_cast< value_type >( 0.63 ) ) ,
      c21 ( static_cast< value_type >( -0.5668800000000000e+01 ) ) ,
      a21 ( static_cast< value_type >( 0.1544000000000000e+01 ) ) ,
      c31 ( static_cast< value_type >( -0.2430093356833875e+01 ) ) ,
      c32 ( static_cast< value_type >( -0.2063599157091915e+00 ) ) ,
      a31 ( static_cast< value_type >( 0.9466785280815826e+00 ) ) ,
      a32 ( static_cast< value_type >( 0.2557011698983284e+00 ) ) ,
      c41 ( static_cast< value_type >( -0.1073529058151375e+00 ) ) ,
      c42 ( static_cast< value_type >( -0.9594562251023355e+01 ) ) ,
      c43 ( static_cast< value_type >( -0.2047028614809616e+02 ) ) ,
      a41 ( static_cast< value_type >( 0.3314825187068521e+01 ) ) ,
      a42 ( static_cast< value_type >( 0.2896124015972201e+01 ) ) ,
      a43 ( static_cast< value_type >( 0.9986419139977817e+00 ) ) ,
      c51 ( static_cast< value_type >( 0.7496443313967647e+01 ) ) ,
      c52 ( static_cast< value_type >( -0.1024680431464352e+02 ) ) ,
      c53 ( static_cast< value_type >( -0.3399990352819905e+02 ) ) ,
      c54 ( static_cast< value_type >(  0.1170890893206160e+02 ) ) ,
      a51 ( static_cast< value_type >( 0.1221224509226641e+01 ) ) ,
      a52 ( static_cast< value_type >( 0.6019134481288629e+01 ) ) ,
      a53 ( static_cast< value_type >( 0.1253708332932087e+02 ) ) ,
      a54 ( static_cast< value_type >( -0.6878860361058950e+00 ) ) ,
      c61 ( static_cast< value_type >( 0.8083246795921522e+01 ) ) ,
      c62 ( static_cast< value_type >( -0.7981132988064893e+01 ) ) ,
      c63 ( static_cast< value_type >( -0.3152159432874371e+02 ) ) ,
      c64 ( static_cast< value_type >( 0.1631930543123136e+02 ) ) ,
      c65 ( static_cast< value_type >( -0.6058818238834054e+01 ) ) ,
      d21 ( static_cast< value_type >( 0.1012623508344586e+02 ) ) ,
      d22 ( static_cast< value_type >( -0.7487995877610167e+01 ) ) ,
      d23 ( static_cast< value_type >( -0.3480091861555747e+02 ) ) ,
      d24 ( static_cast< value_type >( -0.7992771707568823e+01 ) ) ,
      d25 ( static_cast< value_type >( 0.1025137723295662e+01 ) ) ,
      d31 ( static_cast< value_type >( -0.6762803392801253e+00 ) ) ,
      d32 ( static_cast< value_type >( 0.6087714651680015e+01 ) ) ,
      d33 ( static_cast< value_type >( 0.1643084320892478e+02 ) ) ,
      d34 ( static_cast< value_type >( 0.2476722511418386e+02 ) ) ,
      d35 ( static_cast< value_type >( -0.6594389125716872e+01 ) )
    {}

    const value_type gamma;
    const value_type d1 , d2 , d3 , d4;
    const value_type c2 , c3 , c4;
    const value_type c21 ;
    const value_type a21;
    const value_type c31 , c32;
    const value_type a31 , a32;
    const value_type c41 , c42 , c43;
    const value_type a41 , a42 , a43;
    const value_type c51 , c52 , c53 , c54;
    const value_type a51 , a52 , a53 , a54;
    const value_type c61 , c62 , c63 , c64 , c65;
    const value_type d21 , d22 , d23 , d24 , d25;
    const value_type d31 , d32 , d33 , d34 , d35;

    static const order_type stepper_order = 4;
    static const order_type error_order = 3;
};

template< class Value , class Coefficients = default_rosenbrock_coefficients< Value > , class Resizer = initially_resizer >
class rosenbrock4
{
private:

public:

    typedef Value value_type;
    typedef boost::numeric::ublas::vector< value_type > state_type;
    typedef state_type deriv_type;
    typedef value_type time_type;
    typedef boost::numeric::ublas::matrix< value_type > matrix_type;
    typedef boost::numeric::ublas::permutation_matrix< size_t > pmatrix_type;
    typedef Resizer resizer_type;
    typedef Coefficients rosenbrock_coefficients;
    typedef stepper_tag stepper_category;
    typedef unsigned short order_type;

    typedef state_wrapper< state_type > wrapped_state_type;
    typedef state_wrapper< deriv_type > wrapped_deriv_type;
    typedef state_wrapper< matrix_type > wrapped_matrix_type;
    typedef state_wrapper< pmatrix_type > wrapped_pmatrix_type;

    typedef rosenbrock4< Value , Coefficients , Resizer > stepper_type;

    const static order_type stepper_order = rosenbrock_coefficients::stepper_order;
    const static order_type error_order = rosenbrock_coefficients::error_order;

    rosenbrock4( void )
    : m_resizer() , m_x_err_resizer() ,
      m_jac() , m_pm() ,
      m_dfdt() , m_dxdt() , m_dxdtnew() ,
      m_g1() , m_g2() , m_g3() , m_g4() , m_g5() ,
      m_cont3() , m_cont4() , m_xtmp() , m_x_err() ,
      m_coef()
    { }

    order_type order() const { return stepper_order; }

    template< class System >
    void do_step( System system , const state_type &x , time_type t , state_type &xout , time_type dt , state_type &xerr )
    {
        // get the system and jacobi function
        typedef typename odeint::unwrap_reference< System >::type system_type;
        typedef typename odeint::unwrap_reference< typename system_type::first_type >::type deriv_func_type;
        typedef typename odeint::unwrap_reference< typename system_type::second_type >::type jacobi_func_type;
        system_type &sys = system;
        deriv_func_type &deriv_func = sys.first;
        jacobi_func_type &jacobi_func = sys.second;

        const size_t n = x.size();

        m_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_impl<state_type> , detail::ref( *this ) , detail::_1 ) );

        for( size_t i=0 ; i<n ; ++i )
            m_pm.m_v( i ) = i;

        deriv_func( x , m_dxdt.m_v , t );
        jacobi_func( x , m_jac.m_v , t , m_dfdt.m_v );

        m_jac.m_v *= -1.0;
        m_jac.m_v += 1.0 / m_coef.gamma / dt * boost::numeric::ublas::identity_matrix< value_type >( n );
        boost::numeric::ublas::lu_factorize( m_jac.m_v , m_pm.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_g1.m_v[i] = m_dxdt.m_v[i] + dt * m_coef.d1 * m_dfdt.m_v[i];
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g1.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a21 * m_g1.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c2 * dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g2.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d2 * m_dfdt.m_v[i] + m_coef.c21 * m_g1.m_v[i] / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g2.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a31 * m_g1.m_v[i] + m_coef.a32 * m_g2.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c3 * dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g3.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d3 * m_dfdt.m_v[i] + ( m_coef.c31 * m_g1.m_v[i] + m_coef.c32 * m_g2.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g3.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a41 * m_g1.m_v[i] + m_coef.a42 * m_g2.m_v[i] + m_coef.a43 * m_g3.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + m_coef.c4 * dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g4.m_v[i] = m_dxdtnew.m_v[i] + dt * m_coef.d4 * m_dfdt.m_v[i] + ( m_coef.c41 * m_g1.m_v[i] + m_coef.c42 * m_g2.m_v[i] + m_coef.c43 * m_g3.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g4.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] = x[i] + m_coef.a51 * m_g1.m_v[i] + m_coef.a52 * m_g2.m_v[i] + m_coef.a53 * m_g3.m_v[i] + m_coef.a54 * m_g4.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
        for( size_t i=0 ; i<n ; ++i )
            m_g5.m_v[i] = m_dxdtnew.m_v[i] + ( m_coef.c51 * m_g1.m_v[i] + m_coef.c52 * m_g2.m_v[i] + m_coef.c53 * m_g3.m_v[i] + m_coef.c54 * m_g4.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , m_g5.m_v );

        for( size_t i=0 ; i<n ; ++i )
            m_xtmp.m_v[i] += m_g5.m_v[i];
        deriv_func( m_xtmp.m_v , m_dxdtnew.m_v , t + dt );
        for( size_t i=0 ; i<n ; ++i )
            xerr[i] = m_dxdtnew.m_v[i] + ( m_coef.c61 * m_g1.m_v[i] + m_coef.c62 * m_g2.m_v[i] + m_coef.c63 * m_g3.m_v[i] + m_coef.c64 * m_g4.m_v[i] + m_coef.c65 * m_g5.m_v[i] ) / dt;
        boost::numeric::ublas::lu_substitute( m_jac.m_v , m_pm.m_v , xerr );

        for( size_t i=0 ; i<n ; ++i )
            xout[i] = m_xtmp.m_v[i] + xerr[i];
    }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt , state_type &xerr )
    {
        do_step( system , x , t , x , dt , xerr );
    }

    /*
     * do_step without error output - just calls above functions with and neglects the error estimate
     */
    template< class System >
    void do_step( System system , const state_type &x , time_type t , state_type &xout , time_type dt )
    {
        m_x_err_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_x_err<state_type> , detail::ref( *this ) , detail::_1 ) );
        do_step( system , x , t , xout , dt , m_x_err.m_v );
    }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt )
    {
        m_x_err_resizer.adjust_size( x , detail::bind( &stepper_type::template resize_x_err<state_type> , detail::ref( *this ) , detail::_1 ) );
        do_step( system , x , t , dt , m_x_err.m_v );
    }

    void prepare_dense_output()
    {
        const size_t n = m_g1.m_v.size();
        for( size_t i=0 ; i<n ; ++i )
        {
            m_cont3.m_v[i] = m_coef.d21 * m_g1.m_v[i] + m_coef.d22 * m_g2.m_v[i] + m_coef.d23 * m_g3.m_v[i] + m_coef.d24 * m_g4.m_v[i] + m_coef.d25 * m_g5.m_v[i];
            m_cont4.m_v[i] = m_coef.d31 * m_g1.m_v[i] + m_coef.d32 * m_g2.m_v[i] + m_coef.d33 * m_g3.m_v[i] + m_coef.d34 * m_g4.m_v[i] + m_coef.d35 * m_g5.m_v[i];
        }
    }

    void calc_state( time_type t , state_type &x ,
            const state_type &x_old , time_type t_old ,
            const state_type &x_new , time_type t_new )
    {
        const size_t n = m_g1.m_v.size();
        time_type dt = t_new - t_old;
        time_type s = ( t - t_old ) / dt;
        time_type s1 = 1.0 - s;
        for( size_t i=0 ; i<n ; ++i )
            x[i] = x_old[i] * s1 + s * ( x_new[i] + s1 * ( m_cont3.m_v[i] + s * m_cont4.m_v[i] ) );
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_impl( x );
        resize_x_err( x );
    }

protected:

    template< class StateIn >
    bool resize_impl( const StateIn &x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_dxdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_dfdt , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_dxdtnew , x , typename is_resizeable<deriv_type>::type() );
        resized |= adjust_size_by_resizeability( m_xtmp , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g1 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g2 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g3 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g4 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_g5 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_cont3 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_cont4 , x , typename is_resizeable<state_type>::type() );
        resized |= adjust_size_by_resizeability( m_jac , x , typename is_resizeable<matrix_type>::type() );
        resized |= adjust_size_by_resizeability( m_pm , x , typename is_resizeable<pmatrix_type>::type() );
        return resized;
    }

    template< class StateIn >
    bool resize_x_err( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_x_err , x , typename is_resizeable<state_type>::type() );
    }

private:

    resizer_type m_resizer;
    resizer_type m_x_err_resizer;

    wrapped_matrix_type m_jac;
    wrapped_pmatrix_type m_pm;
    wrapped_deriv_type m_dfdt , m_dxdt , m_dxdtnew;
    wrapped_state_type m_g1 , m_g2 , m_g3 , m_g4 , m_g5;
    wrapped_state_type m_cont3 , m_cont4;
    wrapped_state_type m_xtmp;
    wrapped_state_type m_x_err;

    const rosenbrock_coefficients m_coef;
};

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_ROSENBROCK4_HPP_INCLUDED