/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "truth_table_utils.hpp"

#include <deque>
#include <iostream>

#include <boost/pending/integer_log2.hpp>

#include <core/utils/conversion_utils.hpp>
#include <core/utils/string_utils.hpp>

namespace cirkit
{

/******************************************************************************
 * Truth table store                                                          *
 ******************************************************************************/

tt_store& tt_store::i()
{
  static tt_store instance;
  return instance;
}

tt_store::tt_store()
{
  assert( sizeof( unsigned long ) == 8 );
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

tt tt_const0()
{
  return boost::dynamic_bitset<>( 64u, 0u );
}

tt tt_const1()
{
  return ~tt_const0();
}

tt tt_nth_var( unsigned i )
{
  if ( i < 6u )
  {
    return tt_store::i()( i );
  }
  else
  {
    tt t( 1u << i, 0ul );
    t.resize( 1u << ( i + 1 ), true );
    return t;
  }
}

unsigned tt_num_vars( const tt& t )
{
  return boost::integer_log2( t.size() );
}

void tt_extend( tt& t, unsigned to )
{
  unsigned nv = tt_num_vars( t );
  tt::size_type s, i;

  while ( nv < to )
  {
    s = t.size();
    t.resize( s << 1u );
    for ( i = 0u; i < s; ++i )
    {
      t[s + i] = t[i];
    }
    ++nv;
  }
}

void tt_shrink( tt& t, unsigned to )
{
  t.resize( 1u << to );
}

void tt_align( tt& t1, tt& t2 )
{
  unsigned nv1 = tt_num_vars( t1 ), nv2 = tt_num_vars( t2 );
  if ( nv1 < nv2 )
  {
    tt_extend( t1, nv2 );
  }
  else if ( nv2 < nv1 )
  {
    tt_extend( t2, nv1 );
  }
}

bool tt_has_var( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );

  if ( i >= n ) { return false; }

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  auto tv = ~tt_nth_var( i );
  tt_extend( tv, n );

  return ( (tc >> (1 << i)) & tv )  != ( tc & tv );
}

boost::dynamic_bitset<> tt_support( const tt& t )
{
  unsigned n = tt_num_vars( t );

  boost::dynamic_bitset<> support( n );
  for ( unsigned i = 0u; i < n; ++i )
  {
    support.set( i, tt_has_var( t, i ) );
  }

  return support;
}

unsigned tt_support_size( const tt& t )
{
  return tt_support( t ).count();
}

tt tt_cof0( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );
  auto tv = ~tt_nth_var( i );
  tt_extend( tv, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  return (tc & tv) | ((tc & tv) << (1 << i));
}

tt tt_cof1( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );
  auto tv = tt_nth_var( i );
  tt_extend( tv, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  return (tc & tv) | ((tc & tv) >> (1 << i));
}

bool tt_cof0_is_const0( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );
  auto tv = ~tt_nth_var( i );
  tt_extend( tv, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  return ( tc & tv ).empty();
}

bool tt_cof0_is_const1( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );
  auto tv = ~tt_nth_var( i );
  tt_extend( tv, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  return ( tc & tv ) == tv;
}

bool tt_cof1_is_const0( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );
  auto tv = tt_nth_var( i );
  tt_extend( tv, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  return ( tc & tv ).empty();
}

bool tt_cof1_is_const1( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );
  auto tv = tt_nth_var( i );
  tt_extend( tv, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  return ( tc & tv ) == tv;
}

bool tt_cofs_opposite( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );
  auto tv = tt_nth_var( i );
  tt_extend( tv, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_extend( tc, tt_store::i().width ); }

  unsigned shift = ( 1u << i );
  return ((tc << shift) & tv) == (~tc & tv);
}

void tt_resize( tt& t, unsigned size )
{
  auto num_vars = tt_num_vars( t );
  if ( num_vars == size ) return;
  else if ( num_vars < size )
  {
    tt_extend( t, size );
  }
  else
  {
    tt_shrink( t, size );
  }
}

bool tt_is_const0( const tt& t )
{
  tt t0 = tt_const0();
  tt_resize( t0, tt_num_vars( t ) );
  return t0 == t;
}

bool tt_is_const1( const tt& t )
{
  tt t1 = tt_const1();
  tt_resize( t1, tt_num_vars( t ) );
  return t1 == t;
}

tt tt_exists( const tt& t, unsigned i )
{
  return tt_cof0( t, i ) | tt_cof1( t, i );
}

tt tt_forall( const tt& t, unsigned i )
{
  return tt_cof0( t, i ) & tt_cof1( t, i );
}

tt tt_permute( const tt& t, unsigned i, unsigned j )
{
  if ( i == j ) return t;

  unsigned n = tt_num_vars( t );
  assert( i < n );
  assert( j < n );

  tt vi = tt_nth_var( i );
  tt vj = tt_nth_var( j );
  tt_extend( vi, n );
  tt_extend( vj, n );

  tt c0 = tt_cof0( t, i );
  tt c1 = tt_cof1( t, i );
  tt c00 = tt_cof0( c0, j );
  tt c01 = tt_cof1( c0, j );
  tt c10 = tt_cof0( c1, j );
  tt c11 = tt_cof1( c1, j );

  auto tt_new = ( ~vi & ( ( ~vj & c00 ) | ( vj & c10 ) ) ) | ( vi & ( ( ~vj & c01 ) | ( vj & c11 ) ) );

  if ( n < 6u )
  {
    tt_shrink( tt_new, n );
  }

  return tt_new;
}

tt tt_remove_var( const tt& t, unsigned i )
{
  unsigned n = tt_num_vars( t );
  assert( n > 0u );
  tt ret = t;

  for ( unsigned j = i; j < n - 1u; ++j )
  {
    ret = tt_permute( ret, j, j + 1u );
  }

  ret.resize( (tt::size_type)1 << ( n - 1 ) );
  return ret;
}

tt tt_flip( const tt& t, unsigned i )
{
  auto n = tt_num_vars( t );
  assert( i < n );

  auto vi = tt_nth_var( i );
  tt_extend( vi, n );

  auto tc = t;
  if ( n < tt_store::i().width ) { tt_shrink( vi, n ); }

  return ((tc << (1 << i)) & vi) | ((tc & vi) >> (1 << i));
}

void tt_to_minbase( tt& t, boost::dynamic_bitset<>* psupport )
{
  auto support = tt_support( t );

  if ( psupport ) *psupport = support;

  auto to_pos = 0u;
  auto pos = support.find_first();

  while ( pos != boost::dynamic_bitset<>::npos )
  {
    t = tt_permute( t, pos, to_pos++ );
    pos = support.find_next( pos );
  }

  /* resize */
  t.resize( 1 << support.count() );
}

void tt_to_minbase_and_discard( tt& t, unsigned max_size, boost::dynamic_bitset<>* psupport )
{
  boost::dynamic_bitset<> support = tt_support( t );

  if ( psupport ) *psupport = support;

  unsigned to_pos = 0u;
  boost::dynamic_bitset<>::size_type pos = support.find_first();

  while ( pos != boost::dynamic_bitset<>::npos )
  {
    if ( to_pos < max_size )
    {
      t = tt_permute( t, pos, to_pos++ );
    }
    else
    {
      t = tt_exists( t, pos );
    }
    pos = support.find_next( pos );
  }

  /* resize */
  t.resize( 1 << std::min( to_pos, max_size ) );
}

void tt_from_minbase( tt& t, const boost::dynamic_bitset<> pattern )
{
  std::deque<unsigned> positions;

  tt_extend( t, pattern.size() );

  boost::dynamic_bitset<>::size_type pos = pattern.find_first();
  while ( pos != boost::dynamic_bitset<>::npos )
  {
    positions.push_front( pos );
    pos = pattern.find_next( pos );
  }

  unsigned support_size = tt_support_size( t );
  assert( positions.size() == support_size );

  unsigned tpos = support_size - 1u;
  for ( const auto& pos : positions )
  {
    assert( pos >= tpos );
    t = tt_permute( t, tpos--, pos );
  }
}

std::string tt_to_hex( const tt& t )
{
  std::string s;
  to_string( t, s );

  std::string result;
  for ( unsigned i = 0u; i < s.length(); i += 4u )
  {
    result += convert_bin2hex( s.substr( i, 4u ) );
  }
  return result;
}

tt tt_from_hex( const std::string& s )
{
  const auto bin = convert_hex2bin( s );
  auto t = tt( bin.size(), 0u );
  for ( auto i = 0u; i < bin.size(); ++i )
  {
    assert( bin[i] == '0' || bin[i] == '1' );
    t[ t.size() - i - 1 ] = ( bin[i] == '1' );
  }
  return t;
}

tt tt_from_hex( const std::string& s, unsigned to )
{
  auto t = tt_from_hex( s );
  const auto num_vars = tt_num_vars( t );
  if ( num_vars < to )
  {
    tt_extend( t, to );
  }
  else if ( num_vars > to )
  {
    tt_shrink( t, to );
  }
  return t;
}

/******************************************************************************
 * truth table from expression                                                *
 ******************************************************************************/

std::pair<tt, unsigned> tt_expression_evaluator::on_const( bool value ) const
{
  return {value ? tt_const1() : tt_const0(), 0u};
}

std::pair<tt, unsigned> tt_expression_evaluator::on_var( unsigned index ) const
{
  return {tt_nth_var( index ), index + 1u};
}

std::pair<tt, unsigned> tt_expression_evaluator::on_inv( const std::pair<tt, unsigned>& value ) const
{
  return {~( value.first ), value.second};
}

std::pair<tt, unsigned> tt_expression_evaluator::on_and( const std::pair<tt, unsigned>& value1, const std::pair<tt, unsigned>& value2 ) const
{
  auto _v1 = value1.first;
  auto _v2 = value2.first;
  tt_align( _v1, _v2 );
  return {_v1 & _v2, std::max( value1.second, value2.second )};
}

std::pair<tt, unsigned> tt_expression_evaluator::on_or( const std::pair<tt, unsigned>& value1, const std::pair<tt, unsigned>& value2 ) const
{
  auto _v1 = value1.first;
  auto _v2 = value2.first;
  tt_align( _v1, _v2 );
  return {_v1 | _v2, std::max( value1.second, value2.second )};
}

std::pair<tt, unsigned> tt_expression_evaluator::on_maj( const std::pair<tt, unsigned>& value1, const std::pair<tt, unsigned>& value2, const std::pair<tt, unsigned>& value3 ) const
{
  auto _v1 = value1.first;
  auto _v2 = value2.first;
  auto _v3 = value3.first;
  tt_align( _v1, _v2 );
  tt_align( _v2, _v3 );
  tt_align( _v1, _v3 );
  return {( _v1 & _v2 ) | ( _v1 & _v3 ) | ( _v2 & _v3 ), std::max( value1.second, std::max( value2.second, value3.second ) )};
}

std::pair<tt, unsigned> tt_expression_evaluator::on_xor( const std::pair<tt, unsigned>& value1, const std::pair<tt, unsigned>& value2 ) const
{
  auto _v1 = value1.first;
  auto _v2 = value2.first;
  tt_align( _v1, _v2 );
  return {_v1 ^ _v2, std::max( value1.second, value2.second )};
}

tt tt_from_expression( const expression_t::ptr& expr )
{
  const auto v = evaluate_expression( expr, tt_expression_evaluator() );
  auto t = v.first;
  tt_shrink( t, v.second );
  return t;
}

tt tt_from_sop_spec( const std::string& spec )
{
  enum class pla_type_t { none, on, off };
  auto pla_type = pla_type_t::none;
  tt f( 1u );

  foreach_string( spec, "\n", [&f, &pla_type]( const std::string& line ) {
      const auto pair = split_string_pair( line, " " );
      const auto p = pair.first;

      switch ( pla_type )
      {
      case pla_type_t::none:
        pla_type = ( pair.second == "1" ) ? pla_type_t::on : pla_type_t::off;
        f = tt( 1u << p.size() );
        if ( pla_type == pla_type_t::off )
        {
          f.flip();
        }
        break;
      case pla_type_t::on:   assert( pair.second == "1" ); break;
      case pla_type_t::off:  assert( pair.second == "0" ); break;
      }

      auto cube = ( pla_type == pla_type_t::on ) ? ~tt( 1 << p.size() ) : tt( 1 << p.size() );
      for ( auto i = 0u; i < p.size(); ++i )
      {
        if ( p[i] == '-' ) continue;
        auto v = ( p[i] == '0' ) != ( pla_type == pla_type_t::off ) ? ~tt_nth_var( i ) : tt_nth_var( i );
        if ( p.size() < 6 )
        {
          tt_shrink( v, p.size() );
        }
        else
        {
          tt_align( v, cube );
        }

        if ( pla_type == pla_type_t::on )
        {
          cube &= v;
        }
        else
        {
          cube |= v;
        }
      }

      if ( pla_type == pla_type_t::on )
      {
        f |= cube;
      }
      else
      {
        f &= cube;
      }
    } );

  return f;
}

std::vector<int> walsh_spectrum( const tt& func )
{
  const auto n = tt_num_vars( func );

  std::vector<int> spectra( func.size(), 0u );
  foreach_bit( func, [&spectra]( unsigned pos ) { spectra[pos] = 1u; } );

  /* butterfly loops */
  for ( auto i = 0u; i < n; ++i )
  {
    auto i1 = 0u;

    const unsigned d = ( 1 << ( n - 1 - i ) );
    /* blocks? */
    for ( auto b = 0u; b < ( 1u << i ); ++b, i1 += d )
    {
      /* block elements */
      for ( auto e = 0u; e < d; ++e, ++i1 )
      {
        const auto i2 = i1 + d;

        const auto v1 = spectra[i1] + spectra[i2];
        const auto v2 = spectra[i1] - spectra[i2];
        spectra[i1] = v1;
        spectra[i2] = v2;
      }
    }
  }

  return spectra;
}

tt tt_maj(tt a, tt b, tt c)
{
  return (a & b) | (b & c) | (a & c);
}

kitty::dynamic_truth_table to_kitty( const tt& tt )
{
  kitty::dynamic_truth_table ret( tt_num_vars( tt ) );
  boost::to_block_range( tt, ret.begin() );
  return ret;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


// -----------------------------------------------------------------------
// RTToolbox - DKFZ radiotherapy quantitative evaluation library
//
// Copyright (c) German Cancer Research Center (DKFZ),
// Software development for Integrated Diagnostics and Therapy (SIDT).
// ALL RIGHTS RESERVED.
// See rttbCopyright.txt or
// http://www.dkfz.de/en/sidt/projects/rttb/copyright.html
//
// This software is distributed WITHOUT ANY WARRANTY; without even
// the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
// PURPOSE.  See the above copyright notices for more information.
//
//------------------------------------------------------------------------

#include "rttbBoostMaskVoxelizationThread.h"

#include "rttbInvalidParameterException.h"

#include <boost/geometry.hpp>
#include <boost/make_shared.hpp>

namespace rttb
{
	namespace masks
	{
		namespace boost
		{
			BoostMaskVoxelizationThread::BoostMaskVoxelizationThread(const BoostPolygonMap& APolygonMap,
                const VoxelIndexVector& aGlobalBoundingBox, BoostArrayMapPointer anArrayMap, ::boost::shared_ptr<std::mutex> aMutex, bool strict) : _geometryCoordinateBoostPolygonMap(APolygonMap),
                _globalBoundingBox(aGlobalBoundingBox), _resultVoxelization(anArrayMap), _mutex(aMutex), _strict(strict)
			{
			}

			void BoostMaskVoxelizationThread::operator()()
			{
				rttb::VoxelGridIndex3D minIndex = _globalBoundingBox.at(0);
				rttb::VoxelGridIndex3D maxIndex = _globalBoundingBox.at(1);
				const unsigned int globalBoundingBoxSize0 = maxIndex[0] - minIndex[0] + 1;
				const unsigned int globalBoundingBoxSize1 = maxIndex[1] - minIndex[1] + 1;

                std::map<double, ::boost::shared_ptr<BoostArray2D> > voxelizationMapInThread;

                 for (auto & it : _geometryCoordinateBoostPolygonMap)
				{
                    BoostArray2D maskArray(::boost::extents[globalBoundingBoxSize0][globalBoundingBoxSize1]);

					BoostPolygonVector boostPolygonVec = it.second;

					for (unsigned int x = 0; x < globalBoundingBoxSize0; ++x)
					{
						for (unsigned int y = 0; y < globalBoundingBoxSize1; ++y)
						{
							rttb::VoxelGridIndex3D currentIndex;
							currentIndex[0] = x + minIndex[0];
							currentIndex[1] = y + minIndex[1];
							currentIndex[2] = 0;

							//Get intersection polygons of the dose voxel and the structure
							BoostPolygonDeque polygons = getIntersections(currentIndex, boostPolygonVec);

							//Calc areas of all intersection polygons
							double volumeFraction = calcArea(polygons);
                            volumeFraction = correctForErrorAndStrictness(volumeFraction, _strict);
                            if (volumeFraction < 0 || volumeFraction > 1 )
                            {
                                throw rttb::core::InvalidParameterException("Mask calculation failed! The volume fraction should >= 0 and <= 1!");
                            }

							maskArray[x][y] = volumeFraction;
						}
					}

                    voxelizationMapInThread.insert(std::pair<double, BoostArray2DPointer>(it.first, ::boost::make_shared<BoostArray2D>(maskArray)));
				}
                //insert gathered values into voxelization map
                std::unique_lock<std::mutex> lock(*_mutex);
                _resultVoxelization->insert(voxelizationMapInThread.begin(), voxelizationMapInThread.end());

			}

			/*Get intersection polygons of the contour and a voxel polygon*/
			BoostMaskVoxelizationThread::BoostPolygonDeque BoostMaskVoxelizationThread::getIntersections(
			    const rttb::VoxelGridIndex3D&
			    aVoxelIndex3D, const BoostPolygonVector& intersectionSlicePolygons)
			{
				BoostMaskVoxelizationThread::BoostPolygonDeque polygonDeque;

				BoostRing2D voxelPolygon = get2DContour(aVoxelIndex3D);
				::boost::geometry::correct(voxelPolygon);

				BoostPolygonVector::const_iterator it;

				for (it = intersectionSlicePolygons.begin(); it != intersectionSlicePolygons.end(); ++it)
				{
					BoostPolygon2D contour = *it;
					::boost::geometry::correct(contour);

					BoostPolygonDeque intersection;
					::boost::geometry::intersection(voxelPolygon, contour, intersection);
					polygonDeque.insert(polygonDeque.end(), intersection.begin(), intersection.end());
				}

				return polygonDeque;
			}

			BoostMaskVoxelizationThread::BoostRing2D BoostMaskVoxelizationThread::get2DContour(
			    const rttb::VoxelGridIndex3D& aVoxelGrid3D)
			{
				BoostRing2D polygon;

				BoostPoint2D point1(aVoxelGrid3D[0] - 0.5, aVoxelGrid3D[1] - 0.5);
				::boost::geometry::append(polygon, point1);

				BoostPoint2D point2(aVoxelGrid3D[0] + 0.5, aVoxelGrid3D[1] - 0.5);
				::boost::geometry::append(polygon, point2);

				BoostPoint2D point3(aVoxelGrid3D[0] + 0.5, aVoxelGrid3D[1] + 0.5);
				::boost::geometry::append(polygon, point3);

				BoostPoint2D point4(aVoxelGrid3D[0] - 0.5, aVoxelGrid3D[1] + 0.5);
				::boost::geometry::append(polygon, point4);

				::boost::geometry::append(polygon, point1);

				return polygon;

			}

			/*Calculate the intersection area*/
			double BoostMaskVoxelizationThread::calcArea(const BoostPolygonDeque& aPolygonDeque)
			{
				double area = 0;

				BoostPolygonDeque::const_iterator it;

				for (it = aPolygonDeque.begin(); it != aPolygonDeque.end(); ++it)
				{
					area += ::boost::geometry::area(*it);
				}

				return area;
			}

            double BoostMaskVoxelizationThread::correctForErrorAndStrictness(double volumeFraction, bool strict) const
            {
                if (strict){
                    if (volumeFraction > 1 && (volumeFraction - 1) <= errorConstant)
                    {
                        volumeFraction = 1;
                    }
                }
                else {
                    if (volumeFraction > 1){
                        volumeFraction = 1;
                    }
                    else if (volumeFraction < 0){
                        volumeFraction = 0;
                    }
                }
                return volumeFraction;
            }

        }
	}
}


#ifndef STAN_MATH_PRIM_SCAL_FUN_LOCSCALE_CONSTRAIN_HPP
#define STAN_MATH_PRIM_SCAL_FUN_LOCSCALE_CONSTRAIN_HPP

#include <boost/math/tools/promotion.hpp>
#include <stan/math/prim/scal/fun/identity_constrain.hpp>
#include <stan/math/prim/scal/fun/abs.hpp>
#include <stan/math/prim/scal/meta/size_of.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <cmath>
#include <limits>

namespace stan {
namespace math {

/**
 * Return the linearly transformed value for the specified unconstrained input
 * and specified location and scale.
 *
 *
The transform applied is
 *
 *
\f$f(x) = mu + sigma * x\f$
 *
 *
where mu is the location and sigma is the scale.
 *
 *
If the location is zero and the scale is one this
 * reduces to identity_constrain(x).
 *
 * @tparam T type of scalar
 * @tparam M type of mean
 * @tparam S type of scale
 * @param[in] x Unconstrained scalar input
 * @param[in] mu location of constrained output
 * @param[in] sigma scale of constrained output
 * @return linear transformed value correspdonding to inputs
 * @throw std::domain_error if sigma <= 0
 * @throw std::domain_error if mu is not finite
 */
template <typename T, typename M, typename S>
inline typename boost::math::tools::promote_args<T, M, S>::type
locscale_constrain(const T& x, const M& mu, const S& sigma) {
  check_finite("locscale_constrain", "location", mu);
  if (sigma == 1) {
    if (mu == 0)
      return identity_constrain(x);
    return mu + x;
  }
  check_positive_finite("locscale_constrain", "scale", sigma);
  return mu + sigma * x;
}

/**
 * Return the linearly transformed value for the specified unconstrained input
 * and specified location and scale, incrementing the specified
 * reference with the log absolute Jacobian determinant of the
 * transform.
 *
 *
The transform applied is
 *
 *
\f$f(x) = mu + sigma * x\f$
 *
 *
where mu is the location and sigma is the scale.
 *
 * If the location is zero and scale is one, this function
 * reduces to identity_constraint(x, lp).
 *
 * @tparam T type of scalar
 * @tparam M type of mean
 * @tparam S type of scale
 * @param[in] x Unconstrained scalar input
 * @param[in] mu location of constrained output
 * @param[in] sigma scale of constrained output
 * @param[in,out] lp Reference to log probability to increment.
 * @return linear transformed value corresponding to inputs
 * @throw std::domain_error if sigma <= 0
 * @throw std::domain_error if mu is not finite
 */
template <typename T, typename M, typename S>
inline typename boost::math::tools::promote_args<T, M, S>::type
locscale_constrain(const T& x, const M& mu, const S& sigma, T& lp) {
  using std::log;
  check_finite("locscale_constrain", "location", mu);
  if (sigma == 1) {
    if (mu == 0)
      return identity_constrain(x);
    return mu + x;
  }
  check_positive_finite("locscale_constrain", "scale", sigma);
  lp += size_of(x) * log(sigma);
  return mu + sigma * x;
}

}  // namespace math

}  // namespace stan

#endif


#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
#define KDL_CHAINIKSOLVERPOS_GN_HPP
/**
 \file   chainiksolverpos_lma.hpp
 \brief  computing inverse position kinematics using Levenberg-Marquardt.
*/

/**************************************************************************
    begin                : May 2012
    copyright            : (C) 2012 Erwin Aertbelien
    email                : firstname.lastname@mech.kuleuven.ac.be

 History (only major changes)( AUTHOR-Description ) :

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

#include "chainiksolver.hpp"
#include "chain.hpp"
#include <Eigen/Dense>

namespace KDL
{

/**
 * \brief Solver for the inverse position kinematics that uses Levenberg-Marquardt.
 *
 * The robustness and speed of this solver is improved in several ways:
 *   - by using a Levenberg-Marquardt method that automatically adapts the damping when
 *     computing the inverse damped least squares inverse velocity kinematics.
 *   - by using an internal implementation of forward position kinematics and the
 *     Jacobian kinematics.  This implementation is more numerically robust,
 *     is able to cache previous computations, and implements an \f$ \mathcal{O}(N) \f$
 *     algorithm for the computation of the Jacobian (with \f$N\f$, the number of joints, and for
 *     a fixed size task space).
 *   - by providing a way to specify the weights in task space, you can weigh rotations wrt translations.
 *     This is important e.g. to specify that rotations do not matter for the problem at hand, or to
 *     specify how important you judge rotations w.r.t. translations, typically in S.I.-units, ([m],[rad]),
 *     the rotations are over-specified, this can be avoided using the weight matrix. Weights also
 *     make the solver more robust .
 *   - only the constructors call memory allocation.
 *
 * De general principles behind the optimisation is inspired on:
 *   Jorge Nocedal, Stephen J. Wright, Numerical Optimization,Springer-Verlag New York, 1999.

 * \ingroup KinematicFamily
 */
class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
{
private:
	typedef double ScalarType;
    typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
    typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> VectorXq;
public:

    static const int E_GRADIENT_JOINTS_TOO_SMALL = -100;
    static const int E_INCREMENT_JOINTS_TOO_SMALL = -101;

    /**
	 * \brief constructs an ChainIkSolverPos_LMA solver.
	 *
	 * The default parameters are choosen to be applicable to industrial-size robots
	 * (e.g. 0.5 to 3 meters range in task space), with an accuracy that is more then
	 * sufficient for typical industrial applications.
	 *
	 * Weights are applied in task space, i.e. the kinematic solver minimizes:
	 * \f$ E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \f$, with \f$\mathbf{L}\f$ a diagonal matrix.
	 *
	 * \param _chain specifies the kinematic chain.
	 * \param _L specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector.
	 * \param _eps specifies the desired accuracy in task space; after weighing with
	 *        the weight matrix, it is applied on \f$E\f$.
	 * \param _maxiter specifies the maximum number of iterations.
	 * \param _eps_joints specifies that the algorithm has to stop when the computed joint angle increments are
	 *        smaller then _eps_joints.  This is to avoid unnecessary computations up to _maxiter when the joint angle
	 *        increments are so small that they effectively (in floating point) do not change the joint angles any more.  The default
	 *        is a few digits above numerical accuracy.
     */
    ChainIkSolverPos_LMA(
    		const KDL::Chain& _chain,
    		const Eigen::Matrix<double,6,1>& _L,
    		double _eps=1E-5,
    		int _maxiter=500,
    		double _eps_joints=1E-15
    );

    /**
     * \brief identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix.
     *
     *  \f$\mathbf{L} = \mathrm{diag}\left( \begin{bmatrix} 1 & 1 & 1 & 0.01 & 0.01 & 0.01 \end{bmatrix} \right) \f$.
     */
    ChainIkSolverPos_LMA(
    		const KDL::Chain& _chain,
    		double _eps=1E-5,
    		int _maxiter=500,
    		double _eps_joints=1E-15
    );

    /**
     * \brief computes the inverse position kinematics.
     *
     * \param q_init initial joint position.
     * \param T_base_goal goal position expressed with respect to the robot base.
     * \param q_out  joint position that achieves the specified goal position (if successful).
     * \return E_NOERROR if successful,
     *         E_GRADIENT_JOINTS_TOO_SMALL the gradient of \f$ E \f$ towards the joints is to small,
     *         E_INCREMENT_JOINTS_TOO_SMALL if joint position increments are to small,
     *         E_MAX_ITER_EXCEEDED if number of iterations is exceeded.
     */
    virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);

    /**
     * \brief destructor.
     */
    virtual ~ChainIkSolverPos_LMA();

    /**
     * \brief for internal use only.
     *
     * Only exposed for test and diagnostic purposes.
     */
    void compute_fwdpos(const VectorXq& q);

    /**
     * \brief for internal use only.
     * Only exposed for test and diagnostic purposes.
     * compute_fwdpos(q) should always have been called before.
     */
    void compute_jacobian(const VectorXq& q);

    /**
     * \brief for internal use only.
     * Only exposed for test and diagnostic purposes.
     */
    void display_jac(const KDL::JntArray& jval);

    /// @copydoc KDL::SolverI::strError()
    virtual const char* strError(const int error) const;

private:
    const KDL::Chain& chain;
    unsigned int nj;
    unsigned int ns;

public:

    /**
     * \brief contains the last number of  iterations for an execution of CartToJnt.
     */
    int lastNrOfIter;

    /**
     * \brief contains the last value for \f$ E \f$ after an execution of CartToJnt.
     */
    double lastDifference;

    /**
     * \brief contains the last value for the (unweighted) translational difference after an execution of CartToJnt.
     */
    double lastTransDiff;

    /**
     * \brief contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.
     */
    double lastRotDiff;

    /**
     * \brief contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt.
     */
    VectorXq lastSV;

    /**
     * \brief for internal use only.
     *
     * contains the last value for the Jacobian after an execution of compute_jacobian.
     */
    MatrixXq jac;

    /**
     * \brief for internal use only.
     *
     * contains the gradient of the error criterion after an execution of CartToJnt.
     */
    VectorXq grad;
    /**
     * \brief for internal use only.
     *
     * contains the last value for the position of the tip of the robot (head) with respect to the base, after an execution of compute_jacobian.
     */
    KDL::Frame T_base_head;

    /**
     * \brief display information on each iteration step to the console.
     */
    bool display_information;
private:
    // additional specification of the inverse position kinematics problem:
    unsigned int maxiter;
    double eps;
    double eps_joints;
    Eigen::Matrix<ScalarType,6,1> L;

    // state of compute_fwdpos and compute_jacobian:
    std::vector<KDL::Frame> T_base_jointroot;
    std::vector<KDL::Frame> T_base_jointtip;
					// need 2 vectors because of the somewhat strange definition of segment.hpp
					// you could also recompute jointtip out of jointroot,
    				// but then you'll need more expensive cos/sin functions.

    // the following are state of CartToJnt that is pre-allocated:

    VectorXq q;
    MatrixXq A;
    VectorXq tmp;
    Eigen::LDLT<MatrixXq> ldlt;
    Eigen::JacobiSVD<MatrixXq> svd;
    VectorXq diffq;
    VectorXq q_new;
    VectorXq original_Aii;
};

}; // namespace KDL

#endif


#include <iostream>
#include <cassert>
#include <cstdlib>
#include <ctime>
#include <cmath>
#include <random>
#include <armadillo>
#include "port_authority.hpp"
#include "mpi.h"

using namespace std;
using namespace pauth;
using namespace arma;

int main() {

  int taskid, numtasks;
  MPI_Init(nullptr, nullptr);
  MPI_Comm_size(MPI_COMM_WORLD, &numtasks);
  MPI_Comm_rank(MPI_COMM_WORLD, &taskid);

  srand(time(NULL));
  const auto delta_max = static_cast<double>( (rand() % 200 + 50) / 100.0 );
  const auto spring_const = static_cast<double>( (rand() % 1990 + 10) / 500.0 );
  const auto T = static_cast<double>( (rand() % 200000 + 10) / 1000.0 );
  const auto p0x = static_cast<double>( (rand() % 100 - 200) / 50.0 );
  const auto p0y = static_cast<double>( (rand() % 100 - 200) / 50.0 );
  const auto p0z = static_cast<double>( (rand() % 100 - 200) / 50.0 );
  const auto E0x = static_cast<double>( (rand() % 100 - 200) / 20.0 );
  const auto E0y = static_cast<double>( (rand() % 100 - 200) / 20.0 );
  const auto E0z = static_cast<double>( (rand() % 100 - 200) / 20.0 );
  const unsigned nsteps = 20000000;

  if (taskid == 0) {
    cout << "3D linear dipole test started.\n";
    cout << '\n';
    cout << "delta_max = " << delta_max << '\n';
    cout << "k         = " << spring_const << '\n';
    cout << "T         = " << T << '\n';
    cout << "p0x       = " << p0x << '\n';
    cout << "p0y       = " << p0y << '\n';
    cout << "p0z       = " << p0z << '\n';
    cout << "E0x       = " << E0x << '\n';
    cout << "E0y       = " << E0y << '\n';
    cout << "E0z       = " << E0z << '\n';
    cout << '\n';
  }

  const double dx = 0.1;
  const double kB = 1.0;
  const molecular_id id = molecular_id::Test1;
  const size_t N = 1;
  const size_t D = 3;
  const double L = 1.0;
  const metric m = euclidean;
  const bc boundary = no_bc;
  const mat inv_chi = arma::eye(3, 3);
  const vec E0({E0x, E0y, E0z});
  const vec pequil = inv_chi.i() * E0;
  dipole_strain_linear_3d_potential strain_pot(inv_chi);
  dipole_electric_potential electric_pot({E0x, E0y, E0z}, {0, 1, 2});

  metropolis sim(id, N, D, L, continuous_trial_move(delta_max),
                 {&strain_pot, &electric_pot}, T, kB, m, boundary,
                 metropolis_acc, hardware_entropy_seed_gen, true);
  sim.set_positions(pequil, 0);
  const double u0 = accessors::U(sim);
  sim.set_positions({p0x, p0y, p0z}, 0);

  metropolis_suite msuite(sim, 0, 1, info_lvl_flag::QUIET);

  msuite.add_variable_to_average("px", [](const metropolis &sim) {
    return sim.positions()(0, 0);
  });
  msuite.add_variable_to_average("py", [](const metropolis &sim) {
    return sim.positions()(1, 0);
  });
  msuite.add_variable_to_average("pz", [](const metropolis &sim) {
    return sim.positions()(2, 0);
  });
  msuite.add_variable_to_average("px^2", [](const metropolis &sim) {
    auto x = sim.positions()(0, 0);
    return x*x;
  });
  msuite.add_variable_to_average("py^2", [](const metropolis &sim) {
    auto y = sim.positions()(1, 0);
    return y*y;
  });
  msuite.add_variable_to_average("pz^2", [](const metropolis &sim) {
    auto z = sim.positions()(2, 0);
    return z*z;
  });
  msuite.add_variable_to_average("U", accessors::U);
  msuite.add_variable_to_average("delta(x - x0)", [=](const metropolis &sim) {
    for (unsigned i = 0; i < 3; ++i) {
      const double pi = sim.positions()(i, 0);
      if (pi < pequil(i) - dx || pi > pequil(i) + dx) return 0;
    }
    return 1;
  });

  msuite.simulate(nsteps);

  if(taskid == 0) {

    auto averages = msuite.averages();

    const double exp_px = averages["px"];
    const double exp_py = averages["py"];
    const double exp_pz = averages["pz"];

    const double exp_pxsq = averages["px^2"];
    const double exp_pysq = averages["py^2"];
    const double exp_pzsq = averages["pz^2"];

    const double exp_E = averages["U"];

    vec eigval;
    mat eigvec;
    eig_sym(eigval, eigvec, inv_chi);

    vec Ev = eigvec * E0;
    double Z_an = 1.0;
    for(unsigned i = 0; i < 3; ++i)
      Z_an *= sqrt(2 * M_PI * kB * T / eigval(i)) *
              exp(Ev(i)*Ev(i) / (2 * eigval(i) * kB * T));

    const auto exp_pxsq_an = (kB * T / eigval(0));
    const auto exp_pysq_an = (kB * T / eigval(1));
    const auto exp_pzsq_an = (kB * T / eigval(2));

    const auto exp_E_an = 3 * kB * T / 2.0;

    cout << "exp_px    =   " << exp_px << " ?= " << pequil(0) << '\n';
    assert(abs((exp_px - pequil(0)) / pequil(0)) < 5e-2);
    cout << "exp_py    =   " << exp_py << " ?= " << pequil(1) << '\n';
    assert(abs((exp_py - pequil(1)) / pequil(1)) < 5e-2);
    cout << "exp_pz    =   " << exp_pz << " ?= " << pequil(2) << '\n';
    assert(abs((exp_pz - pequil(2)) / pequil(2)) < 5e-2);

    cout << "exp_px^2  =   " << exp_pxsq << " ?= " << exp_pxsq_an << '\n';
    assert(abs((exp_pxsq - exp_pxsq_an) / exp_pxsq_an) < 1e-2);
    cout << "exp_py^2  =   " << exp_pysq << " ?= " << exp_pysq_an << '\n';
    assert(abs((exp_pysq - exp_pysq_an) / exp_pysq_an) < 1e-2);
    cout << "exp_pz^2  =   " << exp_pzsq << " ?= " << exp_pzsq_an << '\n';
    assert(abs((exp_pzsq - exp_pzsq_an) / exp_pzsq_an) < 1e-2);

    cout << "exp_E    =   " << exp_E << " ?= " << exp_E_an << '\n';
    assert(abs((exp_E - exp_E_an) / exp_E_an) < 1e-2);
    cout << "Z        =   "
         << (2.0 * dx * exp(-u0 / (kB * T)) / averages["delta(x - x0)"])
         << " ?= " << Z_an << '\n';
    assert(abs( ((pow(2.0 * dx, 3) * exp(-u0 / (kB * T)) / averages["delta(x - x0)"]) -
                Z_an) / Z_an ) < 1e-2);
    cout << "---------------------------------------------\n";

  }

  if(taskid == 0) cout << "3D linear dipole test passed.\n";

  MPI_Finalize();

  return 0;

}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Core>

#include <iostream>

double summation(const Eigen::MatrixXd& X, const Eigen::MatrixXd& Y) {
#ifndef _OPENMP
    std::cerr << "Warning: OpenMP is disabled." << std::endl;
#endif
    double s = 0;
    size_t i, j;
#ifdef _OPENMP
#pragma omp parallel for private(j) reduction(+:s)
#endif
    for (i = 0; i < X.rows(); ++i) {
        for (j = 0; j < Y.rows(); ++j) {
            if (0 < X(i) * Y(j)) {
                s += std::log10((X(i) + Y(j)) * (X(i) + Y(j))) + std::sqrt(X(i) * Y(j));
            }
        }
    }
    return s;
}

PYBIND11_MODULE(openmp_cpp, m) {
    m.def("summation", &summation);
}


/// \file nuslam.hpp
/// \brief A package for Extended Kalman Filter Slam implementation.

#ifndef NUSLAM_INCLUDE_GUARD_HPP
#define NUSLAM_INCLUDE_GUARD_HPP

#include "rigid2d/diff_drive.hpp"
#include "rigid2d/rigid2d.hpp"

#include <armadillo>
#include <cmath>
#include <iosfwd>
#include <map>
#include <utility>
#include <vector>

namespace nuslam
{
  using rigid2d::Config2D;
  using rigid2d::Twist2D;

  /// \brief A Mesurement vector Vector
  struct Measurement
  {
    /// \param r - the distance to landmark
    double r;
    /// \param phi - the relative bearing of landmark
    double phi;
    /// \param id - the measured landmark id
    int id;

    /// \brief create zero-measurement
    Measurement();

    /// \brief create a measurement with r,phi inputs
    /// \param x_ - x input of the measurement
    /// \param y_ - y input of the measurement
    /// \param id_ - the measured landmark id
    explicit Measurement(double x_, double y_, int id_);

    /// \brief create a measurement vector
    /// \return the measurement vector
    arma::mat compute_z();
  };

  /// \brief initialize the guess of the robot's state and covariance matrix.
  /// Start with a guess for the robot state (0, 0, 0) and zero covariance matrix.
  class EKF
  {
  private:
    arma::mat q_t = arma::mat(3, 1);
    arma::mat m_t;

    // arma::mat xi = arma::mat(3, 1);
    // arma::mat cov = arma::mat(3, 3);

    arma::mat Q_mat = arma::mat(3, 3);
    arma::mat R_mat = arma::mat(2, 2);

    arma::mat xi_predict = arma::mat(3, 1);
    arma::mat cov_predict = arma::mat(3, 3);

    std::map<int, int> id2landmark;

  public:
    /// \brief initialize the combined state vector.
    /// Start with a guess for the robot state (0, 0, 0) and zero e map state.
    EKF();

    /// \brief updates the landmarks matrix and landmark covariance.
    /// \param meas - the measured landmark.
    void add_new_measurement(const Measurement &meas);

    /// \brief check if the measured landmark exists in the landmark dictionary.
    /// \param landmark_id - the id of the measured landmark.
    /// \return bool - is the measured exists?
    bool check_landmarks(const int landmark_id);

    /// \brief updates the landmarks matrix and landmark covariance.
    /// \param meas - the measured landmark.
    void update_landmark(const Measurement &meas);

    /// \brief gets the state q_t.
    /// \param twist - the twist of the robot.
    /// \return the updated state q_t
    arma::mat get_new_state(const Twist2D &twist);

    /// \brief gets derivative of g with respect to the state ξ.
    /// \param twist - the twist of the robot.
    arma::mat get_transition(const Twist2D &twist);

    /// \brief predicts the next step - finds the estimated state and covariance.
    /// \param twist - the twist of the robot.
    void predict(const Twist2D &twist);

    /// \brief compute the measurement h for range and bearing to landmark.
    /// \param index - the index of the measured landmark in the landmark matrix.
    /// \return the h matrix.
    arma::mat get_h(int index);

    /// \brief compute the derivative of h with respect to the state.
    /// \param index - the index of the measured landmark in the landmark matrix.
    /// \return the H matrix.
    arma::mat get_H(int index);

    /// \brief updates the next step.
    /// \param meas - the measured landmark.
    void update(std::vector<Measurement> meas);

    /// \brief add a new zero row and column to a matrix (used when adding new
    /// landmark). \param mat - the current matrix. \return the updated matrix.
    arma::mat update_matrix_size(arma::mat mat);

    /// \brief run the Extended Kalman Filter algorithm.
    /// \param twist - the twist of the robot.
    /// \param meas -  the current measurement.
    void run_ekf(const Twist2D &twist, const std::vector<Measurement> &meas);

    /// \brief output the robot state
    /// \return the robot state
    arma::mat output_state();

    /// \brief output the map state
    /// \return the map state
    arma::mat output_map_state();
  };
} // namespace nuslam

#endif


//
//  matrix_boost.hpp
//  jerome
//
//  Created by Anton Leuski on 5/13/16.
//  Copyright © 2016 Anton Leuski & ICT/USC. All rights reserved.
//
//  This file is part of Jerome.
//
//  Licensed under the Apache License, Version 2.0 (the "License");
//  you may not use this file except in compliance with the License.
//  You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
//

#ifndef __jerome_type_matrix_boost_hpp__
#define __jerome_type_matrix_boost_hpp__

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wshorten-64-to-32"

#define BOOST_UBLAS_MOVE_SEMANTICS

#include <boost/numeric/ublas/vector_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/operation.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/range/algorithm/max_element.hpp>

#pragma clang diagnostic pop

namespace boost {
  namespace numeric {
    namespace ublas {

      // (op v) [i] = op( v [i] )
      template<class OP, class E>
      BOOST_UBLAS_INLINE
      typename boost::numeric::ublas::vector_unary_traits<E, OP>::result_type
      apply_to_all (const boost::numeric::ublas::vector_expression<E> &e, const OP& op = OP() ) {
        typedef typename boost::numeric::ublas::vector_unary_traits<E, OP>::expression_type expression_type;
        return expression_type (e ());
      }

      template<class T>
      struct scalar_log:
      public scalar_unary_functor<T> {
        typedef typename scalar_unary_functor<T>::argument_type argument_type;
        typedef typename scalar_unary_functor<T>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument_type t) {
          return std::log(t);
        }
      };

      template<class T>
      struct scalar_log_plus1:
      public scalar_unary_functor<T> {
        typedef typename scalar_unary_functor<T>::argument_type argument_type;
        typedef typename scalar_unary_functor<T>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument_type t) {
          return std::log(t+1.0);
        }
      };

      template<class T>
      struct scalar_exp:
      public scalar_unary_functor<T> {
        typedef typename scalar_unary_functor<T>::argument_type argument_type;
        typedef typename scalar_unary_functor<T>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument_type t) {
          return std::exp(t);
        }
      };

      template<class T1, class T2>
      struct scalar_pow:
      public scalar_binary_functor<T1, T2> {
        typedef typename scalar_binary_functor<T1, T2>::argument1_type argument1_type;
        typedef typename scalar_binary_functor<T1, T2>::argument2_type argument2_type;
        typedef typename scalar_binary_functor<T1, T2>::result_type result_type;

        static BOOST_UBLAS_INLINE
        result_type apply (argument1_type t1, argument2_type t2) {
          return std::pow((double)t1,(double)t2);
        }
      };

      // (log v) [i] = log (v [i])
      template<class E>
      BOOST_UBLAS_INLINE
      typename vector_unary_traits<E, scalar_log<typename E::value_type> >::result_type
      log (const vector_expression<E> &e) {
        typedef typename vector_unary_traits<E, scalar_log<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (log v) [i] = log (v [i])
      template<class E>
      BOOST_UBLAS_INLINE
      typename vector_unary_traits<E, scalar_log_plus1<typename E::value_type> >::result_type
      log_plus1 (const vector_expression<E> &e) {
        typedef typename vector_unary_traits<E, scalar_log_plus1<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (exp v) [i] = exp (v [i])
      template<class E>
      BOOST_UBLAS_INLINE
      typename vector_unary_traits<E, scalar_exp<typename E::value_type> >::result_type
      exp (const vector_expression<E> &e) {
        typedef typename vector_unary_traits<E, scalar_exp<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (log m) [i] [j] = log (m [i] [j])
      template<class E>
      BOOST_UBLAS_INLINE
      typename matrix_unary1_traits<E, scalar_log<typename E::value_type> >::result_type
      log (const matrix_expression<E> &e) {
        typedef typename matrix_unary1_traits<E, scalar_log<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (exp m) [i] [j] = exp (m [i] [j])
      template<class E>
      BOOST_UBLAS_INLINE
      typename matrix_unary1_traits<E, scalar_exp<typename E::value_type> >::result_type
      exp (const matrix_expression<E> &e) {
        typedef typename matrix_unary1_traits<E, scalar_exp<typename E::value_type> >::expression_type expression_type;
        return expression_type (e ());
      }

      // (pow(v, t) ) [i] = pow(v [i], t)
      template<class E1, class T2>
      BOOST_UBLAS_INLINE
      typename enable_if< is_convertible<T2, typename E1::value_type >,
      typename vector_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::result_type
      >::type
      element_pow (const vector_expression<E1> &e1,
                   const T2 &e2) {
        typedef typename vector_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::expression_type expression_type;
        return expression_type (e1 (), e2);
      }

      // (pow(m, t)) [i] [j] = pow(m [i] [j], t)
      template<class E1, class T2>
      BOOST_UBLAS_INLINE
      typename enable_if< is_convertible<T2, typename E1::value_type>,
      typename matrix_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::result_type
      >::type
      element_pow (const matrix_expression<E1> &e1,
                   const T2 &e2) {
        typedef typename matrix_binary_scalar2_traits<E1, const T2, scalar_pow<typename E1::value_type, T2> >::expression_type expression_type;
        return expression_type (e1 (), e2);
      }

      template<class M>
      BOOST_UBLAS_INLINE
      matrix_vector_slice<M> diag(M &data) {
        const typename M::size_type s = std::min(data.size1(),data.size2());
        return matrix_vector_slice<M>(data, slice(0,1,s), slice(0,1,s));
      };

      template<class M>
      BOOST_UBLAS_INLINE
      const matrix_vector_slice<const M> diag(const M &data) {
        const typename M::size_type s = std::min(data.size1(),data.size2());
        return matrix_vector_slice<const M>(data, slice(0,1,s), slice(0,1,s));
      };

    }
  }
}

namespace jerome {

  namespace ublas = boost::numeric::ublas;

  template<class T> using Vector = ublas::vector<T>;
  // sparse array
  template<class T> using SparseVector = ublas::compressed_vector<T>;

  typedef ublas::matrix<WeightValue, ublas::column_major>     WeightMatrix;
  typedef ublas::matrix_row<WeightMatrix>                     WeightMatrixRow;
  typedef ublas::matrix_row<const WeightMatrix>               WeightMatrixConstRow;
  typedef ublas::matrix_column<WeightMatrix>                  WeightMatrixColumn;
  typedef ublas::matrix_column<const WeightMatrix>            WeightMatrixConstColumn;
  typedef ublas::vector<WeightMatrix::value_type>             WeightVector;
  typedef ublas::compressed_vector<WeightMatrix::value_type>	SparseWeightVector;

  typedef ublas::symmetric_matrix<WeightMatrix::value_type, ublas::upper>			SymmetricWeightMatrix;
  typedef ublas::matrix_range<SymmetricWeightMatrix>					SymmetricWeightMatrixRange;

  template <class M>
  struct traits {
    typedef typename M::size_type size_type;
    typedef typename M::value_type value_type;
  };

  struct MatrixSize {
    traits<WeightMatrix>::size_type rowCount;
    traits<WeightMatrix>::size_type columnCount;

    MatrixSize() = default;

    MatrixSize(const WeightMatrix& m)
    : rowCount(m.size1()), columnCount(m.size2())
    {}
  };

  typedef ublas::scalar_matrix<WeightMatrix::value_type>
  WeightMatrixScalar;

  typedef ublas::scalar_vector<WeightMatrix::value_type>
  WeightVectorScalar;

  inline WeightMatrixScalar WeightMatrixZero(const MatrixSize& size)
  {
    return WeightMatrixScalar(size.rowCount, size.columnCount, 0);
  }

  inline WeightMatrixScalar WeightMatrixOnes(const MatrixSize& size)
  {
    return WeightMatrixScalar(size.rowCount, size.columnCount, 1);
  }

  inline WeightVectorScalar WeightVectorZero(traits<WeightVector>::size_type size)
  {
    return WeightVectorScalar(size, 0);
  }

  inline WeightVectorScalar WeightVectorOnes(traits<WeightVector>::size_type size)
  {
    return WeightVectorScalar(size, 1);
  }

  template <typename M, typename C>
  inline auto column(M&& m, C c)
  -> decltype(ublas::matrix_column<typename std::remove_reference<M>::type>(std::forward<M>(m), c))
  {
    return ublas::matrix_column<typename std::remove_reference<M>::type>(std::forward<M>(m), c);
  }

  template <typename M, typename C>
  inline auto row(M&& m, C c)
  -> decltype(ublas::matrix_row<typename std::remove_reference<M>::type>(std::forward<M>(m), c))
  {
    return ublas::matrix_row<typename std::remove_reference<M>::type>(std::forward<M>(m), c);
  }

  template <typename M>
  inline auto sum(const M& m)
  -> decltype(ublas::sum(m))
  {
    return ublas::sum(m);
  }

  template <typename A, typename B>
  inline auto prod(const A& a, const B& b)
  -> decltype(ublas::prod(a, b))
  {
    return ublas::prod(a, b);
  }

  template <typename A, typename B>
  inline auto outer_prod(const A& a, const B& b)
  -> decltype(ublas::outer_prod(a, b))
  {
    return ublas::outer_prod(a, b);
  }

  template <typename A, typename B>
  inline auto element_pow(const A& a, B b)
  -> decltype(ublas::element_pow(a, b))
  {
    return ublas::element_pow(a, b);
  }

  template <typename A, typename B>
  inline auto element_prod(const A& a, const B& b)
  -> decltype(ublas::element_prod(a, b))
  {
    return ublas::element_prod(a, b);
  }

  template <typename M>
  inline SparseWeightVector sparse_log_plus1(const M& m)
  {
    SparseWeightVector result(m.size());
    for (auto a_it = m.begin(), a_end = m.end(); a_it != a_end; ++a_it) {
      result(a_it.index()) = std::log(1.0+ (*a_it));
    }

    return result;
  }

  template <typename M>
  inline auto max_element(const M& m)
  -> decltype(*boost::max_element(m))
  {
    return *boost::max_element(m);
  }

  template <typename M, typename S>
  inline void resize(M&& m, const S& s)
  {
    m.resize(s);
  }

  template <typename V>
  inline void append_vector_to_vector(const V& src, V&& dst, const typename traits<V>::size_type& off) {
    for(auto i = src.begin(), e = src.end(); i != e; ++i) {
      dst[i.index()+off] = *i;
    }
  }

  template <typename V>
  inline void append_sparse_vector_to_sparse_vector(const V& src, V&& dst,
                                                    const typename traits<V>::size_type& off) {
    for(auto i = src.begin(), e = src.end(); i != e; ++i) {
      dst[i.index()+off] = *i;
    }
  }

  template <typename X, typename I, typename V>
  inline auto set_value_at_index_in_vector(const X& x, const I& i, V&& v)
  -> decltype(v[i])
  {
    return v[i] = x;
  }

  template <typename I, typename V>
  inline auto increment_value_at_index_in_vector(const I& i, V&& v)
  -> decltype(v[i])
  {
    return v[i] += 1;
  }

  template <typename T, typename M>
  inline auto matrix_cast(M&& m)
  -> decltype(m)
  {
    return m;
  }

  template <typename V, typename OP>
  inline void for_each(const V& v, OP&& op) {
    for (auto E = v.begin(), __end = v.end(); E != __end; ++E) {
      op(E.index(), *E);
    }
  }

  // dense vectors only!!!!
  template <typename V, typename Q>
  inline auto element_div(const V& v, const Q& q)
  -> decltype(ublas::element_div(v, q))
  {
    // ubleas::element_div causes divide by zero. I guess, it tries to compute
    // 1/|Dl| for each values v even for 0 ones....
    // return element_div(term.tfs() * lambda().value(),
    // field.documentLengths());
    return ublas::element_div(v, q);
  }

  template <typename A, typename B, typename C>
  inline void sparse_outer_prod_add_to(const A& a, const B& b, C&& c) {
    for (auto a_it = a.begin(), a_end = a.end(); a_it != a_end; ++a_it) {
      const auto a_index = a_it.index();
      const auto a_value = *a_it;
      for (auto b_it = b.begin(), b_end = b.end(); b_it != b_end; ++b_it) {
        c(a_index, b_it.index()) += a_value * (*b_it);
      }
    }
  }

  template <typename A, typename B, typename C>
  inline void sparse_scale_add_to(const A& a, const B& b, C&& c) {
    for (auto a_it = a.begin(), a_end = a.end(); a_it != a_end; ++a_it) {
      c(a_it.index()) += (*a_it) * b;
    }
  }

  // boost UBLAS is horrible at taking advantage of sparse vectors.
  // Every element-wise operation you apply to sparse vectors is applied
  // to every vector element including zero ones. So we have to implement
  // them ourselves.
  template <typename A, typename B, typename C>
  inline SparseWeightVector sparse_scale_div_by(const A& a, const B& b, const C& c) {
    SparseWeightVector result(a.size());
    for (auto a_it = a.begin(), a_end = a.end(); a_it != a_end; ++a_it) {
      result(a_it.index()) = (*a_it) * b / c(a_it.index());
    }
    return result;
  }

}

namespace boost { namespace numeric { namespace ublas {

  std::ostream& operator << (std::ostream& outs, const jerome::WeightMatrix& obj);
  std::ostream& operator << (std::ostream& outs, const jerome::SymmetricWeightMatrix& obj);
  std::ostream& operator << (std::ostream& outs, const jerome::SymmetricWeightMatrixRange& obj);
  std::ostream& operator << (std::ostream& outs, const jerome::WeightVector& obj);

  // fascinating shit: when looking for an overloaded operator, compiler looks into the
  // namespace of the object parameter. Aparently, if you have a typedef, the compiler
  // ignores the namespace of the typedef and goes directly for the namespace of the class used
  // in the typedef. in this case it's boost:numeric::ublas.

  // if you do not put the operator in the namespace, the compiler will not find it.
}}}

#endif // __jerome_type_matrix_boost_hpp__


/*

CandyPoker
https://github.com/sweeterthancandy/CandyPoker

MIT License

Copyright (c) 2019 Gerry Candy

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 "ps/base/algorithm.h"
#include "ps/base/rank_hasher.h"

#include <boost/range/algorithm.hpp>
#include <map>

namespace ps{

/*

   The idea of this is to 2-tuple of permutations (C,S),
   so for a vector of n-players, we apply the permutation
                        C : c -> c',
   ie
                (p1,p2,p3) -> (p1', p2', p3'),
   and then apply the suit permutation S : s -> s'
                (p1', p2', p3') -> (p1'', p2'', p3''),
   so that given and set of players, we can can an injective
   mapping to a small subset. Ei, AA vs KK == KK vs AA
   with the player orientation swapped around.
        Noting that we can complete ignore any permutation
   of suits for all practical purposes, as always just want
   to find a suit permutation so that it's injective

                    eval(AA,KK vs AA,KK)

        =           eval(AA vs AA ) * w0 +
                    eval(AA vs KK ) * w1 +
                    eval(KK vs AA ) * w2 +
                    eval(KK vs KK ) * w3

        =           eval(AA vs AA ) * w0 +
                    eval(AA vs KK ) * w1 +
            Inverse(eval(AA vs KK )) * w2 +
                    eval(KK vs KK ) * w3

 */
struct rank_info{
        size_t index;
        holdem_id hid;
        rank_hasher::rank_hash_t rank_hash;
        bool is_suited;
        friend bool operator==(rank_info const& l, rank_info const& r)noexcept{
                return ( ! ( l < r ) ) && ( ! ( r < l ) );
        }
        friend bool operator!=(rank_info const& l, rank_info const& r)noexcept{
                return ( ! (l == r) );
        }
        friend bool operator<(rank_info const& l, rank_info const& r)noexcept{
                if( l.rank_hash != r.rank_hash ){
                        return l.rank_hash <  r.rank_hash;
                }
                return l.is_suited <  r.is_suited;
        }
};
struct suit_perm_info{
        std::array<suit_id, 4> suit_perm;
        std::vector<int> player_perm;
        holdem_hand_vector hv;
        size_t card_mask;
        friend bool operator<(suit_perm_info const& l, suit_perm_info const& r)noexcept{
                #if 0
                if( l.card_mask != r.card_mask )
                        return l.card_mask < r.card_mask;
                #endif
                return l.hv < r.hv;
        }
};

std::tuple<
        std::vector<int>,
        std::vector<holdem_id>
> permutate_for_the_better( std::vector<holdem_id> const& players )
{
        enum{ Debug = false };

        std::vector< rank_info > ri;
        for(size_t idx=0;idx!=players.size();++idx){
                auto h =  holdem_hand_decl::get( players[idx] ) ;
                auto rank_hash = rank_hasher::create_from_cards(h.first(), h.second());
                ri.emplace_back(rank_info{idx, players[idx], rank_hash, h.is_suited()});
        }
        boost::sort( ri );

        std::vector< std::vector<rank_info> > level_sets;

        level_sets.emplace_back();
        level_sets.back().push_back( ri[0] );
        for(size_t idx=1; idx < ri.size();++idx){
                if( level_sets.back().front() == ri[idx]){
                        // same set
                        level_sets.back().push_back(ri[idx]);
                } else {
                        // start new set
                        level_sets.emplace_back();
                        level_sets.back().push_back(ri[idx]);
                }
        }

        if( Debug ){
                PS_LOG(trace) << "begin level_sets";
                for(auto const& ls : level_sets ){
                        holdem_hand_vector hv;
                        for(auto const& _ : ls){
                                hv.push_back(_.hid);
                        }
                        PS_LOG(trace) << "    -" << hv;
                }
                PS_LOG(trace) << "end   level_sets";
        }

        std::vector< std::vector< rank_info > > rank_permutations;
        rank_permutations.emplace_back();

        for(auto& ls : level_sets){
                if( ls.size() == 1 ){
                        for(auto& _ : rank_permutations ){
                                _.push_back(ls.back());
                        }
                } else {

                        decltype(rank_permutations) next;

                        // should be sorted anyway
                        boost::sort(ls);
                        do{
                                for(auto rp : rank_permutations ){
                                        for(auto const& item : ls ){
                                                rp.push_back(item);
                                        }
                                        next.push_back(rp);
                                }
                        }while(boost::next_permutation(ls, [](auto const& l, auto const& r){ return l.index < r.index; }));
                        rank_permutations = std::move(next);
                }
        }

        if( Debug ){
                PS_LOG(trace) << "rank_permutations.size() => " << rank_permutations.size();
                PS_LOG(trace) << "begin rank_permutations";
                for(auto const& rp : rank_permutations ){
                        holdem_hand_vector hv;
                        for(auto const& _ : rp){
                                hv.push_back(_.hid);
                        }
                        PS_LOG(trace) << "    -" << hv;
                }
                PS_LOG(trace) << "end   rank_permutations";
        }

        std::vector<suit_perm_info> suit_perm_vec;

        std::array<suit_id, 4> suits = { 0, 1, 2, 3};
        for(auto const& rp : rank_permutations ){
                boost::sort(suits);
                do{
                        std::vector<int> player_perm;
                        holdem_hand_vector hv;
                        size_t mask = 0;

                        for(auto const& _ : rp){

                                auto h =  holdem_hand_decl::get( _.hid ) ;

                                rank_id r0 = h.first().rank();
                                suit_id s0 = h.first().suit();
                                rank_id r1 = h.second().rank();
                                suit_id s1 = h.second().suit();

                                suit_id m0 = suits[s0];
                                suit_id m1 = suits[s1];

                                holdem_id mhid = holdem_hand_decl::make_id(r0, m0, r1, m1);

                                mask |= static_cast<size_t>(1) << mhid;

                                player_perm.push_back(_.index);
                                hv.push_back(mhid);
                        }

                        suit_perm_vec.push_back(suit_perm_info{suits, std::move(player_perm),
                                                               hv, mask});

                }while(boost::next_permutation(suits));
        }

        boost::sort(suit_perm_vec);
        if( Debug ){
                PS_LOG(trace) << "begin rank_permutations";
                for(auto const& sp : suit_perm_vec ){
                        PS_LOG(trace) << "    -" << sp.hv;
                }
                PS_LOG(trace) << "end   rank_permutations";
                PS_LOG(trace) << "hv = " << suit_perm_vec.front().hv;
        }

        return { suit_perm_vec.front().player_perm, suit_perm_vec.front().hv };

}

#if 0
std::tuple<
        std::vector<int>,
        std::vector<holdem_id>
> permutate_for_the_better( std::vector<holdem_id> const& players ){
        // first create vector of n, and token_n = hh_n
        //      (0,hh_0), (1,hh_1), ... (n,hh_n),
        // where first h is greater handk the second h
        std::vector< std::tuple< size_t, std::string> > player_perm;
        for(size_t i=0;i!=players.size();++i){
                auto h =  holdem_hand_decl::get( players[i] ) ;
                player_perm.emplace_back(i, h.first().rank().to_string() +
                                            h.second().rank().to_string() );
        }
        // sort it by the token
        boost::sort(player_perm, [](auto const& left, auto const& right){
                return std::get<1>(left) < std::get<1>(right);
        });

        // new work out the perm used to create it
        std::vector<int> perm;
        for(size_t i=0;i!=players.size();++i){
                perm.emplace_back( std::get<0>(player_perm[i]) );
        }

        std::vector<std::vector<holdem_hand_decl> > decls;

        // now we allocate suits, starting with 0 etc
        std::array< int, 4> rev_suit_map{-1,-1,-1,-1};
        int suit_iter = 0; // using the fact we know suits \in {0,1,2,3}
        // allocate pocket pairs common types first

        // AA KK -> AaAb KcKa
        //       -> AaAb KcKb -> AbAa KcKb

        std::map<int, int> pp_count;
        for(size_t i=0;i!=players.size();++i){
                auto h =  holdem_hand_decl::get( players[perm[i]] ) ;
                if( h.first().rank() != h.second().rank()){
                        continue;
                }
                ++pp_count[h.first().suit()];
                ++pp_count[h.second().suit()];
        }

        for(size_t i=0;i!=players.size();++i){
                auto h =  holdem_hand_decl::get( players[perm[i]] ) ;

                auto a = &h.first();
                auto b = &h.second();
                // TODO pocket pair
                #if 1
                if( a->rank() == b->rank()){
                        if( pp_count[a->suit()] > pp_count[b->suit()] ){
                                std::swap(a,b);
                        }
                }
                #endif

                if(     rev_suit_map[a->suit()] == -1 )
                        rev_suit_map[a->suit()] = suit_iter++;
                if(     rev_suit_map[b->suit()] == -1 )
                        rev_suit_map[b->suit()] = suit_iter++;
        }

        // TODO remove this, unneeded
        for(size_t i=0;i != 4;++i){
                if(     rev_suit_map[i] == -1 )
                        rev_suit_map[i] = suit_iter++;
        }

        // crate map
        std::vector< int> suit_perms;
        for(size_t i=0;i != 4;++i){
                suit_perms.emplace_back(rev_suit_map[i]);
        }

        std::vector<holdem_id> perm_hands;
        for(size_t i=0;i != players.size();++i){
                auto h =  holdem_hand_decl::get( players[perm[i]] ) ;

                #if 0
                if( h.first().rank() == h.second().rank() ){
                        if( suit_perms[h.first().suit()] > suit_perms[h.second().suit()] ){
                                perm_hands.emplace_back(
                                        holdem_hand_decl::make_id(
                                                h.first().rank(),
                                                suit_perms[h.second().suit()],
                                                h.second().rank(),
                                                suit_perms[h.first().suit()]));
                        } else {
                                perm_hands.emplace_back(
                                        holdem_hand_decl::make_id(
                                                h.first().rank(),
                                                suit_perms[h.first().suit()],
                                                h.second().rank(),
                                                suit_perms[h.second().suit()]));
                        }
                } else{
                #endif
                        perm_hands.emplace_back(
                                holdem_hand_decl::make_id(
                                        h.first().rank(),
                                        suit_perms[h.first().suit()],
                                        h.second().rank(),
                                        suit_perms[h.second().suit()]));
                #if 0
                }
                #endif

        }
        return std::make_tuple( perm, perm_hands);
}
#endif

} // ps


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

// Copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAX_INTERVAL_GAP_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAX_INTERVAL_GAP_HPP

#include <cstddef>
#include <queue>
#include <utility>
#include <vector>

#include <boost/core/ref.hpp>
#include <boost/range.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/type_traits/remove_reference.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/util/math.hpp>
#include <boost/geometry/algorithms/detail/sweep.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace max_interval_gap
{

// the class Interval must provide the following:
// * must define the type value_type
// * must define the type difference_type
// * must have the methods:
//   value_type get<Index>() const
//   difference_type length() const
// where an Index value of 0 (resp., 1) refers to the left (resp.,
// right) endpoint of the interval

template <typename Interval>
class sweep_event
{
public:
    typedef Interval interval_type;
    typedef typename Interval::value_type time_type;

    sweep_event(Interval const& interval, bool start_event = true)
        : m_interval(boost::cref(interval))
        , m_start_event(start_event)
    {}

    inline bool is_start_event() const
    {
        return m_start_event;
    }

    inline interval_type const& interval() const
    {
        return m_interval;
    }

    inline time_type time() const
    {
        return (m_start_event)
            ? interval().template get<0>()
            : interval().template get<1>();
    }

    inline bool operator<(sweep_event const& other) const
    {
        if (! math::equals(time(), other.time()))
        {
            return time() < other.time();
        }
        // a start-event is before an end-event with the same event time
        return is_start_event() && ! other.is_start_event();
    }

private:
    boost::reference_wrapper<Interval const> m_interval;
    bool m_start_event;
};

template <typename Event>
struct event_greater
{
    inline bool operator()(Event const& event1, Event const& event2) const
    {
        return event2 < event1;
    }
};

struct initialization_visitor
{
    template <typename Range, typename PriorityQueue, typename EventVisitor>
    static inline void apply(Range const& range,
                             PriorityQueue& queue,
                             EventVisitor&)
    {
        BOOST_GEOMETRY_ASSERT(queue.empty());

        // it is faster to build the queue directly from the entire
        // range, rather than insert elements one after the other
        PriorityQueue pq(boost::begin(range), boost::end(range));
        std::swap(pq, queue);
    }
};

template <typename Event>
class event_visitor
{
    typedef typename Event::time_type event_time_type;
    typedef typename Event::interval_type::difference_type difference_type;

    typedef typename boost::remove_const
        <
            typename boost::remove_reference
                <
                    event_time_type
                >::type
        >::type bare_time_type;

public:
    event_visitor()
        : m_overlap_count(0)
        , m_max_gap_left(0)
        , m_max_gap_right(0)
    {}

    template <typename PriorityQueue>
    inline void apply(Event const& event, PriorityQueue& queue)
    {
        if (event.is_start_event())
        {
            ++m_overlap_count;
            queue.push(Event(event.interval(), false));
        }
        else
        {
            --m_overlap_count;
            if (m_overlap_count == 0 && ! queue.empty())
            {
                // we may have a gap
                BOOST_GEOMETRY_ASSERT(queue.top().is_start_event());

                event_time_type next_event_time
                    = queue.top().interval().template get<0>();
                difference_type gap = next_event_time - event.time();
                if (gap > max_gap())
                {
                    m_max_gap_left = event.time();
                    m_max_gap_right = next_event_time;
                }
            }
        }
    }

    bare_time_type const& max_gap_left() const
    {
        return m_max_gap_left;
    }

    bare_time_type const& max_gap_right() const
    {
        return m_max_gap_right;
    }

    difference_type max_gap() const
    {
        return m_max_gap_right - m_max_gap_left;
    }

private:
    std::size_t m_overlap_count;
    bare_time_type m_max_gap_left, m_max_gap_right;
};

}} // namespace detail::max_interval_gap
#endif // DOXYGEN_NO_DETAIL

// Given a range of intervals I1, I2, ..., In, maximum_gap() returns
// the maximum length of an interval M that satisfies the following
// properties:
//
// 1. M.left >= min(I1, I2, ..., In)
// 2. M.right <= max(I1, I2, ..., In)
// 3. intersection(interior(M), Ik) is the empty set for all k=1, ..., n
// 4. length(M) is maximal
//
// where M.left and M.right denote the left and right extreme values
// for the interval M, and length(M) is equal to M.right - M.left.
//
// If M does not exist (or, alternatively, M is identified as the
// empty set), 0 is returned.
//
// The algorithm proceeds for performing a sweep: the left endpoints
// are inserted into a min-priority queue with the priority being the
// value of the endpoint. The sweep algorithm maintains an "overlap
// counter" that counts the number of overlaping intervals at any
// specific sweep-time value.
// There are two types of events encountered during the sweep:
// (a) a start event: the left endpoint of an interval is found.
//     In this case the overlap count is increased by one and the
//     right endpoint of the interval in inserted into the event queue
// (b) an end event: the right endpoint of an interval is found.
//     In this case the overlap count is decreased by one. If the
//     updated overlap count is 0, then we could expect to have a gap
//     in-between intervals. This gap is measured as the (absolute)
//     distance of the current interval right endpoint (being
//     processed) to the upcoming left endpoint of the next interval
//     to be processed (if such an interval exists). If the measured
//     gap is greater than the current maximum gap, it is recorded.
// The initial maximum gap is initialized to 0. This value is returned
// if no gap is found during the sweeping procedure.

template <typename RangeOfIntervals, typename T>
inline typename boost::range_value<RangeOfIntervals>::type::difference_type
maximum_gap(RangeOfIntervals const& range_of_intervals,
            T& max_gap_left, T& max_gap_right)
{
    typedef typename boost::range_value<RangeOfIntervals>::type interval_type;
    typedef detail::max_interval_gap::sweep_event<interval_type> event_type;

    // create a min-priority queue for the events
    std::priority_queue
        <
            event_type,
            std::vector<event_type>,
            detail::max_interval_gap::event_greater<event_type>
        > queue;

    // define initialization and event-process visitors
    detail::max_interval_gap::initialization_visitor init_visitor;
    detail::max_interval_gap::event_visitor<event_type> sweep_visitor;

    // perform the sweep
    geometry::sweep(range_of_intervals,
                    queue,
                    init_visitor,
                    sweep_visitor);

    max_gap_left = sweep_visitor.max_gap_left();
    max_gap_right = sweep_visitor.max_gap_right();
    return sweep_visitor.max_gap();
}

template <typename RangeOfIntervals>
inline typename boost::range_value<RangeOfIntervals>::type::difference_type
maximum_gap(RangeOfIntervals const& range_of_intervals)
{
    typedef typename boost::remove_const
        <
            typename boost::remove_reference
                <
                    typename boost::range_value
                        <
                            RangeOfIntervals
                        >::type::value_type
                >::type
        >::type value_type;

    value_type left, right;

    return maximum_gap(range_of_intervals, left, right);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_MAX_INTERVAL_GAP_HPP