/* 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 "bdd_utils.hpp"

#include <functional>
#include <unordered_map>
#include <unordered_set>

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/numeric.hpp>

#include <cudd.h>
#include <cuddInt.h>

#include <core/utils/range_utils.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Functions                                                                  *
 ******************************************************************************/

BDD make_cube( Cudd& manager, const std::vector<BDD>& vars )
{
  return boost::accumulate( vars, manager.bddOne(), []( const BDD& x1, const BDD& x2 ) { return x1 & x2; } );
}

BDD make_cube( Cudd& manager, const std::string& cube )
{
  std::vector<BDD> vars;

  for ( auto i = 0u; i < cube.size(); ++i )
  {
    if ( cube[i] == '0' )
    {
      vars.push_back( ~manager.bddVar( i ) );
    }
    else if ( cube[i] == '1' )
    {
      vars.push_back( manager.bddVar( i ) );
    }
    else
    {
      assert( cube[i] == '-' );
    }
  }
  return make_cube( manager, vars );
}

bool is_selfdual( const Cudd& manager, const BDD& f )
{
  /* negative literals */
  std::vector<BDD> lits( manager.ReadSize() );
  for ( auto i = 0u; i < lits.size(); ++i )
  {
    lits[i] = !manager.bddVar( i );
  }

  return f.VectorCompose( lits ) == !f;
}

bool is_monotone( const Cudd& manager, const BDD& f )
{
  for ( auto i = 0; i < manager.ReadSize(); ++i )
  {
    if ( !f.Increasing( i ) ) { return false; }
  }
  return true;
}

/*
 * Implementation according to
 * Knuth TAOCP Exercise 7.1.4-106
 * [T. Horiyama and T. Ibaraki, Artificial Intelligence 136 (2002), 189-213]
 */
bool is_horn( const Cudd& manager, const BDD& f, const BDD& g, const BDD& h )
{
  if ( f > g ) { return is_horn( manager, g, f, h ); }
  if ( f == manager.bddZero() || h == manager.bddOne() ) { return true; }
  if ( g == manager.bddOne() || h == manager.bddZero() ) { return false; }

  assert( f != manager.bddOne() );
  assert( g != manager.bddZero() );

  BDD fl( manager, cuddE( f.getRegularNode() ) );
  BDD fh( manager, cuddT( f.getRegularNode() ) );
  BDD gl( manager, cuddE( g.getRegularNode() ) );
  BDD gh( manager, cuddT( g.getRegularNode() ) );
  BDD hl( manager, cuddE( h.getRegularNode() ) );
  BDD hh( manager, cuddT( h.getRegularNode() ) );

  return is_horn( manager, fl, gl, hl ) && is_horn( manager, fl, gh, hl ) && is_horn( manager, fh, gl, hl ) && is_horn( manager, fh, gh, hh );
}

bool is_horn( const Cudd& manager, const BDD& f )
{
  return is_horn( manager, f, f, f );
}

#define EXTRA_BDD_COMPARE_TAG 0x96
#define EXTRA_BDD_COMPARE_EQ ((DdNode*)1)
#define EXTRA_BDD_COMPARE_IC ((DdNode*)2)
#define EXTRA_BDD_COMPARE_LT ((DdNode*)3)
#define EXTRA_BDD_COMPARE_GT ((DdNode*)4)

DdNode * Extra_bddCompare( DdManager * dd, DdNode * f, DdNode * g )
{
  DdNode * one = DD_ONE( dd );
  DdNode * zero = Cudd_Not( one );

  if ( f == g ) { return EXTRA_BDD_COMPARE_EQ; }
  if ( f == zero || g == one ) { return EXTRA_BDD_COMPARE_LT; }
  if ( f == one || g == zero ) { return EXTRA_BDD_COMPARE_GT; }

  DdNode * r = nullptr;
  /*if ( ( r = cuddConstantLookup( dd, EXTRA_BDD_COMPARE_TAG, f, g, nullptr ) ) )
  {
    return r;
  }*/

  auto fl = Cudd_NotCond( Cudd_E( Cudd_Regular( f ) ), Cudd_IsComplement( f ) );
  auto fh = Cudd_NotCond( Cudd_T( Cudd_Regular( f ) ), Cudd_IsComplement( f ) );
  auto gl = Cudd_NotCond( Cudd_E( Cudd_Regular( g ) ), Cudd_IsComplement( g ) );
  auto gh = Cudd_NotCond( Cudd_T( Cudd_Regular( g ) ), Cudd_IsComplement( g ) );

  auto rl = Extra_bddCompare( dd, fl, gl );
  if ( rl == EXTRA_BDD_COMPARE_IC ) { r = EXTRA_BDD_COMPARE_IC; }
  else
  {
    auto rh = Extra_bddCompare( dd, fh, gh );
    if ( rh == EXTRA_BDD_COMPARE_IC ) { r = EXTRA_BDD_COMPARE_IC; }
    else if ( rl == EXTRA_BDD_COMPARE_EQ ) { r = rh; }
    else if ( rh == EXTRA_BDD_COMPARE_EQ ) { r = rl; }
    else if ( rl == rh ) { r = rl; }
    else { r = EXTRA_BDD_COMPARE_IC; }
  }

  //cuddCacheInsert( dd, EXTRA_BDD_COMPARE_TAG, f, g, nullptr, r );
  return r;
}

bool Extra_bddUnate( DdManager * dd, DdNode * f, std::vector<int>& ps )
{
  /* Initialize */
  ps.resize( Cudd_ReadSize( dd ) );
  boost::fill( ps, 0 );

  if ( Cudd_IsConstant( f ) ) return true;

  auto fr = Cudd_Regular( f );

  auto fl = Cudd_NotCond( Cudd_E( fr ), Cudd_IsComplement( f ) );
  auto fh = Cudd_NotCond( Cudd_T( fr ), Cudd_IsComplement( f ) );

  if ( !Extra_bddUnate( dd, fl, ps ) || !Extra_bddUnate( dd, fh, ps ) ) { return false; }

  auto r = Extra_bddCompare( dd, fl, fh );
  if ( r == EXTRA_BDD_COMPARE_IC ) { return false; }

  if ( r == EXTRA_BDD_COMPARE_LT )
  {
    if ( ps[fr->index] < 0 ) { return false; }
    ps[fr->index] = 1;
    return true;
  }
  if ( r == EXTRA_BDD_COMPARE_GT )
  {
    if ( ps[fr->index] > 0 ) { return false; }
    ps[fr->index] = -1;
    return true;
  }

  /* this should not happen */
  assert( r == EXTRA_BDD_COMPARE_EQ );
  assert( false );
  return false;
}

bool is_unate( const Cudd& manager, const BDD& f, std::vector<int>& ps )
{
  return Extra_bddUnate( manager.getManager(), f.getNode(), ps );
}

void collect_nodes( DdNode * f, std::unordered_set<DdNode*>& visited )
{
  assert( f );
  assert( !Cudd_IsComplement( f ) );

  /* node visited before? */
  if ( visited.find( f ) != visited.end() ) { return; }

  /* mark node as visited */
  visited.insert( f );

  /* terminate? */
  if cuddIsConstant( f ) { return; }

  /* recur */
  collect_nodes( cuddT( f ), visited );
  collect_nodes( Cudd_Regular( cuddE( f ) ), visited );
}

void collect_nodes_and_count( DdNode * f, std::unordered_map<DdNode*, unsigned>& visited )
{
  assert( f );
  assert( !Cudd_IsComplement( f ) );

  /* node visited before? */
  auto it = visited.find( f );
  if ( it != visited.end() )
  {
    /* visited once more */
    it->second++;
    return;
  }

  /* mark node as visited */
  visited.insert( {f, 1u} );

  /* terminate? */
  if cuddIsConstant( f ) { return; }

  /* recur */
  collect_nodes_and_count( cuddT( f ), visited );
  collect_nodes_and_count( Cudd_Regular( cuddE( f ) ), visited );
}

void collect_nodes_and_count_ignore_complemented( const DdNode * f, std::unordered_map<DdNode*, unsigned>& visited )
{
  assert( f );

  auto fr = Cudd_Regular( f );

  /* node visited before? */
  auto it = visited.find( fr );
  if ( it != visited.end() )
  {
    /* visited once more */
    if ( !Cudd_IsComplement( f ) )
    {
      it->second++;
    }
    return;
  }

  /* mark node as visited */
  visited.insert( {fr, (Cudd_IsComplement( f ) ? 0u : 1u)} );

  /* terminate? */
  if cuddIsConstant( fr ) { return; }

  /* recur */
  collect_nodes_and_count_ignore_complemented( cuddT( fr ), visited );
  collect_nodes_and_count_ignore_complemented( cuddE( fr ), visited );
}

std::vector<unsigned> level_sizes( DdManager * dd, const std::vector<DdNode*>& fs )
{
  using namespace std::placeholders;

  std::vector<unsigned> sizes( Cudd_ReadSize( dd ) );

  /* collect all nodes in the BDDs */
  std::unordered_set<DdNode*> visited;
  for ( const auto* f : fs )
  {
    collect_nodes( Cudd_Regular( f ), visited );
  }

  for ( const auto* node : visited )
  {
    if ( !cuddIsConstant( node ) )
    {
      sizes[node->index]++;
    }
  }

  return sizes;
}

std::vector<unsigned> level_sizes( const Cudd& manager, const std::vector<BDD>& fs )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> fs_native( fs.size() );
  boost::copy( fs | transformed( std::bind( &BDD::getRegularNode, _1 ) ), fs_native.begin() );

  return level_sizes( manager.getManager(), fs_native );
}

unsigned maximum_fanout( DdManager* manager, const std::vector<DdNode*>& fs )
{
  using boost::adaptors::map_values;

  /* collect all nodes in the BDDs and count */
  std::unordered_map<DdNode*, unsigned> visited;
  for ( const auto* f : fs )
  {
    collect_nodes_and_count_ignore_complemented( f, visited );
  }

  /* erase constant nodes from visited list */
  visited.erase( DD_ONE( manager ) );
  visited.erase( Cudd_Not( DD_ONE( manager ) ) );

  if ( visited.empty() )
  {
    return 0u;
  }
  else
  {
    return *boost::max_element( visited | map_values );
  }
}

unsigned maximum_fanout( const Cudd& manager, const std::vector<BDD>& fs )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> fs_native( fs.size() );
  boost::copy( fs | transformed( std::bind( &BDD::getNode, _1 ) ), fs_native.begin() );

  return maximum_fanout( manager.getManager(), fs_native );
}

void count_complement_edges_rec( DdManager* manager, DdNode* f, unsigned& count, std::unordered_set<DdNode*>& visited )
{
  if ( visited.find( f ) != visited.end() )
  {
    return;
  }

  if ( Cudd_IsComplement( f ) )
  {
    ++count;
  }

  if ( Cudd_IsConstant( f ) )
  {
    return;
  }

  const auto fr = Cudd_Regular( f );

  count_complement_edges_rec( manager, cuddT( fr ), count, visited );
  count_complement_edges_rec( manager, cuddE( fr ), count, visited );

  visited.insert( f );
}

unsigned count_complement_edges( DdManager* manager, const std::vector<DdNode*>& fs )
{
  std::unordered_set<DdNode*> visited;

  auto count = 0u;

  for ( const auto& f : fs )
  {
    count_complement_edges_rec( manager, f, count, visited );
  }

  return count;
}

unsigned count_complement_edges( const Cudd& manager, const std::vector<BDD>& fs )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> fs_native( fs.size() );
  boost::copy( fs | transformed( std::bind( &BDD::getNode, _1 ) ), fs_native.begin() );

  return count_complement_edges( manager.getManager(), fs_native );
}

BDD make_eq_rec( const Cudd& manager, const std::vector<BDD>& vars, unsigned pos, int k, std::map<std::pair<unsigned, int>, BDD>& visited )
{
  /* terminal */
  if ( pos == vars.size() )
  {
    return ( k == 0 ) ? manager.bddOne() : manager.bddZero();
  }

  /* cannot be 1 anymore */
  if ( ( k < 0 ) || ( k > static_cast<int>( vars.size() - pos ) ) )
  {
    return manager.bddZero();
  }

  /* cached? */
  const auto it = visited.find( {pos, k} );
  if ( it != visited.end() ) { return it->second; }

  const auto f = ( vars[pos] & make_eq_rec( manager, vars, pos + 1, k - 1, visited ) ) |
                 ( ~vars[pos] & make_eq_rec( manager, vars, pos + 1, k, visited ) );

  visited.insert( {{pos, k}, f} );

  return f;
}

BDD make_eq( const Cudd& manager, const std::vector<BDD>& vars, unsigned k )
{
  std::map<std::pair<unsigned, int>, BDD> visited;
  return make_eq_rec( manager, vars, 0, (int)k, visited );
}

DdNode * bdd_copy_rec( DdManager* mgr_from, DdNode* from, DdManager* mgr_to, std::map<DdNode*, DdNode*>& visited, const std::vector<unsigned>& index_map )
{
  assert( !Cudd_IsComplement( from ) );

  if ( Cudd_IsConstant( from ) )
  {
    if ( from == DD_ONE( mgr_from ) )
    {
      return DD_ONE( mgr_to );
    }
    else
    {
      return Cudd_Not( DD_ONE( mgr_to ) );
    }
  }

  /* visited table */
  const auto it = visited.find( from );
  if ( it != visited.end() ) { return it->second; }

  auto * var = Cudd_bddIthVar( mgr_to, index_map.at( from->index ) );

  auto * high = Cudd_NotCond( bdd_copy_rec( mgr_from, Cudd_Regular( cuddT( from ) ), mgr_to, visited, index_map ), Cudd_IsComplement( cuddT( from ) ) );
  auto * low  = Cudd_NotCond( bdd_copy_rec( mgr_from, Cudd_Regular( cuddE( from ) ), mgr_to, visited, index_map ), Cudd_IsComplement( cuddE( from ) ) );

  /* create new node */
  auto* node = Cudd_bddIte( mgr_to, var, high, low );

  visited.insert( {from, node} );
  return node;
}

std::vector<DdNode*> bdd_copy( DdManager* mgr_from, const std::vector<DdNode*>& from, DdManager* mgr_to, std::vector<unsigned>& index_map )
{
  if ( index_map.empty() )
  {
    index_map.resize( Cudd_ReadSize( mgr_from ) );
    boost::iota( index_map, 0u );
  }

  std::map<DdNode*, DdNode*> visited;

  std::vector<DdNode*> ret;

  for ( auto* node : from )
  {
    ret.push_back( Cudd_NotCond( bdd_copy_rec( mgr_from, Cudd_Regular( node ), mgr_to, visited, index_map ), Cudd_IsComplement( node ) ) );
  }

  return ret;
}

std::vector<BDD> bdd_copy( const Cudd& mgr_from, const std::vector<BDD>& from, const Cudd& mgr_to, std::vector<unsigned>& index_map )
{
  using namespace std::placeholders;
  using boost::adaptors::transformed;

  std::vector<DdNode*> from_native( from.size() );
  boost::copy( from | transformed( std::bind( &BDD::getNode, _1 ) ), from_native.begin() );

  auto orig = bdd_copy( mgr_from.getManager(), from_native, mgr_to.getManager(), index_map );
  std::vector<BDD> ret;

  for ( auto* node : orig )
  {
    ret.push_back( BDD( mgr_to, node ) );
  }

  return ret;
}

bdd_function_t compute_characteristic( const bdd_function_t& bdd, bool inputs_first )
{
  const auto& mgr_from = bdd.first;
  const auto& from     = bdd.second;

  Cudd mgr;
  ntimes( mgr_from.ReadSize() + from.size(), [&]() { mgr.bddVar(); } );

  std::vector<unsigned> index_map( mgr_from.ReadSize() );
  boost::iota( index_map, inputs_first ? 0u : from.size() );

  const auto fs = bdd_copy( mgr_from, from, mgr, index_map );

  auto f = mgr.bddOne();

  const auto offset = inputs_first ? mgr_from.ReadSize() : 0u;
  for ( auto i = 0u; i < fs.size(); ++i )
  {
    f &= mgr.bddVar( offset + i ).Xnor( fs[i] );
  }

  return {mgr, {f}};
}

cube_vec_t bdd_to_cubes( DdManager* manager, DdNode* f )
{
  DdGen * gen;
  int * ddcube;
  CUDD_VALUE_TYPE value;

  cube_vec_t cubes;

  Cudd_ForeachCube( manager, f, gen, ddcube, value )
  {
    boost::dynamic_bitset<> bits( manager->size ), care( manager->size );
    for ( auto i = 0; i < manager->size; ++i )
    {
      switch ( ddcube[i] )
      {
      case 0:
        care[i] = true;
        break;
      case 1:
        bits[i] = true;
        care[i] = true;
        break;
      default:
        break;
      }
    }
    cubes.push_back( cube( bits, care ) );
  }

  return cubes;
}

cube_vec_t bdd_to_cubes( const Cudd& manager, BDD f )
{
  return bdd_to_cubes( manager.getManager(), f.getNode() );
}

/******************************************************************************
 * new BDD operations                                                         *
 ******************************************************************************/

DdNode* bdd_up_rec( DdManager* manager, DdNode* f )
{
  /* terminal case */
  if ( Cudd_IsConstant( f ) ) { return f; }

  const auto* F = Cudd_Regular( f );

  if ( F->ref != 1 )
  {
    auto* r = cuddCacheLookup1( manager, bdd_up_rec, f );
    if ( r )
    {
      return r;
    }
  }

  const auto top = manager->perm[F->index];

  auto* fl = Cudd_NotCond( cuddE( F ), Cudd_IsComplement( f ) );
  auto* fh = Cudd_NotCond( cuddT( F ), Cudd_IsComplement( f ) );
  auto* t1 = Cudd_bddOr( manager, fl, fh );
  if ( !t1 )
  {
    return nullptr;
  }
  cuddRef( t1 );

  auto* rh = bdd_up_rec( manager, t1 );
  if ( !rh )
  {
    Cudd_IterDerefBdd( manager, t1 );
    return nullptr;
  }
  cuddRef( rh );

  auto* rl = bdd_up_rec( manager, fl );
  if ( !rl )
  {
    Cudd_IterDerefBdd( manager, t1 );
    Cudd_IterDerefBdd( manager, rh );
  }
  cuddRef( rl );

  DdNode* r;
  if ( rl == rh )
  {
    r = rh;
  }
  else
  {
    if ( Cudd_IsComplement( rh ) )
    {
      r = cuddUniqueInter( manager, (int)top, Cudd_Not( rh ), Cudd_Not( rl ) );
      if ( !r )
      {
        Cudd_IterDerefBdd( manager, t1 );
        Cudd_IterDerefBdd( manager, rh );
        Cudd_IterDerefBdd( manager, rl );
        return nullptr;
      }
      r = Cudd_Not( r );
    }
    else
    {
      r = cuddUniqueInter( manager, (int)top, rh, rl );
      if ( !r )
      {
        Cudd_IterDerefBdd( manager, t1 );
        Cudd_IterDerefBdd( manager, rh );
        Cudd_IterDerefBdd( manager, rl );
        return nullptr;
      }
    }
  }
  cuddDeref( t1 );
  cuddDeref( rh );
  cuddDeref( rl );
  if ( F->ref != 1 )
  {
    cuddCacheInsert1( manager, bdd_up, f, r );
  }
  return r;
}

DdNode* bdd_up( DdManager *manager, DdNode *f )
{
  DdNode *res;

  do
  {
    manager->reordered = 0;
    res = bdd_up_rec( manager, f );
  }
  while ( manager->reordered == 1 );
  return res;
}

BDD bdd_up( Cudd& manager, const BDD& f )
{
  auto* r = bdd_up( f.manager(), f.getNode() );
  return BDD( manager, r );
}

}

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


/*ckwg +29
 * Copyright 2015, 2019 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * 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.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * 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 AUTHORS 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.
 */

/**
 * \file
 * \brief core homography template implementations
 */

#include "homography.h"

#include <cmath>

#include <vital/exceptions/math.h>

#include <Eigen/LU>

namespace kwiver {
namespace vital {

namespace //anonymous
{

/// Private helper method for point transformation via homography matrix
template < typename T >
Eigen::Matrix< T, 2, 1 >
h_map_point( Eigen::Matrix< T, 3, 3 > const& h, Eigen::Matrix< T, 2, 1 > const& p )
{
  Eigen::Matrix< T, 3, 1 > out_pt = h * Eigen::Matrix< T, 3, 1 > ( p[0], p[1], 1.0 );

  if ( fabs( out_pt[2] ) <= Eigen::NumTraits< T >::dummy_precision() )
  {
    throw point_maps_to_infinity();
  }
  return Eigen::Matrix< T, 2, 1 > ( out_pt[0] / out_pt[2], out_pt[1] / out_pt[2] );
}

} // end anonymous namespace

/// Construct an identity homography
template < typename T >
homography_< T >
::homography_()
  : h_( matrix_t::Identity() )
{
}

/// Construct from a provided transformation matrix
template < typename T >
homography_< T >
::homography_( Eigen::Matrix< T, 3, 3 > const& mat )
  : h_( mat )
{
}

/// Conversion Copy constructor -- float specialization
template < >
template < >
homography_< float >
::homography_( homography_< float > const& other )
  : h_( other.get_matrix() )
{
}

/// Conversion Copy constructor -- double specialization
template < >
template < >
homography_< double >
::homography_( homography_< double > const& other )
  : h_( other.get_matrix() )
{
}

/// Construct from a generic homography
template < typename T >
homography_< T >
::homography_( homography const& base )
  : h_( base.matrix().template cast< T > () )
{
}

/// Construct from a generic homography -- double specialization
template < >
homography_< double >
::homography_( homography const& base )
  : h_( base.matrix() )
{
}

/// Create a clone of outself as a shared pointer
template < typename T >
transform_2d_sptr
homography_< T >
::clone() const
{
  return std::make_shared< homography_< T > >( *this );
}

/// Get a double-typed copy of the underlying matrix transformation
template < typename T >
Eigen::Matrix< double, 3, 3 >
homography_< T >
::matrix() const
{
  return this->h_.template cast< double > ();
}

/// Specialization for homographies with native double type
template < >
Eigen::Matrix< double, 3, 3 >
homography_< double >
::matrix() const
{
  return this->h_;
}

/// Normalize homography transformation in-place
template < typename T >
homography_sptr
homography_< T >
::normalize() const
{
  matrix_t norm = this->get_matrix();

  if ( fabs( norm( 2, 2 ) ) >= Eigen::NumTraits< T >::dummy_precision() )
  {
    norm /= norm( 2, 2 );
  }
  return homography_sptr( new homography_< T > ( norm ) );
}

/// Inverse the homography transformation returning a new transformation
template < typename T >
homography_sptr
homography_< T >
::inverse() const
{
  matrix_t inv;
  bool isvalid;

  this->h_.computeInverseWithCheck( inv, isvalid );
  if ( ! isvalid )
  {
    throw non_invertible_matrix();
  }
  return homography_sptr( new homography_< T > ( inv ) );
}

/// Map a 2D double-type point using this homography
template < typename T >
Eigen::Matrix< double, 2, 1 >
homography_< T >
::map( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  // Explicitly refer to templated version of method so as to not infinitely
  // recurse.
  Eigen::Matrix< double, 3, 3 > m = h_.template cast< double > ();

  return h_map_point( m, p );
}

/// Map a 2D double-type point using this homography -- double specialization
template < >
Eigen::Matrix< double, 2, 1 >
homography_< double >
::map( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Get the underlying matrix transformation
template < typename T >
typename homography_< T >::matrix_t &
homography_< T >
::get_matrix()
{
  return this->h_;
}

/// Get a const new copy of the underlying matrix transformation.
template < typename T >
typename homography_< T >::matrix_t const &
homography_< T >
::get_matrix() const
{
  return this->h_;
}

/// Map a 2D point using this homography -- generic version
template < typename T >
Eigen::Matrix< T, 2, 1 >
homography_< T >
::map_point( Eigen::Matrix< T, 2, 1 > const& p ) const
{
  return h_map_point< T > ( h_.template cast< T > (), p );
}

/// Map a 2D point using this homography -- float specialization
template < >
Eigen::Matrix< float, 2, 1 >
homography_< float >
::map_point( Eigen::Matrix< float, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Map a 2D point using this homography -- double specialization
template < >
Eigen::Matrix< double, 2, 1 >
homography_< double >
::map_point( Eigen::Matrix< double, 2, 1 > const& p ) const
{
  return h_map_point( h_, p );
}

/// Custom f2f_homography multiplication operator.
template < typename T >
homography_< T >
homography_< T >
::operator*( homography_< T > const& rhs ) const
{
  return homography_< T > ( h_ * rhs.h_ );
}

// ===========================================================================
// Other Functions
// ---------------------------------------------------------------------------

/// homography_<T> output stream operator
template < typename T >
std::ostream&
operator<<( std::ostream& s, homography_< T > const& h )
{
  s << h.get_matrix();
  return s;
}

/// Output stream operator for \p homography instances
std::ostream&
operator<<( std::ostream& s, homography const& h )
{
  s << h.matrix();
  return s;
}

// ===========================================================================
// Template class instantiation
// ---------------------------------------------------------------------------
/// \cond DoxygenSuppress
#define INSTANTIATE_HOMOGRAPHY( T )              \
  template class homography_< T >;               \
  template VITAL_EXPORT std::ostream&            \
  operator<<( std::ostream&,                     \
              homography_< T > const& )

INSTANTIATE_HOMOGRAPHY( float );
INSTANTIATE_HOMOGRAPHY( double );
#undef INSTANTIATE_HOMOGRAPHY
/// \endcond

} } // end vital namespace


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_LINALG_FUNCTIONS_FACTORIZATIONS_GENEIG_HPP_INCLUDED
#define NT2_LINALG_FUNCTIONS_FACTORIZATIONS_GENEIG_HPP_INCLUDED

#include <nt2/linalg/functions/geneig.hpp>
#include <nt2/include/functions/divides.hpp>
#include <nt2/include/functions/ggev_w.hpp>
#include <nt2/include/functions/ggev_wvr.hpp>
#include <nt2/include/functions/ggev_wvrvl.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/from_diag.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/linalg/details/utility/lapack_verify.hpp>
#include <nt2/linalg/options.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/core/utility/assign_swap.hpp>
#include <nt2/core/container/dsl/as_terminal.hpp>
#include <nt2/sdk/meta/as_real.hpp>
#include <boost/dispatch/attributes.hpp>

namespace nt2 { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT  ( geneig_, tag::cpu_
                            , (A0)
                            , (scalar_<unspecified_<A0> >)
                              (scalar_<unspecified_<A0> >)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A0& a1) const
    {
      return a1/a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( geneig_, tag::cpu_
                            , (A0)(A1)
                            , (scalar_<unspecified_<A0> >)
                              (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A0& a1, const A1&) const
    {
      return a1/a0;
    }
  };

  BOOST_DISPATCH_IMPLEMENT  ( geneig_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (scalar_<unspecified_<A0> >)
                              (scalar_<unspecified_<A0> >)
                              (unspecified_<A1>)
                              (unspecified_<A2>)
                            )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE result_type operator()(const A0& a0, const A0& a1, const A1&, const A2&) const
    {
      return a1/a0;
    }
  };

  //============================================================================
  //Geneig computations
  //============================================================================
  BOOST_DISPATCH_IMPLEMENT  ( geneig_, tag::cpu_
                            , (A0)(N0)(A1)(N1)
                            , ((node_<A0, nt2::tag::geneig_
                                    , N0, nt2::container::domain
                                      >
                              ))
                              ((node_<A1, nt2::tag::tie_
                                    , N1, nt2::container::domain
                                     >
                              ))
                            )
  {
    typedef void  result_type;
    typedef typename boost::proto::result_of::child_c<A0&,0>::value_type child0;
    typedef typename child0::value_type                                  type_t;
    typedef typename nt2::meta::as_real<type_t>::type                   rtype_t;
    typedef typename nt2::meta::as_complex<rtype_t>::type               ctype_t;
    typedef rtype_t T;
    typedef nt2::memory::container<tag::table_,  type_t, nt2::_2D>   o_semantic;
    typedef nt2::memory::container<tag::table_, rtype_t, nt2::_2D>   r_semantic;
    typedef nt2::memory::container<tag::table_, ctype_t, nt2::_2D>   c_semantic;

    BOOST_FORCEINLINE result_type operator()( A0& a0, A1& a1 ) const
    {
      eval(a0, a1, N0(), N1());
    }

    //==========================================================================
    /// INTERNAL ONLY - W = GENEIG(A)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_3(a0, a1
             , nt2::policy<ext::vector_>());
    }

    //==========================================================================
    /// INTERNAL ONLY - W = GENEIG(A, B, matrix_/vector_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<1> const&
              ) const
    {
      eval1_3(a0, a1
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    //==========================================================================
    /// INTERNAL ONLY - [VR, W]= GENEIG(A, B)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::matrix_>());
    }

    /// INTERNAL ONLY: 1o 3i
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, w
                           , boost::proto::child_c<0>(a1));
      size_t n = height(a);
      container::table <type_t>  beta(nt2::of_size(n,1));
      w.resize(nt2::of_size(n,1)); ;
      NT2_LAPACK_VERIFY(nt2::ggev_w( boost::proto::value(a)
                                   , boost::proto::value(b)
                                   , boost::proto::value(w)
                                   , boost::proto::value(beta)));
      assign_swap(boost::proto::child_c<0>(a1), w);
      boost::proto::child_c<0>(a1) /= beta;
    }

    /// INTERNAL ONLY: 1o 3i
    BOOST_FORCEINLINE
    void eval1_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      size_t n = height(a);
      container::table <type_t> beta(nt2::of_size(n,1));
      container::table <ctype_t> alpha(nt2::of_size(n,1));

      NT2_LAPACK_VERIFY(nt2::ggev_w( boost::proto::value(a)
                                   , boost::proto::value(b)
                                   , boost::proto::value(alpha)
                                   , boost::proto::value(beta)));
      boost::proto::child_c<0>(a1) = from_diag(alpha/beta);
      //currently from_diag doesnt support aliasing
    }

    //==========================================================================
    /// INTERNAL ONLY - [alpha, beta] = GENEIG(A, B, alphabeta_) or
    ///                 [vr, w]       = GENEIG(A, B, matrix_/vector_)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_3(a0, a1
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    /// INTERNAL ONLY: 2o 3i
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::alphabeta_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, alpha
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (o_semantic, beta
                           , boost::proto::child_c<1>(a1));
      size_t n = height(a);
      beta.resize(nt2::of_size(n,1));
      alpha.resize(nt2::of_size(n,1)); ;
      NT2_LAPACK_VERIFY(nt2::ggev_w( boost::proto::value(a)
                                   , boost::proto::value(b)
                                   , boost::proto::value(alpha)
                                   , boost::proto::value(beta)));
      assign_swap(boost::proto::child_c<0>(a1), alpha);
      assign_swap(boost::proto::child_c<1>(a1), beta);

    }

    /// INTERNAL ONLY: 2o 3i
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, w
                           , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      size_t n = height(a);
      w.resize(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      container::table <type_t>  beta(nt2::of_size(n,1));
      NT2_LAPACK_VERIFY(nt2::ggev_wvr( boost::proto::value(a)
                                     , boost::proto::value(b)
                                     , boost::proto::value(w)
                                     , boost::proto::value(beta)
                                     , boost::proto::value(vr)));
      assign_swap(boost::proto::child_c<1>(a1), w);
      boost::proto::child_c<1>(a1) /= beta;
      assign_swap(boost::proto::child_c<0>(a1), vr);
    }

    /// INTERNAL ONLY: 2o 3i
    BOOST_FORCEINLINE
    void eval2_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      size_t n = height(a);
      container::table <type_t> beta(nt2::of_size(n,1));
      container::table <ctype_t> alpha(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      NT2_LAPACK_VERIFY(nt2::ggev_wvr( boost::proto::value(a)
                                     , boost::proto::value(b)
                                     , boost::proto::value(alpha)
                                     , boost::proto::value(beta)
                                     , boost::proto::value(vr)));
      boost::proto::child_c<1>(a1) = from_diag(alpha/beta);
      assign_swap(boost::proto::child_c<0>(a1), vr);
      //from_diag doesnt support aliasing currently
    }

    //==========================================================================
    /// INTERNAL ONLY - [alpha, beta] = GENEIG(A, B, alphabeta_, matrix_/vectors)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<4> const&
              , boost::mpl::long_<2> const&
              ) const
    {
      eval2_4(a0, a1
             , boost::proto::value(boost::proto::child_c<3>(a0)));
    }

    /// INTERNAL ONLY: 2o 4i
    BOOST_FORCEINLINE
    void eval2_4 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      eval2_3(a0, a1
             , nt2::policy<ext::alphabeta_>());
    }

    /// INTERNAL ONLY: 2o 4i
    BOOST_FORCEINLINE
    void eval2_4 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      size_t n = height(a);
      container::table <type_t> beta(nt2::of_size(n,1));
      container::table <ctype_t> alpha(nt2::of_size(n,1));
      NT2_LAPACK_VERIFY(nt2::ggev_w( boost::proto::value(a)
                                   , boost::proto::value(b)
                                   , boost::proto::value(alpha)
                                   , boost::proto::value(beta)
                                   ));
      boost::proto::child_c<0>(a1) = from_diag(alpha);
      boost::proto::child_c<1>(a1) = from_diag(beta);
    }

    //==========================================================================
    /// INTERNAL ONLY - [vr, w, vl] = GENEIG(A, B)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_3(a0, a1, nt2::policy<ext::matrix_>());
    }

  //==========================================================================
    /// INTERNAL ONLY - [vr, alpha, beta] = GENEIG(A, B, alphabeta_) or
    ///                 [vr, w, vl]       = GENEIG(A, B, matrix_/vectors)
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_3(a0, a1, boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::alphabeta_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      size_t n = height(a);
      nt2::container::table <type_t> beta(nt2::of_size(n,1));
      nt2::container::table <ctype_t> alpha(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      NT2_LAPACK_VERIFY(nt2::ggev_wvr( boost::proto::value(a)
                                     , boost::proto::value(b)
                                     , boost::proto::value(alpha)
                                     , boost::proto::value(beta)
                                     , boost::proto::value(vr)
                                     ));
      boost::proto::child_c<1>(a1) = nt2::from_diag(alpha);
      boost::proto::child_c<2>(a1) = nt2::from_diag(beta);
      assign_swap(boost::proto::child_c<0>(a1), vr);
    }

    /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, alpha
                           , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, vl
                           , boost::proto::child_c<2>(a1));
      size_t n = height(a);
      alpha.resize(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      container::table <type_t>  beta(nt2::of_size(n,1));
      NT2_LAPACK_VERIFY(nt2::ggev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(b)
                                       , boost::proto::value(alpha)
                                       , boost::proto::value(beta)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      assign_swap(boost::proto::child_c<1>(a1), alpha);
      boost::proto::child_c<1>(a1) /= beta;
      assign_swap(boost::proto::child_c<0>(a1), vr);
      assign_swap(boost::proto::child_c<2>(a1), vl);
    }

    /// INTERNAL ONLY: 3o 3i
    BOOST_FORCEINLINE
    void eval3_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, vl
                           , boost::proto::child_c<2>(a1));
      size_t n = height(a);
      container::table <type_t> beta(nt2::of_size(n,1));
      container::table <ctype_t> alpha(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      vl.resize(nt2::of_size(n,n));
      NT2_LAPACK_VERIFY(nt2::ggev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(b)
                                       , boost::proto::value(alpha)
                                       , boost::proto::value(beta)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      boost::proto::child_c<1>(a1) = from_diag(alpha/beta);
      assign_swap(boost::proto::child_c<0>(a1), vr);
      assign_swap(boost::proto::child_c<2>(a1), vl);
      //from_diag doesnt support aliasing currently
    }

   //==========================================================================
    /// INTERNAL ONLY - [vr, alpha, beta, vl] = GENEIG(A, B) or
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<2> const&
              , boost::mpl::long_<4> const&
              ) const
    {
      eval4_3(a0, a1
             , nt2::policy<ext::matrix_>());
    }
    //==========================================================================
    /// INTERNAL ONLY - [vr, alpha, beta] = GENEIG(A, B, alphabeta_, matrix_/vectors) or
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<4> const&
              , boost::mpl::long_<3> const&
              ) const
    {
      eval3_4(a0, a1
             , boost::proto::value(boost::proto::child_c<2>(a0))
             , boost::proto::value(boost::proto::child_c<3>(a0)));
    }

   /// INTERNAL ONLY: 3o 4i
    BOOST_FORCEINLINE
    void eval3_4 ( A0& a0, A1& a1
                 , nt2::policy<ext::alphabeta_> const &
                 , nt2::policy<ext::matrix_>  const &
                 ) const
    {
      eval3_3(a0, a1, nt2::policy<ext::alphabeta_>());
    }

   /// INTERNAL ONLY: 3o 4i
    BOOST_FORCEINLINE
    void eval3_4 ( A0& a0, A1& a1
                 , nt2::policy<ext::alphabeta_> const &
                 , nt2::policy<ext::vector_>  const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, alpha
                           , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (o_semantic, beta
                           , boost::proto::child_c<2>(a1));
      size_t n = height(a);
      beta.resize(nt2::of_size(n,1));
      alpha.resize(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      NT2_LAPACK_VERIFY(nt2::ggev_wvr( boost::proto::value(a)
                                     , boost::proto::value(b)
                                     , boost::proto::value(alpha)
                                     , boost::proto::value(beta)
                                     , boost::proto::value(vr)
                                     ));
      assign_swap(boost::proto::child_c<1>(a1), alpha);
      assign_swap(boost::proto::child_c<2>(a1), beta);
      assign_swap(boost::proto::child_c<0>(a1), vr);

    }

    //==========================================================================
    /// INTERNAL ONLY - [vr, alpha, beta, vl] = GENEIG(A, B, matrix_/vectors) or
    BOOST_FORCEINLINE
    void eval ( A0& a0, A1& a1
              , boost::mpl::long_<3> const&
              , boost::mpl::long_<4> const&
              ) const
    {
      eval4_3(a0, a1
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

    /// INTERNAL ONLY: 4o 3i
    BOOST_FORCEINLINE
    void eval4_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::vector_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, alpha
                           , boost::proto::child_c<1>(a1));
      NT2_AS_TERMINAL_OUT  (o_semantic, beta
                           , boost::proto::child_c<2>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, vl
                           , boost::proto::child_c<3>(a1));
      size_t n = height(a);
      alpha.resize(nt2::of_size(n,1));
      beta.resize(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      vl.resize(nt2::of_size(n,n));
      NT2_LAPACK_VERIFY(nt2::ggev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(b)
                                       , boost::proto::value(alpha)
                                       , boost::proto::value(beta)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      assign_swap(boost::proto::child_c<0>(a1), vr);
      assign_swap(boost::proto::child_c<3>(a1), vl);
      assign_swap(boost::proto::child_c<1>(a1), alpha);
      assign_swap(boost::proto::child_c<2>(a1), beta);
    }

     /// INTERNAL ONLY: 4o 3i
    BOOST_FORCEINLINE
    void eval4_3 ( A0& a0, A1& a1
                 , nt2::policy<ext::matrix_> const &
                 ) const
    {
      nt2::container::table<type_t> wa, wb;
      NT2_AS_TERMINAL_INOUT(o_semantic, a
                           , boost::proto::child_c<0>(a0), wa);
      NT2_AS_TERMINAL_INOUT(o_semantic, b
                           , boost::proto::child_c<1>(a0), wb);
      NT2_AS_TERMINAL_OUT  (c_semantic, vr
                           , boost::proto::child_c<0>(a1));
      NT2_AS_TERMINAL_OUT  (c_semantic, vl
                           , boost::proto::child_c<3>(a1));
      size_t n = height(a);
      nt2::container::table <type_t> beta(nt2::of_size(n,1));
      nt2::container::table <ctype_t> alpha(nt2::of_size(n,1));
      vr.resize(nt2::of_size(n,n));
      vl.resize(nt2::of_size(n,n));
      NT2_LAPACK_VERIFY(nt2::ggev_wvrvl( boost::proto::value(a)
                                       , boost::proto::value(b)
                                       , boost::proto::value(alpha)
                                       , boost::proto::value(beta)
                                       , boost::proto::value(vr)
                                       , boost::proto::value(vl)
                                       ));
      assign_swap(boost::proto::child_c<0>(a1), vr);
      assign_swap(boost::proto::child_c<3>(a1), vl);
      boost::proto::child_c<1>(a1) = from_diag(alpha);
      boost::proto::child_c<2>(a1) = from_diag(beta);
      //from_diag doesnt support aliasing currently
    }

     /// INTERNAL ONLY: 4o 3i
    template < class T > BOOST_FORCEINLINE
    void eval4_3 ( A0& a0, A1& a1
                 , T const &
                 ) const
    {
      eval4_3(a0, a1
             , boost::proto::value(boost::proto::child_c<2>(a0)));
    }

  };
} }

#endif


// boost\math\distributions\geometric.hpp

// Copyright John Maddock 2010.
// Copyright Paul A. Bristow 2010.

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

// geometric distribution is a discrete probability distribution.
// It expresses the probability distribution of the number (k) of
// events, occurrences, failures or arrivals before the first success.
// supported on the set {0, 1, 2, 3...}

// Note that the set includes zero (unlike some definitions that start at one).

// The random variate k is the number of events, occurrences or arrivals.
// k argument may be integral, signed, or unsigned, or floating point.
// If necessary, it has already been promoted from an integral type.

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

// See http://en.wikipedia.org/wiki/geometric_distribution
// http://documents.wolfram.com/v5/Add-onsLinks/StandardPackages/Statistics/DiscreteDistributions.html
// http://mathworld.wolfram.com/GeometricDistribution.html

#ifndef BOOST_MATH_SPECIAL_GEOMETRIC_HPP
#define BOOST_MATH_SPECIAL_GEOMETRIC_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/beta.hpp> // for ibeta(a, b, x) == Ix(a, b).
#include <boost/math/distributions/complement.hpp> // complement.
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks domain_error & logic_error.
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/tools/roots.hpp> // for root finding.
#include <boost/math/distributions/detail/inv_discrete_quantile.hpp>

#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/if.hpp>

#include <limits> // using std::numeric_limits;
#include <utility>

#if defined (BOOST_MSVC)
#  pragma warning(push)
// This believed not now necessary, so commented out.
//#  pragma warning(disable: 4702) // unreachable code.
// in domain_error_imp in error_handling.
#endif

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

      template <class RealType, class Policy>
      inline bool check_dist(const char* function, const RealType& p, RealType* result, const Policy& pol)
      {
        return check_success_fraction(function, p, result, pol);
      }

      template <class RealType, class Policy>
      inline bool check_dist_and_k(const char* function,  const RealType& p, RealType k, RealType* result, const Policy& pol)
      {
        if(check_dist(function, p, result, pol) == false)
        {
          return false;
        }
        if( !(boost::math::isfinite)(k) || (k < 0) )
        { // Check k failures.
          *result = policies::raise_domain_error<RealType>(
            function,
            "Number of failures argument is %1%, but must be >= 0 !", k, pol);
          return false;
        }
        return true;
      } // Check_dist_and_k

      template <class RealType, class Policy>
      inline bool check_dist_and_prob(const char* function, RealType p, RealType prob, RealType* result, const Policy& pol)
      {
        if((check_dist(function, p, result, pol) && detail::check_probability(function, prob, result, pol)) == false)
        {
          return false;
        }
        return true;
      } // check_dist_and_prob
    } //  namespace geometric_detail

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

      geometric_distribution(RealType p) : m_p(p)
      { // Constructor stores success_fraction p.
        RealType result;
        geometric_detail::check_dist(
          "geometric_distribution<%1%>::geometric_distribution",
          m_p, // Check success_fraction 0 <= p <= 1.
          &result, Policy());
      } // geometric_distribution constructor.

      // Private data getter class member functions.
      RealType success_fraction() const
      { // Probability of success as fraction in range 0 to 1.
        return m_p;
      }
      RealType successes() const
      { // Total number of successes r = 1 (for compatibility with negative binomial?).
        return 1;
      }

      // Parameter estimation.
      // (These are copies of negative_binomial distribution with successes = 1).
      static RealType find_lower_bound_on_p(
        RealType trials,
        RealType alpha) // alpha 0.05 equivalent to 95% for one-sided test.
      {
        static const char* function = "boost::math::geometric<%1%>::find_lower_bound_on_p";
        RealType result = 0;  // of error checks.
        RealType successes = 1;
        RealType failures = trials - successes;
        if(false == detail::check_probability(function, alpha, &result, Policy())
          && geometric_detail::check_dist_and_k(
          function, RealType(0), failures, &result, Policy()))
        {
          return result;
        }
        // Use complement ibeta_inv function for lower bound.
        // This is adapted from the corresponding binomial formula
        // here: http://www.itl.nist.gov/div898/handbook/prc/section2/prc241.htm
        // This is a Clopper-Pearson interval, and may be overly conservative,
        // see also "A Simple Improved Inferential Method for Some
        // Discrete Distributions" Yong CAI and K. KRISHNAMOORTHY
        // http://www.ucs.louisiana.edu/~kxk4695/Discrete_new.pdf
        //
        return ibeta_inv(successes, failures + 1, alpha, static_cast<RealType*>(0), Policy());
      } // find_lower_bound_on_p

      static RealType find_upper_bound_on_p(
        RealType trials,
        RealType alpha) // alpha 0.05 equivalent to 95% for one-sided test.
      {
        static const char* function = "boost::math::geometric<%1%>::find_upper_bound_on_p";
        RealType result = 0;  // of error checks.
        RealType successes = 1;
        RealType failures = trials - successes;
        if(false == geometric_detail::check_dist_and_k(
          function, RealType(0), failures, &result, Policy())
          && detail::check_probability(function, alpha, &result, Policy()))
        {
          return result;
        }
        if(failures == 0)
        {
           return 1;
        }// Use complement ibetac_inv function for upper bound.
        // Note adjusted failures value: *not* failures+1 as usual.
        // This is adapted from the corresponding binomial formula
        // here: http://www.itl.nist.gov/div898/handbook/prc/section2/prc241.htm
        // This is a Clopper-Pearson interval, and may be overly conservative,
        // see also "A Simple Improved Inferential Method for Some
        // Discrete Distributions" Yong CAI and K. Krishnamoorthy
        // http://www.ucs.louisiana.edu/~kxk4695/Discrete_new.pdf
        //
        return ibetac_inv(successes, failures, alpha, static_cast<RealType*>(0), Policy());
      } // find_upper_bound_on_p

      // Estimate number of trials :
      // "How many trials do I need to be P% sure of seeing k or fewer failures?"

      static RealType find_minimum_number_of_trials(
        RealType k,     // number of failures (k >= 0).
        RealType p,     // success fraction 0 <= p <= 1.
        RealType alpha) // risk level threshold 0 <= alpha <= 1.
      {
        static const char* function = "boost::math::geometric<%1%>::find_minimum_number_of_trials";
        // Error checks:
        RealType result = 0;
        if(false == geometric_detail::check_dist_and_k(
          function, p, k, &result, Policy())
          && detail::check_probability(function, alpha, &result, Policy()))
        {
          return result;
        }
        result = ibeta_inva(k + 1, p, alpha, Policy());  // returns n - k
        return result + k;
      } // RealType find_number_of_failures

      static RealType find_maximum_number_of_trials(
        RealType k,     // number of failures (k >= 0).
        RealType p,     // success fraction 0 <= p <= 1.
        RealType alpha) // risk level threshold 0 <= alpha <= 1.
      {
        static const char* function = "boost::math::geometric<%1%>::find_maximum_number_of_trials";
        // Error checks:
        RealType result = 0;
        if(false == geometric_detail::check_dist_and_k(
          function, p, k, &result, Policy())
          &&  detail::check_probability(function, alpha, &result, Policy()))
        {
          return result;
        }
        result = ibetac_inva(k + 1, p, alpha, Policy());  // returns n - k
        return result + k;
      } // RealType find_number_of_trials complemented

    private:
      //RealType m_r; // successes fixed at unity.
      RealType m_p; // success_fraction
    }; // template <class RealType, class Policy> class geometric_distribution

    typedef geometric_distribution<double> geometric; // Reserved name of type double.

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

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

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

    // median implemented via quantile(half) in derived accessors.

    template <class RealType, class Policy>
    inline RealType mode(const geometric_distribution<RealType, Policy>&)
    { // Mode of geometric distribution = zero.
      BOOST_MATH_STD_USING // ADL of std functions.
      return 0;
    } // mode

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

    template <class RealType, class Policy>
    inline RealType skewness(const geometric_distribution<RealType, Policy>& dist)
    { // skewness of geometric distribution = 2-p / (sqrt(r(1-p))
      BOOST_MATH_STD_USING // ADL of std functions.
      RealType p = dist.success_fraction();
      return (2 - p) / sqrt(1 - p);
    } // skewness

    template <class RealType, class Policy>
    inline RealType kurtosis(const geometric_distribution<RealType, Policy>& dist)
    { // kurtosis of geometric distribution
      // http://en.wikipedia.org/wiki/geometric is kurtosis_excess so add 3
      RealType p = dist.success_fraction();
      return 3 + (p*p - 6*p + 6) / (1 - p);
    } // kurtosis

     template <class RealType, class Policy>
    inline RealType kurtosis_excess(const geometric_distribution<RealType, Policy>& dist)
    { // kurtosis excess of geometric distribution
      // http://mathworld.wolfram.com/Kurtosis.html table of kurtosis_excess
      RealType p = dist.success_fraction();
      return (p*p - 6*p + 6) / (1 - p);
    } // kurtosis_excess

    // RealType standard_deviation(const geometric_distribution<RealType, Policy>& dist)
    // standard_deviation provided by derived accessors.
    // RealType hazard(const geometric_distribution<RealType, Policy>& dist)
    // hazard of geometric distribution provided by derived accessors.
    // RealType chf(const geometric_distribution<RealType, Policy>& dist)
    // chf of geometric distribution provided by derived accessors.

    template <class RealType, class Policy>
    inline RealType pdf(const geometric_distribution<RealType, Policy>& dist, const RealType& k)
    { // Probability Density/Mass Function.
      BOOST_FPU_EXCEPTION_GUARD
      BOOST_MATH_STD_USING  // For ADL of math functions.
      static const char* function = "boost::math::pdf(const geometric_distribution<%1%>&, %1%)";

      RealType p = dist.success_fraction();
      RealType result = 0;
      if(false == geometric_detail::check_dist_and_k(
        function,
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      if (k == 0)
      {
        return p; // success_fraction
      }
      RealType q = 1 - p;  // Inaccurate for small p?
      // So try to avoid inaccuracy for large or small p.
      // but has little effect > last significant bit.
      //cout << "p *  pow(q, k) " << result << endl; // seems best whatever p
      //cout << "exp(p * k * log1p(-p)) " << p * exp(k * log1p(-p)) << endl;
      //if (p < 0.5)
      //{
      //  result = p *  pow(q, k);
      //}
      //else
      //{
      //  result = p * exp(k * log1p(-p));
      //}
      result = p * pow(q, k);
      return result;
    } // geometric_pdf

    template <class RealType, class Policy>
    inline RealType cdf(const geometric_distribution<RealType, Policy>& dist, const RealType& k)
    { // Cumulative Distribution Function of geometric.
      static const char* function = "boost::math::cdf(const geometric_distribution<%1%>&, %1%)";

      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      RealType p = dist.success_fraction();
      // Error check:
      RealType result = 0;
      if(false == geometric_detail::check_dist_and_k(
        function,
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      if(k == 0)
      {
        return p; // success_fraction
      }
      //RealType q = 1 - p;  // Bad for small p
      //RealType probability = 1 - std::pow(q, k+1);

      RealType z = boost::math::log1p(-p, Policy()) * (k + 1);
      RealType probability = -boost::math::expm1(z, Policy());

      return probability;
    } // cdf Cumulative Distribution Function geometric.

      template <class RealType, class Policy>
      inline RealType cdf(const complemented2_type<geometric_distribution<RealType, Policy>, RealType>& c)
      { // Complemented Cumulative Distribution Function geometric.
      BOOST_MATH_STD_USING
      static const char* function = "boost::math::cdf(const geometric_distribution<%1%>&, %1%)";
      // k argument may be integral, signed, or unsigned, or floating point.
      // If necessary, it has already been promoted from an integral type.
      RealType const& k = c.param;
      geometric_distribution<RealType, Policy> const& dist = c.dist;
      RealType p = dist.success_fraction();
      // Error check:
      RealType result = 0;
      if(false == geometric_detail::check_dist_and_k(
        function,
        p,
        k,
        &result, Policy()))
      {
        return result;
      }
      RealType z = boost::math::log1p(-p, Policy()) * (k+1);
      RealType probability = exp(z);
      return probability;
    } // cdf Complemented Cumulative Distribution Function geometric.

    template <class RealType, class Policy>
    inline RealType quantile(const geometric_distribution<RealType, Policy>& dist, const RealType& x)
    { // Quantile, percentile/100 or Percent Point geometric function.
      // Return the number of expected failures k for a given probability p.

      // Inverse cumulative Distribution Function or Quantile (percentile / 100) of geometric Probability.
      // k argument may be integral, signed, or unsigned, or floating point.

      static const char* function = "boost::math::quantile(const geometric_distribution<%1%>&, %1%)";
      BOOST_MATH_STD_USING // ADL of std functions.

      RealType success_fraction = dist.success_fraction();
      // Check dist and x.
      RealType result = 0;
      if(false == geometric_detail::check_dist_and_prob
        (function, success_fraction, x, &result, Policy()))
      {
        return result;
      }

      // Special cases.
      if (x == 1)
      {  // Would need +infinity failures for total confidence.
        result = policies::raise_overflow_error<RealType>(
            function,
            "Probability argument is 1, which implies infinite failures !", Policy());
        return result;
       // usually means return +std::numeric_limits<RealType>::infinity();
       // unless #define BOOST_MATH_THROW_ON_OVERFLOW_ERROR
      }
      if (x == 0)
      { // No failures are expected if P = 0.
        return 0; // Total trials will be just dist.successes.
      }
      // if (P <= pow(dist.success_fraction(), 1))
      if (x <= success_fraction)
      { // p <= pdf(dist, 0) == cdf(dist, 0)
        return 0;
      }
      if (x == 1)
      {
        return 0;
      }

      // log(1-x) /log(1-success_fraction) -1; but use log1p in case success_fraction is small
      result = boost::math::log1p(-x, Policy()) / boost::math::log1p(-success_fraction, Policy()) - 1;
      // Subtract a few epsilons here too?
      // to make sure it doesn't slip over, so ceil would be one too many.
      return result;
    } // RealType quantile(const geometric_distribution dist, p)

    template <class RealType, class Policy>
    inline RealType quantile(const complemented2_type<geometric_distribution<RealType, Policy>, RealType>& c)
    {  // Quantile or Percent Point Binomial function.
       // Return the number of expected failures k for a given
       // complement of the probability Q = 1 - P.
       static const char* function = "boost::math::quantile(const geometric_distribution<%1%>&, %1%)";
       BOOST_MATH_STD_USING
       // Error checks:
       RealType x = c.param;
       const geometric_distribution<RealType, Policy>& dist = c.dist;
       RealType success_fraction = dist.success_fraction();
       RealType result = 0;
       if(false == geometric_detail::check_dist_and_prob(
          function,
          success_fraction,
          x,
          &result, Policy()))
       {
          return result;
       }

       // Special cases:
       if(x == 1)
       {  // There may actually be no answer to this question,
          // since the probability of zero failures may be non-zero,
          return 0; // but zero is the best we can do:
       }
       if (-x <= boost::math::powm1(dist.success_fraction(), dist.successes(), Policy()))
       {  // q <= cdf(complement(dist, 0)) == pdf(dist, 0)
          return 0; //
       }
       if(x == 0)
       {  // Probability 1 - Q  == 1 so infinite failures to achieve certainty.
          // Would need +infinity failures for total confidence.
          result = policies::raise_overflow_error<RealType>(
             function,
             "Probability argument complement is 0, which implies infinite failures !", Policy());
          return result;
          // usually means return +std::numeric_limits<RealType>::infinity();
          // unless #define BOOST_MATH_THROW_ON_OVERFLOW_ERROR
       }
       // log(x) /log(1-success_fraction) -1; but use log1p in case success_fraction is small
       result = log(x) / boost::math::log1p(-success_fraction, Policy()) - 1;
      return result;

    } // quantile complement

 } // namespace math
} // namespace boost

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

#if defined (BOOST_MSVC)
# pragma warning(pop)
#endif

#endif // BOOST_MATH_SPECIAL_GEOMETRIC_HPP


// This file is part of the dune-gdt project:
//   http://users.dune-project.org/projects/dune-gdt
// Copyright holders: Felix Schindler
// License: BSD 2-Clause License (http://opensource.org/licenses/BSD-2-Clause)

#ifndef DUNE_GDT_OPERATORS_DARCY_HH
#define DUNE_GDT_OPERATORS_DARCY_HH

#include <limits>

#include <boost/numeric/conversion/cast.hpp>

#include <dune/geometry/quadraturerules.hh>

#include <dune/stuff/common/exceptions.hh>
#include <dune/stuff/common/type_utils.hh>
#include <dune/stuff/functions/interfaces.hh>
#include <dune/stuff/la/container.hh>
#include <dune/stuff/la/solver.hh>

#include <dune/gdt/discretefunction/default.hh>
#include <dune/gdt/exceptions.hh>
#include <dune/gdt/spaces/cg/interface.hh>
#include <dune/gdt/spaces/rt/interface.hh>

#include "interfaces.hh"

namespace Dune {
namespace GDT {
namespace Exceptions {

class darcy_operator_error : public operator_error {};

} // namespace Exceptions
namespace Operators {

// forward, to be used in the traits
template< class GridViewImp, class FunctionImp >
class Darcy;

namespace internal {

template< class GridViewImp, class FunctionImp >
class DarcyTraits
{
  static_assert(Stuff::is_localizable_function< FunctionImp >::value,
                "FunctionImp has to be derived from Stuff::IsLocalizableFunction!");
  static_assert(std::is_same< typename GridViewImp::ctype, typename FunctionImp::DomainFieldType >::value,
                "Types do not match!");
  static_assert(GridViewImp::dimension == FunctionImp::dimDomain, "Dimensions do not match!");
public:
  typedef Darcy< GridViewImp, FunctionImp >    derived_type;
  typedef GridViewImp                          GridViewType;
  typedef typename FunctionImp::RangeFieldType FieldType;
}; // class DarcyTraits

} // namespace internal

/**
  * \note Only works for scalar valued function atm.
  **/
template< class GridViewImp, class FunctionImp >
class Darcy
  : public OperatorInterface< internal::DarcyTraits< GridViewImp, FunctionImp > >
{
public:
  typedef internal::DarcyTraits< GridViewImp, FunctionImp >  Traits;
  typedef typename Traits::GridViewType                      GridViewType;
  typedef typename Traits::FieldType                         FieldType;
  typedef typename GridViewType::template Codim< 0 >::Entity EntityType;
  typedef typename GridViewType::ctype                       DomainFieldType;
  static const size_t                                        dimDomain = GridViewType::dimension;

  Darcy(const GridViewType& grd_vw, const FunctionImp& function)
    : grid_view_(grd_vw)
    , function_(function)
  {}

  /**
   * \brief Applies the operator.
   * \note  See redirect_apply for the implementation (depending on the type of the range space).
   * \sa    redirect_apply
   */
  template< class S, class V, size_t r, size_t rC >
  void apply(const Stuff::LocalizableFunctionInterface< EntityType, DomainFieldType, dimDomain, FieldType, r, rC >& source,
             DiscreteFunction< S, V >& range) const
  {
    redirect_apply(range.space(), source, range);
  }

private:
  /**
   * \brief Does an L2 projection of '- function * \gradient source' onto range.
   */
  template< class T, class S, class V >
  void redirect_apply(const Spaces::CGInterface< T, dimDomain, dimDomain, 1 >& /*space*/,
                      const Stuff::LocalizableFunctionInterface< EntityType, DomainFieldType, dimDomain, FieldType, 1, 1 >& source,
                      DiscreteFunction< S, V >& range) const
  {
    typedef typename Stuff::LA::Container< FieldType >::MatrixType MatrixType;
    typedef typename Stuff::LA::Container< FieldType >::VectorType VectorType;
    MatrixType lhs(range.space().mapper().size(),
                   range.space().mapper().size(),
                   range.space().compute_volume_pattern());
    VectorType rhs(range.space().mapper().size());

    // walk the grid
    const auto entity_it_end = grid_view_.template end< 0 >();
    for (auto entity_it = grid_view_.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      const auto local_function = function_.local_function(entity);
      const auto local_source = source.local_function(entity);
      const auto basis = range.space().base_function_set(entity);
      // do a volume quadrature
      const size_t integrand_order = std::max(local_function->order() + ssize_t(local_source->order()) - 1,
                                              basis.order())
                                     + basis.order();
      const auto& quadrature = QuadratureRules< DomainFieldType, dimDomain >::rule(entity.type(),
                                                                                boost::numeric_cast< int >(integrand_order));
      const auto quadrature_it_end = quadrature.end();
      for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
        const auto xx = quadrature_it->position();
        const auto quadrature_weight = quadrature_it->weight();
        const auto integration_element = entity.geometry().integrationElement(xx);
        const auto function_value = local_function->evaluate(xx);
        const auto source_gradient = local_source->jacobian(xx);
        const auto basis_value = basis.evaluate(xx);
        for (size_t ii = 0; ii < basis.size(); ++ii) {
          const size_t global_ii = range.space().mapper().mapToGlobal(entity, ii);
          rhs.add_to_entry(global_ii,
                           integration_element * quadrature_weight
                           * -1.0 * function_value * (source_gradient[0] * basis_value[ii]));
          for (size_t jj = 0; jj < basis.size(); ++jj) {
            const size_t global_jj = range.space().mapper().mapToGlobal(entity, jj);
            lhs.add_to_entry(global_ii,
                             global_jj,
                             integration_element * quadrature_weight * (basis_value[ii] * basis_value[jj]));
          }
        }
      } // do a volume quadrature
    } // walk the grid

    // solve
    try {
      Stuff::LA::Solver< MatrixType >(lhs).apply(rhs, range.vector());
    } catch (Stuff::Exceptions::linear_solver_failed& ee) {
      DUNE_THROW(Exceptions::darcy_operator_error,
                 "Application of the Darcy operator failed because a matrix could not be inverted!\n\n"
                 << "This was the original error: " << ee.what());
    }
  } // ... redirect_apply(...)

  template< class T, class S, class V >
  void redirect_apply(const Spaces::RTInterface< T, dimDomain, dimDomain, 1 >& /*space*/,
                      const Stuff::LocalizableFunctionInterface< EntityType, DomainFieldType, dimDomain, FieldType, 1 >& source,
                      DiscreteFunction< S, V >& range) const
  {
    static_assert(Spaces::RTInterface< T, dimDomain, 1 >::polOrder == 0, "Untested!");
    const auto& rtn0_space = range.space();
    auto& range_vector = range.vector();
    const auto infinity = std::numeric_limits< FieldType >::infinity();
    for (size_t ii = 0; ii < range_vector.size(); ++ii)
      range_vector[ii] = infinity;
    // walk the grid
    const auto entity_it_end = grid_view_.template end< 0 >();
    for (auto entity_it = grid_view_.template begin< 0 >(); entity_it != entity_it_end; ++entity_it) {
      const auto& entity = *entity_it;
      const auto local_DoF_indices = rtn0_space.local_DoF_indices(entity);
      const auto global_DoF_indices = rtn0_space.mapper().globalIndices(entity);
      assert(global_DoF_indices.size() == local_DoF_indices.size());
      const auto local_function = function_.local_function(entity);
      const auto local_source = source.local_function(entity);
      const auto local_basis = rtn0_space.base_function_set(entity);
      // walk the intersections
      const auto intersection_it_end = grid_view_.iend(entity);
      for (auto intersection_it = grid_view_.ibegin(entity);
           intersection_it != intersection_it_end;
           ++intersection_it) {
        const auto& intersection = *intersection_it;
        if (intersection.neighbor() && !intersection.boundary()) {
          const auto neighbor_ptr = intersection.outside();
          const auto& neighbor = *neighbor_ptr;
          if (grid_view_.indexSet().index(entity) < grid_view_.indexSet().index(neighbor)) {
            const auto local_function_neighbor = function_.local_function(neighbor);
            const auto local_source_neighbor = source.local_function(neighbor);
            const size_t local_intersection_index = intersection.indexInInside();
            const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
            // do a face quadrature
            FieldType lhs = 0;
            FieldType rhs = 0;
            const size_t integrand_order = local_function->order();
            const auto& quadrature = QuadratureRules< DomainFieldType, dimDomain - 1 >::rule(intersection.type(),
                                                                                boost::numeric_cast< int >(integrand_order));
            const auto quadrature_it_end = quadrature.end();
            for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
              const auto xx_intersection = quadrature_it->position();
              const auto normal = intersection.unitOuterNormal(xx_intersection);
              const FieldType integration_factor = intersection.geometry().integrationElement(xx_intersection);
              const FieldType weigth = quadrature_it->weight();
              const auto xx_entity = intersection.geometryInInside().global(xx_intersection);
              const auto xx_neighbor = intersection.geometryInOutside().global(xx_intersection);
              // evaluate
              auto function_value = local_function->evaluate(xx_entity);
              function_value *= 0.5;
              auto function_value_neighbor = local_function_neighbor->evaluate(xx_neighbor);
              function_value_neighbor *= 0.5;
              function_value += function_value_neighbor;
              auto source_gradient = local_source->jacobian(xx_entity);
              source_gradient *= 0.5;
              auto source_gradient_neighbor = local_source_neighbor->jacobian(xx_neighbor);
              source_gradient_neighbor *= 0.5;
              source_gradient += source_gradient_neighbor;
              const auto basis_values = local_basis.evaluate(xx_entity);
              const auto basis_value = basis_values[local_DoF_index];
              // compute integrals
              lhs += integration_factor * weigth * (basis_value * normal);
              rhs += integration_factor * weigth * -1.0 * function_value * (source_gradient[0] * normal);
            } // do a face quadrature
            // set DoF
            const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
            assert(!(range_vector[global_DoF_index] < infinity));
            range_vector[global_DoF_index] = rhs / lhs;
          }
        } else if (intersection.boundary() && !intersection.neighbor()) {
          const size_t local_intersection_index = intersection.indexInInside();
          const size_t local_DoF_index = local_DoF_indices[local_intersection_index];
          // do a face quadrature
          FieldType lhs = 0;
          FieldType rhs = 0;
          const size_t integrand_order = local_function->order();
          const auto& quadrature = QuadratureRules< DomainFieldType, dimDomain - 1 >::rule(intersection.type(),
                                                                                boost::numeric_cast< int >(integrand_order));
          const auto quadrature_it_end = quadrature.end();
          for (auto quadrature_it = quadrature.begin(); quadrature_it != quadrature_it_end; ++quadrature_it) {
            const auto xx_intersection = quadrature_it->position();
            const auto normal = intersection.unitOuterNormal(xx_intersection);
            const auto integration_factor = intersection.geometry().integrationElement(xx_intersection);
            const auto weigth = quadrature_it->weight();
            const auto xx_entity = intersection.geometryInInside().global(xx_intersection);
            // evalaute
            const auto function_value = local_function->evaluate(xx_entity);
            const auto source_gradient = local_source->jacobian(xx_entity);
            const auto basis_values = local_basis.evaluate(xx_entity);
            const auto basis_value = basis_values[local_DoF_index];
            // compute integrals
            lhs += integration_factor * weigth * (basis_value * normal);
            rhs += integration_factor * weigth * -1.0 * function_value * (source_gradient[0] * normal);
          } // do a face quadrature
          // set DoF
          const size_t global_DoF_index = global_DoF_indices[local_DoF_index];
          assert(!(range_vector[global_DoF_index] < infinity));
          range_vector[global_DoF_index] = rhs / lhs;
        } else
          DUNE_THROW(Stuff::Exceptions::internal_error, "Unknown intersection type!");
      } // walk the intersections
    } // walk the grid
  } // ... redirect_apply(...)

  const GridViewType& grid_view_;
  const FunctionImp& function_;
}; // class Darcy

} // namespace Operators
} // namespace GDT
} // namespace Dune

#endif // DUNE_GDT_OPERATORS_DARCY_HH


// smooth: Lie Theory for Robotics
// https://github.com/pettni/smooth
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// 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.

#ifndef SMOOTH__CONCEPTS_HPP_
#define SMOOTH__CONCEPTS_HPP_

/**
 * @file concepts.hpp Library concept definitions.
 */

#include <concepts>

#include <Eigen/Core>

namespace smooth
{

/**
 * @brief A concept defining a (smooth) manifold.
 *
 * - `M::Scalar` scalar type
 * - `M::SizeAtCompileTime` tangent space dimension (compile time, -1 if dynamic)
 * - `M.size()` : tangent space dimension (runtime)
 * - `M + T -> M` : geodesic addition
 * - `M - M -> T` : inverse of geodesic addition (in practice only used for infinitesimal values)
 *
 * Where `T = Eigen::Matrix<Scalar, SizeAtCompileTime, 1>` is the tangent type
 */
template<typename M>
concept Manifold =
requires
{
  typename M::Scalar;
  typename M::PlainObject;
  {M::SizeAtCompileTime}->std::convertible_to<Eigen::Index>;  // degrees of freedom at compile time
} &&
requires(const M & m1, const M & m2, const Eigen::Matrix<typename M::Scalar, M::SizeAtCompileTime, 1> & a)
{
  {m1.size()}->std::convertible_to<Eigen::Index>;             // degrees of freedom at runtime
  {m1 + a}->std::convertible_to<typename M::PlainObject>;
  {m1 - m2}->std::convertible_to<Eigen::Matrix<typename M::Scalar, M::SizeAtCompileTime, 1>>;
  {m1.template cast<double>()};
};

/**
 * @brief Lie group concept.
 *
 * Requires the exp and log maps, and the upper and lowercase adjoints.
 */
template<typename G>
concept LieGroup = Manifold<G> &&
// static constants
requires {
  typename G::Tangent;
  {G::Dof}->std::convertible_to<Eigen::Index>;
} &&
(G::Dof >= 1) &&
(G::SizeAtCompileTime == G::Dof) &&
(G::Tangent::SizeAtCompileTime == G::Dof) &&
// member methods
requires(const G & g1, const G & g2)
{
  {g1.inverse()}->std::convertible_to<typename G::PlainObject>;
  {g1 * g2}->std::convertible_to<typename G::PlainObject>;
  {g1.log()}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, 1>>;
  {g1.Ad()}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
} &&
// static methods
requires(const Eigen::Matrix<typename G::Scalar, G::Dof, 1> & a)
{
  {G::Identity()}->std::convertible_to<typename G::PlainObject>;
  {G::exp(a)}->std::convertible_to<typename G::PlainObject>;
  {G::ad(a)}->std::convertible_to<Eigen::Matrix<typename G::Scalar, G::Dof, G::Dof>>;
};

} // namespace smooth

#endif  // SMOOTH__CONCEPTS_HPP_