// (C) Copyright Renaud Detry   2007-2011.
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//#define TRSL_USE_BSD_BETTER_RANDOM_GENERATOR
//#define TRSL_USE_SYSTEMATIC_INTUITIVE_ALGORITHM

#include <trsl/sort_iterator.hpp>
#include <trsl/is_picked_systematic.hpp>
#include <trsl/random_permutation_iterator.hpp>
#include <trsl/ppfilter_iterator.hpp>

#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <vector>
#include <iostream>
#include <numeric> // accumulate

// In this example, population elements are floats, and an element's
// weight is the element itself. The weight accessor is:
struct wac
{
  double operator()(float const& e) const
    {
      return e;
    }
};

int main()
{

  //-----------------------//
  // Generate a population //
  //-----------------------//

#define C_ARRAY_ITERATOR_SAMPLING
//#define STD_VECTOR_ITERATOR_SAMPLING

#if defined(C_ARRAY_ITERATOR_SAMPLING)
  float population[] = { 0, 1, 4, 3, 5, 8, 2 };
  const size_t POPULATION_SIZE = sizeof(population)/sizeof(float);
  const size_t SAMPLE_SIZE = 3;

  typedef float* population_iterator;
  population_iterator populationIteratorBegin(population),
    populationIteratorEnd(population + POPULATION_SIZE);
#elif defined(STD_VECTOR_ITERATOR_SAMPLING)
  boost::mt19937 rng;
  boost::uniform_real<float> dist(0, 100);
  boost::variate_generator<
    boost::mt19937&,
    boost::uniform_real<float>
    > uni(rng, dist);

  const size_t POPULATION_SIZE = 100;
  std::vector<float> population(POPULATION_SIZE);
  std::generate(population.begin(), population.end(), uni);
  const size_t SAMPLE_SIZE = 10;

  typedef std::vector<float>::iterator population_iterator;
  population_iterator populationIteratorBegin = population.begin(),
    populationIteratorEnd = population.end();
#else
#  error Please choose an example.
#endif

  std::cout << "Population ("
            << std::distance(populationIteratorBegin,
                             populationIteratorEnd)
            << " elements):" << std::endl;
  std::copy(populationIteratorBegin,
            populationIteratorEnd,
            std::ostream_iterator<float>(std::cout, " "));
  std::cout << std::endl;

  {
    //----------------------------//
    // Sample from the population //
    //----------------------------//

    typedef trsl::is_picked_systematic<
      float,
      double,
      wac
      > is_picked;

    typedef trsl::persistent_filter_iterator
      <is_picked, population_iterator> sample_iterator;

    is_picked predicate(SAMPLE_SIZE,
                        std::accumulate(populationIteratorBegin,
                                        populationIteratorEnd,
                                        float(0)),
                        wac());

    sample_iterator sampleIteratorBegin(predicate,
                                        populationIteratorBegin,
                                        populationIteratorEnd);
    sample_iterator sampleIteratorEnd(predicate,
                                      populationIteratorEnd,
                                      populationIteratorEnd);

    std::cout << "Sample of " << SAMPLE_SIZE << " elements:" << std::endl;
    std::copy(sampleIteratorBegin,
              sampleIteratorEnd,
              std::ostream_iterator<float>(std::cout, " "));
    std::cout << std::endl;
  }

  {
    //-------------------------------------------------------------------//
    // Sample from the population (robust to patterns in the population) //
    //-------------------------------------------------------------------//

    typedef trsl::is_picked_systematic<
    float,
    double,
    wac
    > is_picked;

    typedef trsl::ppfilter_iterator
    <is_picked, population_iterator> sample_iterator;

    is_picked predicate(SAMPLE_SIZE,
                        std::accumulate(populationIteratorBegin,
                                        populationIteratorEnd,
                                        float(0)),
                        wac());

    sample_iterator sampleIteratorBegin(predicate,
                                        populationIteratorBegin,
                                        populationIteratorEnd);
    sample_iterator sampleIteratorEnd = sampleIteratorBegin.end();

    std::cout << "Probabilistic sample of " << SAMPLE_SIZE << " elements:" << std::endl;
    std::copy(sampleIteratorBegin,
              sampleIteratorEnd,
              std::ostream_iterator<float>(std::cout, " "));
    std::cout << std::endl;
  }

  {
    //-------------------------------------//
    // Get a permutation of the population //
    //-------------------------------------//

    typedef trsl::reorder_iterator
      <population_iterator> permutation_iterator;

    {
      permutation_iterator pi =
        trsl::random_permutation_iterator(populationIteratorBegin,
                                          populationIteratorEnd);

      std::cout << "Permutation:" << std::endl;
      std::copy(pi,
                pi.end(),
                std::ostream_iterator<float>(std::cout, " "));
      std::cout << std::endl;
    }
    {
      permutation_iterator pi =
        trsl::random_permutation_iterator(populationIteratorBegin,
                                          populationIteratorEnd,
                                          SAMPLE_SIZE);

      std::cout << "Permutation of a subset of " << SAMPLE_SIZE
                << " elements:" << std::endl;
      std::copy(pi,
                pi.end(),
                std::ostream_iterator<float>(std::cout, " "));
      std::cout << std::endl;
    }
  }

  {
    //---------------------//
    // Sort the population //
    //---------------------//

    typedef trsl::reorder_iterator
      <population_iterator> permutation_iterator;

    {
      permutation_iterator pi =
        trsl::sort_iterator(populationIteratorBegin,
                            populationIteratorEnd);

      std::cout << "Sorted population:" << std::endl;
      std::copy(pi,
                pi.end(),
                std::ostream_iterator<float>(std::cout, " "));
      std::cout << std::endl;
    }
    {
      permutation_iterator pi =
        trsl::sort_iterator(populationIteratorBegin,
                            populationIteratorEnd,
                            std::less<float>(), SAMPLE_SIZE);

      std::cout << "Sorted " << SAMPLE_SIZE
                << " smallest elements:" << std::endl;
      std::copy(pi,
                pi.end(),
                std::ostream_iterator<float>(std::cout, " "));
      std::cout << std::endl;
    }
  }

  return 0;
}


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/is_included.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>

STF_CASE_TPL (" is_included integer",  STF_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::is_included;

  using r_t = decltype(is_included(T(),T()));

  // specific values tests
  STF_EQUAL(is_included(bs::Mone<T>(),bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_included(bs::One<T>(), bs::One<T>()), r_t(true));
  STF_EQUAL(is_included(bs::One<T>(),bs::Mone<T>()), r_t(true));
  STF_EQUAL(is_included(bs::One<T>(),bs::Three<T>()), r_t(true));
  STF_EQUAL(is_included(bs::One<T>(),bs::Two<T>()), r_t(false));
  STF_EQUAL(is_included(bs::One<T>(),bs::Zero<T>()), r_t(false));
  STF_EQUAL(is_included(bs::Zero<T>(), bs::Zero<T>()), r_t(true));
} // end of test for integer_


// Petter Strandmark 2013
// petter.strandmark@gmail.com

// Cataldo Azzariti 2016
// cataldo.azzariti@gmail.com

#include <iostream>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

// sight_level of the opponent algorithm
int max_level = 2;
// flag for saving moves
bool save_move = false;

#include <mcts.h>

#include "connect_four.h"

void main_program()
{
  using namespace std;

  bool human_player = false;   // toggle true-false for human player
  int games_won_P1 = 0;
  int games_won_P2 = 0;
  int games_drawn = 0;
  int moves_per_player = 0;
  int games_to_play = 100;    // choose as desired
  const int MAX_SIGHT = 5;
  vector<vector<ConnectFourState::Move>> TS_sight_array;
  vector<ConnectFourState::Move> moves_chosen;
  RowVectorXd lambda_evidence(MAX_SIGHT);
  RowVectorXd prior(MAX_SIGHT);
  vector<double> updated_post;

  /* TOGGLE A-KEY ON-OFF */
  //char a_key; // to make algos wait    // toggle on-off
  MatrixXd link_matrix(MAX_SIGHT, MAX_SIGHT);
  vector<vector<double>> TS_belief_sight;
  vector<double> game_break;
  for (int i=0; i<MAX_SIGHT; i++){
    game_break.push_back(-9999);
  }
  vector<ConnectFourState::Move> game_break_SA;
  for (int i=0; i<MAX_SIGHT; i++){
    game_break_SA.push_back(-9999);
  }
  string filename = "";  // to allow file savings

  /* Initialize prior and link matrix */
  link_matrix << 0.6,0.15,0.05,0.05,0.05,
                 0.2,0.6,0.15,0.05,0.05,
                 0.1,0.15,0.6,0.15,0.1,
                 0.05,0.05,0.15,0.6,0.2,
                 0.05,0.05,0.05,0.15,0.6;

  prior << 0.2,0.2,0.2,0.2,0.2;

  ofstream out5;
  filename = "Sight_";
  filename += (char)(max_level + '0');
  filename += "/structure.txt";
  out5.open(filename);
  out5 << "the link matrix is: " << endl;
  out5 << link_matrix << endl << endl;
  out5 << "Prior is: " << endl;
  out5 << prior << endl << endl;
  out5.close();

  // Initialize algorithms parameters
  MCTS::ComputeOptions player1_options, player2_options;
  player1_options.max_iterations = 100000;
  player1_options.verbose = false;   //to be changed back to true eventually
  player2_options.max_iterations =  100000;   //put back to 100k!!!***
  player2_options.verbose = false;   //to be changed back to true eventually

  // Outer loop for each game
  for (int i=0; i<games_to_play; i++){

    ConnectFourState state;
    moves_per_player = 0;
    while (state.has_moves()) {

      /* toggle on-off to suppress output to console */
      //cout << endl << "State: " << state << endl;

      ConnectFourState::Move move = ConnectFourState::no_move;
      if (state.player_to_move == 1) {

	/* We assume the opponent move first */
	vector<ConnectFourState::Move> sight_array =
	  MCTS::sight_array(state, MAX_SIGHT, player1_options);
	TS_sight_array.push_back(sight_array);
	move = MCTS::compute_move_capped(state, player1_options);

	/* save move for hit-rate analysis */
	if (save_move) {
	  filename = "Sight_";
	  filename += (char)(max_level + '0');
	  filename += "/moves_inferred.txt";
	  out5.open(filename, std::fstream::app);
	  out5 << " " << move << endl;
	  out5.close();
	}

	state.do_move(move);
	moves_per_player++;
	moves_chosen.push_back(move);

	/* Probabilistic Update */
	prior = MCTS::update_prior(move, sight_array, prior, MAX_SIGHT,
				   link_matrix);
	// store time series in a matrix
	updated_post.clear();
	for (int i = 0; i < MAX_SIGHT; i++){
	  updated_post.push_back(prior(i));
	}
	TS_belief_sight.push_back(updated_post);
      }

      /* Part for human player, if present. Not used for DSEM but left it
	 anyways */
      else {
	if (human_player) {
	  while (true) {
	    cout << "Input your move: ";
	    move = ConnectFourState::no_move;
	    cin >> move;
	    try {
	      state.do_move(move);
	      break;
	    }
	    catch (std::exception& ) {
	      cout << "Invalid move." << endl;
	    }
	    moves_per_player++;
	  }
	}

	/* Unconstrained or Adaptative algo part. We assume it moves second. */
	else {

	  /* TOGGLE ON FOR ADAPTATIVE !!!!!!!!!!!! */
	  move = MCTS::compute_adaptative_move_UCT(state, MAX_SIGHT,
						   updated_post,player2_options);

	  /* Old piece of algo, which used traditional MCTS, replaced by
	     adaptative */
	  //move = MCTS::compute_move(state, player2_options);
	  /* old piece of algo replaced by adaptative */

	  state.do_move(move);
	  moves_per_player++;

	}
      }

      /* Toggle on-off, to check-point algorithms to analyse
	 intermediate trees.  */
      //cout << "Press a key to continue: ";
      //cin >> a_key;
    }

    /* toggle on-off - to control output to console*/
    //cout << endl << "Final state: " << state << endl;

    /* Part to signal game end to data-containing arrays */
    TS_belief_sight.push_back(game_break);
    TS_sight_array.push_back(game_break_SA);
    moves_chosen.push_back(-9999);

    // Lambda evidence
    ofstream out1;
    filename = "Sight_";
    filename += (char)(max_level + '0');
    filename += "/lambda_evidence.txt";
    out1.open(filename, std::fstream::app);
    for (unsigned int i = 0; i < MAX_SIGHT; i++){
      out1 << -9999 << " " ;
    }
    out1 << endl;
    out1.close();

    // Move inferred
    filename = "Sight_";
    filename += (char)(max_level + '0');
    filename += "/moves_inferred.txt";
    out1.open(filename, std::fstream::app);
    out1 << -9999 << endl;
    //    out1 << " " << endl;
    out1.close();

    // % win
    ofstream out7;
    filename = "Sight_";
    filename += (char)(max_level + '0');
    filename += "/TS_%_win.txt";
    out7.open(filename, std::fstream::app);
    out7 << -9999;
    out7 << endl;
    out7.close();

    // % visits
    filename = "Sight_";
    filename += (char)(max_level + '0');
    filename += "/TS_%_visits.txt";
    out7.open(filename, std::fstream::app);
    out7 << -9999;
    out7 << endl;
    out7.close();

    // Moves per player
    filename = "Sight_";
    filename += (char)(max_level + '0');
    filename += "/moves_per_player.txt";
    out7.open(filename, std::fstream::app);
    if (state.get_result(2) == 1.0) {
      out7 << "W ";
    }
    else
      out7 << "L ";
    out7 << moves_per_player;
    out7 << endl;
    out7.close();
    /* End of part to signal game end to data-containing arrays */

    /* Assign victory to the players. */
    if (state.get_result(2) == 1.0) {
      games_won_P1++;
      /* toggle on-off - to control output to console */
      //cout << "Player 1 wins!" << endl;
    }
    else if (state.get_result(1) == 1.0) {
      games_won_P2++;
      /* toggle on-off - to control output to console */
      //cout << "Player 2 wins!" << endl;
    }
    else {
      games_drawn++;
      /* toggle on-off - to control output to console */
      //cout << "Nobody wins!" << endl;
    }

    /* runtime tracker */
    cerr <<".";
    if (!(i%5))
      cerr <<i<<endl;
  }

  /* SAVE THE RELEVANT DATA */
  /* Save sight array */
  ofstream out;
  filename = "Sight_";
  filename += (char)(max_level + '0');
  filename += "/TS_sight_array.txt";
  out.open(filename);
  for (unsigned int i = 0; i < TS_sight_array.size(); i++){
    for (int j = 0; j < MAX_SIGHT; j++){
      out << TS_sight_array[i][j];
    }
    out << endl;
  }
  out.close();
  /* part to save sight array */

  /* Save TS belief sight */
  ofstream out2;
  filename = "Sight_";
  filename += (char)(max_level + '0');
  filename += "/TS_belief_sight.txt";
  out2.open(filename);
  for (unsigned int i = 0; i < TS_belief_sight.size(); i++){
    for (int j = 0; j < MAX_SIGHT; j++){
      out2 << setprecision(2) << fixed << TS_belief_sight[i][j];
      out2 << "  ";
    }
    out2 << endl;
  }
  out2.close();
  /* Save TS belief sight */

  /* Save moves_chosen */
  ofstream out1;
  filename = "Sight_";
  filename += (char)(max_level + '0');
  filename += "/moves_chosen.txt";
  out1.open(filename);
  for (unsigned int i = 0; i < moves_chosen.size(); i++){
    out1 << moves_chosen[i] << endl;
  }
  out1.close();
  /* save moves_chosen */

  /* Final Output. */
  cout << "Sight level is: " << max_level << endl;
  cout << "Player 1 is CAPPED." << endl;
  cout << "Using ADAPTATIVE algo." << endl;
  cout << "Player 1 won: " << games_won_P1 << " games."<< endl;
  cout << "Player 2 won: " << games_won_P2 << " games."<< endl;
  cout << "Drawn games: " << games_drawn << " games."<< endl;

  /* Save final output to file. */
  filename = "Sight_";
  filename += (char)(max_level + '0');
  filename += "/structure.txt";
  out5.open(filename, std::fstream::app);
  out5 << "Sight level is: " << max_level << endl << endl;
  out5 << "Player 1 is CAPPED." << endl;
  out5 << "Using ADAPTATIVE algo." << endl;
  out5 << "Total games is: " << games_to_play << endl;
  out5 << "Player 1 won: " << games_won_P1 << " games."<< endl;
  out5 << "Player 2 won: " << games_won_P2 << " games."<< endl;
  out5 << "Drawn games: " << games_drawn << " games."<< endl;
  out5.close();

}

/* Main program. */
int main()
{
  try {
    main_program();
  }
  catch (std::runtime_error& error) {
    std::cerr << "ERROR: " << error.what() << std::endl;
    return 1;
  }
}
/* END OF MAIN PROGRAM */


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

/// @project        Open Space Toolkit ▸ Physics
/// @file           OpenSpaceToolkit/Physics/Time/Interval.cpp
/// @author         Lucas Brémond <lucas@loftorbital.com>
/// @license        Apache License 2.0

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

#include <OpenSpaceToolkit/Physics/Time/Interval.hpp>

#include <OpenSpaceToolkit/Core/Error.hpp>
#include <OpenSpaceToolkit/Core/Utilities.hpp>

#include <boost/regex.hpp>

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

namespace ostk
{
namespace physics
{
namespace time
{

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

                                Interval::Interval                          (   const   Instant&                    aLowerBound,
                                                                                const   Instant&                    anUpperBound,
                                                                                const   Interval::Type&             anIntervalType                              )
                                :   math::obj::Interval<Instant>(aLowerBound, anUpperBound, anIntervalType)
{

}

std::ostream&                   operator <<                                 (           std::ostream&               anOutputStream,
                                                                                const   Interval&                   anInterval                                  )
{

    ostk::core::utils::Print::Header(anOutputStream, "Interval") ;

    ostk::core::utils::Print::Line(anOutputStream) << "Start:" << (anInterval.isDefined() ? anInterval.accessStart().toString(Scale::UTC) : "Undefined") ;
    ostk::core::utils::Print::Line(anOutputStream) << "End:" << (anInterval.isDefined() ? anInterval.accessEnd().toString(Scale::UTC) : "Undefined") ;

    ostk::core::utils::Print::Footer(anOutputStream) ;

    return anOutputStream ;

}

bool                            Interval::isDefined                         ( ) const
{
    return math::obj::Interval<Instant>::isDefined() && (this->getType() != Interval::Type::Undefined) ;
}

const Instant&                  Interval::accessStart                       ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Interval") ;
    }

    return this->accessLowerBound() ;

}

const Instant&                  Interval::accessEnd                         ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Interval") ;
    }

    return this->accessUpperBound() ;

}

Instant                         Interval::getStart                          ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Interval") ;
    }

    return this->accessLowerBound() ;

}

Instant                         Interval::getEnd                            ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Interval") ;
    }

    return this->accessUpperBound() ;

}

Duration                        Interval::getDuration                       ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Interval") ;
    }

    return Duration::Between(this->accessLowerBound(), this->accessUpperBound()) ;

}

Instant                         Interval::getCenter                         ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Interval") ;
    }

    return this->accessLowerBound() + this->getDuration() / 2.0 ;

}

String                          Interval::toString                         (   const   Scale&                      aTimeScale                                  ) const
{

    using ostk::physics::time::DateTime ;

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Interval") ;
    }

    switch (this->getType())
    {

        case Interval::Type::Closed:
            return String::Format("[{0} - {1}] [{2}]", this->accessLowerBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), this->accessUpperBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), StringFromScale(aTimeScale)) ;

        case Interval::Type::Open:
            return String::Format("]{0} - {1}[ [{2}]", this->accessLowerBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), this->accessUpperBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), StringFromScale(aTimeScale)) ;

        case Interval::Type::HalfOpenLeft:
            return String::Format("]{0} - {1}] [{2}]", this->accessLowerBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), this->accessUpperBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), StringFromScale(aTimeScale)) ;

        case Interval::Type::HalfOpenRight:
            return String::Format("[{0} - {1}[ [{2}]", this->accessLowerBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), this->accessUpperBound().getDateTime(aTimeScale).toString(DateTime::Format::Standard), StringFromScale(aTimeScale)) ;

        default:
            break ;

    }

    throw ostk::core::error::runtime::Wrong("Interval type") ;

    return String::Empty() ;

}

Array<Instant>                  Interval::generateGrid                      (   const   Duration&                   aTimeStep                                   ) const
{

    return this->generateArrayWithStep(aTimeStep) ;

    // using ostk::core::types::Index ;
    // using ostk::core::types::Size ;

    // if (!this->isDefined())
    // {
    //     throw ostk::core::error::runtime::Undefined("Interval") ;
    // }

    // if (!aTimeStep.isDefined())
    // {
    //     throw ostk::core::error::runtime::Undefined("Time step") ;
    // }

    // if (aTimeStep.isZero())
    // {
    //     throw ostk::core::error::RuntimeError("Time step is zero.") ;
    // }

    // Array<Instant> grid = Array<Instant>::Empty() ;

    // if (aTimeStep.isPositive())
    // {

    //     Instant instant = this->accessLowerBound() ;

    //     while (instant <= this->accessUpperBound())
    //     {

    //         grid.add(instant) ;

    //         instant += aTimeStep ;

    //     }

    //     if (grid.accessLast() < this->accessUpperBound())
    //     {
    //         grid.add(this->accessUpperBound()) ;
    //     }

    // }
    // else
    // {

    //     Instant instant = this->accessUpperBound() ;

    //     while (instant >= this->accessLowerBound())
    //     {

    //         grid.add(instant) ;

    //         instant += aTimeStep ;

    //     }

    //     if (grid.accessLast() > this->accessLowerBound())
    //     {
    //         grid.add(this->accessLowerBound()) ;
    //     }

    // }

    // return grid ;

}

Interval                        Interval::Undefined                         ( )
{
    return Interval(Instant::Undefined(), Instant::Undefined(), Interval::Type::Undefined) ;
}

Interval                        Interval::Closed                            (   const   Instant&                    aLowerBound,
                                                                                const   Instant&                    anUpperBound                                )
{

    if (!aLowerBound.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Lower bound") ;
    }

    if (!anUpperBound.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Upper bound") ;
    }

    return Interval(aLowerBound, anUpperBound, Interval::Type::Closed) ;

}

Interval                        Interval::Centered                          (   const   Instant&                    aCentralInstant,
                                                                                const   Duration&                   aDuration,
                                                                                const   Interval::Type&             anIntervalType                              )
{

    if (!aCentralInstant.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Central instant") ;
    }

    if (!aDuration.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Duration") ;
    }

    if (!aDuration.isPositive())
    {
        throw ostk::core::error::RuntimeError("Duration is negative.") ;
    }

    if (anIntervalType == Interval::Type::Undefined)
    {
        throw ostk::core::error::runtime::Undefined("Interval type") ;
    }

    return Interval((aCentralInstant - aDuration / 2.0), (aCentralInstant + aDuration / 2.0), anIntervalType) ;

}

Interval                        Interval::Parse                             (   const   String&                     aString                                     )
{

    using ostk::physics::time::DateTime ;

    if (aString.isEmpty())
    {
        throw ostk::core::error::runtime::Undefined("String") ;
    }

    boost::smatch match ;

    if (boost::regex_match(aString, match, boost::regex("^([\\[\\]])([\\d]{4}-[\\d]{2}-[\\d]{2} [\\d]{2}:[\\d]{2}:[\\d]{2}) - ([\\d]{4}-[\\d]{2}-[\\d]{2} [\\d]{2}:[\\d]{2}:[\\d]{2})([\\[\\]]) \\[([\\w]+)\\]$")))
    {

        const String openingBracket = String(match[1]) ;
        const String startInstantString = String(match[2]) ;
        const String endInstantString = String(match[3]) ;
        const String closingBracket = String(match[4]) ;
        const String timeScaleString = String(match[5]) ;

        const Scale timeScale = ScaleFromString(timeScaleString) ;

        const Instant startInstant = Instant::DateTime(DateTime::Parse(startInstantString, DateTime::Format::Standard), timeScale) ;
        const Instant endInstant = Instant::DateTime(DateTime::Parse(endInstantString, DateTime::Format::Standard), timeScale) ;

        Interval::Type type = Interval::Type::Undefined ;

        if ((openingBracket == "[") && (closingBracket == "]"))
        {
            type = Interval::Type::Closed ;
        }
        else if ((openingBracket == "]") && (closingBracket == "["))
        {
            type = Interval::Type::Open ;
        }
        else if ((openingBracket == "]") && (closingBracket == "]"))
        {
            type = Interval::Type::HalfOpenLeft ;
        }
        else if ((openingBracket == "[") && (closingBracket == "["))
        {
            type = Interval::Type::HalfOpenRight ;
        }
        else
        {
            throw ostk::core::error::runtime::Wrong("Brackets") ;
        }

        return Interval(startInstant, endInstant, type) ;

    }
    else
    {
        throw ostk::core::error::RuntimeError("Cannot parse interval string [{}].", aString) ;
    }

    return Interval::Undefined() ;

}

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

}
}
}

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


#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/array.hpp>
#include <array>
#include <utility>
#include <algorithm>
#include <iostream>

int main()
{
  enum { topLeft, topRight, bottomRight, bottomLeft };

  std::array<std::pair<int, int>, 4> edges{{
    std::make_pair(topLeft, topRight),
    std::make_pair(topRight, bottomRight),
    std::make_pair(bottomRight, bottomLeft),
    std::make_pair(bottomLeft, topLeft)
  }};

  typedef boost::adjacency_list<boost::setS, boost::vecS,
    boost::undirectedS> graph;
  graph g{edges.begin(), edges.end(), 4};

  boost::array<int, 4> predecessors;
  predecessors[bottomRight] = bottomRight;

  boost::breadth_first_search(g, bottomRight,
    boost::visitor(
      boost::make_bfs_visitor(
        boost::record_predecessors(predecessors.begin(),
          boost::on_tree_edge{}))));

  int p = topLeft;
  while (p != bottomRight)
  {
    std::cout << p << '\n';
    p = predecessors[p];
  }
  std::cout << p << '\n';
}


#ifndef ST_LAYER_HPP_
#define ST_LAYER_HPP_

#include <boost/shared_ptr.hpp>
#include <gflags/gflags.h>
#include <glog/logging.h>

#include <cmath>

#include "caffe/blob.hpp"
#include "caffe/common.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"

namespace caffe {

template <typename Dtype>
class SpatialTransformerLayer : public Layer<Dtype> {

public:
	explicit SpatialTransformerLayer(const LayerParameter& param)
      : Layer<Dtype>(param), global_debug(false) {}
	virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
	virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

	virtual inline const char* type() const { return "SpatialTransformer"; }
	virtual inline int ExactNumBottomBlobs() const { return 2; }
	virtual inline int ExactNumTopBlobs() const { return 1; }

protected:
	virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
	virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
	virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);
	virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

private:
	inline Dtype abs(Dtype x) {
		if(x < 0) return -x; return x;
	}
	inline Dtype max(Dtype x, Dtype y) {
		if(x < y) return y; return x;
	}

	Dtype transform_forward_cpu(const Dtype* pic, Dtype px, Dtype py);
	void transform_backward_cpu(Dtype dV, const Dtype* U, const Dtype px,
			const Dtype py, Dtype* dU, Dtype& dpx, Dtype& dpy);

	string transform_type_;
	string sampler_type_;

	int output_H_;
	int output_W_;

	int N, C, H, W;

	Blob<Dtype>* input_grid;	// Store the projected coordinates on input map
					// for all output pixels when forwarding
	Blob<Dtype>* output_grid;	// Only used in CPU version

	bool global_debug;
};

}  // namespace caffe

#endif  // CAFFE_COMMON_HPP_


//=======================================================================
// Copyright 2007 Aaron Windsor
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/test/minimal.hpp>

using namespace boost;

struct VertexIndexUpdater
{
  template <typename Graph>
  void reset(Graph& g)
  {
    typename property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);
    typename graph_traits<Graph>::vertex_iterator vi, vi_end;
    typename graph_traits<Graph>::vertices_size_type cnt = 0;
    for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
      put(index, *vi, cnt++);
  }
};

struct NoVertexIndexUpdater
{
  template <typename Graph> void reset(Graph& g) {}
};

template <typename Graph, typename VertexIndexUpdater>
void test_K_5(VertexIndexUpdater vertex_index)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

  Graph g;
  vertex_t v1 = add_vertex(g);
  vertex_t v2 = add_vertex(g);
  vertex_t v3 = add_vertex(g);
  vertex_t v4 = add_vertex(g);
  vertex_t v5 = add_vertex(g);
  vertex_index.reset(g);

  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v1, v2, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v1, v3, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v1, v4, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v1, v5, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v2, v3, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v2, v4, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v2, v5, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v3, v4, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v3, v5, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));

  //This edge should make the graph non-planar
  add_edge(v4, v5, g);
  BOOST_CHECK(!boyer_myrvold_planarity_test(g));

}

template <typename Graph, typename VertexIndexUpdater>
void test_K_3_3(VertexIndexUpdater vertex_index)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

  Graph g;
  vertex_t v1 = add_vertex(g);
  vertex_t v2 = add_vertex(g);
  vertex_t v3 = add_vertex(g);
  vertex_t v4 = add_vertex(g);
  vertex_t v5 = add_vertex(g);
  vertex_t v6 = add_vertex(g);
  vertex_index.reset(g);

  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v1, v4, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v1, v5, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v1, v6, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v2, v4, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v2, v5, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v2, v6, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v3, v4, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  add_edge(v3, v5, g);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));

  //This edge should make the graph non-planar
  add_edge(v3, v6, g);
  BOOST_CHECK(!boyer_myrvold_planarity_test(g));

}

// This test creates a maximal planar graph on num_vertices vertices,
// then, if num_vertices is at least 5, adds an additional edge to
// create a non-planar graph.

template <typename Graph, typename VertexIndexUpdater>
void test_maximal_planar(VertexIndexUpdater vertex_index, std::size_t num_vertices)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

  Graph g;
  std::vector<vertex_t> vmap;
  for(std::size_t i = 0; i < num_vertices; ++i)
    vmap.push_back(add_vertex(g));

  vertex_index.reset(g);

  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  //create a cycle
  for(std::size_t i = 0; i < num_vertices; ++i)
    {
      add_edge(vmap[i], vmap[(i+1) % num_vertices], g);
      BOOST_CHECK(boyer_myrvold_planarity_test(g));
    }

  //triangulate the interior of the cycle.
  for(std::size_t i = 2; i < num_vertices - 1; ++i)
    {
      add_edge(vmap[0], vmap[i], g);
      BOOST_CHECK(boyer_myrvold_planarity_test(g));
    }

  //triangulate the exterior of the cycle.
  for(std::size_t i = 3; i < num_vertices; ++i)
    {
      add_edge(vmap[1], vmap[i], g);
      BOOST_CHECK(boyer_myrvold_planarity_test(g));
    }

  //Now add an additional edge, forcing the graph to be non-planar.
  if (num_vertices > 4)
    {
      add_edge(vmap[2], vmap[4], g);
      BOOST_CHECK(!boyer_myrvold_planarity_test(g));
    }

}

int test_main(int, char* [])
{
  typedef adjacency_list
    <vecS,
    vecS,
    undirectedS,
    property<vertex_index_t, int>
    >
    VVgraph_t;

  typedef adjacency_list
    <vecS,
    listS,
    undirectedS,
    property<vertex_index_t, int>
    >
    VLgraph_t;

  typedef adjacency_list
    <listS,
    vecS,
    undirectedS,
    property<vertex_index_t, int>
    >
    LVgraph_t;

  typedef adjacency_list
    <listS,
    listS,
    undirectedS,
    property<vertex_index_t, int>
    >
    LLgraph_t;

  typedef adjacency_list
    <setS,
    setS,
    undirectedS,
    property<vertex_index_t, int>
    >
    SSgraph_t;

  test_K_5<VVgraph_t>(NoVertexIndexUpdater());
  test_K_3_3<VVgraph_t>(NoVertexIndexUpdater());
  test_maximal_planar<VVgraph_t>(NoVertexIndexUpdater(), 3);
  test_maximal_planar<VVgraph_t>(NoVertexIndexUpdater(), 6);
  test_maximal_planar<VVgraph_t>(NoVertexIndexUpdater(), 10);
  test_maximal_planar<VVgraph_t>(NoVertexIndexUpdater(), 20);
  test_maximal_planar<VVgraph_t>(NoVertexIndexUpdater(), 50);

  test_K_5<VLgraph_t>(VertexIndexUpdater());
  test_K_3_3<VLgraph_t>(VertexIndexUpdater());
  test_maximal_planar<VLgraph_t>(VertexIndexUpdater(), 3);
  test_maximal_planar<VLgraph_t>(VertexIndexUpdater(), 6);
  test_maximal_planar<VLgraph_t>(VertexIndexUpdater(), 10);
  test_maximal_planar<VLgraph_t>(VertexIndexUpdater(), 20);
  test_maximal_planar<VLgraph_t>(VertexIndexUpdater(), 50);

  test_K_5<LVgraph_t>(NoVertexIndexUpdater());
  test_K_3_3<LVgraph_t>(NoVertexIndexUpdater());
  test_maximal_planar<LVgraph_t>(NoVertexIndexUpdater(), 3);
  test_maximal_planar<LVgraph_t>(NoVertexIndexUpdater(), 6);
  test_maximal_planar<LVgraph_t>(NoVertexIndexUpdater(), 10);
  test_maximal_planar<LVgraph_t>(NoVertexIndexUpdater(), 20);
  test_maximal_planar<LVgraph_t>(NoVertexIndexUpdater(), 50);

  test_K_5<LLgraph_t>(VertexIndexUpdater());
  test_K_3_3<LLgraph_t>(VertexIndexUpdater());
  test_maximal_planar<LLgraph_t>(VertexIndexUpdater(), 3);
  test_maximal_planar<LLgraph_t>(VertexIndexUpdater(), 6);
  test_maximal_planar<LLgraph_t>(VertexIndexUpdater(), 10);
  test_maximal_planar<LLgraph_t>(VertexIndexUpdater(), 20);
  test_maximal_planar<LLgraph_t>(VertexIndexUpdater(), 50);

  test_K_5<SSgraph_t>(VertexIndexUpdater());
  test_K_3_3<SSgraph_t>(VertexIndexUpdater());
  test_maximal_planar<SSgraph_t>(VertexIndexUpdater(), 3);
  test_maximal_planar<SSgraph_t>(VertexIndexUpdater(), 6);
  test_maximal_planar<SSgraph_t>(VertexIndexUpdater(), 10);
  test_maximal_planar<SSgraph_t>(VertexIndexUpdater(), 20);
  test_maximal_planar<SSgraph_t>(VertexIndexUpdater(), 50);

  return 0;
}


/**
* This file is part of Intrinsic3D.
*
* Copyright (c) 2019, NVIDIA CORPORATION. All rights reserved.
* Copyright (c) 2019, Technical University of Munich. 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 the name of NVIDIA CORPORATION nor the names of its
*      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 "AS IS" AND ANY
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
* PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE COPYRIGHT OWNER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
* EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
* PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
* PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY
* OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include <nv/rgbd/processing.h>

#include <iostream>

#include <Eigen/Geometry>
#include <opencv2/imgproc.hpp>

namespace nv
{

    void threshold(cv::Mat &depth, float depth_min, float depth_max)
	{
        cv::threshold(depth, depth, static_cast<double>(depth_min), 0.0, cv::THRESH_TOZERO);
        cv::threshold(depth, depth, static_cast<double>(depth_max), 0.0, cv::THRESH_TOZERO_INV);
	}

	cv::Mat computeVertexMap(const Mat3f &K, const cv::Mat &depth)
	{
		if (depth.empty() || depth.type() != CV_32FC1)
			return cv::Mat();

		float cx = K(0, 2);
		float cy = K(1, 2);
		float fx_inv = 1.0f / K(0, 0);
		float fy_inv = 1.0f / K(1, 1);

        cv::Mat vertex_map = cv::Mat::zeros(depth.size(), CV_32FC3);
		for (int y = 0; y < depth.rows; ++y)
		{
			for (int x = 0; x < depth.cols; ++x)
			{
				float d = depth.at<float>(y, x);
				float x0 = (float(x) - cx) * fx_inv;
				float y0 = (float(y) - cy) * fy_inv;
                vertex_map.at<cv::Vec3f>(y, x) = cv::Vec3f(x0 * d, y0 * d, d);
			}
		}
        return vertex_map;
	}

    cv::Mat computeNormals(const cv::Mat &vertex_map, float depth_threshold)
	{
        if (vertex_map.empty() || vertex_map.type() != CV_32FC3)
			return cv::Mat();

        int w = vertex_map.cols;
        int h = vertex_map.rows;
		cv::Mat normals = cv::Mat::zeros(h, w, CV_32FC3);
        const float* ptr_vert = reinterpret_cast<const float*>(vertex_map.data);

		// depth threshold to avoid computing properties over depth-discontinuities
        for (int y = 1; y < h - 1; ++y)
		{
            for (int x = 1; x < w - 1; ++x)
			{
                size_t off = static_cast<size_t>((y*w + x) * 3);
                Vec3f vert(ptr_vert[off], ptr_vert[off + 1], ptr_vert[off + 2]);
                if (vert[2] == 0.0f)
					continue;

				// determine tangent vectors
                size_t off_x0 = static_cast<size_t>((y*w + x - 1) * 3);
                Vec3f vert_x0(ptr_vert[off_x0], ptr_vert[off_x0 + 1], ptr_vert[off_x0 + 2]);
                size_t off_x1 = static_cast<size_t>((y*w + x + 1) * 3);
                Vec3f vert_x1(ptr_vert[off_x1], ptr_vert[off_x1 + 1], ptr_vert[off_x1 + 2]);
                size_t off_y0 = static_cast<size_t>(((y - 1)*w + x) * 3);
                Vec3f vert_y0(ptr_vert[off_y0], ptr_vert[off_y0 + 1], ptr_vert[off_y0 + 2]);
                size_t off_y1 = static_cast<size_t>(((y + 1)*w + x) * 3);
                Vec3f vert_y1(ptr_vert[off_y1], ptr_vert[off_y1 + 1], ptr_vert[off_y1 + 2]);
                if (vert_x0[2] == 0.0f || vert_x1[2] == 0.0f || vert_y0[2] == 0.0f || vert_y1[2] == 0.0f)
					continue;

                Vec3f tangent_x = vert_x1 - vert_x0;
                Vec3f tangent_y = vert_y1 - vert_y0;
                if (tangent_x.norm() < depth_threshold && tangent_y.norm() < depth_threshold)
				{
					// compute normal using cross product
                    Vec3f n = (tangent_y.cross(tangent_x)).normalized();
					normals.at<cv::Vec3f>(y, x) = cv::Vec3f(n[0], n[1], n[2]);
				}
			}
		}

		return normals;
	}

    cv::Mat computeNormals(const Mat3f &K, const cv::Mat &depth, float depth_threshold)
	{
        cv::Mat vertex_map = computeVertexMap(K, depth);
        cv::Mat normals = computeNormals(vertex_map, depth_threshold);
		return normals;
	}

    cv::Mat resizeDepth(const Camera &input_cam, const cv::Mat &input_depth, const Camera &output_cam)
	{
        if (input_depth.empty() || input_depth.type() != CV_32FC1)
			return cv::Mat();
        int w = output_cam.width();
        int h = output_cam.height();
        if (input_depth.cols == w && input_depth.rows == h)
		{
			// TODO check if intrinsics of both cameras are the same
            return input_depth.clone();
		}

        const Mat3f input_K = input_cam.intrinsics();
        const Mat3f output_K = output_cam.intrinsics();

        float in_cx = input_K(0, 2);
        float in_cy = input_K(1, 2);
        float in_fx = input_K(0, 0);
        float in_fy = input_K(1, 1);

        float out_cx = output_K(0, 2);
        float out_cy = output_K(1, 2);
        float out_fx_inv = 1.0f / output_K(0, 0);
        float out_fy_inv = 1.0f / output_K(1, 1);

        cv::Mat depth_out = cv::Mat::zeros(h, w, CV_32FC1);
		for (int y = 0; y < h; ++y)
		{
			for (int x = 0; x < w; ++x)
			{
				// compute lookup coordinates for input depth
                float x0 = (float(x) - out_cx) * out_fx_inv;
                float y0 = (float(y) - out_cy) * out_fy_inv;
				// 3d point (depth and color are registered/aligned -> depth value shouldn't matter)
				Vec3f p(x0, y0, 1.0f);
				// project 3d point into depth image
				Vec2f p2;
                p2[0] = (in_fx * p[0] / p[2]) + in_cx;
                p2[1] = (in_fy * p[1] / p[2]) + in_cy;
				Vec2i p2i = (p2 + Vec2f::Constant(0.5f)).cast<int>();
                if (p2i[0] < 0 || p2i[1] < 0 || p2i[0] >= input_depth.cols || p2i[1] >= input_depth.rows)
					continue;
                // lookup depth in input depth (using linear interpolation)
                float d_in = interpolate<float>(input_depth, p2[0], p2[1], 0);
				// check depth and store it
                if (d_in == 0.0f)
					continue;
                depth_out.at<float>(y, x) = d_in;
			}
		}

        return depth_out;
	}

    cv::Mat erodeDiscontinuities(const cv::Mat &depth_in, int window_size, float max_depth_diff)
	{
        if (depth_in.empty() || depth_in.type() != CV_32FC1)
			return cv::Mat();

        if (window_size <= 0)
		{
			// no erosion -> return copy of input depth
            return depth_in.clone();
		}

		// valid depth values
        int h = depth_in.rows;
        int w = depth_in.cols;
        const float* ptr_in = reinterpret_cast<const float*>(depth_in.data);
        cv::Mat depth_out = depth_in.clone();
        float* ptr_out = reinterpret_cast<float*>(depth_out.data);

		for (int y = 0; y < h; ++y)
		{
			for (int x = 0; x < w; ++x)
			{
                size_t idx = static_cast<size_t>(y*w + x);
                float d_ref = ptr_in[idx];
                if (d_ref == 0.0f)
				{
                    ptr_out[idx] = 0.0f;
					continue;
				}

				bool valid = true;
                for (int v = std::max(0, y - window_size); v <= std::min(y + window_size, h - 1); ++v)
				{
                    for (int u = std::max(0, x - window_size); u <= std::min(x + window_size, w - 1); ++u)
					{
                        size_t off = static_cast<size_t>(v*w + u);
                        float d = ptr_in[off];
                        if (d == 0.0f || std::abs(d - d_ref) > max_depth_diff)
						{
							valid = false;
							break;
						}
					}
					if (!valid)
						break;
				}
				if (!valid)
                    ptr_out[idx] = 0.0f;
			}
		}
        return depth_out;
	}

	template<typename T>
	T interpolate(const cv::Mat &img, float x, float y, int channel)
	{
		int w = img.cols;
		int h = img.rows;
        T val_cur = static_cast<T>(0.0);
        const T* ptr_img = reinterpret_cast<const T*>(img.data);
		int nc = img.channels();

		//bilinear interpolation
		int x0 = static_cast<int>(std::floor(x));
		int y0 = static_cast<int>(std::floor(y));
		int x1 = x0 + 1;
		int y1 = y0 + 1;

        float x1_weight = x - static_cast<float>(x0);
        float y1_weight = y - static_cast<float>(y0);
        float x0_weight = 1.0f - x1_weight;
        float y0_weight = 1.0f - y1_weight;

		if (x0 < 0 || x0 >= w)
            x0_weight = 0.0f;
		if (x1 < 0 || x1 >= w)
            x1_weight = 0.0f;
		if (y0 < 0 || y0 >= h)
            y0_weight = 0.0f;
		if (y1 < 0 || y1 >= h)
            y1_weight = 0.0f;
        float w00 = x0_weight * y0_weight;
        float w10 = x1_weight * y0_weight;
        float w01 = x0_weight * y1_weight;
        float w11 = x1_weight * y1_weight;

        float sumWeights = w00 + w10 + w01 + w11;
        float sum = 0.0f;
        if (w00 > 0.0f)
            sum += static_cast<float>(ptr_img[(y0*w + x0) * nc + channel]) * w00;
        if (w01 > 0.0f)
            sum += static_cast<float>(ptr_img[(y1*w + x0) * nc + channel]) * w01;
        if (w10 > 0.0f)
            sum += static_cast<float>(ptr_img[(y0*w + x1) * nc + channel]) * w10;
        if (w11 > 0.0f)
            sum += static_cast<float>(ptr_img[(y1*w + x1) * nc + channel]) * w11;

        if (sumWeights > 0.0f)
            val_cur = static_cast<T>(sum / sumWeights);

        return val_cur;
	}
	template unsigned char interpolate(const cv::Mat &img, float x, float y, int channel);
	template unsigned short interpolate(const cv::Mat &img, float x, float y, int channel);
	template int interpolate(const cv::Mat &img, float x, float y, int channel);
	template float interpolate(const cv::Mat &img, float x, float y, int channel);
	template double interpolate(const cv::Mat &img, float x, float y, int channel);

	Vec3b interpolateRGB(const cv::Mat &color, float x, float y)
	{
		// lookup color using bilinear interpolation
		unsigned char r = interpolate<unsigned char>(color, x, y, 2);
		unsigned char g = interpolate<unsigned char>(color, x, y, 1);
		unsigned char b = interpolate<unsigned char>(color, x, y, 0);
		return Vec3b(r, g, b);
	}

} // namespace nv


// The contents of this file are in the public domain. See LICENSE_FOR_EXAMPLE_PROGRAMS.txt
/*
	This example shows "Minimalistic CNN-based model" for gender prediction from
	face images. The original idea and definition of the model is given in the
	document available at this address:
	http://www.eurecom.fr/fr/publication/4768/download/mm-publi-4768.pdf

	This model is a gender classifier trained using a private dataset of about
	200k different face images, , with a balanced distribution between female
	and male genders. It was generated according to the network definition and
	settings given in the paper for gender prediction from face images. Even if
	the dataset used for the training is different from that used by G. Antipov
	et al, the classification results on the LFW evaluation are similar overall.

	To take up the authors' proposal to join the results of three networks, a
	simplification was made by finally presenting RGB images, thus simulating
	three "grayscale" networks via the three image planes. Better results could
	be probably obtained with a more complex and deeper network, but the performance
	of the classification is nevertheless surprising compared to the simplicity
	of the network used and thus its very small size.

	The model was trained using about 200k images, with a balanced distribution
	between female and male genders. Faces extracted from a non-public database
	were augmented, after alignment according to the 5-point model provided
	by Dlib. The CNN pretrained model he model was saved just after training so
	that if necessary it could be fine-tuned using another database.

	Finally, this gender model is provided for free by Cydral and is licensed
	under the Creative Commons Zero v1.0 Universal.
*/

#include "dlib/data_io.h"
#include "dlib/string.h"
#include <dlib/cmd_line_parser.h>
#include <dlib/image_transforms.h>
#include <dlib/dir_nav.h>
#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/image_processing/frontal_face_detector.h>

#include <iostream>
#include <iterator>

using namespace std;
using namespace dlib;

const char* VERSION = "1.1";

// ----------------------------------------------------------------------------------------

// This block of statements defines the network as proposed for the CNN Model I.
// We however removed the "dropout" regularization on the activations of convolutional layers.
template <int N, template <typename> class BN, int stride, typename SUBNET>
using block = BN<con<N, 3, 3, stride, stride, relu<BN<con<N, 3, 3, stride, stride, SUBNET>>>>>;

template <int N, typename SUBNET> using res_ = relu<block<N, bn_con, 1, SUBNET>>;
template <int N, typename SUBNET> using ares_ = relu<block<N, affine, 1, SUBNET>>;

template <typename SUBNET> using alevel1 = avg_pool<2, 2, 2, 2, ares_<64, SUBNET>>;
template <typename SUBNET> using alevel2 = avg_pool<2, 2, 2, 2, ares_<32, SUBNET>>;

using agender_type = loss_multiclass_log<fc<2, multiply<relu<fc<16, multiply<alevel1<alevel2< input_rgb_image_sized<32>>>>>>>>>;

// ----------------------------------------------------------------------------------------

#define PBSTR "||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||"
#define PBWIDTH 40
void display_progressbar(float percentage)
{
	uint32_t val = (int)(percentage * 100);
	uint32_t lpad = (int)(percentage * PBWIDTH);
	uint32_t rpad = PBWIDTH - lpad;
	printf("\r%3d%% [%.*s%*s]", val, lpad, PBSTR, rpad, "");
	fflush(stdout);
}

// ----------------------------------------------------------------------------------------

// An helper function to load dataset for testing.
enum label_ : uint16_t
{
	female_label,
	male_label,
};
struct image_info
{
	enum label_ label_image;
	string filename;
};

std::vector<image_info> get_image_listing(
	const std::string& images_folder,
	const enum label_& label
)
{
	std::vector<image_info> results;
	image_info temp;
	temp.label_image = label;

	auto dir = directory(images_folder);
	for (auto image_file : dir.get_files())
	{
		temp.filename = image_file;
		results.push_back(temp);
	}

	return results;
}
std::vector<image_info> get_train_listing_females(const std::string& ifolder) { return get_image_listing(ifolder, female_label); }
std::vector<image_info> get_train_listing_males(const std::string& ifolder) { return get_image_listing(ifolder, male_label); }

int main(int argc, char** argv) try {
	// Use a parser to set parameters.
	command_line_parser parser;

	parser.add_option("h", "");
	parser.add_option("help", "Displays this information");
	parser.add_option("version", "Display version");
	parser.add_option("test-folders", "Test the gender classifier ./females ./males", 2);
	parser.add_option("test-image", "Test the gender classifier ./image.jpg", 1);
	parser.parse(argc, argv);

	parser.check_incompatible_options("help", "version");
	parser.check_incompatible_options("help", "test-folders");
	parser.check_incompatible_options("help", "test-image");
	parser.check_incompatible_options("version", "test-folders");
	parser.check_incompatible_options("version", "test-image");
	parser.check_incompatible_options("test-folders", "test-image");

	if (parser.option("help") || parser.option("h"))
	{
		cout << "Usage: ./dnn_gender_classifier_ex [options]\n";
		parser.print_options(cout);
		cout << endl << "Note:" << endl << "You will need two different models to test this classifier.";
		cout << endl << "First, download and then decompress the gender model from: " << endl;
		cout << "http://dlib.net/files/dnn_gender_classifier_v1.dat.bz2" << endl;
		cout << "Then, you also need the face landmarking model file from:" << endl;
		cout << "http://dlib.net/files/shape_predictor_5_face_landmarks.dat.bz2" << endl;
		cout << endl << endl;
		return EXIT_SUCCESS;
	}
	if (parser.option("version"))
	{
		cout << "dnn_gender_classifier_ex v" << VERSION
			 << "\nCompiled: " << __TIME__ << " " << __DATE__ << endl << endl;
		return EXIT_SUCCESS;
	}

	if (parser.option("test-folders"))
	{
		// Initialize networks.
		frontal_face_detector detector = get_frontal_face_detector();
		shape_predictor sp;
		deserialize("shape_predictor_5_face_landmarks.dat") >> sp;

		static const char* model_net_filename = "dnn_gender_classifier_v1.dat";
		agender_type net;
		deserialize(model_net_filename) >> net;

		// Load test images.
		auto female_images = get_train_listing_females(parser.option("test-folders").argument(0));
		cout << "female examples: " << female_images.size() << endl;

		auto male_images = get_train_listing_males(parser.option("test-folders").argument(1));
		cout << "male examples: " << male_images.size() << endl << endl;

		// Test all images.
		matrix<rgb_pixel> in;
		int32_t num_right = 0, num_wrong = 0, nb_elts = 0;
		cout << "analysing female faces in progress: " << endl;
		for (auto& img : female_images)
		{
			display_progressbar((nb_elts++ / (float)female_images.size()));
			load_image(in, img.filename);
			for (auto face : detector(in))
			{
				auto shape = sp(in, face);
				if (shape.num_parts())
				{
					matrix<rgb_pixel> face_chip;
					extract_image_chip(in, get_face_chip_details(shape, 32), face_chip);
					if (net(face_chip) == female_label)
						++num_right;
					else
						++num_wrong;
				}
			}
		}
		display_progressbar(1.0f);
		// ---
		cout << endl << "analysing male faces in progress: " << endl;
		nb_elts = 0;
		for (auto& img : male_images)
		{
			display_progressbar((nb_elts++ / (float)male_images.size()));
			load_image(in, img.filename);
			for (auto face : detector(in))
			{
				auto shape = sp(in, face);
				if (shape.num_parts())
				{
					matrix<rgb_pixel> face_chip;
					extract_image_chip(in, get_face_chip_details(shape, 32), face_chip);
					if (net(face_chip) == male_label)
						++num_right;
					else
						++num_wrong;
				}
			}
		}
		display_progressbar(1.0f);
		cout << endl;

		// And at final, display accuracy.
		cout << "testing num_right: " << num_right << endl;
		cout << "testing num_wrong: " << num_wrong << endl;
		cout << "testing accuracy:  " << num_right / (double)(num_right + num_wrong) << endl;
		return EXIT_SUCCESS;
	}

	if (parser.option("test-image"))
	{
		// Initialize networks.
		frontal_face_detector detector = get_frontal_face_detector();
		shape_predictor sp;
		deserialize("shape_predictor_5_face_landmarks.dat") >> sp;

		static const char* model_net_filename = "dnn_gender_classifier_v1.dat";
		agender_type net;
		deserialize(model_net_filename) >> net;

		// Load the source image.
		matrix<rgb_pixel> in;
		load_image(in, parser.option("test-image").argument(0));

		// As proposed in the paper, use Softmax for the last layer.
		softmax<agender_type::subnet_type> snet;
		snet.subnet() = net.subnet();

		// Evaluate the gender
		int32_t cur_face = 0;
		float confidence = 0.0f;
		enum label_ gender;
		for (auto face : detector(in))
		{
			auto shape = sp(in, face);
			if (shape.num_parts())
			{
				matrix<rgb_pixel> face_chip;
				extract_image_chip(in, get_face_chip_details(shape, 32), face_chip);
				matrix<float, 1, 2> p = mat(snet(face_chip));
				if (p(0) < p(1))
				{
					gender = male_label;
					confidence = p(1);
				}
				else
				{
					gender = female_label;
					confidence = p(0);
				}
				cout << "face#" << cur_face++ << " - detected gender: " << ((gender == female_label) ? "female" : "male");
				cout << std::fixed << std::setprecision(1) << " [" << (confidence * 100.0f) << "%]" << endl;
			}
		}
		return EXIT_SUCCESS;
	}
}
catch(std::exception& e)
{
    cout << e.what() << endl;
}


/**
 *          Copyright Matthias Walter 2010.
 * Distributed under the Boost Software License, Version 1.0.
 *    (See accompanying file LICENSE_1_0.txt or copy at
 *          http://www.boost.org/LICENSE_1_0.txt)
 **/

#ifndef VIOLATOR_SEARCH_HPP_
#define VIOLATOR_SEARCH_HPP_

#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "algorithm.hpp"
#include "matroid.hpp"
#include "signing.hpp"
#include "logger.hpp"

namespace unimod
{
  namespace detail
  {

    inline matroid_element_set find_smallest_irregular_minor(const decomposed_matroid* decomposition, bool collect_extra_elements = true)
    {
      if (decomposition->is_leaf())
      {
        const decomposed_matroid_leaf* leaf = (decomposed_matroid_leaf*) decomposition;

        if (leaf->is_regular())
          return matroid_element_set();

        matroid_element_set result;
        std::copy(leaf->elements().begin(), leaf->elements().end(), std::inserter(result, result.end()));
        if (collect_extra_elements)
          std::copy(leaf->extra_elements().begin(), leaf->extra_elements().end(), std::inserter(result, result.end()));
        return result;
      }
      else
      {
        const decomposed_matroid_separator* separator = (decomposed_matroid_separator*) decomposition;

        matroid_element_set first_elements = find_smallest_irregular_minor(separator->first(), collect_extra_elements);
        matroid_element_set second_elements = find_smallest_irregular_minor(separator->second(), collect_extra_elements);
        if (first_elements.empty())
          return second_elements;
        else if (second_elements.empty())
          return first_elements;
        else
          return (first_elements.size() < second_elements.size()) ? first_elements : second_elements;
      }
    }

    template <typename InputIterator, typename OutputIterator1, typename OutputIterator2>
    std::pair <OutputIterator1, OutputIterator2> split_elements(InputIterator first, InputIterator beyond, OutputIterator1 rows,
        OutputIterator2 columns)
    {
      for (; first != beyond; ++first)
      {
        if (*first > 0)
          *columns++ = *first;
        else
          *rows++ = *first;
      }
      return std::make_pair(rows, columns);
    }

    template <typename MatrixType>
    void create_indirect_matroid(const MatrixType& input_matrix, const matroid_element_set& row_elements, const matroid_element_set& column_elements,
        integer_matroid& sub_matroid, submatrix_indices& sub_indices)
    {
      sub_matroid.resize(row_elements.size(), column_elements.size());
      submatrix_indices::vector_type row_vector(row_elements.size());
      submatrix_indices::vector_type column_vector(column_elements.size());

      size_t index = row_elements.size() - 1;
      for (matroid_element_set::const_iterator iter = row_elements.begin(); iter != row_elements.end(); ++iter)
      {
        sub_matroid.name1(index) = *iter;
        row_vector[index] = -1 - *iter;
        --index;
      }

      index = 0;
      for (matroid_element_set::const_iterator iter = column_elements.begin(); iter != column_elements.end(); ++iter)
      {
        sub_matroid.name2(index) = *iter;
        column_vector[index] = -1 + *iter;
        ++index;
      }

      sub_indices.rows = submatrix_indices::indirect_array_type(row_vector.size(), row_vector);
      sub_indices.columns = submatrix_indices::indirect_array_type(column_vector.size(), column_vector);

    }

    class violator_strategy
    {
    public:
      violator_strategy(const integer_matrix& input_matrix, const matroid_element_set& row_elements, const matroid_element_set& column_elements,
          logger& log) :
        _input_matrix(input_matrix), _row_elements(row_elements), _column_elements(column_elements), _log(log)
      {
        if (log.is_progressive() || _log.is_verbose())
        {
          std::cout << "\nMatrix is NOT totally unimodular. Searching the violating submatrix...\n" << std::endl;
        }
      }

      /**
       * Destructor
       */

      virtual ~violator_strategy()
      {

      }

      virtual void search() = 0;

      inline void create_matrix(submatrix_indices& indices) const
      {
        integer_matroid sub_matroid;
        create_indirect_matroid(_input_matrix, _row_elements, _column_elements, sub_matroid, indices);
      }

    protected:

      virtual void shrink(const matroid_element_set& row_elements, const matroid_element_set& column_elements)
      {
#ifndef NDEBUG
        typedef boost::numeric::ublas::matrix_indirect <const integer_matrix, submatrix_indices::indirect_array_type> indirect_matrix_t;

        integer_matroid matroid;
        submatrix_indices sub_indices;

        create_indirect_matroid(_input_matrix, row_elements, column_elements, matroid, sub_indices);
        indirect_matrix_t sub_matrix(_input_matrix, sub_indices.rows, sub_indices.columns);

        if (is_totally_unimodular(sub_matrix))
        {
          std::cout << "submatrix is t.u., but should not:" << std::endl;
          matrix_print(sub_matrix);

          assert (false);
        }
#endif

        _row_elements = row_elements;
        _column_elements = column_elements;

      }

      inline bool test(const matroid_element_set& row_elements, const matroid_element_set& column_elements)
      {
        typedef boost::numeric::ublas::matrix_indirect <const integer_matrix, submatrix_indices::indirect_array_type> indirect_matrix_t;

        integer_matroid matroid;
        submatrix_indices sub_indices;

        create_indirect_matroid(_input_matrix, row_elements, column_elements, matroid, sub_indices);
        indirect_matrix_t sub_matrix(_input_matrix, sub_indices.rows, sub_indices.columns);

        /// Signing test

        decomposed_matroid* decomposition;
        integer_matrix matrix(sub_matrix);

        if (_log.is_progressive() || _log.is_verbose())
        {
          std::cout << "Testing a " << row_elements.size() << " x " << column_elements.size() << " submatrix." << std::endl;
        }

        if (!is_signed_matrix(matrix))
        {
          if (_log.is_progressive() || _log.is_verbose())
          {
            std::cout << "Submatrix did not pass the signing test. It is NOT totally unimodular.\n" << std::endl;
          }
          shrink(row_elements, column_elements);
          return false;
        }

        /// Remove sign from matrix
        support_matrix(matrix);

        /// Matroid decomposition
        bool is_tu;
        boost::tie(is_tu, decomposition) = decompose_binary_matroid(matroid, matrix, matroid_element_set(), true, _log);

        if (is_tu)
        {
          if (_log.is_progressive() || _log.is_verbose())
          {
            std::cout << "\nSubmatrix is totally unimodular.\n" << std::endl;
          }
          delete decomposition;
          return true;
        }

        matroid_element_set rows, columns, elements = detail::find_smallest_irregular_minor(decomposition);
        delete decomposition;

        detail::split_elements(elements.begin(), elements.end(), std::inserter(rows, rows.end()), std::inserter(columns, columns.end()));

        if (_log.is_progressive() || _log.is_verbose())
        {
          if (rows.size() < row_elements.size() || columns.size() < column_elements.size())
          {
            std::cout << "\nThe " << row_elements.size() << " x " << column_elements.size() << " submatrix is NOT totally unimodular. A "
                << rows.size() << " x " << columns.size() << " non-totally unimodular submatrix was identified, too.\n" << std::endl;
          }
          else
          {
            std::cout << "\nThe " << row_elements.size() << " x " << column_elements.size() << " submatrix is NOT totally unimodular.\n" << std::endl;

          }
        }

        if (rows.size() < row_elements.size() || columns.size() < column_elements.size())
          shrink(rows, columns);
        else
          shrink(row_elements, column_elements);

        return false;
      }

      inline bool test_forbidden(const matroid_element_set& forbidden_elements)
      {
        /// Setup rows and columns
        matroid_element_set rows, columns;
        for (matroid_element_set::const_iterator iter = _row_elements.begin(); iter != _row_elements.end(); ++iter)
        {
          if (forbidden_elements.find(*iter) == forbidden_elements.end())
          {
            rows.insert(*iter);
          }
        }
        for (matroid_element_set::const_iterator iter = _column_elements.begin(); iter != _column_elements.end(); ++iter)
        {
          if (forbidden_elements.find(*iter) == forbidden_elements.end())
          {
            columns.insert(*iter);
          }
        }

        return test(rows, columns);
      }

    protected:
      const integer_matrix& _input_matrix;
      matroid_element_set _row_elements;
      matroid_element_set _column_elements;
      logger& _log;
    };

    class single_violator_strategy: public violator_strategy
    {
    public:
      single_violator_strategy(const integer_matrix& input_matrix, const matroid_element_set& row_elements,
          const matroid_element_set& column_elements, logger& log) :
        violator_strategy(input_matrix, row_elements, column_elements, log)
      {

      }

      /**
       * Destructor
       */

      virtual ~single_violator_strategy()
      {

      }

      virtual void search()
      {
        std::vector <int> all_elements;
        std::copy(_row_elements.begin(), _row_elements.end(), std::back_inserter(all_elements));
        std::copy(_column_elements.begin(), _column_elements.end(), std::back_inserter(all_elements));

        for (std::vector <int>::const_iterator iter = all_elements.begin(); iter != all_elements.end(); ++iter)
        {
          if (_row_elements.find(*iter) == _row_elements.end() && _column_elements.find(*iter) == _column_elements.end())
            continue;

          matroid_element_set rows(_row_elements);
          matroid_element_set columns(_column_elements);
          rows.erase(*iter);
          columns.erase(*iter);
          test(rows, columns);
        }
      }
    };

    class greedy_violator_strategy: public violator_strategy
    {
    public:
      greedy_violator_strategy(const integer_matrix& input_matrix, const matroid_element_set& row_elements,
          const matroid_element_set& column_elements, logger& log) :
        violator_strategy(input_matrix, row_elements, column_elements, log)
      {

      }

      /**
       * Destructor
       */

      virtual ~greedy_violator_strategy()
      {

      }

      /**
       * Tests minors given in a vector of sets.
       *
       * @param bundles Vector of Sets containing the removed elements.
       * @return true iff a test failed, i.e. the submatrix was not totally unimodular.
       */

      bool test_bundles(const std::vector <matroid_element_set>& bundles)
      {
        for (std::vector <matroid_element_set>::const_iterator bundle_iter = bundles.begin(); bundle_iter != bundles.end(); ++bundle_iter)
        {
          if (!test_forbidden(*bundle_iter))
          {
            return true;
          }
        }
        return false;
      }

      virtual void search()
      {
        for (float rate = 0.8f; rate > 0.02f; rate *= 0.5f)
        {
          size_t row_amount, column_amount;
          if (rate > 0.04f)
          {
            row_amount = int(_row_elements.size() * rate);
            column_amount = int(_column_elements.size() * rate);
            if (row_amount == 0 || column_amount == 0)
            {
              row_amount = 1;
              column_amount = 1;
              rate = 0.03f;
            }
          }
          else
          {
            row_amount = 1;
            column_amount = 1;
          }

          if (_log.is_progressive() || _log.is_verbose())
            std::cout << "\nGreedy loop starting, forbidden sets will have size " << row_amount << " and " << column_amount << std::endl;

          typedef std::vector <matroid_element_set::value_type> matroid_element_vector;
          matroid_element_vector shuffled_rows, shuffled_columns;
          std::copy(_row_elements.begin(), _row_elements.end(), std::back_inserter(shuffled_rows));
          std::copy(_column_elements.begin(), _column_elements.end(), std::back_inserter(shuffled_columns));

          std::random_shuffle(shuffled_rows.begin(), shuffled_rows.end());
          std::random_shuffle(shuffled_columns.begin(), shuffled_columns.end());

          std::vector <matroid_element_set> bundles;

          for (matroid_element_vector::const_iterator iter = shuffled_rows.begin(); iter + row_amount <= shuffled_rows.end(); iter += row_amount)
          {
            bundles.push_back(matroid_element_set());
            std::copy(iter, iter + row_amount, std::inserter(bundles.back(), bundles.back().end()));
          }

          for (matroid_element_vector::const_iterator iter = shuffled_columns.begin(); iter + column_amount <= shuffled_columns.end(); iter
              += column_amount)
          {
            bundles.push_back(matroid_element_set());
            std::copy(iter, iter + column_amount, std::inserter(bundles.back(), bundles.back().end()));
          }

          if (test_bundles(bundles))
          {
            rate *= 2.0f;
          }
        }
      }
    };

  }
}

#endif /* VIOLATOR_SEARCH_HPP_ */


//  (C) Copyright John Maddock 2005.
//  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)

#include <pch_light.hpp>

#define BOOST_MATH_OVERFLOW_ERROR_POLICY ignore_error
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

#include <cmath>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std{ using ::sqrt; }
#endif

//
// test_boundaries:
// This is an accuracy test, sets the two arguments to hypot to just
// above or just below various boundary conditions, and checks the accuracy
// of the result.  The values computed at double precision will use a
// different computation method to those computed at float precision:
// as long as these compute the same values then everything's OK.
//
// Tolerance is 2*epsilon, expressed here as a percentage:
//
static const float tolerance = 200 * (std::numeric_limits<float>::epsilon)();
const float boundaries[] = {
   0,
   1,
   2,
   (std::numeric_limits<float>::max)()/2,
   (std::numeric_limits<float>::min)(),
   std::numeric_limits<float>::epsilon(),
   std::sqrt((std::numeric_limits<float>::max)()) / 2,
   std::sqrt((std::numeric_limits<float>::min)()),
   std::sqrt((std::numeric_limits<float>::max)()) / 4,
   std::sqrt((std::numeric_limits<float>::min)()) * 2,
};

void do_test_boundaries(float x, float y)
{
   float expected = static_cast<float>((boost::math::hypot)(
#ifndef BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
      static_cast<long double>(x),
      static_cast<long double>(y)));
#else
      static_cast<double>(x),
      static_cast<double>(y)));
#endif
   float found = (boost::math::hypot)(x, y);
   BOOST_CHECK_CLOSE(expected, found, tolerance);
}

void test_boundaries(float x, float y)
{
   do_test_boundaries(x, y);
   do_test_boundaries(-x, y);
   do_test_boundaries(-x, -y);
   do_test_boundaries(x, -y);
}

void test_boundaries(float x)
{
   for(unsigned i = 0; i < sizeof(boundaries)/sizeof(float); ++i)
   {
      test_boundaries(x, boundaries[i]);
      test_boundaries(x, boundaries[i] + std::numeric_limits<float>::epsilon()*boundaries[i]);
      test_boundaries(x, boundaries[i] - std::numeric_limits<float>::epsilon()*boundaries[i]);
   }
}

void test_boundaries()
{
   for(unsigned i = 0; i < sizeof(boundaries)/sizeof(float); ++i)
   {
      test_boundaries(boundaries[i]);
      test_boundaries(boundaries[i] + std::numeric_limits<float>::epsilon()*boundaries[i]);
      test_boundaries(boundaries[i] - std::numeric_limits<float>::epsilon()*boundaries[i]);
   }
}

void test_spots()
{
   static const float zero = 0;
   for(unsigned i = 0; i < sizeof(boundaries)/sizeof(float); ++i)
   {
      BOOST_CHECK_EQUAL(boost::math::hypot(boundaries[i], zero), std::fabs(boundaries[i]));
      BOOST_CHECK_EQUAL(boost::math::hypot(-boundaries[i], zero), std::fabs(-boundaries[i]));
      BOOST_CHECK_EQUAL(boost::math::hypot(boundaries[i], -zero), std::fabs(boundaries[i]));
      BOOST_CHECK_EQUAL(boost::math::hypot(-boundaries[i], -zero), std::fabs(-boundaries[i]));
      for(unsigned j = 0; j < sizeof(boundaries)/sizeof(float); ++j)
      {
         BOOST_CHECK_EQUAL(boost::math::hypot(boundaries[i], boundaries[j]), boost::math::hypot(boundaries[j], boundaries[i]));
         BOOST_CHECK_EQUAL(boost::math::hypot(boundaries[i], boundaries[j]), boost::math::hypot(boundaries[i], -boundaries[j]));
         BOOST_CHECK_EQUAL(boost::math::hypot(-boundaries[i], -boundaries[j]), boost::math::hypot(-boundaries[j], -boundaries[i]));
         BOOST_CHECK_EQUAL(boost::math::hypot(-boundaries[i], -boundaries[j]), boost::math::hypot(-boundaries[i], boundaries[j]));
      }
   }
   if((std::numeric_limits<float>::has_infinity) && (std::numeric_limits<float>::has_quiet_NaN))
   {
      static const float nan = std::numeric_limits<float>::quiet_NaN();
      static const float inf = std::numeric_limits<float>::infinity();
      BOOST_CHECK_EQUAL(boost::math::hypot(inf, nan), inf);
      BOOST_CHECK_EQUAL(boost::math::hypot(-inf, nan), inf);
      BOOST_CHECK_EQUAL(boost::math::hypot(nan, inf), inf);
      BOOST_CHECK_EQUAL(boost::math::hypot(nan, -inf), inf);
      for(unsigned j = 0; j < sizeof(boundaries)/sizeof(float); ++j)
      {
         BOOST_CHECK_EQUAL(boost::math::hypot(boundaries[j], inf), inf);
         BOOST_CHECK_EQUAL(boost::math::hypot(-boundaries[j], inf), inf);
         BOOST_CHECK_EQUAL(boost::math::hypot(inf, boundaries[j]), inf);
         BOOST_CHECK_EQUAL(boost::math::hypot(inf, -boundaries[j]), inf);
         BOOST_CHECK_EQUAL(boost::math::hypot(boundaries[j], -inf), inf);
         BOOST_CHECK_EQUAL(boost::math::hypot(-boundaries[j], -inf), inf);
         BOOST_CHECK_EQUAL(boost::math::hypot(-inf, boundaries[j]), inf);
         BOOST_CHECK_EQUAL(boost::math::hypot(-inf, -boundaries[j]), inf);
      }
   }
}

BOOST_AUTO_TEST_CASE( test_main )
{
   BOOST_MATH_CONTROL_FP;
   test_boundaries();
   test_spots();
}


//#define CGAL_USE_BOOST_BIMAP

#include <fstream>
#include <vector>
#include <deque>
#include <boost/config.hpp>
#include <boost/version.hpp>

// CGAL headers
#include <CGAL/Bbox_2.h>
#include <CGAL/assertions_behaviour.h>
#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Timer.h>
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
#include <CGAL/IO/WKT.h>
#endif

//#define CGAL_TESTING_POLYLINE_SIMPLIFICATION
//#define CGAL_POLYLINE_SIMPLIFICATION_TRACE_LEVEL 15

bool lAppToLog = false ;
void Polyline_simplification_2_external_trace( std::string m )
{
  std::ofstream out("polysim_log.txt", ( lAppToLog ? std::ios::app | std::ios::ate : std::ios::trunc | std::ios::ate ) );
  out << std::setprecision(19) << m << std::endl << std::flush ;
  lAppToLog = true ;
}

void error_handler ( char const* what, char const* expr, char const* file, int line, char const* msg )
{
  std::cerr << "CGAL error: " << what << " violation!" << std::endl
       << "Expr: " << expr << std::endl
       << "File: " << file << std::endl
       << "Line: " << line << std::endl;
  if ( msg != 0)
      std::cerr << "Explanation:" << msg << std::endl;

  throw std::runtime_error("CGAL Error");
}

#include <CGAL/Constrained_Delaunay_triangulation_2.h>
#include <CGAL/Constrained_triangulation_plus_2.h>
#include <CGAL/Polyline_simplification_2/simplify.h>
#include <CGAL/Polyline_simplification_2/Squared_distance_cost.h>
#include <CGAL/Polyline_simplification_2/Scaled_squared_distance_cost.h>

// Qt headers
#include <QtGui>
#include <QString>
#include <QActionGroup>
#include <QFileDialog>
#include <QInputDialog>
#include <QDragEnterEvent>
#include <QDropEvent>
#include <QSlider>
#include <QProgressBar>

// GraphicsView items and event filters (input classes)
#include <CGAL/Qt/GraphicsViewPolylineInput.h>
#include <CGAL/Qt/Polyline_simplification_2_graphics_item.h>
#include <CGAL/Qt/Converter.h>

// the two base classes
#include "ui_Polyline_simplification_2.h"
#include <CGAL/Qt/DemosMainWindow.h>

// for viewportsBbox(QGraphicsScene*)
#include <CGAL/Qt/utility.h>

typedef CGAL::Exact_predicates_inexact_constructions_kernel K;

typedef K::Point_2         Point_2;
typedef K::Segment_2       Segment_2;
typedef K::Iso_rectangle_2 Iso_rectangle_2;

typedef CGAL::Polyline_simplification_2::Vertex_base_2<K> Vb;
typedef CGAL::Constrained_triangulation_face_base_2<K>    Fb;
typedef CGAL::Triangulation_data_structure_2<Vb,Fb>       TDS;
typedef CGAL::Exact_predicates_tag                        Itag;
typedef CGAL::Constrained_Delaunay_triangulation_2<K,TDS, Itag> CDT;

typedef CGAL::Constrained_triangulation_plus_2<CDT>       PCT;

namespace PS2 = CGAL::Polyline_simplification_2 ;

enum Mode { ABS_P, ABS_E, REL_P, REL_E, MIXED_P, MIXED_E } ;

class MainWindow :
  public CGAL::Qt::DemosMainWindow,
  public Ui::Polyline_simplification_2
{
  Q_OBJECT

private:

  PCT                                                m_pct;
  QGraphicsScene                                    mScene;
  CGAL::Qt::PolylineSimplificationGraphicsItem<PCT> * mGI;
  CGAL::Qt::GraphicsViewPolylineInput<K> *          mPI;

private:

public:

  MainWindow();

protected:
  void dragEnterEvent(QDragEnterEvent *event);
  void dropEvent(QDropEvent *event);
private:

  void loadPoly(QString);
  void loadOSM (QString);
  void loadWKT (QString);

protected Q_SLOTS:
void open(QString);

public Q_SLOTS:

  void processInput(CGAL::Object o);

  void on_actionShowTriangulation_toggled(bool checked);

  void on_actionInsertPolyline_toggled(bool checked);

  void on_actionClear_triggered();

  void on_actionRecenter_triggered();

  void on_actionLoadConstraints_triggered();

  void on_actionSimplify_triggered();

  Mode getSimplificationMode() ;

  double getThreshold() ;

Q_SIGNALS:
  void changed();
};

MainWindow::MainWindow()
  : DemosMainWindow()
{
  CGAL::set_error_handler  (error_handler);
  CGAL::set_warning_handler(error_handler);

  setupUi(this);

  setAcceptDrops(true);

  // Add a GraphicItem for the PS triangulation
  mGI = new CGAL::Qt::PolylineSimplificationGraphicsItem<PCT>(&m_pct);

  QObject::connect(this, SIGNAL(changed()), mGI, SLOT(modelChanged()));

  mGI->setVerticesPen(QPen(Qt::black, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  mGI->setUnremovableVerticesPen(QPen(Qt::blue, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
  mGI->setZValue(-1);
  mGI->setVisibleEdges(false);
  mGI->setVisibleConstraints(true);

  mScene.addItem(mGI);

  // Setup input handlers. They get events before the mScene gets them
  // and the input they generate is passed to the triangulation with
  // the signal/slot mechanism
  mPI = new CGAL::Qt::GraphicsViewPolylineInput<K>(this, &mScene, 0, true); // inputs polylines which are not closed
  this->on_actionInsertPolyline_toggled(true);
  QObject::connect(mPI, SIGNAL(generate(CGAL::Object)), this, SLOT(processInput(CGAL::Object)));

  //
  // Manual handling of actions
  //
  QObject::connect(this->actionQuit, SIGNAL(triggered()), this, SLOT(close()));

  // We put mutually exclusive actions in an QActionGroup
  //  QActionGroup* ag = new QActionGroup(this);
  //  ag->addAction(this->actionInsertPolyline);

  this->actionShowTriangulation->setChecked(false);

  //
  // Setup the mScene and the view
  //
  mScene.setItemIndexMethod(QGraphicsScene::NoIndex);
  mScene.setSceneRect(-100, -100, 100, 100);
  this->graphicsView->setScene(&mScene);
  this->graphicsView->setMouseTracking(true);

  // Turn the vertical axis upside down
  this->graphicsView->scale(1, -1);

  // The navigation adds zooming and translation functionality to the
  // QGraphicsView
  this->addNavigation(this->graphicsView);

  this->setupStatusBar();
  this->setupOptionsMenu();
  this->addAboutDemo(":/cgal/help/about_Polyline_simplification_2.html");
  this->addAboutCGAL();
  this->setupExportSVG(action_Export_SVG, graphicsView);

  this->addRecentFiles(this->menuFile, this->actionQuit);
  connect(this, SIGNAL(openRecentFile(QString)), this, SLOT(open(QString)));
}

void
MainWindow::dragEnterEvent(QDragEnterEvent *event)
{
  if (event->mimeData()->hasFormat("text/uri-list"))
    event->acceptProposedAction();
}

void
MainWindow::dropEvent(QDropEvent *event)
{
  QString filename = event->mimeData()->urls().at(0).path();
  open(filename);
  event->acceptProposedAction();
}

void
MainWindow::processInput(CGAL::Object o)
{
  std::list<Point_2> points;
  if(CGAL::assign(points, o))
  {
    if(points.size() >= 2)
    {
      m_pct.insert_constraint(points.begin(), points.end());
#if 0
      std::ofstream out("polygon.txt");
      out.precision(12);
      out << points.size() << std::endl;
      for(std::list<Point_2>::iterator it = points.begin(); it!= points.end(); ++it){
        out << *it << std::endl;
      }
#endif
      Q_EMIT( changed());
    }
  }
  Q_EMIT( changed());
}

/*
 *  Qt Automatic Connections
 *  https://doc.qt.io/qt-5/designer-using-a-ui-file.html#automatic-connections
 *
 *  setupUi(this) generates connections to the slots named
 *  "on_<action_name>_<signal_name>"
 */
void
MainWindow::on_actionInsertPolyline_toggled(bool checked)
{
  if(checked){
    mScene.installEventFilter(mPI);
  } else {
    mScene.removeEventFilter(mPI);
  }
}

void
MainWindow::on_actionShowTriangulation_toggled(bool checked)
{
  mGI->setVisibleEdges(checked);
  update();
}

void
MainWindow::on_actionClear_triggered()
{
  m_pct.clear();
  mGI->modelChanged();
  Q_EMIT( changed());
}

Mode MainWindow::getSimplificationMode()
{
  bool lP     = rbuttonPercentage->isChecked();
  int  lError = rbuttonAbsError  ->isChecked() ? 0 : ( rbuttonRelError  ->isChecked() ? 1 : 2 ) ;

  return lP ? lError == 0 ? ABS_P : ( lError == 1 ? REL_P : MIXED_P )
            : lError == 0 ? ABS_E : ( lError == 1 ? REL_E : MIXED_E ) ;
}

double MainWindow::getThreshold()
{
  double lValue = this->textValue->text().toDouble() ;
  return rbuttonPercentage->isChecked() ? lValue / 100.0
           : rbuttonAbsError->isChecked() ? lValue * lValue
              : lValue  ;
}

void MainWindow::on_actionSimplify_triggered()
{
  // wait cursor
  QApplication::setOverrideCursor(Qt::WaitCursor);

  try
  {
    switch( getSimplificationMode() )
    {
    case ABS_P   : simplify(m_pct, PS2::Squared_distance_cost(),  PS2::Stop_below_count_ratio_threshold(getThreshold())   ) ;
 break ;
    case ABS_E   : simplify(m_pct, PS2::Squared_distance_cost(), PS2::Stop_above_cost_threshold(getThreshold()) ) ; break ;
    case REL_P   : simplify(m_pct, PS2::Scaled_squared_distance_cost(),  PS2::Stop_below_count_ratio_threshold(getThreshold()) ) ; break ;
    case REL_E   : simplify(m_pct, PS2::Scaled_squared_distance_cost(), PS2::Stop_above_cost_threshold(getThreshold()) ) ; break ;
    case MIXED_P : simplify(m_pct, PS2::Hybrid_squared_distance_cost<double>(1.0), PS2::Stop_below_count_ratio_threshold(getThreshold())) ; break ;
 case MIXED_E : simplify(m_pct, PS2::Hybrid_squared_distance_cost<double>(1.0), PS2::Stop_above_cost_threshold(getThreshold())) ; break ;

      break ;
    }

    statusBar()->showMessage(QString("Simplification done"));
  }
  catch(...)
  {
    statusBar()->showMessage(QString("Exception ocurred"));
  }

   // default cursor
  QApplication::restoreOverrideCursor();
  mGI->modelChanged();
  Q_EMIT( changed());
}

void
MainWindow::open(QString fileName)
{
  if(! fileName.isEmpty()){
    if(fileName.endsWith(".osm")){
      loadOSM(fileName);
      this->addToRecentFiles(fileName);
    }
    else if(fileName.endsWith(".wkt")){
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
      loadWKT(fileName);
      this->addToRecentFiles(fileName);
#endif
    }
  }
}

void
MainWindow::on_actionLoadConstraints_triggered()
{
  QString fileName = QFileDialog::getOpenFileName(this,
                                                  tr("Open Constraint File"),
                                                  "../data"
                                                #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                                                  ,tr("Polylines (*.osm *.wkt);;")
                                                #endif
                                                  );
  open(fileName);
}

std::string trim_right ( std::string str )
{
  if ( str.length() > 0 )
  {
    std::size_t pos = str.find_last_not_of( " " ) ;
    if ( pos != std::string::npos )
      return str.substr(0,pos+1);
  }

  return std::string("") ;
}

void MainWindow::loadWKT(QString
                         #if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
                         fileName
                         #endif
                         )
{
#if BOOST_VERSION >= 105600 && (! defined(BOOST_GCC) || BOOST_GCC >= 40500)
    typedef std::vector<Point_2> MultiPoint;

  typedef std::vector<Point_2> LineString;
  typedef std::deque<LineString> MultiLineString;

  typedef CGAL::Polygon_2<K> Polygon_2;
  typedef CGAL::Polygon_with_holes_2<K> Polygon_with_holes_2;
  typedef std::deque<Polygon_with_holes_2> MultiPolygon;

  std::ifstream ifs(qPrintable(fileName));
  MultiPoint points;
  MultiLineString polylines;
  MultiPolygon polygons;
  CGAL::read_WKT(ifs, points,polylines,polygons);

  m_pct.clear();
  mGI->modelChanged();

  if(! points.empty()){
    std::cout << "Ignore " << points.size() << " isolated points" << std::endl;
  }
  for(LineString poly : polylines){
    if ( poly.size() > 2 ){
      m_pct.insert_constraint(poly.begin(), poly.end());
    }
  }
 for(Polygon_with_holes_2 poly : polygons){
   m_pct.insert_constraint(poly.outer_boundary().vertices_begin(), poly.outer_boundary().vertices_end());
   for(Polygon_with_holes_2::Hole_const_iterator it = poly.holes_begin(); it != poly.holes_end(); ++it){
     const Polygon_2& hole = *it;
      m_pct.insert_constraint(hole);
    }
 }

  Q_EMIT( changed());

  actionRecenter->trigger();
#endif
}

void MainWindow::loadOSM(QString fileName)
{
  m_pct.clear();
  mGI->modelChanged();

  try
  {
    std::ifstream ifs(qPrintable(fileName));

    std::string line ;

    std::vector<Point_2> poly ;

    while ( std::getline(ifs,line) )
    {
      line = trim_right(line);

      if ( line.size() > 0 )
      {
        if ( line.find(':') != std::string::npos )
        {
          if ( poly.size() > 0 )
          {
            if ( poly.front() == poly.back() && poly.size() >= 4 )
            {
              if ( is_simple_2(poly.begin(), poly.end() - 1) )
                m_pct.insert_constraint(poly.begin(), poly.end() - 1 ) ;
            }
            else if ( poly.size() >= 2 )
            {
              if ( is_simple_2(poly.begin(), poly.end()) )
                m_pct.insert_constraint(poly.begin(), poly.end() ) ;
            }
          }
          poly.clear();
        }
        else
        {
          double x,y ;

          std::string::size_type pos = line.find(',');
          if ( pos != std::string::npos )
            line[pos]= ' ' ;

          std::istringstream ss(line);

          ss >> x >> y ;

          poly.push_back( Point_2(x,y) );
        }
      }
    }

    if ( poly.size() > 0 )
    {
      if ( poly.front() == poly.back() )
      {
        m_pct.insert_constraint(poly.begin(), poly.end() - 1 ) ;
      }
      else
      {
        m_pct.insert_constraint(poly.begin(), poly.end() ) ;
      }
    }
  }
  catch(...)
  {
    statusBar()->showMessage(QString("Exception ocurred"));
  }

  Q_EMIT( changed());

  actionRecenter->trigger();
}

void
MainWindow::on_actionRecenter_triggered()
{
  this->graphicsView->setSceneRect(mGI->boundingRect());
  this->graphicsView->fitInView(mGI->boundingRect(), Qt::KeepAspectRatio);
}

#include "Polyline_simplification_2.moc"

int main(int argc, char **argv)
{
  QApplication app(argc, argv);

  app.setOrganizationDomain("geometryfactory.com");
  app.setOrganizationName("GeometryFactory");
  app.setApplicationName("Polyline_simplification_2 demo");

  // Import resources from libCGALQt5.
  CGAL_QT_INIT_RESOURCES;

  MainWindow mainWindow;
  mainWindow.show();
  return app.exec();
}