// What do we all need?
//
// External datastructures (information):
// 1. Graph
//      a. Should provide an range over
//      factors() and
//      variables()
//      b. Should comply with Boost.Graph.AdjacencyGraph, AdjacencyMatrixGraph and VertexListGraph
// 2. SlaveMinimizer should support:
//      a. get(slave_minimzer, factor)(Messages, MultiAssignment)
// 3. SampleSpaceMap should support:
//      a. for (it = get(sample_space_map, variable).first;
//              it!= get(sample_space_map, variable).second; ++it)
//
// Internal datastructures:
//
//    2. A data structure for variable assignments : MultiAssignment
//    a PropertyMap with key as undirected edge -> sample_space_element (bool or size_t)
//    3. A data structure for Lagrange dual variables: Messages
//    a PropertyMap with key as undirected edge and sample_space_element -> energy_type
//    4. A data structure for `MessageGradients`
//    a PropertyMap with key as undirected edge and sample_space_element -> energy_type
//    5. A data structure to record disagreement among factors Disagreement
// factor -> boolean
//
// Pseudo code:
// initialize() Messages with 0
// while max_iterations > 0:
//     for each factor in graph that is in Disagreement[factor]
//         minimize_slave_problem(Messages) to update MultiAssignment
//     for each variable in graph:
//         disagrees = false
//         if disagreement() in MultiAssignment:
//             resolve_disagreement() to update Messages
//             disagrees = true
//         for factors in adjacent_vertices(variable):
//             Disagreement[factor] = disagrees
//
#include <boost/graph/graph_traits.hpp>
#include <boost/foreach.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/typeof/typeof.hpp>
#include <boost/range/size.hpp>
#include <boost/type_traits.hpp>
#include <boost/unordered_map.hpp>
#include <boost/utility/result_of.hpp>
#include "debugflags.h"
#include "visualiser.h"

#include "gtsam/discrete/Assignment.h"

namespace occgrid {

struct on_disagreeing_factor { };
struct on_disagreeing_variable { };
struct on_disagreement_computation { };

template <typename Graph, typename Messages, typename MultiAssignment, typename EnergyType>
struct subgradient_update
  : public boost::base_visitor<subgradient_update<Graph, Messages, MultiAssignment, EnergyType> >
{
  typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef on_disagreeing_variable event_filter;
  subgradient_update(
      Messages& messages,
      MultiAssignment& multi_assignment,
      EnergyType stepsize)
    : messages_(messages),
    multi_assignment_(multi_assignment),
    stepsize_(stepsize)
  { }

  void operator()(vertex_descriptor x, const Graph& g_) {
    typedef typename boost::property_traits<Messages>::key_type msg_key_type;
    BOOST_FOREACH(vertex_descriptor f, adjacent_vertices(x, g_))
      messages_[msg_key_type(f, x, get(multi_assignment_, std::make_pair(f, x)))] += stepsize_;
  }
  private:
  Messages& messages_;
  const MultiAssignment& multi_assignment_;
  EnergyType stepsize_;
};

template <typename G, typename Minimizer, typename Messages, typename MultiAssignment>
struct laser_factor_disagreement_visitor
: public boost::base_visitor<laser_factor_disagreement_visitor<G, Minimizer, Messages, MultiAssignment> >
{
  typedef on_disagreeing_factor event_filter;
  typedef typename boost::graph_traits<G>::vertex_descriptor vertex_descriptor;
  laser_factor_disagreement_visitor(
      const Minimizer& minmizer,
      Messages& messages,
      MultiAssignment& multi_assignment)
    : minmizer_(minmizer),
    messages_(messages),
    multi_assignment_(multi_assignment)
  { }

  void operator()(vertex_descriptor f, const G& g) {
    get(minmizer_, f)(multi_assignment_, messages_);
  }

  private:
    const Minimizer minmizer_;
    Messages& messages_;
    MultiAssignment& multi_assignment_;
};

template <typename Graph, typename MultiAssignment>
bool disagrees(
    typename boost::graph_traits<Graph>::vertex_descriptor x,
    const Graph& g,
    MultiAssignment& multi_assignment)
{
  typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  bool allsame = true;
  bool first_iter = true;
  vertex_descriptor f1;
  BOOST_FOREACH(vertex_descriptor f, adjacent_vertices(x, g)) {
    if (first_iter) {
      f1 = f;
      first_iter = false;
    }
    if (multi_assignment[std::make_pair(f, x)]
        != multi_assignment[std::make_pair(f1, x)]) {
      allsame = false;
      break;
    }
  }
  return (! allsame);
}

template <typename G, typename MultiAssignment>
struct DisagreementTracker {
  typedef typename MultiAssignment::key_type key_type;
  typedef typename MultiAssignment::value_type value_type;
  typedef typename MultiAssignment::reference reference;
  typedef typename boost::writable_property_map_tag category;
  typedef typename boost::graph_traits<G>::vertex_descriptor vertex_descriptor;
  DisagreementTracker(const G& g,
      MultiAssignment& multi_assignment
      ) : g_(g) , multi_assignment_(multi_assignment),
  disagreeing_vertices_(), variable_disagrement_count_(0),
  is_cache_valid_(false)
  {
    // Initialize
    BOOST_FOREACH(vertex_descriptor f, factors(g_))
      disagreeing_vertices_[f] = true;
  }

  reference get(typename MultiAssignment::key_type k) const {
    return occgrid::get(multi_assignment_, k);
  }

  void put(typename MultiAssignment::key_type k, typename MultiAssignment::value_type v) {
    occgrid::put(multi_assignment_, k, v);
    is_cache_valid_ = false;
  }

  bool is_disagreement(const vertex_descriptor& x) const {
    if (( ! is_factor(x, g_)) && (! is_cache_valid_)) {
      update_cache();
    }
    return disagreeing_vertices_[x];
  }

  size_t disagreement_count() const {
    if (! is_cache_valid_) {
      update_cache();
    }
    return variable_disagrement_count_;
  }

  private:
    void update_cache() const {
      std::cout << "Updating disagreement cache" << std::endl;
      variable_disagrement_count_ = 0;
      BOOST_FOREACH(vertex_descriptor xi, variables(g_)) {
        disagreeing_vertices_[xi] = disagrees(xi, g_, multi_assignment_);
        if (disagreeing_vertices_[xi]) {
          BOOST_FOREACH(vertex_descriptor fi, adjacent_vertices(xi, g_))
            disagreeing_vertices_[fi] = true; // any disagreement is total disagreement
          variable_disagrement_count_ += 1;
        }
      }
      is_cache_valid_ = true;
    }
    const G& g_;
    MultiAssignment& multi_assignment_;
    mutable boost::unordered_map<vertex_descriptor, bool> disagreeing_vertices_;
    mutable size_t variable_disagrement_count_;
    mutable bool is_cache_valid_;
};

template <typename DisagreementTracker>
bool is_disagreement(
    const DisagreementTracker& dt,
    typename DisagreementTracker::key_type::first_type x)
{
  return dt.is_disagreement(x);
}

template <typename G, typename MultiAssignment>
size_t disagreement_count(const DisagreementTracker<G, MultiAssignment>& dt) {
  return dt.disagreement_count();
}

template <typename Messages, typename G, typename sample_space_type>
typename boost::property_traits<Messages>::value_type
vertex_energy(const Messages& msgs,
    const G& g,
    typename boost::graph_traits<G>::vertex_descriptor x,
    sample_space_type xv)
{
  typedef typename boost::graph_traits<G>::vertex_descriptor vertex_descriptor;
  typedef typename boost::property_traits<Messages>::value_type energy_type;
  typedef typename boost::property_traits<Messages>::key_type msg_key_type;
  energy_type venergy(0);
  BOOST_FOREACH(vertex_descriptor f, adjacent_vertices(x, g))
    venergy += msgs[msg_key_type(f, x, xv)];
  return venergy;
}

template <typename FactorGraph, typename DualDecompositionVisitor, typename DisagreementTracker>
size_t dual_decomposition_traversal(const FactorGraph& g_,
    DualDecompositionVisitor visitors_,
    DisagreementTracker& disagreement_tracker_)
{
  typedef typename boost::graph_traits<FactorGraph>::vertex_descriptor vertex_descriptor;
  BOOST_FOREACH(vertex_descriptor f, factors(g_)) {
    if (is_disagreement(disagreement_tracker_, f)) {
      // minimize_slave_problem(Messages) to update MultiAssignment
      invoke_visitors(visitors_, f, g_, on_disagreeing_factor());
    }
  }
  std::cout << "Factor traversal done" << std::endl;

  BOOST_FOREACH(vertex_descriptor x, variables(g_)) {
    if (is_disagreement(disagreement_tracker_, x)) {
      // update messages to resolve disagreements
      invoke_visitors(visitors_, x, g_, on_disagreeing_variable());
    }
  }
  return disagreement_count(disagreement_tracker_);
}

template <typename G, typename SampleSpaceMap, typename Messages, typename DisagreementTracker>
typename boost::property_traits<SampleSpaceMap>::value_type::first_type::value_type
assignment(
    const G& g_,
    typename boost::graph_traits<G>::vertex_descriptor x,
    const Messages& messages_,
    const SampleSpaceMap& sample_space_map_,
    const DisagreementTracker& disagreement_tracker_)
{
  typedef typename boost::graph_traits<G>::vertex_descriptor vertex_descriptor;
  typedef typename boost::property_traits<Messages>::value_type energy_type;
  typedef typename boost::property_traits<SampleSpaceMap>::value_type::first_type::value_type sample_space_type;
  sample_space_type assign = (((double)rand() / RAND_MAX) < 0.5) ? 0 : 1;
  if (is_disagreement(disagreement_tracker_, x)) {
    // maximum energy assignment
    energy_type max_energy = vertex_energy(messages_, g_, x, assign);
    BOOST_FOREACH(sample_space_type xv, get(sample_space_map_, x)) {
      energy_type xv_energy = vertex_energy(messages_, g_, x, xv);
      assign = (max_energy >= xv_energy) ?  assign : xv;
    }

  } else {
    // There is agreement, take any agreed assignment
    BOOST_FOREACH(vertex_descriptor f, adjacent_vertices(x, g_)) {
      assign = get(disagreement_tracker_, std::make_pair(f, x));
      break;
    }
  }
  return assign;
}
template <typename Graph, typename SlaveMinimizer, typename SampleSpaceMap, typename DisagreementMap, typename Messages, typename SampleSpaceType, typename EnergyType>
struct SubgradientDualDecomposition {
  typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  SubgradientDualDecomposition(
      const Graph& g,
      const SlaveMinimizer& slave_minimizer,
      const SampleSpaceMap& sample_space_map,
      DisagreementMap& disagreement_tracker,
      Messages& messages,
      EnergyType init_step_size
      )
    : g_(g),
    slave_minimizer_(slave_minimizer),
    sample_space_map_(sample_space_map),
    disagreement_tracker_(disagreement_tracker),
    messages_(messages),
    init_step_size_(init_step_size),
    laser_vis_(slave_minimizer, messages_, disagreement_tracker_)
  {
  }

  bool operator()(size_t i) {
    EnergyType step_i_x = init_step_size_ / (i + 1);
    subgradient_update<Graph, Messages, DisagreementMap, EnergyType>
      subg_update(messages_, disagreement_tracker_, step_i_x);

    size_t count = dual_decomposition_traversal(
        g_,
        std::make_pair(laser_vis_, subg_update),
        disagreement_tracker_);

    return (count == 0);
  }

  SampleSpaceType assignment(
      typename boost::graph_traits<Graph>::vertex_descriptor x)
  {
    return occgrid::assignment(g_, x, messages_, sample_space_map_, disagreement_tracker_);
  }

  EnergyType marginal(
      typename boost::graph_traits<Graph>::vertex_descriptor x,
      SampleSpaceType value)
  {
    if ( ! is_disagreement(disagreement_tracker_, x)) {
      BOOST_FOREACH(vertex_descriptor f, adjacent_vertices(x, g_)) {
        SampleSpaceType assign = get(disagreement_tracker_, std::make_pair(f, x));
        // Agreement. Take any assignment and decide on energy values.
        return (assign == value) ? 0 : std::numeric_limits<EnergyType>::infinity();
      }
      // Agreement but there is no factor associated. This is unexplored
      // region.
      return - std::log(0.5);
    }

    // Disgreement
    // maximum energy assignment
    EnergyType min_energy = std::numeric_limits<EnergyType>::infinity(),
    max_energy = 0;
    BOOST_FOREACH(SampleSpaceType xv, get(sample_space_map_, x)) {
      EnergyType xv_energy = vertex_energy(messages_, g_, x, xv);
      if (max_energy < xv_energy)
        max_energy = xv_energy;
      if (min_energy > xv_energy)
        min_energy = xv_energy;
    }

    EnergyType xv_energy = vertex_energy(messages_, g_, x, value);
    EnergyType retenergy = (max_energy - xv_energy);
    assert(retenergy >= 0);
    return retenergy;
  }

  private:
      const Graph& g_;
      const SlaveMinimizer& slave_minimizer_;
      const SampleSpaceMap& sample_space_map_;
      DisagreementMap& disagreement_tracker_;
      Messages messages_;
      EnergyType init_step_size_;

      laser_factor_disagreement_visitor<Graph, SlaveMinimizer, Messages, DisagreementMap> laser_vis_;
};

template <typename Graph, typename Functor, typename DisagreementMap, typename sample_space_type>
void iterate_dualdecomposition(
    const Graph& g_,
    Functor& func,
    DisagreementMap& disagreement_tracker_,
    size_t max_iter,
    double max_clock
    )
{
  typedef typename boost::graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  bool converged = false;

  // main loop
  clock_t st = clock();
  for (size_t i = 0;
      (i < max_iter) && (not converged) && clock() < (max_clock - st);
      ++i)
  {
    converged = func(i);

    // Debugging logistics

    size_t count = disagreement_count(disagreement_tracker_);
    std::cout << "Variable disagreement count:" << count << std::endl;

    gtsam::Assignment<gtsam::Index> best_assign;
    std::vector<double> marg(num_variables(g_));
    BOOST_FOREACH(vertex_descriptor x, variables(g_)) {
      best_assign[x] = func.assignment(x);
      marg[x] = std::exp(-func.marginal(x, 1));
    }
    double energy = g_.g_(best_assign);
    clock_t et = clock();
    std::cout << "<Energy>\t" << ((float)(et - st)) / CLOCKS_PER_SEC << "\t" << energy << std::endl;

    global_vis_.setMarginals(marg);
    global_vis_.show(10);

    std::vector<sample_space_type> disagreeing_cells;
    BOOST_FOREACH(vertex_descriptor x, variables(g_))
      if (is_disagreement(disagreement_tracker_, x))
        disagreeing_cells.push_back(x);
    global_vis2_.highlightCells(disagreeing_cells);
    global_vis2_.reset();
    global_vis2_.show(1);
  }
  global_vis_.show();
  global_vis_.save("/tmp/dualdecomposition.png");
}
} // namespace occgrid


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <boost/bind.hpp>
#include <boost/make_shared.hpp>
#include <memory>
#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

#include "Tudat/Basics/testMacros.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/orbitalElementConversions.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/physicalConstants.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/unitConversions.h"
#include "Tudat/Mathematics/NumericalIntegrators/rungeKutta4Integrator.h"
#include "Tudat/Astrodynamics/BasicAstrodynamics/stateVectorIndices.h"
#include "Tudat/Basics/basicTypedefs.h"
#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/SimulationSetup/PropagationSetup/dynamicsSimulator.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/body.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EstimationSetup/createNumericalSimulator.h"

namespace tudat
{

namespace unit_tests
{

using namespace tudat::propagators;
using namespace tudat::simulation_setup;
using namespace tudat::numerical_integrators;

BOOST_AUTO_TEST_SUITE( test_body_custom_state_propagation )

double getDummyCustomState1(
        const double currentTime, const double currentCustomState )
{
    return -0.02;
}

double getDummyCustomState2(
        const double currentTime, const double currentCustomState )
{
    return -0.02 * currentTime;
}

double getDummyCustomState3(
        const double currentTime, const double currentCustomState )
{
    return -0.002 * currentCustomState;
}

double getDummyCustomState4(
        const double currentTime, const double currentCustomState )
{
    return -0.00002 * currentCustomState * currentTime;
}

// Test custom state propagation, linearly decreasing with time
BOOST_AUTO_TEST_CASE( testSingleCustomStatePropagation )
{
    // Crate bodyMap
    NamedBodyMap bodyMap;

    // Create settings for propagation
    double initialCustomState = 500.0;
    std::shared_ptr< CustomStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< CustomStatePropagatorSettings< double > >(
                std::bind( &getDummyCustomState1, std::placeholders::_1, std::placeholders::_2 ), initialCustomState,
                std::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) );

    // Define numerical integrator settings.
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1.0 );

    // Create dynamics simulation object.
    SingleArcDynamicsSimulator< double, double > dynamicsSimulator(
                bodyMap, integratorSettings, propagatorSettings, true, false, false );

    // Test propagated solution.
    std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( );
         stateIterator != integratedState.end( ); stateIterator++ )
    {
        BOOST_CHECK_EQUAL( stateIterator->second.rows( ), 1 );
        BOOST_CHECK_SMALL( std::fabs( stateIterator->second( 0 ) - ( 500.0 - 0.02 * stateIterator->first ) ), 1.0E-9 );
    }
}

// Test custom state propagation, quadratically decreasing with time
BOOST_AUTO_TEST_CASE( testSingleCustomStatePropagation2 )
{
    // Crate bodyMap
    NamedBodyMap bodyMap;

    // Create settings for propagation
    double initialCustomState = 500.0;
    std::shared_ptr< CustomStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< CustomStatePropagatorSettings< double > >(
                std::bind( &getDummyCustomState2, std::placeholders::_1, std::placeholders::_2 ), initialCustomState,
                std::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) );

    // Define numerical integrator settings.
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1.0 );

    // Create dynamics simulation object.
    SingleArcDynamicsSimulator< double, double > dynamicsSimulator(
                bodyMap, integratorSettings, propagatorSettings, true, false, false );

    // Test propagated solution.
    std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( );
         stateIterator != integratedState.end( ); stateIterator++ )
    {
        BOOST_CHECK_EQUAL( stateIterator->second.rows( ), 1 );
        BOOST_CHECK_SMALL( std::fabs( stateIterator->second( 0 ) -
                                      ( 500.0 - 0.5 * 0.02 * stateIterator->first  * stateIterator->first ) ), 1.0E-9 );
    }
}

// Test custom state propagation, exponentially decreasing with time
BOOST_AUTO_TEST_CASE( testSingleCustomStatePropagation3 )
{
    // Crate bodyMap
    NamedBodyMap bodyMap;

    // Create settings for propagation
    double initialCustomState = 500.0;
    std::shared_ptr< CustomStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< CustomStatePropagatorSettings< double > >(
                std::bind( &getDummyCustomState3, std::placeholders::_1, std::placeholders::_2 ), initialCustomState,
                std::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) );

    // Define numerical integrator settings.
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 1.0 );

    // Create dynamics simulation object.
    SingleArcDynamicsSimulator< double, double > dynamicsSimulator(
                bodyMap, integratorSettings, propagatorSettings, true, false, false );

    // Test propagated solution.
    std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( );
         stateIterator != integratedState.end( ); stateIterator++ )
    {
        BOOST_CHECK_EQUAL( stateIterator->second.rows( ), 1 );
        BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ), 500.0 * std::exp( -stateIterator->first * 0.002 ), 1.0E-9 );
    }
}

// Test custom state propagation, exponentially decreasing with square of time
BOOST_AUTO_TEST_CASE( testSingleCustomStatePropagation4 )
{
    // Crate bodyMap
    NamedBodyMap bodyMap;

    // Create settings for propagation
    double initialCustomState = 500.0;
    std::shared_ptr< CustomStatePropagatorSettings< double > > propagatorSettings =
            std::make_shared< CustomStatePropagatorSettings< double > >(
                std::bind( &getDummyCustomState4, std::placeholders::_1, std::placeholders::_2 ), initialCustomState,
                std::make_shared< PropagationTimeTerminationSettings >( 100.0 ) );

    // Define numerical integrator settings.
    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >( rungeKutta4, 0.0, 0.01 );

    // Create dynamics simulation object.
    SingleArcDynamicsSimulator< double, double > dynamicsSimulator(
                bodyMap, integratorSettings, propagatorSettings, true, false, false );

    // Test propagated solution.
    std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );
    for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( );
         stateIterator != integratedState.end( ); stateIterator++ )
    {

        BOOST_CHECK_EQUAL( stateIterator->second.rows( ), 1 );
        BOOST_CHECK_CLOSE_FRACTION( stateIterator->second( 0 ),
                                    500.0 * std::exp( -0.5 * stateIterator->first * stateIterator->first * 0.00002 ), 1.0E-9 );
    }
}

// Test if custom mass rate is properly computed in multi-type propagation
// (majority of test copied form unitTestMultiTypeStatePropagation).
BOOST_AUTO_TEST_CASE( testMultiTypeCustomStatePropagation )
{
    using namespace simulation_setup;
    using namespace propagators;
    using namespace numerical_integrators;
    using namespace orbital_element_conversions;
    using namespace basic_mathematics;
    using namespace gravitation;
    using namespace numerical_integrators;
    using namespace unit_conversions;

    // Load Spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Set simulation end epoch.
    const double simulationEndEpoch = tudat::physical_constants::JULIAN_DAY;

    // Set numerical integration fixed step size.
    const double fixedStepSize = 10.0;

    // Define body settings for simulation.
    std::map< std::string, std::shared_ptr< BodySettings > > bodySettings;
    bodySettings[ "Earth" ] = std::make_shared< BodySettings >( );
    bodySettings[ "Earth" ]->ephemerisSettings = std::make_shared< ConstantEphemerisSettings >(
                Eigen::Vector6d::Zero( ), "SSB", "J2000" );
    bodySettings[ "Earth" ]->gravityFieldSettings = std::make_shared< GravityFieldSettings >( central_spice );

    // Create Earth object
    NamedBodyMap bodyMap = createBodies( bodySettings );

    // Create spacecraft object.
    bodyMap[ "Asterix" ] = std::make_shared< simulation_setup::Body >( );
    bodyMap[ "Asterix" ]->setEphemeris( std::make_shared< ephemerides::TabulatedCartesianEphemeris< > >(
                                            std::shared_ptr< interpolators::OneDimensionalInterpolator
                                            < double, Eigen::Vector6d  > >( ), "Earth", "J2000" ) );

    // Finalize body creation.
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "J2000" );

    // Define propagator settings variables.
    SelectedAccelerationMap accelerationMap;
    std::vector< std::string > bodiesToPropagate;
    std::vector< std::string > centralBodies;

    // Define propagation settings.
    std::map< std::string, std::vector< std::shared_ptr< AccelerationSettings > > > accelerationsOfAsterix;
    accelerationsOfAsterix[ "Earth" ].push_back( std::make_shared< AccelerationSettings >(
                                                     basic_astrodynamics::central_gravity ) );
    accelerationMap[ "Asterix" ] = accelerationsOfAsterix;
    bodiesToPropagate.push_back( "Asterix" );
    centralBodies.push_back( "Earth" );

    // Create acceleration models and propagation settings.
    basic_astrodynamics::AccelerationMap accelerationModelMap = createAccelerationModelsMap(
                bodyMap, accelerationMap, bodiesToPropagate, centralBodies );

    // Set Keplerian elements for Asterix.
    Eigen::Vector6d asterixInitialStateInKeplerianElements;
    asterixInitialStateInKeplerianElements( semiMajorAxisIndex ) = 7500.0E3;
    asterixInitialStateInKeplerianElements( eccentricityIndex ) = 0.1;
    asterixInitialStateInKeplerianElements( inclinationIndex ) = convertDegreesToRadians( 85.3 );
    asterixInitialStateInKeplerianElements( argumentOfPeriapsisIndex )
            = convertDegreesToRadians( 235.7 );
    asterixInitialStateInKeplerianElements( longitudeOfAscendingNodeIndex )
            = convertDegreesToRadians( 23.4 );
    asterixInitialStateInKeplerianElements( trueAnomalyIndex ) = convertDegreesToRadians( 139.87 );

    // Convert Asterix state from Keplerian elements to Cartesian elements.
    double earthGravitationalParameter = bodyMap.at( "Earth" )->getGravityFieldModel( )->getGravitationalParameter( );
    Eigen::VectorXd systemInitialState = convertKeplerianToCartesianElements(
                asterixInitialStateInKeplerianElements,
                earthGravitationalParameter );

    std::shared_ptr< SingleArcPropagatorSettings< double > > translationalPropagatorSettings =
            std::make_shared< TranslationalStatePropagatorSettings< double > >
            ( centralBodies, accelerationModelMap, bodiesToPropagate, systemInitialState,
              std::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) );

    // Create mass rate model and mass propagation settings
    std::map< std::string, std::shared_ptr< basic_astrodynamics::MassRateModel > > massRateModels;
    massRateModels[ "Vehicle" ] = std::make_shared< basic_astrodynamics::CustomMassRateModel >(
                [ ]( const double ){ return -0.01; } );
    Eigen::VectorXd initialMass = Eigen::VectorXd( 1 );
    initialMass( 0 ) = 500.0;
    std::shared_ptr< SingleArcPropagatorSettings< double > > massPropagatorSettings =
            std::make_shared< MassPropagatorSettings< double > >(
                std::vector< std::string >{ "Asterix" }, massRateModels, initialMass,
                std::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) );

    // Create custom state derivative model settings
    double initialCustomState = 500.0;
    std::shared_ptr< SingleArcPropagatorSettings< double > > customPropagatorSettings =
            std::make_shared< CustomStatePropagatorSettings< double > >(
                std::bind( &getDummyCustomState1, std::placeholders::_1, std::placeholders::_2 ), initialCustomState,
                std::make_shared< PropagationTimeTerminationSettings >( 1000.0 ) );

    // Create total propagator settings, depending on current case.
    std::shared_ptr< PropagatorSettings< double > > propagatorSettings;

    std::vector< std::shared_ptr< SingleArcPropagatorSettings< double > > >  propagatorSettingsList;
    propagatorSettingsList.push_back( translationalPropagatorSettings );
    propagatorSettingsList.push_back( massPropagatorSettings );
    propagatorSettingsList.push_back( customPropagatorSettings );

    propagatorSettings = std::make_shared< MultiTypePropagatorSettings< double > >(
                propagatorSettingsList,
                std::make_shared< PropagationTimeTerminationSettings >( simulationEndEpoch ) );

    std::shared_ptr< IntegratorSettings< > > integratorSettings =
            std::make_shared< IntegratorSettings< > >
            ( rungeKutta4, 0.0, fixedStepSize );

    // Create simulation object and propagate dynamics.
    SingleArcDynamicsSimulator< > dynamicsSimulator(
                bodyMap, integratorSettings, propagatorSettings, true, false, true );

    std::map< double, Eigen::VectorXd > integratedState = dynamicsSimulator.getEquationsOfMotionNumericalSolution( );

    // Test propagated solution.
    for( std::map< double, Eigen::VectorXd >::const_iterator stateIterator = integratedState.begin( );
         stateIterator != integratedState.end( ); stateIterator++ )
    {
        BOOST_CHECK_EQUAL( stateIterator->second.rows( ), 8 );
        BOOST_CHECK_SMALL( std::fabs( stateIterator->second( 6 ) - ( 500.0 - 0.01 * stateIterator->first ) ), 1.0E-9 );
        BOOST_CHECK_SMALL( std::fabs( stateIterator->second( 7 ) - ( 500.0 - 0.02 * stateIterator->first ) ), 1.0E-9 );
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat


#include <k52/dsp/transform/wavelet/fast_wavelet_transform.h>
#include <k52/dsp/transform/fast_fourier_transform.h>
#include <boost/tuple/tuple_comparison.hpp>
#include <stdexcept>

using ::std::vector;
using ::std::conj;
using ::std::complex;
using ::std::invalid_argument;
using ::k52::dsp::FastWaveletTransform;
using ::k52::dsp::IScale;

//TODO in c++11 there is a default hash implementation for tuples,
// so this should be removed and k52::dsp::FastWaveletTransform::ScaledWaveletKey
// made private when k52 migrate to c++11
namespace boost
{
size_t hash_value(k52::dsp::FastWaveletTransform::ScaledWaveletKey const & t)
{
    std::size_t seed = 0;
    boost::hash_combine( seed, t.get<0>().get() );
    boost::hash_combine( seed, t.get<1>() );
    boost::hash_combine( seed, t.get<2>() );
    return seed;
}
}

namespace k52
{
namespace dsp
{

FastWaveletTransform::FastWaveletTransform(
        IScale::shared_ptr scale,
        size_t sequence_size,
        double cache_data_limit_in_megabytes
)
        : i_scale_(scale)
{
    i_circular_convolution_ = FourierBasedCircularConvolution::shared_ptr(
            new FourierBasedCircularConvolution(
                    IFourierTransform::shared_ptr(
                            new FastFourierTransform(sequence_size)
                    )
            )
    );

    cache_ = ScaledWaveletCache::Create(cache_data_limit_in_megabytes);
}

vector< vector< complex< double > > > FastWaveletTransform::Transform(
        const vector< std::complex< double > > &sequence,
        IWavelet::shared_ptr wavelet) const
{
    vector< double > scales = i_scale_->GetScales();

    vector< vector< complex< double > > > result(scales.size());

    for (size_t i = 0; i < scales.size(); ++i)
    {
        result[i] = TransformOneScale(sequence, wavelet, scales[i]);
        //TODO this is a hack to allow filling of wavelet samples with N/2 shift - think of a better solution
        std::rotate(result[i].begin(), result[i].begin() + result[i].size() / 2, result[i].end());
    }

    return result;
}

std::vector< std::complex< double > > FastWaveletTransform::TransformOneScale(
        const std::vector< std::complex< double > > &sequence, IWavelet::shared_ptr wavelet, double scale) const
{
    if (scale <= 0)
    {
        throw invalid_argument("scale <= 0");
    }

    return i_circular_convolution_->EvaluateConvolution(sequence,
        GetWaveletSamplesForConvolution(wavelet, scale, sequence.size()));
}

std::vector< std::complex< double > > FastWaveletTransform::GetWaveletSamplesForConvolution(
        IWavelet::shared_ptr wavelet, double scale, size_t N) const
{
    ScaledWaveletKey cache_key = boost::make_tuple(wavelet, scale, N);

    if (cache_ && cache_->IsCached(cache_key))
    {
        return cache_->GetCachedValue(cache_key);
    }
    else
    {
        vector< complex< double > > wavelet_samples = CalculateWaveletSamplesForConvolution(wavelet, scale, N);
        if (cache_)
        {
            cache_->AddValue(cache_key, wavelet_samples);
        }
        return wavelet_samples;
    }
}

std::vector< std::complex< double > > FastWaveletTransform::CalculateWaveletSamplesForConvolution(
    IWavelet::shared_ptr wavelet, double scale, size_t N) const
{
    vector< complex< double > > wavelet_samples(N);
    for (size_t n = 0; n < N; ++n)
    {
        double t = n - double(N) / 2.;
        wavelet_samples[n] = conj(GetScaledWaveletValue(wavelet, scale, -t));
    }
    return wavelet_samples;
}

complex< double > FastWaveletTransform::GetScaledWaveletValue(
        IWavelet::shared_ptr wavelet, double scale, double t) const
{
    return wavelet->GetValue(t / scale) / sqrt(scale);
}

} // namespace dsp
} // namespace k52


#include <iostream>
#include <Eigen/Dense>
#include <chrono>
#include "DriverParser.h"
#include "Object3D.h"
#include "CameraSpecifications.h"
#include "CameraPictureTaker.h"

using Eigen::MatrixXd;
int main(int argc, char* argv[]) {
    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

    if (argc != 3) {
        std::cout << "invalid command line usage" << std::endl;
        return 1;
    }

    std::string fileName = argv[1];
    std::string outPutFileName = argv[2];
    DriverParser parser(fileName);
    parser.parse();

    CameraSpecification *specs = parser.getCameraSpecs();

    std::vector<Object3D> models;

    for (int i = 0; i < parser.getTransformations()->size(); ++i) {
        Object3D object3D(parser.getTransformations()->at(i).getFileName());
        object3D.performTransformation(&parser.getTransformations()->at(i));

        models.push_back(object3D);
    }

    CameraPictureTaker imageMaker(specs, &models, parser.getLighting(), parser.getSpheres());
    imageMaker.printImageToFile(outPutFileName);

    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();

    std::cout << "Execution time: "  << (double) std::chrono::duration_cast<std::chrono::microseconds>(end - begin).count() / 1000000.0 << 's' << std::endl;
}


/**
 * Copyright (c) 2012 Andrew Prock. All rights reserved.
 * $Id: CardSet.cpp 2649 2012-06-30 04:53:24Z prock $
 */
#include <cstdio>
#include <cassert>
#include <algorithm>
#include <iostream>
#include <limits>
#include <vector>
#include <set>
#include <boost/array.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <pokerstove/util/combinations.h>
#include "Rank.h"
#include "Suit.h"
#include "Card.h"
#include "CardSet.h"
#include "PokerEvaluation.h"
#include "PokerEvaluationTables.h"

using namespace std;
using namespace boost;
using namespace pokerstove;
using namespace pokerstove;

// some suit mask macros
#define SMASK(n)   (static_cast<int>(_cardmask >> (n)*Rank::NUM_RANK) & 0x1FFF)

#if 1
#define C()        (static_cast<int>(_cardmask >> (0)*Rank::NUM_RANK) & 0x1FFF)
#define D()        (static_cast<int>(_cardmask >> (1)*Rank::NUM_RANK) & 0x1FFF)
#define H()        (static_cast<int>(_cardmask >> (2)*Rank::NUM_RANK) & 0x1FFF)
#define S()        (static_cast<int>(_cardmask >> (3)*Rank::NUM_RANK) & 0x1FFF)
#else
#define C()        SMASK(0)
#define D()        SMASK(1)
#define H()        SMASK(2)
#define S()        SMASK(3)
#endif

#define RMASK()    (C() | D() | H() | S())

#define LOWBALL_ROTATE_RANKS(ranks)             \
  ((((ranks) & ~(1 << Rank::AceVal())) << 1)    \
   | (((ranks) >> Rank::AceVal()) & 0x01))

/**
 * ctors
 */

CardSet::CardSet()
    : _cardmask(0)
{}

CardSet::CardSet(const CardSet& cs)
    : _cardmask(cs._cardmask)
{}

CardSet::CardSet(const Card& c)
    : _cardmask(ONE64 << c._card)
{}

CardSet::CardSet(const string& c)
    : _cardmask(0)
{
    fromString(c);
}

string CardSet::str() const
{
    string out = "";
    uint64_t v = _cardmask;
    while (v) {
        Card card(lastbit64(v));
        out += card.str();
        v &= v - 1; // clear the least significant bit set
    }
    return out;
}

// TODO: use same loop as in ::str()
string CardSet::rankstr() const
{
    string out = "";
    for (size_t i=0; i<STANDARD_DECK_SIZE; i++)
    {
        uint64_t mask = ONE64 << i;
        if (_cardmask & mask)
        {
            Card c(i);
            out += c.rank().str();
        }
    }
    sort(out.begin(), out.end());
    return out;
}

string CardSet::toRankBitString() const
{
    string out = "";
    for (size_t i=0; i<STANDARD_DECK_SIZE; i++)
    {
        if (_cardmask & ONE64 << i)
            out += "1";
        else
            out += ".";
        if (((i+1)%Rank::NUM_RANK) == 0)
            out += " ";
    }
    return out;
}

size_t CardSet::countSuits() const
{
    int ns = 0;
    for (size_t i=0; i<Suit::NUM_SUIT; i++)
        if (SMASK(i) > 0)
            ns++;
    return ns;
}

size_t CardSet::countMaxSuit() const
{
    vector<size_t> suit(Suit::NUM_SUIT);
    suit[0] = (nRanksTable[SMASK(0)]);
    suit[1] = (nRanksTable[SMASK(1)]);
    suit[2] = (nRanksTable[SMASK(2)]);
    suit[3] = (nRanksTable[SMASK(3)]);

    return *std::max_element(suit.begin(),suit.end());
}

size_t CardSet::size() const
{
    uint64_t v = _cardmask;
    size_t c;
    for (c = 0; v; c++)
    {
        v &= v - 1; // clear the least significant bit set
    }
    return c;
}

void CardSet::fromString(const string& instr)
{
	clear ();

	for (size_t i=0; i<instr.size(); i+=2) {
        // skip whitespace
        if (instr[i] == ' ') {
            i -= 1;
            continue;
        }
        int code = Rank::rank_code(instr[i]) + Suit::suit_code(instr[i+1]) * Rank::NUM_RANK;
        uint64_t mask = (ONE64 << code);
		if (_cardmask & mask) {
			clear();  // card duplication is an error, no hand parsed
			return;
		}
        _cardmask |= mask;
    }
}

CardSet& CardSet::insert(const CardSet& c) {
  _cardmask |= c._cardmask;
  return *this;
}

CardSet CardSet::rotateSuits(int c, int d, int h, int s) const {
  return CardSet(
          static_cast<uint64_t>(suitMask(Suit::Clubs())) << Rank::NUM_RANK * c |
          static_cast<uint64_t>(suitMask(Suit::Diamonds())) << Rank::NUM_RANK * d |
          static_cast<uint64_t>(suitMask(Suit::Hearts())) << Rank::NUM_RANK * h |
          static_cast<uint64_t>(suitMask(Suit::Spades())) << Rank::NUM_RANK * s);
}

void CardSet::flipSuits() {
  *this = rotateSuits(3, 2, 1, 0);
}

CardSet CardSet::canonize() const {
  int smasks[Suit::NUM_SUIT];
  int i = 0;
  for (Suit s = Suit::begin(); s < Suit::end(); ++s)
    smasks[i++] = suitMask(s);

  sort(smasks, smasks + Suit::NUM_SUIT);

  return CardSet(
          static_cast<uint64_t>(smasks[3]) |
          static_cast<uint64_t>(smasks[2]) << Rank::NUM_RANK |
          static_cast<uint64_t>(smasks[1]) << Rank::NUM_RANK * 2 |
          static_cast<uint64_t>(smasks[0]) << Rank::NUM_RANK * 3);
}

CardSet CardSet::canonize(const CardSet& other) const
{
    CardSet cother = other.canonize();
    vector<int> perms = findSuitPermutation(other, cother);
    CardSet hand = *this;
    CardSet chand = hand.rotateSuits(perms[0],perms[1],perms[2],perms[3]);
    return chand;
}

// This is super slow, make it faster
bool CardSet::insertRanks(const CardSet& rset)
{
    // try the fastest way first
    if ((_cardmask & rset._cardmask) == 0)
    {
        _cardmask |= rset._cardmask;
        return true;
    }

    // now try shifting by flipping around suits
    for (size_t s=1; s< Suit::NUM_SUIT; s++)
    {
        CardSet flipped(rset);
        flipped.flipSuits();
        if ((_cardmask & flipped._cardmask) == 0)
        {
            _cardmask |= flipped._cardmask;
            return true;
        }
    }

    // ok, just do it the slow way
    for (Rank r=Rank::Two(); r<=Rank::Ace(); ++r)
    {
        size_t nrin = rset.count(r);
        if (nrin == 0)
            continue;

        if (count(r) + nrin > Suit::NUM_SUIT)
            return false;

        for (Suit s=Suit::Clubs(); s<=Suit::Spades(); ++s)
            //for (Suit s=Suit::Spades(); s>=Suit::Spades(); --s)
        {
            Card c(r,s);
            if (!contains(c))
            {
                insert(c);
                nrin--;
            }
            if (nrin == 0)
                break;
        }
    }
    return true;
}

CardSet CardSet::canonizeRanks() const {
    // this is very slow, optimize if it winds up in an inner loop
    CardSet ret;
    string ranks = rankstr();
    for (size_t i = 0; i < ranks.size(); i++) {
        for (Suit s = Suit::Clubs(); s <= Suit::Spades(); ++s) {
            Card candidate(Rank(ranks.substr(i, 1)), s);
            if (!ret.contains(candidate)) {
                ret.insert(candidate);
                break;
            }
        }
    }
    return ret;
}

CardSet& CardSet::insert(const Card& c)
{
    _cardmask |= (ONE64 << c.code());
    return *this;
}

vector<Card> CardSet::cards() const
{
    vector<Card> out(size());
    size_t n = 0;
    for (size_t i=0; i<STANDARD_DECK_SIZE; i++)
        if (_cardmask & (ONE64 << i))
            out[n++] = Card(i);
    return out;
}

vector<CardSet> CardSet::cardSets() const
{
    vector<CardSet> out(size());
    size_t n = 0;
    for (size_t i=0; i<STANDARD_DECK_SIZE; i++)
        if (_cardmask & (ONE64 << i))
            out[n++] = CardSet(ONE64<<i);
    return out;
}

CardSet& CardSet::remove(const CardSet& c)
{
    _cardmask ^= c._cardmask;
    return *this;
}

CardSet& CardSet::remove(const Card& c)
{
    _cardmask ^= ONE64<<c.code();
    return *this;
}

bool CardSet::contains(const CardSet& c) const
{
    return (c._cardmask == (_cardmask & c._cardmask));
}

bool CardSet::contains(const Card& c) const
{
    uint64_t bit = ONE64 << c.code();
    return ((bit & _cardmask) != 0);
}

bool CardSet::contains(const Rank& r) const
{
    return (RMASK() & r.rankBit()) > 0;
}

Card CardSet::find(const Rank& r) const
{
    for (uint8_t i=0; i<Suit::NUM_SUIT; i++)
    {
        if (SMASK(i) & r.rankBit())
            return Card(r, Suit(i));
    }
    return Card();
}

bool CardSet::contains(const Suit& s) const
{
    if (SMASK(s.code()) > 0)
        return true;
    return false;
}

int CardSet::rankMask() const
{
    return RMASK();
}

size_t CardSet::countRanks() const
{
    return nRanksTable[C() | D() | H() | S()];
}

int CardSet::suitMask(const Suit& s) const
{
    return SMASK(s.code());
}

size_t CardSet::count(const Rank& r) const
{
#if 1
    // this version is faster, if less obvious
    int c = C();
    int d = D();
    int h = H();
    int s = S();

    int four_mask = c & d & h & s;
    if (four_mask & r.rankBit())
        return 4;

    int three_mask =
        ((c&d)|(h&s)) &
        ((c&h)|(d&s));
    if (three_mask & r.rankBit())
        return 3;

    int rankmask = c | d | h | s;
    int two_mask = rankmask ^(c ^ d ^ h ^ s);
    if (two_mask & r.rankBit())
        return 2;

    if (rankmask & r.rankBit())
        return 1;

    return 0;
#else
    size_t ret = 0;
    for (size_t i=0; i<Suit::NUM_SUIT; i++)
        if (r.rankBit() & SMASK(i))
            ret++;
    return ret;
#endif
}

bool CardSet::hasStraight() const
{
    if (straightTable[RMASK()] > 0)
        return true;
    return false;
}

// this is a rough draft of the evaluateHigh routine, the idea is that
// it should be AFAP.  However, this is a general 1-7 card evaluator, so
// it cannot be too fast.  We could unwrap the method calls here for the
// PokerHand, maybe it would speed things up.
//
// note, there are no function calls in this function
PokerEvaluation CardSet::evaluateHigh() const
{
    // first the easy stuff
    int c = C();
    int d = D();
    int h = H();
    int s = S();
    int rankmask = c | d | h | s;

    if (nRanksTable[rankmask] >= 5)
    {
        int sranks = 0;
        int suitindex=-1;
        if (nRanksTable[c] >= 5)
        {
            suitindex = Suit::ClubVal();
            sranks = c;
        }
        else if (nRanksTable[d] >= 5)
        {
            suitindex = Suit::DiamondVal();
            sranks = d;
        }
        else if (nRanksTable[h] >= 5)
        {
            suitindex = Suit::HeartVal();
            sranks = h;
        }
        else if (nRanksTable[s] >= 5)
        {
            suitindex = Suit::SpadeVal();
            sranks = s;
        }
        if (suitindex >= 0)
        {
            int strval = straightTable[sranks];
            if (strval > 0)
                return PokerEvaluation((STRAIGHT_FLUSH<<VSHIFT) ^ strval<<MAJOR_SHIFT);
            else
                return PokerEvaluation((FLUSH<<VSHIFT) ^ topFiveRanksTable[sranks]);
        }
        int strval = straightTable[rankmask];
        if (strval > 0)
            return PokerEvaluation((STRAIGHT<<VSHIFT) ^(strval<<MAJOR_SHIFT));
    }

    int ncards = nRanksTable[c] + nRanksTable[d] + nRanksTable[h] + nRanksTable[s];
    int ndups = ncards - nRanksTable[rankmask];

    switch (ndups)
    {
        case 0:     // no pair
        {
            return PokerEvaluation((NO_PAIR<<VSHIFT) ^ topFiveRanksTable[rankmask]);
        }
        break;

        case 1:     // one pair
        {
            int two_mask = rankmask ^(c ^ d ^ h ^ s);
            int topind = topRankTable[two_mask];
            int kickers = topThreeRanksTable[rankmask ^(0x01<<topind)];
            return PokerEvaluation((ONE_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^ kickers);
        }
        break;

        case 2:
        {
            int two_mask = rankmask ^(c ^ d ^ h ^ s);

            if (two_mask)   // two pair
            {
                int topind = topRankTable[two_mask];
                int botind = botRankTable[two_mask];
                int kicker = topRankTable[rankmask ^ two_mask];
                if (kicker >= 0)
                    return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT) ^(0x01<<kicker));
                else
                    return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
            }
            else
            {
                int three_mask =
                    ((c&d)|(h&s)) &
                    ((c&h)|(d&s));
                int topind = topRankTable[three_mask];
                int kickers = rankmask ^(0x01<<topind);
                int kbits = 0;
                if (kickers > 0)
                    kbits   = 0x01<<topRankTable[kickers];
                if (kbits >= 0 && ((kickers^kbits) > 0))
                    kbits      ^= 0x01<<topRankTable[kickers^kbits];
                return PokerEvaluation((THREE_OF_A_KIND<<VSHIFT) ^(topind << MAJOR_SHIFT) ^ kbits);
            }
        }
        break;

        default:
        {
            int four_mask = c & d & h & s;
            if (four_mask)
            {
                int topind = topRankTable[four_mask];
                int kicker = rankmask;
                kicker ^= (0x01<<topind);
                kicker  = topRankTable[kicker];
                if (kicker >= 0)
                    return PokerEvaluation((FOUR_OF_A_KIND<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(0x01<<kicker));
                else
                    return PokerEvaluation((FOUR_OF_A_KIND<<VSHIFT) ^(topind<<MAJOR_SHIFT));
            }

            int two_mask = rankmask ^(c ^ d ^ h ^ s);
            if (nRanksTable[two_mask] != ndups)
            {
                int three_mask =
                    ((c&d)|(h&s)) &
                    ((c&h)|(d&s));
                int topind = topRankTable[three_mask];
                if (two_mask > 0)
                {
                    int botind = topRankTable[two_mask];
                    return PokerEvaluation((FULL_HOUSE<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
                }
                else
                {
                    int botind = topRankTable[three_mask ^ 0x01<<topind];
                    return PokerEvaluation((FULL_HOUSE<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
                }
            }

            int topind = topRankTable[two_mask];
            int botind = topRankTable[two_mask ^ 0x01<<topind];
            int kicker = rankmask ^ 0x01<<topind ^ 0x01<<botind;
            kicker  = topRankTable[kicker];
            if (kicker >= 0)
                return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT) ^(0x01<<kicker));
            else
                return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
        }
        break;
    }

    cerr << "oops\n";
    return PokerEvaluation(0);
}

// same as evaluateHigh, but with the non-flush logic
// elided
PokerEvaluation CardSet::evaluateHighFlush() const
{
    // first the easy stuff
    int c = C();
    int d = D();
    int h = H();
    int s = S();
    int rankmask = c | d | h | s;

    if (nRanksTable[rankmask] >= 5)
    {
        int sranks = 0;
        int suitindex=-1;
        if (nRanksTable[c] >= 5)
        {
            suitindex = Suit::ClubVal();
            sranks = c;
        }
        else if (nRanksTable[d] >= 5)
        {
            suitindex = Suit::DiamondVal();
            sranks = d;
        }
        else if (nRanksTable[h] >= 5)
        {
            suitindex = Suit::HeartVal();
            sranks = h;
        }
        else if (nRanksTable[s] >= 5)
        {
            suitindex = Suit::SpadeVal();
            sranks = s;
        }
        if (suitindex >= 0)
        {
            int strval = straightTable[sranks];
            if (strval > 0)
                return PokerEvaluation((STRAIGHT_FLUSH<<VSHIFT) ^ strval<<MAJOR_SHIFT);
            else
                return PokerEvaluation((FLUSH<<VSHIFT) ^ topFiveRanksTable[sranks]);
        }
    }
    return PokerEvaluation(0);
}

// evaluteHighRanks, same as evaluateHigh, but with the flush logic
// elided
PokerEvaluation CardSet::evaluateHighRanks() const
{
    // first the easy stuff
    int c = C();
    int d = D();
    int h = H();
    int s = S();
    int rankmask = c | d | h | s;

    if (nRanksTable[rankmask] >= 5)
    {
        int strval = straightTable[rankmask];
        if (strval > 0)
            return PokerEvaluation((STRAIGHT<<VSHIFT) ^(strval<<MAJOR_SHIFT));
    }

    int ncards = nRanksTable[c] + nRanksTable[d] + nRanksTable[h] + nRanksTable[s];
    int ndups = ncards - nRanksTable[rankmask];

    switch (ndups)
    {
        case 0:     // no pair
        {
            return PokerEvaluation((NO_PAIR<<VSHIFT) ^ topFiveRanksTable[rankmask]);
        }
        break;

        case 1:     // one pair
        {
            int two_mask = rankmask ^(c ^ d ^ h ^ s);
            int topind = topRankTable[two_mask];
            int kickers = topThreeRanksTable[rankmask ^(0x01<<topind)];
            return PokerEvaluation((ONE_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^ kickers);
        }
        break;

        case 2:
        {
            int two_mask = rankmask ^(c ^ d ^ h ^ s);

            if (two_mask)   // two pair
            {
                int topind = topRankTable[two_mask];
                int botind = botRankTable[two_mask];
                int kicker = topRankTable[rankmask ^ two_mask];
                if (kicker >= 0)
                    return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT) ^(0x01<<kicker));
                else
                    return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
            }
            else
            {
                int three_mask =
                    ((c&d)|(h&s)) &
                    ((c&h)|(d&s));
                int topind = topRankTable[three_mask];
                int kickers = rankmask ^(0x01<<topind);
                int kbits = 0;
                if (kickers > 0)
                    kbits   = 0x01<<topRankTable[kickers];
                if (kbits >= 0 && ((kickers^kbits) > 0))
                    kbits      ^= 0x01<<topRankTable[kickers^kbits];
                return PokerEvaluation((THREE_OF_A_KIND<<VSHIFT) ^(topind << MAJOR_SHIFT) ^ kbits);
            }
        }
        break;

        default:
        {
            int four_mask = c & d & h & s;
            if (four_mask)
            {
                int topind = topRankTable[four_mask];
                int kicker = rankmask;
                kicker ^= (0x01<<topind);
                kicker  = topRankTable[kicker];
                if (kicker >= 0)
                    return PokerEvaluation((FOUR_OF_A_KIND<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(0x01<<kicker));
                else
                    return PokerEvaluation((FOUR_OF_A_KIND<<VSHIFT) ^(topind<<MAJOR_SHIFT));
            }

            int two_mask = rankmask ^(c ^ d ^ h ^ s);
            if (nRanksTable[two_mask] != ndups)
            {
                int three_mask =
                    ((c&d)|(h&s)) &
                    ((c&h)|(d&s));
                int topind = topRankTable[three_mask];
                if (two_mask > 0)
                {
                    int botind = topRankTable[two_mask];
                    return PokerEvaluation((FULL_HOUSE<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
                }
                else
                {
                    int botind = topRankTable[three_mask ^ 0x01<<topind];
                    return PokerEvaluation((FULL_HOUSE<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
                }
            }

            int topind = topRankTable[two_mask];
            int botind = topRankTable[two_mask ^ 0x01<<topind];
            int kicker = rankmask ^ 0x01<<topind ^ 0x01<<botind;
            kicker  = topRankTable[kicker];
            if (kicker >= 0)
                return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT) ^(0x01<<kicker));
            else
                return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
        }
        break;
    }

    cerr << "oops\n";
    return PokerEvaluation(0);
}

// this is just evaluateHigh without the straight or flush code
PokerEvaluation CardSet::evaluatePairing() const
{
    // first the easy stuff
    int c = C();
    int d = D();
    int h = H();
    int s = S();
    int rankmask = c | d | h | s;
    int ncards = nRanksTable[c] + nRanksTable[d] + nRanksTable[h] + nRanksTable[s];
    int ndups = ncards - nRanksTable[rankmask];

    switch (ndups)
    {
        case 0:     // no pair
        {
            return PokerEvaluation((NO_PAIR<<VSHIFT) ^ topFiveRanksTable[rankmask]);
        }
        break;

        case 1:     // one pair
        {
            int two_mask = rankmask ^(c ^ d ^ h ^ s);
            int topind = topRankTable[two_mask];
            int kickers = topThreeRanksTable[rankmask ^(0x01<<topind)];
            return PokerEvaluation((ONE_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^ kickers);
        }
        break;

        case 2:
        {
            int two_mask = rankmask ^(c ^ d ^ h ^ s);

            if (two_mask)   // two pair
            {
                int topind = topRankTable[two_mask];
                int botind = botRankTable[two_mask];
                int kicker = topRankTable[rankmask ^ two_mask];
                if (kicker >= 0)
                    return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT) ^(0x01<<kicker));
                else
                    return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
            }
            else
            {
                int three_mask =
                    ((c&d)|(h&s)) &
                    ((c&h)|(d&s));
                int topind = topRankTable[three_mask];
                int kickers = rankmask ^(0x01<<topind);
                int kbits = 0;
                if (kickers > 0)
                    kbits   = 0x01<<topRankTable[kickers];
                if (kbits >= 0 && ((kickers^kbits) > 0))
                    kbits      ^= 0x01<<topRankTable[kickers^kbits];
                return PokerEvaluation((THREE_OF_A_KIND<<VSHIFT) ^(topind << MAJOR_SHIFT) ^ kbits);
            }
        }
        break;

        default:
        {
            int four_mask = c & d & h & s;
            if (four_mask)
            {
                int topind = topRankTable[four_mask];
                int kicker = rankmask;
                kicker ^= (0x01<<topind);
                kicker  = topRankTable[kicker];
                if (kicker >= 0)
                    return PokerEvaluation((FOUR_OF_A_KIND<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(0x01<<kicker));
                else
                    return PokerEvaluation((FOUR_OF_A_KIND<<VSHIFT) ^(topind<<MAJOR_SHIFT));
            }

            int two_mask = rankmask ^(c ^ d ^ h ^ s);
            if (nRanksTable[two_mask] != ndups)
            {
                int three_mask =
                    ((c&d)|(h&s)) &
                    ((c&h)|(d&s));
                int topind = topRankTable[three_mask];
                if (two_mask > 0)
                {
                    int botind = topRankTable[two_mask];
                    return PokerEvaluation((FULL_HOUSE<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
                }
                else
                {
                    int botind = topRankTable[three_mask ^ 0x01<<topind];
                    return PokerEvaluation((FULL_HOUSE<<VSHIFT) ^(topind<<MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
                }
            }

            int topind = topRankTable[two_mask];
            int botind = topRankTable[two_mask ^ 0x01<<topind];
            int kicker = rankmask ^ 0x01<<topind ^ 0x01<<botind;
            kicker  = topRankTable[kicker];
            if (kicker >= 0)
                return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT) ^(0x01<<kicker));
            else
                return PokerEvaluation((TWO_PAIR<<VSHIFT) ^(topind << MAJOR_SHIFT) ^(botind << MINOR_SHIFT));
        }
        break;
    }

    cerr << "oops\n";
    return PokerEvaluation(0);
}

// This one is not fully optimized
PokerEvaluation CardSet::evaluateLow2to7() const
{
    PokerEvaluation high;

    // if there are five or fewer cards, we just evaluate the high,
    // fix the wheel, and take the complement
    switch (size())
    {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
            high = evaluateHigh();
            break;

        case 5:
            high = evaluateHigh();
            high.fixWheel2to7(rankMask());
            break;

        default:
            // this is a slow way to handle the general case.
            // TODO: specialize the code for the 6 and 7 card cases.
            vector<Card> cards = this->cards();
            combinations combo(size(), FULL_HAND_SIZE);
            PokerEvaluation best;
            do
            {
                CardSet candidate;
                for (size_t i=0; i<static_cast<size_t>(FULL_HAND_SIZE); i++)
                    candidate.insert(cards[combo[i]]);
                PokerEvaluation e = candidate.evaluateLow2to7();
                if (e > best)
                    best = e;
            }
            while (combo.next());
            return best;

    }

    high.flip();
    return high;
}

// This one is not fully optimized
PokerEvaluation CardSet::evaluateRanksLow2to7() const
{
    PokerEvaluation high;
    PokerEvaluation h;

    // if there are five or fewer cards, we just evaluate the high,
    // fix the wheel, and take the complement
    switch (size())
    {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
            high = evaluateHighRanks();
            break;

        case 5:
            high = evaluateHighRanks();
            high.fixWheel2to7(rankMask());
            break;

        default:
            // this is a slow way to handle the general case.
            // TODO: specialize the code for the 6 and 7 card cases.
            vector<Card> cards = this->cards();
            combinations combo(size(), FULL_HAND_SIZE);
            PokerEvaluation best;
            do
            {
                CardSet candidate;
                for (size_t i=0; i<static_cast<size_t>(FULL_HAND_SIZE); i++)
                    candidate.insert(cards[combo[i]]);
                PokerEvaluation e = candidate.evaluateRanksLow2to7();
                if (e > best)
                    best = e;
            }
            while (combo.next());
            return best;

    }

    high.flip();
    return high;
}

// This one is not fully optimized
PokerEvaluation CardSet::evaluateSuitsLow2to7() const
{
    PokerEvaluation high;

    // if there are five or fewer cards, we just evaluate the high,
    // fix the wheel, and take the complement
    switch (size())
    {
        case 0:
        case 1:
        case 2:
        case 3:
        case 4:
            high = evaluateHighFlush();
            break;

        case 5:
            high = evaluateHighFlush();
            high.fixWheel2to7(rankMask());
            break;

        default:
            // this is a slow way to handle the general case.
            // TODO: specialize the code for the 6 and 7 card cases.
            vector<Card> cards = this->cards();
            combinations combo(size(), FULL_HAND_SIZE);
            PokerEvaluation best;
            do
            {
                CardSet candidate;
                for (size_t i=0; i<static_cast<size_t>(FULL_HAND_SIZE); i++)
                    candidate.insert(cards[combo[i]]);
                PokerEvaluation e = candidate.evaluateSuitsLow2to7();
                if (e > best)
                    best = e;
            }
            while (combo.next());
            return best;

    }

    high.flip();
    return high;
}

// This one is not fully optimized
PokerEvaluation CardSet::evaluateLowA5() const
{
    //PokerEvaluation::generateLowballLookupA5();

    // this is a rank only evaluator, so we just get
    // the rank masks and then fill accordingly.  We
    // also have to shift the rank mask according to the
    // the fact that the ace swings low
    // this could probably be much faster

    int c = C();
    int d = D();
    int h = H();
    int s = S();
    int rankmask = c | d | h | s;

    int ncards = nRanksTable[c] + nRanksTable[d] + nRanksTable[h] + nRanksTable[s];
    int ndups = ncards - nRanksTable[rankmask];
    int nranks = nRanksTable[rankmask];

    // first the easy cases
    // 1) no duplicate ranks
    // 2) 5 or more ranks
    if (ndups == 0 || nranks >= FULL_HAND_SIZE)
    {
        PokerEvaluation ret((NO_PAIR<<VSHIFT) ^ lowballA5Ranks[rankmask] ^ ACE_LOW_BIT);
        ret.flip();
        return ret;
    }
    // another easy case
    // *) we have five or fewer cards
    else if (ncards <= FULL_HAND_SIZE)
    {
        PokerEvaluation ret = evaluatePairing();
        ret.playAceLow();
        ret.flip();
        return ret;
    }
    else
    {
        // now the general case
        // the algorithm is as follows:
        // cycle through the "pairing list" till a total of five
        // cards are found
        uint64_t chash = 0;// = rankmask;
        int rset[4];

        rset[0]  = rankmask;
        rset[1]  = rankmask ^(c ^ d ^ h ^ s);             // mask of cards with 2/4 ranks
        rset[2]  = ((c&d)|(h&s)) & ((c&h)|(d&s));         // mask of cards with 3 ranks
        rset[3]  =  c & d & h & s;                        // fourmask
        rset[1] |= rset[2];       // add the threes back to teh twos

        rset[0] = LOWBALL_ROTATE_RANKS(rset[0]);
        rset[1] = LOWBALL_ROTATE_RANKS(rset[1]);
        rset[2] = LOWBALL_ROTATE_RANKS(rset[2]);
        rset[3] = LOWBALL_ROTATE_RANKS(rset[3]);

        // we add pairs first, then trips, then quads
        int nc=0;
        for (int i=0; i<4; i++)
        {
            // do the ace first
            if (rset[i] & (0x01<<Rank::AceVal()))
            {
                chash |= (ONE64<<(Rank::AceVal()+Rank::NUM_RANK*i));
                nc++;
            }
            if (nc == FULL_HAND_SIZE)
            {
                break;
            }

            // now the rest of the ranks
            for (int r=0; r<11; r++)
            {
                if (rset[i] & (0x01<<r))
                {
                    chash |= (ONE64<<(r+Rank::NUM_RANK*i));
                    nc++;
                }
                if (nc == FULL_HAND_SIZE)
                {
                    break;
                }
            }
            if (nc == FULL_HAND_SIZE)
            {
                break;
            }
        }

        CardSet paird(chash);
        PokerEvaluation ret(paird.evaluatePairing().code() | ACE_LOW_BIT);
        ret.flip();
        return ret;
    }
}

PokerEvaluation CardSet::evaluate8LowA5() const
{
    // bits 2-8+A are set here
    const int LOW_MASK = 0x107F;

    int c = C() & LOW_MASK;
    int d = D() & LOW_MASK;
    int h = H() & LOW_MASK;
    int s = S() & LOW_MASK;

    int rankmask = c | d | h | s;
    int nranks = nRanksTable[rankmask];

    if (nranks >= FULL_HAND_SIZE)
    {
        PokerEvaluation ret(((NO_PAIR<<VSHIFT) ^ lowballA5Ranks[rankmask]) | ACE_LOW_BIT);
        ret.flip();
        return ret;
    }

    PokerEvaluation e;
    return e;
}

PokerEvaluation CardSet::evaluate3CP() const
{
    int c = C();
    int d = D();
    int h = H();
    int s = S();
    int rankmask = c | d | h | s;

    int ncards = nRanksTable[c] + nRanksTable[d] + nRanksTable[h] + nRanksTable[s];
    if (ncards > 3)
        throw std::invalid_argument("3CP evaluator only works on three cards");

    // check for three flush
    bool threeflush = countSuits() == 1 ? true : false;

    // then three straight
    const int THREE_WHEEL = 0x01<<Rank::AceVal() | 0x01<<Rank::TwoVal() | 0x01<<Rank::ThreeVal();
    int  topr = topRankTable[rankmask];
    int  botr = botRankTable[rankmask];
    int  strr = -1;
    bool threestr = false;
    if (topr - botr == 2 && nRanksTable[rankmask] == 3)
    {
        threestr = true;
        strr = topr;
    }
    else if (rankmask == THREE_WHEEL)
    {
        threestr = true;
        strr = Rank::ThreeVal();
    }

    if (threeflush)
    {
        if (threestr)
            return PokerEvaluation((THREE_STRAIGHT_FLUSH<<VSHIFT) ^ strr<<MAJOR_SHIFT);
        else
            return PokerEvaluation((THREE_FLUSH<<VSHIFT) ^ rankmask);
    }
    else if (threestr)
    {
        return PokerEvaluation((THREE_STRAIGHT<<VSHIFT) ^(strr<<MAJOR_SHIFT));
    }
    else
    {
        return evaluatePairing();
    }
}

// three card badugis are better than two card badugis
static inline bool badugiless(int c1, int c2)
{
    int n1 = nRanksTable[c1];
    int n2 = nRanksTable[c2];
    if (n1 == n2)
        return c1 < c2;
    return n1 > n2;
}

/**
 * Badugi hand evaluator. To make this faster, I need a
 * bottomRankMask.
 *
 * To do a four card evaluation it suffices to pass through the suits
 * twice, once in each direction.  To do a general evaluation, all
 * suits must be traversed in all orders so that 4*3*2*1=24 suit
 * traversals will be done.  Suits of size zero can be ruled out and
 * those with size one can be ruled in.
 */
PokerEvaluation CardSet::evaluateBadugi() const
{
    // get our ranks orgainzed in lowball order by suit
    boost::array<int,4> suits =
    {
        {
            LOWBALL_ROTATE_RANKS(C()),
            LOWBALL_ROTATE_RANKS(D()),
            LOWBALL_ROTATE_RANKS(H()),
            LOWBALL_ROTATE_RANKS(S())
        }
    };

    // We try to save some time by being smart about which suits we loop
    // over.  Empty suits are ignored, and suits with one rank are used
    // as is.  At the end we sort to make next_permutation to work properly.
    boost::array<int,4> ind;
    int bmust = 0;
    size_t k=0;
    for (size_t i=0; i<suits.size(); i++)
    {
        switch (nRanksTable[suits[i]])
        {
            case 1:
                bmust |= suits[i];
                // fall through
            case 0:
                break;
            default:
                ind[k++] = suits[i];
        };
    }
    sort(ind.begin(),ind.begin()+k);

    // now enter into the fray to find the best possible badugi.  suit
    // order in this loop matters, so we cycle through the permutations
    // of suit order.
    //
    // zeroing minbadugi works because it has no ranks and a one rank
    // badugi is less than a zero rank badugi.
    int minbadugi = 0;
    do
    {
        // explanation of bit ops, b=current badugi, s=suit being considered
        // (b|s)-b                    set of ranks in suit s not in the badugi yet
        // b |= botRankMask[(b|s)-b]  for each relevant suit do this:
        int branks = bmust;
        for (size_t i=0; i<k; i++)
            branks |= bottomRankMask[(branks|ind[i])-branks];

        if (badugiless(branks, minbadugi))
            minbadugi = branks;
    }
    while (next_permutation(ind.begin(),ind.begin()+k));

    // encode the badugi, it's a low hand where aces plays low.  we set
    // the number of missing suits in the badugi as the "major rank". the
    // value is chosen so that more cards maps to a lower value
    PokerEvaluation ret(((NO_PAIR<<VSHIFT) ^ minbadugi) | ACE_LOW_BIT);
    ret.setMajorRank(Suit::NUM_SUIT - nRanksTable[minbadugi]);
    ret.flip();
    return ret;
}

bool CardSet::isPaired() const  // returns true if *any* two cards match rank
{
    int c = C();
    int d = D();
    int h = H();
    int s = S();
    int rankmask = c | d | h | s;

    int two_mask = rankmask ^(c ^ d ^ h ^ s);
    if (two_mask > 0)
        return true;
    return false;
}

size_t CardSet::countMaxRank() const         // returns true if trips or quads
{
    int c = C();
    int d = D();
    int h = H();
    int s = S();

    int four_mask = c & d & h & s;
    if (four_mask)
        return 4;

    int three_mask =
        ((c&d)|(h&s)) &
        ((c&h)|(d&s));
    if (three_mask)
        return 3;

    int rankmask = c | d | h | s;
    int two_mask = rankmask ^(c ^ d ^ h ^ s);
    if (two_mask)
        return 2;

    if (rankmask > 0)
        return 1;

    return 0;
}

bool CardSet::isTripped() const         // returns true if trips or quads
{
    int c = C();
    int d = D();
    int h = H();
    int s = S();

    int three_mask =
        ((c&d)|(h&s)) &
        ((c&h)|(d&s));

    if (three_mask > 0)
        return true;
    return false;
}

size_t CardSet::count(const Suit& s) const
{
    return nRanksTable[SMASK(s.code())];
}

Rank CardSet::flushRank(const Suit& s) const
{
    return Rank(topRankTable[SMASK(s.code())]);
}

Rank CardSet::topRank() const
{
    return Rank(topRankTable[RMASK()]);
}

Rank CardSet::bottomRank() const
{
    return Rank(botRankTable[RMASK()]);
}

double clampedChoose(int n, int m)
{
    if (n < m)
        return 0;
    return static_cast<size_t>(boost::math::binomial_coefficient<double>(n,m));
}

int CardSet::evaluateStraightOuts() const
{
    int sval = straightTable[RMASK()];
    if (sval > 0)         // straight on board, all cards make straight
        return STANDARD_DECK_SIZE;
    if (sval == -2)       // open-ended, eight outs
        return 8;
    if (sval == -1)       // gut-shot, four outs
        return 4;
    if (sval == -3)       // runner-runner, "1" out
        return 1;
    return 0;
}

// original version in r2488
size_t CardSet::rankColex() const
{
    size_t ret  = 0;
    int slot = 0;
    int sz   = 1;
    int c    = C();
    int d    = D();
    int h    = H();
    int s    = S();
    int rbit = 0x01;

    for (int i=0; i<13; i++)
    {
        if (rbit&c) ret += clampedChoose(slot++,sz++);
        if (rbit&d) ret += clampedChoose(slot++,sz++);
        if (rbit&h) ret += clampedChoose(slot++,sz++);
        if (rbit&s) ret += clampedChoose(slot++,sz++);
        slot++;
        rbit <<= 1;
    }
    return ret;
}

std::ostream& operator<<(std::ostream& sout, const pokerstove::CardSet& e)
{
    sout << e.str();
    return sout;
}

vector<int> pokerstove::findSuitPermutation(const CardSet& source, const CardSet& dest) {
    vector<int> rot(4, -1);
    vector<int> taken(4, 0);

    int i = 0;
    for (Suit s = Suit::begin(); s < Suit::end(); ++s, ++i) {
        int j = 0;
        for (Suit t = Suit::begin(); t < Suit::end(); ++t, ++j) {
            if (source.suitMask(s) == dest.suitMask(t)) {
                if (taken[j] == 0) {
                    rot[i] = j;
                    taken[j] = 1;
                    break;
                }
            }
        }
    }
    return rot;
}

CardSet pokerstove::canonizeToBoard(const CardSet& board, const CardSet& hand)
{
    CardSet cboard = board.canonize();
    vector<int> perms = findSuitPermutation(board, cboard);
    CardSet chand = hand.rotateSuits(perms[0],perms[1],perms[2],perms[3]);
    return chand;
}

// some suit mask macros
#undef C
#undef D
#undef H
#undef S
#undef RMASK
#undef SUITMASK

size_t CardSet::colex() const {
    vector<Card> cards = this->cards();
    size_t value = 0;
    for (size_t i = 0; i < cards.size(); i++) {
        size_t code = cards[i].code();
#ifdef __AP_MATH__
        value += tchoose(code, i+1);
#else
        if (code >= i + 1)
            value += static_cast<size_t>(boost::math::binomial_coefficient<double>(code, i + 1));
#endif
    }
    return value;
}


/*
 * Copyright 2021 Francisco Javier Torres Maldonado
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thread>
#include <chrono>
#include <ctime>

#include <fstream>
#include <iostream>
#include <stdlib.h>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <nav_msgs/Odometry.h>
#include <mav_msgs/conversions.h>
#include <mav_msgs/eigen_mav_msgs.h>
#include "bebop_simulator/Matrix3x3.h"
#include "bebop_simulator/Quaternion.h"
#include "bebop_simulator/Vector3.h"
#include "bebop_simulator_msgs/default_topics.h"

#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <std_msgs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <math.h>

static const float DEG_2_RAD = M_PI / 180.0;

namespace bebop_simulator {

    class Launcher{
        public:
            Launcher();
            ~Launcher();

        private:
            double K1_xy_i, K1_z_i, K1_phi_theta_i, K1_psi_i;
            double K2_xy_i, K2_z_i, K2_phi_theta_i, K2_psi_i;
            double K34_xy_i, K34_z_i, K34_phi_theta_i, K34_psi_i;
            double K1_xy_f, K1_z_f, K1_phi_theta_f, K1_psi_f;
            double K2_xy_f, K2_z_f, K2_phi_theta_f, K2_psi_f;
            double K34_xy_f, K34_z_f, K34_phi_theta_f, K34_psi_f;
            double K1_xy_d, K1_z_d, K1_phi_theta_d, K1_psi_d;
            double K2_xy_d, K2_z_d, K2_phi_theta_d, K2_psi_d;
            double K34_xy_d, K34_z_d, K34_phi_theta_d, K34_psi_d;
            double K1_xy, K1_z, K1_phi_theta, K1_psi;
            double K2_xy, K2_z, K2_phi_theta, K2_psi;
            double K34_xy, K34_z, K34_phi_theta, K34_psi;
            double sigma;
            std::string pos_controller_node, csv_odometry, plot, waypoint, control;

            ros::NodeHandle nh1;
            ros::Timer _timer1;
            bool trigger = true;

            void InitializeParams();
            void TimerCallback(const ros::TimerEvent& event);
            void RunNodes();
            std::string getTimeStr();
    };

    Launcher::Launcher() {
      InitializeParams();
      _timer1 = nh1.createTimer(ros::Duration(1.0), &Launcher::TimerCallback, this, false, true);
    }

    Launcher::~Launcher() {}

    void Launcher::InitializeParams() {
      double U_x, U_y, U_z;
      double mass, in_xx, in_xy, in_xz, in_yy, in_yz, in_zz, bf, bm, l;
      double Qp_aa, Qp_bb, Qp_cc, Qp_dd, Qp_ee, Qp_ff;
      double dev_x, dev_y, dev_z, dev_vx, dev_vy, dev_vz;
      double Tsf, H;
      bool use_sim_time, csvFilesStoring, waypoint_filter, EKFActive;
      std::string user_account;
      double csvFilesStoringTime;
      double Gamma, TiempoMargen;
      double TiempoPosicionInicial, TiempoTrayectoria, Dif_Tiempo;
      std::string Ruta, Subcarpeta;
      double X_Inicial, Y_Inicial, Z_Inicial;
      double X_Distancia, Y_Distancia, Z_Distancia;
      bool verbose;
      ros::param::get("~K1_xy/K1_xy_i", K1_xy_i);
      ros::param::get("~K1_z/K1_z_i", K1_z_i);
      ros::param::get("~K1_phi_theta/K1_phi_theta_i", K1_phi_theta_i);
      ros::param::get("~K1_psi/K1_psi_i", K1_psi_i);
      ros::param::get("~K2_xy/K2_xy_i", K2_xy_i);
      ros::param::get("~K2_z/K2_z_i", K2_z_i);
      ros::param::get("~K2_phi_theta/K2_phi_theta_i", K2_phi_theta_i);
      ros::param::get("~K2_psi/K2_psi_i", K2_psi_i);
      ros::param::get("~K34_xy/K34_xy_i", K34_xy_i);
      ros::param::get("~K34_z/K34_z_i", K34_z_i);
      ros::param::get("~K34_phi_theta/K34_phi_theta_i", K34_phi_theta_i);
      ros::param::get("~K34_psi/K34_psi_i", K34_psi_i);
      ros::param::get("~K1_xy/K1_xy_f", K1_xy_f);
      ros::param::get("~K1_z/K1_z_f", K1_z_f);
      ros::param::get("~K1_phi_theta/K1_phi_theta_f", K1_phi_theta_f);
      ros::param::get("~K1_psi/K1_psi_f", K1_psi_f);
      ros::param::get("~K2_xy/K2_xy_f", K2_xy_f);
      ros::param::get("~K2_z/K2_z_f", K2_z_f);
      ros::param::get("~K2_phi_theta/K2_phi_theta_f", K2_phi_theta_f);
      ros::param::get("~K2_psi/K2_psi_f", K2_psi_f);
      ros::param::get("~K34_xy/K34_xy_f", K34_xy_f);
      ros::param::get("~K34_z/K34_z_f", K34_z_f);
      ros::param::get("~K34_phi_theta/K34_phi_theta_f", K34_phi_theta_f);
      ros::param::get("~K34_psi/K34_psi_f", K34_psi_f);
      ros::param::get("~K1_xy/K1_xy_d", K1_xy_d);
      ros::param::get("~K1_z/K1_z_d", K1_z_d);
      ros::param::get("~K1_phi_theta/K1_phi_theta_d", K1_phi_theta_d);
      ros::param::get("~K1_psi/K1_psi_d", K1_psi_d);
      ros::param::get("~K2_xy/K2_xy_d", K2_xy_d);
      ros::param::get("~K2_z/K2_z_d", K2_z_d);
      ros::param::get("~K2_phi_theta/K2_phi_theta_d", K2_phi_theta_d);
      ros::param::get("~K2_psi/K2_psi_d", K2_psi_d);
      ros::param::get("~K34_xy/K34_xy_d", K34_xy_d);
      ros::param::get("~K34_z/K34_z_d", K34_z_d);
      ros::param::get("~K34_phi_theta/K34_phi_theta_d", K34_phi_theta_d);
      ros::param::get("~K34_psi/K34_psi_d", K34_psi_d);
      ros::param::get("~sigma/sigma", sigma);
      ros::param::get("~U_xyz/U_x", U_x);
      ros::param::get("~U_xyz/U_y", U_y);
      ros::param::get("~U_xyz/U_z", U_z);
      ros::param::get("~mass", mass);
      ros::param::get("~inertia/xx", in_xx);
      ros::param::get("~inertia/xy", in_xy);
      ros::param::get("~inertia/xz", in_xz);
      ros::param::get("~inertia/yy", in_yy);
      ros::param::get("~inertia/yz", in_yz);
      ros::param::get("~inertia/zz", in_zz);
      ros::param::get("~bf", bf);
      ros::param::get("~bm", bm);
      ros::param::get("~l", l);
      ros::param::get("~Qp/aa", Qp_aa);
      ros::param::get("~Qp/bb", Qp_bb);
      ros::param::get("~Qp/cc", Qp_cc);
      ros::param::get("~Qp/dd", Qp_dd);
      ros::param::get("~Qp/ee", Qp_ee);
      ros::param::get("~Qp/ff", Qp_ff);
      ros::param::get("~dev_x", dev_x);
      ros::param::get("~dev_y", dev_y);
      ros::param::get("~dev_z", dev_z);
      ros::param::get("~dev_vx", dev_vx);
      ros::param::get("~dev_vy", dev_vy);
      ros::param::get("~dev_vz", dev_vz);
      ros::param::get("~Tsf", Tsf);
      ros::param::get("~H", H);
      ros::param::get("~use_sim_time", use_sim_time);
      ros::param::get("~csvFilesStoring", csvFilesStoring);
      ros::param::get("~csvFilesStoringTime", csvFilesStoringTime);
      ros::param::get("~user_account", user_account);
      ros::param::get("~waypoint_filter", waypoint_filter);
      ros::param::get("~EKFActive", EKFActive);
      ros::param::get("~Gamma", Gamma);
      ros::param::get("~TiempoMargen", TiempoMargen);
      ros::param::get("~Ruta", Ruta);
      ros::param::get("~TiempoPosicionInicial", TiempoPosicionInicial);
      ros::param::get("~TiempoTrayectoria", TiempoTrayectoria);
      ros::param::get("~Dif_Tiempo", Dif_Tiempo);
      ros::param::get("~X_Inicial", X_Inicial);
      ros::param::get("~Y_Inicial", Y_Inicial);
      ros::param::get("~Z_Inicial", Z_Inicial);
      ros::param::get("~X_Distancia", X_Distancia);
      ros::param::get("~Y_Distancia", Y_Distancia);
      ros::param::get("~Z_Distancia", Z_Distancia);
      ros::param::get("~verbose", verbose);

      std::string s_U_x, s_U_y, s_U_z, s_mass, s_in_xx, s_in_xy, s_in_xz, s_in_yy, s_in_yz, s_in_zz,
      s_bf, s_bm, s_l, s_Qp_aa, s_Qp_bb, s_Qp_cc, s_Qp_dd, s_Qp_ee, s_Qp_ff,
      s_dev_x, s_dev_y, s_dev_z, s_dev_vx, s_dev_vy, s_dev_vz, s_use_sim_time,
      s_csvFilesStoring, s_waypoint_filter, s_EKFActive, s_csvFilesStoringTime,
      s_Gamma, s_TiempoMargen, s_TiempoPosicionInicial, s_TiempoTrayectoria,
      s_Dif_Tiempo, s_X_Inicial, s_Y_Inicial, s_Z_Inicial, s_X_Distancia,
      s_Y_Distancia, s_Z_Distancia, s_verbose, s_Tsf, s_H;
      std::ostringstream stream_bf;
      stream_bf << bf;

      s_U_x = std::to_string(U_x);
      s_U_y = std::to_string(U_y);
      s_U_z = std::to_string(U_z);
      s_mass = std::to_string(mass);
      s_in_xx = std::to_string(in_xx);
      s_in_xy = std::to_string(in_xy);
      s_in_xz = std::to_string(in_xz);
      s_in_yy = std::to_string(in_yy);
      s_in_yz = std::to_string(in_yz);
      s_in_zz = std::to_string(in_zz);
      s_bf = stream_bf.str();
      s_bm = std::to_string(bm);
      s_l = std::to_string(l);
      s_Qp_aa = std::to_string(Qp_aa);
      s_Qp_bb = std::to_string(Qp_bb);
      s_Qp_cc = std::to_string(Qp_cc);
      s_Qp_dd = std::to_string(Qp_dd);
      s_Qp_ee = std::to_string(Qp_ee);
      s_Qp_ff = std::to_string(Qp_ff);
      s_dev_x = std::to_string(dev_x);
      s_dev_y = std::to_string(dev_y);
      s_dev_z = std::to_string(dev_z);
      s_dev_vx = std::to_string(dev_vx);
      s_dev_vy = std::to_string(dev_vy);
      s_dev_vz = std::to_string(dev_vz);
      s_Tsf = std::to_string(Tsf);
      s_H = std::to_string(H);
      if (use_sim_time){
        s_use_sim_time = "true";
      }
      else {
        s_use_sim_time = "false";
      }
      if (csvFilesStoring){
        s_csvFilesStoring = "true";
      }
      else {
        s_csvFilesStoring = "false";
      }
      if (waypoint_filter){
        s_waypoint_filter = "true";
      }
      else {
        s_waypoint_filter = "false";
      }
      if (EKFActive){
        s_EKFActive = "true";
      }
      else {
        s_EKFActive = "false";
      }
      s_csvFilesStoringTime = std::to_string(csvFilesStoringTime);
      s_Gamma = std::to_string(Gamma);
      s_TiempoMargen = std::to_string(TiempoMargen);
      s_TiempoPosicionInicial = std::to_string(TiempoPosicionInicial);
      s_TiempoTrayectoria = std::to_string(TiempoTrayectoria);
      s_Dif_Tiempo = std::to_string(Dif_Tiempo);
      s_X_Inicial = std::to_string(X_Inicial);
      s_Y_Inicial = std::to_string(Y_Inicial);
      s_Z_Inicial = std::to_string(Z_Inicial);
      s_X_Distancia = std::to_string(X_Distancia);
      s_Y_Distancia = std::to_string(Y_Distancia);
      s_Z_Distancia = std::to_string(Z_Distancia);
      if (verbose){
        s_verbose = "true";
      }
      else {
        s_verbose = "false";
      }

      pos_controller_node = "rosrun bebop_simulator pos_controller_node_3 _U_xyz/U_x:="
      + s_U_x + " _U_xyz/U_y:=" + s_U_y + " _U_xyz/U_z:=" + s_U_z
      + " _mass:=" + s_mass + " _inertia/xx:=" + s_in_xx + " _inertia/xy:=" + s_in_xy
      + " _inertia/xz:=" + s_in_xz + " _inertia/yy:=" + s_in_yy
      + " _inertia/yz:=" + s_in_yz + " _inertia/zz:=" + s_in_zz
      + " _bf:=" + s_bf + " _bm:=" + s_bm + " _l:=" + s_l
      + " _Qp/aa:=" + s_Qp_aa + " _Qp/bb:=" + s_Qp_bb
      + " _Qp/cc:=" + s_Qp_cc + " _Qp/dd:=" + s_Qp_dd
      + " _Qp/ee:=" + s_Qp_ee + " _Qp/ff:=" + s_Qp_ff
      + " _dev_x:=" + s_dev_x + " _dev_y:=" + s_dev_y
      + " _dev_z:=" + s_dev_z + " _dev_vx:=" + s_dev_vx
      + " _dev_vy:=" + s_dev_vy + " _dev_vz:=" + s_dev_vz
      + " _Tsf:=" + s_Tsf + " _H:=" + s_H
      + " _use_sim_time:=" + s_use_sim_time
      + " _csvFilesStoring:=" + s_csvFilesStoring
      + " _csvFilesStoringTime:=" + s_csvFilesStoringTime
      + " _user_account:=" + user_account
      + " _waypoint_filter:=" + s_waypoint_filter
      + " _EKFActive:=" + s_EKFActive
      + " /command/motor_speed:=/bebop/command/motors"
      + " /odometry:=/bebop/odometry"
      + " /odometry_gt:=/bebop/odometry_gt"
      + " /filteredOutput:=/bebop/filteredOutput"
      + " /referenceAngles:=/bebop/referenceAngles"
      + " /stateErrors:=/bebop/stateErrors"
      + " /smoothedTrajectory:=/bebop/smoothedTrajectory"
      + " /command/trajectory:=/bebop/command/trajectory"
      + " /uTerrComponents:=/bebop/uTerrComponents"
      + " /zVelocityComponents:=/bebop/zVelocityComponents"
      + " /positionAndVelocityErrors:=/bebop/positionAndVelocityErrors"
      + " /angularAndAngularVelocityErrors:=/bebop/angularAndAngularVelocityErrors";

      csv_odometry = "rosrun bebop_simulator csv_odometry _Ruta:="
      + Ruta + " _TiempoMargen:=" + s_TiempoMargen;

      plot = "rosrun bebop_simulator plot_3.py _Ruta:=" + Ruta
      + " _TiempoMargen:=" + s_TiempoMargen
      + " _Gamma:=" + s_Gamma + " _U_xyz/U_x:=" + s_U_x
      + " _U_xyz/U_y:=" + s_U_y + " _U_xyz/U_z:=" + s_U_z;

      waypoint = "rosrun bebop_simulator waypoint_gen _TiempoPosicionInicial:="
      + s_TiempoPosicionInicial + " _TiempoTrayectoria:=" + s_TiempoTrayectoria
      + " _Dif_Tiempo:=" + s_Dif_Tiempo + " _TiempoMargen:=" + s_TiempoMargen
      + " _X_Inicial:=" + s_X_Inicial + " _Y_Inicial:=" + s_Y_Inicial
      + " _Z_Inicial:=" + s_Z_Inicial + " _X_Distancia:=" + s_X_Distancia
      + " _Y_Distancia:=" + s_Y_Distancia + " _Z_Distancia:=" + s_Z_Distancia
      + " _Gamma:=" + s_Gamma + " _verbose:=" + s_verbose
      + " /command/trajectory:=/bebop/command/trajectory"
      + " /Odometry:=/bebop/Odometry"
      + " /OdometryGT:=/bebop/OdometryGT"
      + " /PositionReference:=/bebop/PositionReference"
      + " /PositionWithGamma:=/bebop/PositionWithGamma"
      + " /odometry:=/bebop/odometry"
      + " /odometry_gt:=/bebop/odometry_gt";

      control = "rosrun bebop_simulator csv_control _Ruta:="
      + Ruta + " _TiempoMargen:=" + s_TiempoMargen
      + " _Topico:=/Control"
      + " _Nombre:=Control.csv";

      K1_xy = K1_xy_i;
      K1_z = K1_z_i;
      K1_phi_theta = K1_phi_theta_i;
      K1_psi = K1_psi_i;
      K2_xy = K2_xy_i;
      K2_z = K2_z_i;
      K2_phi_theta = K2_phi_theta_i;
      K2_psi = K2_psi_i;
      K34_xy = K34_xy_i;
      K34_z = K34_z_i;
      K34_phi_theta = K34_phi_theta_i;
      K34_psi = K34_psi_i;

    }

    void Launcher::TimerCallback(const ros::TimerEvent& event) {
      RunNodes();
      K2_xy += K2_xy_d;
      if (K2_xy < K2_xy_f && K2_xy_d > 0){
        return;
      }
      K2_xy = K2_xy_i;
      K2_z += K2_z_d;
      if (K2_z < K2_z_f && K2_z_d > 0){
        return;
      }
      K2_z = K2_z_i;
      K2_phi_theta += K2_phi_theta_d;
      if (K2_phi_theta < K2_phi_theta_f && K2_phi_theta_d > 0){
        return;
      }
      K2_phi_theta = K2_phi_theta_i;
      K2_psi += K2_psi_d;
      if (K2_psi < K2_psi_f && K2_psi_d > 0){
        return;
      }
      K2_psi = K2_psi_i;
      K1_xy += K1_xy_d;
      while (K1_xy >= K2_xy){
        K2_xy += K2_xy_d;
        K2_xy_i = K2_xy;
      }
      if (K1_xy < K1_xy_f && K1_xy_d > 0){
        return;
      }
      K1_xy = K1_xy_i;
      K1_z += K1_z_d;
      while (K1_z >= K2_z){
        K2_z += K2_z_d;
        K2_z_i = K2_z;
      }
      if (K1_z < K1_z_f && K1_z_d > 0){
        return;
      }
      K1_z = K1_z_i;
      K1_phi_theta += K1_phi_theta_d;
      while (K1_phi_theta >= K2_phi_theta){
        K2_phi_theta += K2_phi_theta_d;
        K2_phi_theta_i = K2_phi_theta;
      }
      if (K1_phi_theta < K1_phi_theta_f && K1_phi_theta_d > 0){
        return;
      }
      K1_phi_theta = K1_phi_theta_i;
      K1_psi += K1_psi_d;
      while (K1_psi >= K2_psi){
        K2_psi += K2_psi_d;
        K2_psi_i = K2_psi;
      }
      if (K1_psi < K1_psi_f && K1_psi_d > 0){
        return;
      }
      K1_psi = K1_psi_i;
      K34_xy += K34_xy_d;
      if (K34_xy < K34_xy_f && K34_xy_d > 0){
        return;
      }
      K34_xy = K34_xy_i;
      K34_z += K34_z_d;
      if (K34_z < K34_z_f && K34_z_d > 0){
        return;
      }
      K34_z = K34_z_i;
      K34_phi_theta += K34_phi_theta_d;
      if (K34_phi_theta < K34_phi_theta_f && K34_phi_theta_d > 0){
        return;
      }
      K34_phi_theta = K34_phi_theta_i;
      K34_psi += K34_psi_d;
      if (K34_psi < K34_psi_f && K34_psi_d > 0){
        return;
      }
      else {
        ros::shutdown();
      }
    }

    void Launcher::RunNodes() {
      if (K2_xy <= K1_xy || K2_z <= K1_z || K2_phi_theta <= K1_phi_theta || K2_psi <= K1_psi){
        return;
      }
      std::string s_K1_xy, s_K1_z, s_K1_phi_theta, s_K1_psi,
      s_K2_xy, s_K2_z, s_K2_phi_theta, s_K2_psi,
      s_K34_xy, s_K34_z, s_K34_phi_theta, s_K34_psi,
      s_sigma, s_controller_params;
      std::string run_pos_controller_node, run_csv1, run_csv2, run_csv3, run_csv4,
      run_plot, run_waypoint, run_control;
      s_K1_xy = std::to_string(K1_xy);
      s_K1_z = std::to_string(K1_z);
      s_K1_phi_theta = std::to_string(K1_phi_theta);
      s_K1_psi = std::to_string(K1_psi);
      s_K2_xy = std::to_string(K2_xy);
      s_K2_z = std::to_string(K2_z);
      s_K2_phi_theta = std::to_string(K2_phi_theta);
      s_K2_psi = std::to_string(K2_psi);
      s_K34_xy = std::to_string(K34_xy);
      s_K34_z = std::to_string(K34_z);
      s_K34_phi_theta = std::to_string(K34_phi_theta);
      s_K34_psi = std::to_string(K34_psi);
      s_sigma = std::to_string(sigma);

      s_controller_params = " _K1_xy/K1_xy:=" + s_K1_xy
      + " _K1_z/K1_z:=" + s_K1_z
      + " _K1_phi_theta/K1_phi_theta:=" + s_K1_phi_theta + " _K1_psi/K1_psi:=" + s_K1_psi
      + " _K2_xy/K2_xy:=" + s_K2_xy + " _K2_z/K2_z:=" + s_K2_z
      + " _K2_phi_theta/K2_phi_theta:=" + s_K2_phi_theta + " _K2_psi/K2_psi:=" + s_K2_psi
      + " _K3_xy/K3_xy:=" + s_K34_xy + " _K3_z/K3_z:=" + s_K34_z
      + " _K3_phi_theta/K3_phi_theta:=" + s_K34_phi_theta + " _K3_psi/K3_psi:=" + s_K34_psi
      + " _K4_xy/K4_xy:=" + s_K34_xy + " _K4_z/K4_z:=" + s_K34_z
      + " _K4_phi_theta/K4_phi_theta:=" + s_K34_phi_theta + " _K4_psi/K4_psi:=" + s_K34_psi
      + " _sigma/sigma:=" + s_sigma;

      std::string Subcarpeta = getTimeStr();
      ros::param::set("/Subcarpeta", Subcarpeta.c_str());
      run_pos_controller_node = pos_controller_node + s_controller_params
      + " __name:=pos_controller_node";
      run_csv1 = csv_odometry + " _Topico:=/bebop/Odometry"
      + " _Nombre:=odometry.csv __name:=csv_odometry_1";
      run_csv2 = csv_odometry + " _Topico:=/bebop/OdometryGT"
      + " _Nombre:=odometry_gt.csv __name:=csv_odometry_2";
      run_csv3 = csv_odometry + " _Topico:=/bebop/PositionReference"
      + " _Nombre:=reference.csv __name:=csv_odometry_3";
      run_csv4 = csv_odometry + " _Topico:=/bebop/PositionWithGamma"
      + " _Nombre:=referenceGamma.csv __name:=csv_odometry_4";
      run_plot = plot + s_controller_params + " __name:=plot";
      run_waypoint = waypoint + " __name:=waypoint_gen";
      run_control = control + " __name:=csv_control";

      ROS_INFO(run_waypoint.c_str());
      ROS_INFO(run_pos_controller_node.c_str());
      ROS_INFO(run_csv1.c_str());
      ROS_INFO(run_csv2.c_str());
      ROS_INFO(run_csv3.c_str());
      ROS_INFO(run_csv4.c_str());
      ROS_INFO(run_control.c_str());
      ROS_INFO(run_plot.c_str());

      std::thread th_pos_controller_node(system,run_pos_controller_node.c_str());
      std::thread th_csv1(system,run_csv1.c_str());
      std::thread th_csv2(system,run_csv2.c_str());
      std::thread th_csv3(system,run_csv3.c_str());
      std::thread th_csv4(system,run_csv4.c_str());
      std::thread th_control(system,run_control.c_str());
      std::thread th_plot(system,run_plot.c_str());
      std::thread th_waypoint(system,run_waypoint.c_str());

      th_pos_controller_node.join();
      th_csv1.join();
      th_csv2.join();
      th_csv3.join();
      th_csv4.join();
      th_control.join();
      th_plot.join();
      th_waypoint.join();

    }

    std::string Launcher::getTimeStr() {
      std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
      std::string s(30, '\0');
      std::strftime(&s[0], s.size(), "%Y-%m-%d %H:%M:%S ", std::localtime(&now));
      return s;
    }

}

int main(int argc, char** argv){
  ros::init(argc, argv, "launcher");
  ros::NodeHandle nh;
  bebop_simulator::Launcher launcher_;
  ros::spin();
  return 0;
}


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_HardElasticElectronScatteringDistribution.hpp
//! \author Luke Kersting
//! \brief  The electron hard elastic scattering distribution base class
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_HARD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP
#define MONTE_CARLO_HARD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

// Std Lib Includes
#include <limits>

// Boost Includes
#include <boost/function.hpp>
#include <boost/bind.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ElectronState.hpp"
#include "MonteCarlo_ParticleBank.hpp"
#include "MonteCarlo_ElectronScatteringDistribution.hpp"
#include "MonteCarlo_AdjointElectronScatteringDistribution.hpp"
#include "Utility_TabularOneDDistribution.hpp"

namespace MonteCarlo{

//! The scattering distribution base class
class HardElasticElectronScatteringDistribution : public ElectronScatteringDistribution,
                                                  public AdjointElectronScatteringDistribution
{

public:

  //! Typedef for the  elastic distribution
  typedef Teuchos::Array<Utility::Pair< double,
		       Teuchos::RCP<const Utility::TabularOneDDistribution> > >
  ElasticDistribution;

  //! Constructor
  HardElasticElectronScatteringDistribution(
                 const int atomic_number,
                 const ElasticDistribution& elastic_scattering_distribution);

  //! Destructor
  virtual ~HardElasticElectronScatteringDistribution()
  { /* ... */ }

  //! Evaluate the PDF
  double evaluatePDF( const double incoming_energy,
                      const double scattering_angle_cosine ) const;

  //! Evaluate the PDF
  double evaluatePDF( const unsigned incoming_energy_bin,
                      const double scattering_angle_cosine ) const;

  //! Evaluate the CDF
  double evaluateCDF( const double incoming_energy,
                      const double scattering_angle_cosine ) const;

  //! Return the energy at a given energy bin
  double getEnergy( const unsigned energy_bin ) const;

  // Evaluate the analytical Screened Rutherford PDF
  double evaluateScreenedRutherfordPDF(
                      const double incoming_energy,
                      const long double scattering_angle_cosine ) const;

  //! Evaluate the screened Rutherford cross section ratio above the cutoff mu
  double evaluateScreenedRutherfordCrossSectionRatio(
                                  const double incoming_energy ) const;

  //! Sample an outgoing energy and direction from the distribution
  void sample( const double incoming_energy,
               double& outgoing_energy,
               double& scattering_angle_cosine ) const;

  //! Sample an outgoing energy and direction and record the number of trials
  void sampleAndRecordTrials( const double incoming_energy,
                              double& outgoing_energy,
                              double& scattering_angle_cosine,
                              unsigned& trials ) const;

  //! Randomly scatter the electron
  void scatterElectron( ElectronState& electron,
                        ParticleBank& bank,
                        SubshellType& shell_of_interaction ) const;

  //! Randomly scatter the adjoint electron
  void scatterAdjointElectron( AdjointElectronState& adjoint_electron,
                               ParticleBank& bank,
                               SubshellType& shell_of_interaction ) const;

//protected:

   //! Sample an outgoing direction from the distribution
  void sampleAndRecordTrialsImpl( const double incoming_energy,
                                  double& scattering_angle_cosine,
                                  unsigned& trials ) const;

  //! Evaluate Moliere's atomic screening constant at the given electron energy
  double evaluateMoliereScreeningConstant( const double energy ) const;

  //! Evaluate the scattering angle from the analytical screened Rutherford function
  double evaluateScreenedScatteringAngle( const double energy ) const;

private:

  // Cutoff angle cosine between the distribution and analytical screened Rutherford function
  static double s_rutherford_cutoff;

  // Difference btw cutoff angle cosine and forward peak (mu = 1)
  static double s_delta_cutoff;

  // The fine structure constant (fsc) squared
  static double s_fine_structure_const_squared;

  // A parameter for moliere's screening factor  (1/2*(fsc/0.885)**2)
  static double s_screening_param1;

  // Atomic number (Z) of the target atom
  int d_atomic_number;

  // Atomic number (Z) of the target atom to the 2/3 power (Z^2/3)
  double d_Z_two_thirds_power;

  // A parameter for moliere's screening factor (3.76*fsc**2*Z**2)
  double d_screening_param2;

  // elastic scattering distribution without forward screening data
  ElasticDistribution d_elastic_scattering_distribution;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_HARD_ELASTIC_ELECTRON_SCATTERING_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_HardElasticElectronScatteringDistribution.hpp
//---------------------------------------------------------------------------//