//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//
//
// Revision History:
//   04 April 2001: Added named parameter variant. (Jeremy Siek)
//   01 April 2001: Modified to use new <boost/limits.hpp> header. (JMaddock)
//
#ifndef BOOST_GRAPH_DIJKSTRA_HPP
#define BOOST_GRAPH_DIJKSTRA_HPP

#include <functional>
#include <boost/limits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/relax.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/overloading.hpp>
#include <boost/smart_ptr.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/graph/two_bit_color_map.hpp>
#include <boost/graph/detail/mpi_include.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/vector_property_map.hpp>
#include <boost/type_traits.hpp>
#include <boost/concept/assert.hpp>

#ifdef BOOST_GRAPH_DIJKSTRA_TESTING
#include <boost/pending/mutable_queue.hpp>
#endif // BOOST_GRAPH_DIJKSTRA_TESTING

namespace boost
{

/**
 * @brief Updates a particular value in a queue used by Dijkstra's
 * algorithm.
 *
 * This routine is called by Dijkstra's algorithm after it has
 * decreased the distance from the source vertex to the given @p
 * vertex. By default, this routine will just call @c
 * Q.update(vertex). However, other queues may provide more
 * specialized versions of this routine.
 *
 * @param Q             the queue that will be updated.
 * @param vertex        the vertex whose distance has been updated
 * @param old_distance  the previous distance to @p vertex
 */
template < typename Buffer, typename Vertex, typename DistanceType >
inline void dijkstra_queue_update(
    Buffer& Q, Vertex vertex, DistanceType old_distance)
{
    (void)old_distance;
    Q.update(vertex);
}

template < class Visitor, class Graph > struct DijkstraVisitorConcept
{
    void constraints()
    {
        BOOST_CONCEPT_ASSERT((CopyConstructibleConcept< Visitor >));
        vis.initialize_vertex(u, g);
        vis.discover_vertex(u, g);
        vis.examine_vertex(u, g);
        vis.examine_edge(e, g);
        vis.edge_relaxed(e, g);
        vis.edge_not_relaxed(e, g);
        vis.finish_vertex(u, g);
    }
    Visitor vis;
    Graph g;
    typename graph_traits< Graph >::vertex_descriptor u;
    typename graph_traits< Graph >::edge_descriptor e;
};

template < class Visitors = null_visitor >
class dijkstra_visitor : public bfs_visitor< Visitors >
{
public:
    dijkstra_visitor() {}
    dijkstra_visitor(Visitors vis) : bfs_visitor< Visitors >(vis) {}

    template < class Edge, class Graph > void edge_relaxed(Edge e, Graph& g)
    {
        invoke_visitors(this->m_vis, e, g, on_edge_relaxed());
    }
    template < class Edge, class Graph > void edge_not_relaxed(Edge e, Graph& g)
    {
        invoke_visitors(this->m_vis, e, g, on_edge_not_relaxed());
    }

private:
    template < class Edge, class Graph > void tree_edge(Edge u, Graph& g) {}
};
template < class Visitors >
dijkstra_visitor< Visitors > make_dijkstra_visitor(Visitors vis)
{
    return dijkstra_visitor< Visitors >(vis);
}
typedef dijkstra_visitor<> default_dijkstra_visitor;

namespace detail
{

    template < class UniformCostVisitor, class UpdatableQueue, class WeightMap,
        class PredecessorMap, class DistanceMap, class BinaryFunction,
        class BinaryPredicate >
    struct dijkstra_bfs_visitor
    {
        typedef typename property_traits< DistanceMap >::value_type D;
        typedef typename property_traits< WeightMap >::value_type W;

        dijkstra_bfs_visitor(UniformCostVisitor vis, UpdatableQueue& Q,
            WeightMap w, PredecessorMap p, DistanceMap d,
            BinaryFunction combine, BinaryPredicate compare, D zero)
        : m_vis(vis)
        , m_Q(Q)
        , m_weight(w)
        , m_predecessor(p)
        , m_distance(d)
        , m_combine(combine)
        , m_compare(compare)
        , m_zero(zero)
        {
        }

        template < class Edge, class Graph > void tree_edge(Edge e, Graph& g)
        {
            bool decreased = relax_target(e, g, m_weight, m_predecessor,
                m_distance, m_combine, m_compare);
            if (decreased)
                m_vis.edge_relaxed(e, g);
            else
                m_vis.edge_not_relaxed(e, g);
        }
        template < class Edge, class Graph > void gray_target(Edge e, Graph& g)
        {
            D old_distance = get(m_distance, target(e, g));

            bool decreased = relax_target(e, g, m_weight, m_predecessor,
                m_distance, m_combine, m_compare);
            if (decreased)
            {
                dijkstra_queue_update(m_Q, target(e, g), old_distance);
                m_vis.edge_relaxed(e, g);
            }
            else
                m_vis.edge_not_relaxed(e, g);
        }

        template < class Vertex, class Graph >
        void initialize_vertex(Vertex u, Graph& g)
        {
            m_vis.initialize_vertex(u, g);
        }
        template < class Edge, class Graph > void non_tree_edge(Edge, Graph&) {}
        template < class Vertex, class Graph >
        void discover_vertex(Vertex u, Graph& g)
        {
            m_vis.discover_vertex(u, g);
        }
        template < class Vertex, class Graph >
        void examine_vertex(Vertex u, Graph& g)
        {
            m_vis.examine_vertex(u, g);
        }
        template < class Edge, class Graph > void examine_edge(Edge e, Graph& g)
        {
            // Test for negative-weight edges:
            //
            // Reasons that other comparisons do not work:
            //
            // m_compare(e_weight, D(0)):
            //    m_compare only needs to work on distances, not weights, and
            //    those types do not need to be the same (bug 8398,
            //    https://svn.boost.org/trac/boost/ticket/8398).
            // m_compare(m_combine(source_dist, e_weight), source_dist):
            //    if m_combine is project2nd (as in prim_minimum_spanning_tree),
            //    this test will claim that the edge weight is negative whenever
            //    the edge weight is less than source_dist, even if both of
            //    those are positive (bug 9012,
            //    https://svn.boost.org/trac/boost/ticket/9012).
            // m_compare(m_combine(e_weight, source_dist), source_dist):
            //    would fix project2nd issue, but documentation only requires
            //    that m_combine be able to take a distance and a weight (in
            //    that order) and return a distance.

            // W e_weight = get(m_weight, e);
            // sd_plus_ew = source_dist + e_weight.
            // D sd_plus_ew = m_combine(source_dist, e_weight);
            // sd_plus_2ew = source_dist + 2 * e_weight.
            // D sd_plus_2ew = m_combine(sd_plus_ew, e_weight);
            // The test here is equivalent to e_weight < 0 if m_combine has a
            // cancellation law, but always returns false when m_combine is a
            // projection operator.
            if (m_compare(m_combine(m_zero, get(m_weight, e)), m_zero))
                boost::throw_exception(negative_edge());
            // End of test for negative-weight edges.

            m_vis.examine_edge(e, g);
        }
        template < class Edge, class Graph > void black_target(Edge, Graph&) {}
        template < class Vertex, class Graph >
        void finish_vertex(Vertex u, Graph& g)
        {
            m_vis.finish_vertex(u, g);
        }

        UniformCostVisitor m_vis;
        UpdatableQueue& m_Q;
        WeightMap m_weight;
        PredecessorMap m_predecessor;
        DistanceMap m_distance;
        BinaryFunction m_combine;
        BinaryPredicate m_compare;
        D m_zero;
    };

} // namespace detail

namespace detail
{
    template < class Graph, class IndexMap, class Value, bool KnownNumVertices >
    struct vertex_property_map_generator_helper
    {
    };

    template < class Graph, class IndexMap, class Value >
    struct vertex_property_map_generator_helper< Graph, IndexMap, Value, true >
    {
        typedef boost::iterator_property_map< Value*, IndexMap > type;
        static type build(const Graph& g, const IndexMap& index,
            boost::scoped_array< Value >& array_holder)
        {
            array_holder.reset(new Value[num_vertices(g)]);
            std::fill(array_holder.get(), array_holder.get() + num_vertices(g),
                Value());
            return make_iterator_property_map(array_holder.get(), index);
        }
    };

    template < class Graph, class IndexMap, class Value >
    struct vertex_property_map_generator_helper< Graph, IndexMap, Value, false >
    {
        typedef boost::vector_property_map< Value, IndexMap > type;
        static type build(const Graph& g, const IndexMap& index,
            boost::scoped_array< Value >& array_holder)
        {
            return boost::make_vector_property_map< Value >(index);
        }
    };

    template < class Graph, class IndexMap, class Value >
    struct vertex_property_map_generator
    {
        typedef boost::is_base_and_derived< boost::vertex_list_graph_tag,
            typename boost::graph_traits< Graph >::traversal_category >
            known_num_vertices;
        typedef vertex_property_map_generator_helper< Graph, IndexMap, Value,
            known_num_vertices::value >
            helper;
        typedef typename helper::type type;
        static type build(const Graph& g, const IndexMap& index,
            boost::scoped_array< Value >& array_holder)
        {
            return helper::build(g, index, array_holder);
        }
    };
}

namespace detail
{
    template < class Graph, class IndexMap, bool KnownNumVertices >
    struct default_color_map_generator_helper
    {
    };

    template < class Graph, class IndexMap >
    struct default_color_map_generator_helper< Graph, IndexMap, true >
    {
        typedef boost::two_bit_color_map< IndexMap > type;
        static type build(const Graph& g, const IndexMap& index)
        {
            size_t nv = num_vertices(g);
            return boost::two_bit_color_map< IndexMap >(nv, index);
        }
    };

    template < class Graph, class IndexMap >
    struct default_color_map_generator_helper< Graph, IndexMap, false >
    {
        typedef boost::vector_property_map< boost::two_bit_color_type,
            IndexMap >
            type;
        static type build(const Graph& g, const IndexMap& index)
        {
            return boost::make_vector_property_map< boost::two_bit_color_type >(
                index);
        }
    };

    template < class Graph, class IndexMap > struct default_color_map_generator
    {
        typedef boost::is_base_and_derived< boost::vertex_list_graph_tag,
            typename boost::graph_traits< Graph >::traversal_category >
            known_num_vertices;
        typedef default_color_map_generator_helper< Graph, IndexMap,
            known_num_vertices::value >
            helper;
        typedef typename helper::type type;
        static type build(const Graph& g, const IndexMap& index)
        {
            return helper::build(g, index);
        }
    };
}

// Call breadth first search with default color map.
template < class Graph, class SourceInputIter, class DijkstraVisitor,
    class PredecessorMap, class DistanceMap, class WeightMap, class IndexMap,
    class Compare, class Combine, class DistZero >
inline void dijkstra_shortest_paths_no_init(const Graph& g,
    SourceInputIter s_begin, SourceInputIter s_end, PredecessorMap predecessor,
    DistanceMap distance, WeightMap weight, IndexMap index_map, Compare compare,
    Combine combine, DistZero zero, DijkstraVisitor vis)
{
    typedef detail::default_color_map_generator< Graph, IndexMap >
        ColorMapHelper;
    typedef typename ColorMapHelper::type ColorMap;
    ColorMap color = ColorMapHelper::build(g, index_map);
    dijkstra_shortest_paths_no_init(g, s_begin, s_end, predecessor, distance,
        weight, index_map, compare, combine, zero, vis, color);
}

// Call breadth first search with default color map.
template < class Graph, class DijkstraVisitor, class PredecessorMap,
    class DistanceMap, class WeightMap, class IndexMap, class Compare,
    class Combine, class DistZero >
inline void dijkstra_shortest_paths_no_init(const Graph& g,
    typename graph_traits< Graph >::vertex_descriptor s,
    PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
    IndexMap index_map, Compare compare, Combine combine, DistZero zero,
    DijkstraVisitor vis)
{
    dijkstra_shortest_paths_no_init(g, &s, &s + 1, predecessor, distance,
        weight, index_map, compare, combine, zero, vis);
}

// Call breadth first search
template < class Graph, class SourceInputIter, class DijkstraVisitor,
    class PredecessorMap, class DistanceMap, class WeightMap, class IndexMap,
    class Compare, class Combine, class DistZero, class ColorMap >
inline void dijkstra_shortest_paths_no_init(const Graph& g,
    SourceInputIter s_begin, SourceInputIter s_end, PredecessorMap predecessor,
    DistanceMap distance, WeightMap weight, IndexMap index_map, Compare compare,
    Combine combine, DistZero zero, DijkstraVisitor vis, ColorMap color)
{
    typedef indirect_cmp< DistanceMap, Compare > IndirectCmp;
    IndirectCmp icmp(distance, compare);

    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;

    // Now the default: use a d-ary heap
    boost::scoped_array< std::size_t > index_in_heap_map_holder;
    typedef detail::vertex_property_map_generator< Graph, IndexMap,
        std::size_t >
        IndexInHeapMapHelper;
    typedef typename IndexInHeapMapHelper::type IndexInHeapMap;
    IndexInHeapMap index_in_heap
        = IndexInHeapMapHelper::build(g, index_map, index_in_heap_map_holder);
    typedef d_ary_heap_indirect< Vertex, 4, IndexInHeapMap, DistanceMap,
        Compare >
        MutableQueue;
    MutableQueue Q(distance, index_in_heap, compare);

    detail::dijkstra_bfs_visitor< DijkstraVisitor, MutableQueue, WeightMap,
        PredecessorMap, DistanceMap, Combine, Compare >
        bfs_vis(vis, Q, weight, predecessor, distance, combine, compare, zero);

    breadth_first_visit(g, s_begin, s_end, Q, bfs_vis, color);
}

// Call breadth first search
template < class Graph, class DijkstraVisitor, class PredecessorMap,
    class DistanceMap, class WeightMap, class IndexMap, class Compare,
    class Combine, class DistZero, class ColorMap >
inline void dijkstra_shortest_paths_no_init(const Graph& g,
    typename graph_traits< Graph >::vertex_descriptor s,
    PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
    IndexMap index_map, Compare compare, Combine combine, DistZero zero,
    DijkstraVisitor vis, ColorMap color)
{
    dijkstra_shortest_paths_no_init(g, &s, &s + 1, predecessor, distance,
        weight, index_map, compare, combine, zero, vis, color);
}

// Initialize distances and call breadth first search with default color map
template < class VertexListGraph, class SourceInputIter, class DijkstraVisitor,
    class PredecessorMap, class DistanceMap, class WeightMap, class IndexMap,
    class Compare, class Combine, class DistInf, class DistZero, typename T,
    typename Tag, typename Base >
inline void dijkstra_shortest_paths(const VertexListGraph& g,
    SourceInputIter s_begin, SourceInputIter s_end, PredecessorMap predecessor,
    DistanceMap distance, WeightMap weight, IndexMap index_map, Compare compare,
    Combine combine, DistInf inf, DistZero zero, DijkstraVisitor vis,
    const bgl_named_params< T, Tag, Base >& BOOST_GRAPH_ENABLE_IF_MODELS_PARM(
        VertexListGraph, vertex_list_graph_tag))
{
    boost::two_bit_color_map< IndexMap > color(num_vertices(g), index_map);
    dijkstra_shortest_paths(g, s_begin, s_end, predecessor, distance, weight,
        index_map, compare, combine, inf, zero, vis, color);
}

// Initialize distances and call breadth first search with default color map
template < class VertexListGraph, class DijkstraVisitor, class PredecessorMap,
    class DistanceMap, class WeightMap, class IndexMap, class Compare,
    class Combine, class DistInf, class DistZero, typename T, typename Tag,
    typename Base >
inline void dijkstra_shortest_paths(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
    IndexMap index_map, Compare compare, Combine combine, DistInf inf,
    DistZero zero, DijkstraVisitor vis,
    const bgl_named_params< T, Tag, Base >& BOOST_GRAPH_ENABLE_IF_MODELS_PARM(
        VertexListGraph, vertex_list_graph_tag))
{
    dijkstra_shortest_paths(g, &s, &s + 1, predecessor, distance, weight,
        index_map, compare, combine, inf, zero, vis);
}

// Initialize distances and call breadth first search
template < class VertexListGraph, class SourceInputIter, class DijkstraVisitor,
    class PredecessorMap, class DistanceMap, class WeightMap, class IndexMap,
    class Compare, class Combine, class DistInf, class DistZero,
    class ColorMap >
inline void dijkstra_shortest_paths(const VertexListGraph& g,
    SourceInputIter s_begin, SourceInputIter s_end, PredecessorMap predecessor,
    DistanceMap distance, WeightMap weight, IndexMap index_map, Compare compare,
    Combine combine, DistInf inf, DistZero zero, DijkstraVisitor vis,
    ColorMap color)
{
    typedef typename property_traits< ColorMap >::value_type ColorValue;
    typedef color_traits< ColorValue > Color;
    typename graph_traits< VertexListGraph >::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
    {
        vis.initialize_vertex(*ui, g);
        put(distance, *ui, inf);
        put(predecessor, *ui, *ui);
        put(color, *ui, Color::white());
    }
    for (SourceInputIter it = s_begin; it != s_end; ++it)
    {
        put(distance, *it, zero);
    }

    dijkstra_shortest_paths_no_init(g, s_begin, s_end, predecessor, distance,
        weight, index_map, compare, combine, zero, vis, color);
}

// Initialize distances and call breadth first search
template < class VertexListGraph, class DijkstraVisitor, class PredecessorMap,
    class DistanceMap, class WeightMap, class IndexMap, class Compare,
    class Combine, class DistInf, class DistZero, class ColorMap >
inline void dijkstra_shortest_paths(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
    IndexMap index_map, Compare compare, Combine combine, DistInf inf,
    DistZero zero, DijkstraVisitor vis, ColorMap color)
{
    dijkstra_shortest_paths(g, &s, &s + 1, predecessor, distance, weight,
        index_map, compare, combine, inf, zero, vis, color);
}

// Initialize distances and call breadth first search
template < class VertexListGraph, class SourceInputIter, class DijkstraVisitor,
    class PredecessorMap, class DistanceMap, class WeightMap, class IndexMap,
    class Compare, class Combine, class DistInf, class DistZero >
inline void dijkstra_shortest_paths(const VertexListGraph& g,
    SourceInputIter s_begin, SourceInputIter s_end, PredecessorMap predecessor,
    DistanceMap distance, WeightMap weight, IndexMap index_map, Compare compare,
    Combine combine, DistInf inf, DistZero zero, DijkstraVisitor vis)
{
    dijkstra_shortest_paths(g, s_begin, s_end, predecessor, distance, weight,
        index_map, compare, combine, inf, zero, vis, no_named_parameters());
}

// Initialize distances and call breadth first search
template < class VertexListGraph, class DijkstraVisitor, class PredecessorMap,
    class DistanceMap, class WeightMap, class IndexMap, class Compare,
    class Combine, class DistInf, class DistZero >
inline void dijkstra_shortest_paths(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    PredecessorMap predecessor, DistanceMap distance, WeightMap weight,
    IndexMap index_map, Compare compare, Combine combine, DistInf inf,
    DistZero zero, DijkstraVisitor vis)
{
    dijkstra_shortest_paths(g, &s, &s + 1, predecessor, distance, weight,
        index_map, compare, combine, inf, zero, vis);
}

namespace detail
{

    // Handle defaults for PredecessorMap and
    // Distance Compare, Combine, Inf and Zero
    template < class VertexListGraph, class DistanceMap, class WeightMap,
        class IndexMap, class Params >
    inline void dijkstra_dispatch2(const VertexListGraph& g,
        typename graph_traits< VertexListGraph >::vertex_descriptor s,
        DistanceMap distance, WeightMap weight, IndexMap index_map,
        const Params& params)
    {
        // Default for predecessor map
        dummy_property_map p_map;

        typedef typename property_traits< DistanceMap >::value_type D;
        D inf = choose_param(get_param(params, distance_inf_t()),
            (std::numeric_limits< D >::max)());

        dijkstra_shortest_paths(g, s,
            choose_param(get_param(params, vertex_predecessor), p_map),
            distance, weight, index_map,
            choose_param(
                get_param(params, distance_compare_t()), std::less< D >()),
            choose_param(
                get_param(params, distance_combine_t()), std::plus< D >()),
            inf, choose_param(get_param(params, distance_zero_t()), D()),
            choose_param(get_param(params, graph_visitor),
                make_dijkstra_visitor(null_visitor())),
            params);
    }

    template < class VertexListGraph, class DistanceMap, class WeightMap,
        class IndexMap, class Params >
    inline void dijkstra_dispatch1(const VertexListGraph& g,
        typename graph_traits< VertexListGraph >::vertex_descriptor s,
        DistanceMap distance, WeightMap weight, IndexMap index_map,
        const Params& params)
    {
        // Default for distance map
        typedef typename property_traits< WeightMap >::value_type D;
        typename std::vector< D >::size_type n
            = is_default_param(distance) ? num_vertices(g) : 1;
        std::vector< D > distance_map(n);

        detail::dijkstra_dispatch2(g, s,
            choose_param(distance,
                make_iterator_property_map(
                    distance_map.begin(), index_map, distance_map[0])),
            weight, index_map, params);
    }
} // namespace detail

// Named Parameter Variant
template < class VertexListGraph, class Param, class Tag, class Rest >
inline void dijkstra_shortest_paths(const VertexListGraph& g,
    typename graph_traits< VertexListGraph >::vertex_descriptor s,
    const bgl_named_params< Param, Tag, Rest >& params)
{
    // Default for edge weight and vertex index map is to ask for them
    // from the graph.  Default for the visitor is null_visitor.
    detail::dijkstra_dispatch1(g, s, get_param(params, vertex_distance),
        choose_const_pmap(get_param(params, edge_weight), g, edge_weight),
        choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
        params);
}

} // namespace boost

#include BOOST_GRAPH_MPI_INCLUDE(< boost / graph / distributed / dijkstra_shortest_paths.hpp >)

#endif // BOOST_GRAPH_DIJKSTRA_HPP


/*
  Author(s):      Matthew Celnik (msc37)
  Project:        sweepc (population balance solver)
  Sourceforge:    http://sourceforge.net/projects/mopssuite

  Copyright (C) 2008 Matthew S Celnik.

  File purpose:
    Implementation of the Cell class declared in the
    swp_cell.h header file.

  Licence:
    This file is part of "sweepc".

    sweepc is free software; you can redistribute it and/or
    modify it under the terms of the GNU Lesser General Public License
    as published by the Free Software Foundation; either version 2
    of the License, or (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with this program; if not, write to the Free Software
    Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

  Contact:
    Dr Markus Kraft
    Dept of Chemical Engineering
    University of Cambridge
    New Museums Site
    Pembroke Street
    Cambridge
    CB2 3RA
    UK

    Email:       mk306@cam.ac.uk
    Website:     http://como.cheng.cam.ac.uk
*/

#include "swp_cell.h"
#include "swp_particle_model.h"
#include "swp_birth_process.h"
#include "swp_death_process.h"
#include "swp_sprog_idealgas_wrapper.h"
#include "swp_fixed_mixture.h"

#include <stdexcept>

#include <boost/random/uniform_01.hpp>
#include <boost/random/mersenne_twister.hpp>

using namespace std;

namespace Sweep {

// CONSTRUCTORS AND DESTRUCTORS.

// Default constructor (public).
Cell::Cell(const Sweep::ParticleModel &model, const bool const_gas)
: m_ensemble(), m_model(&model),
  m_smpvol(1.0), m_fixed_chem(false),
  m_constv(false), m_particle_density(0.0), m_adiabatic_flag(false)
{
    if(const_gas)
        m_gas = new Sweep::FixedMixture(fvector(7 + model.Species()->size()), *model.Species());
    else
        m_gas = new Sweep::SprogIdealGasWrapper(*model.Species());

    assert(isValid());
}

// Copy constructor.
Cell::Cell(const Cell &copy)
: m_gas(copy.m_gas->Clone())
{
    *this = copy;
	assert(isValid());
}

// Stream-reading constructor.
Cell::Cell(std::istream &in, const Sweep::ParticleModel &model)
: m_model(&model), m_gas(new Sweep::SprogIdealGasWrapper(*model.Species()))
{
    Deserialize(in, model);
	assert(isValid());
}

// Default destructor.
Cell::~Cell(void)
{
	assert(isValid());
    delete m_gas;
}

// OPERATOR OVERLOADS.

// Assignment operator.
Cell &Cell::operator=(const Sweep::Cell &rhs)
{
    if (this != &rhs) {
	//assert(rhs.isValid());
        delete m_gas;
        m_gas        = rhs.m_gas->Clone();
        m_ensemble   = rhs.m_ensemble;
        m_model      = rhs.m_model;
        m_smpvol     = rhs.m_smpvol;
        m_fixed_chem = rhs.m_fixed_chem;
        m_inflow     = rhs.m_inflow;
        m_outflow    = rhs.m_outflow;
        m_adiabatic_flag = rhs.m_adiabatic_flag;          // flag for adiabatic operation
        m_bulk_heat_capacity = rhs.m_bulk_heat_capacity;  // temperature update variables
	m_particle_heat_capacity = rhs.m_particle_heat_capacity;
        m_particle_density = rhs.m_particle_density;
        m_enthalpies = rhs.m_enthalpies;
	m_constv = rhs.m_constv;                          // flag for constant volume operation
    }
    assert(isValid());
    return *this;
}

/*!
 * @param os    Output stream
 * @param net   Cell object to print
 * @return      Output stream
 */
std::ostream& operator<<(
        std::ostream &os,
        const Sweep::Cell &c)
{
  os << "[Sweep::Cell]\n";
  if (&(c.Particles()) != NULL) os << " with " << c.Particles();
  os << " with [EnvInterface]," <<
          " T=" << c.GasPhase().Temperature() <<
          " P=" << c.GasPhase().Pressure() <<
          " SP0=" << c.GasPhase().SpeciesConcentration(0) <<
          " \n";
  assert(c.isValid());
  return os;
}

// THE PARTICLE ENSEMBLE.

// Returns the particle ensemble.
Ensemble &Cell::Particles(void) {return m_ensemble;}
const Ensemble &Cell::Particles(void) const {return m_ensemble;}

// Returns the particle count.
unsigned int Cell::ParticleCount(void) const
{
    assert(isValid());
    return m_ensemble.Count();
}

// Returns the particle statistical weight.
double Cell::ParticleWeightSum(void) const
{
	return m_ensemble.GetSum(iW);
}

/**
 * Initialise the ensemble to hold particles of the type specified
 * by the model and containing the particular particles contained
 * in the range [particle_list_begin, particle_list_end) and set
 * the sample volume to achieve the statistical weight.
 *
 *
 *@param[in]        particle_list_begin     Iterator to first in range of particle pointers to insert
 *@param[in]        particle_list_end       Iterator to one past end of range of particle pointers to insert
 *@param[in]        statistical_weight      Number of physical particles represented by each computational particle
 *@param[in,out]    rng                     Random number generator
 */
void Cell::SetParticles(
        std::list<Particle*>::iterator particle_list_begin,
        std::list<Particle*>::iterator particle_list_end,
        double statistical_weight, rng_type &rng)
{
    assert(statistical_weight > 0);
    // This puts the particles into the ensemble and clears any scaling
    // stored inside the ensemble
    m_ensemble.SetParticles(particle_list_begin, particle_list_end, rng);

    m_smpvol = 1.0 / statistical_weight;
    assert(isValid());
}

/**
 * Initialise the ensemble to hold particles of the type specified
 * by the model and containing the particular particles contained
 * in the range [particle_list_begin, particle_list_end) and set
 * the sample volume to achieve the statistical weight.
 *
 *
 *@param[in]        particle_list_begin     Iterator to first in range of particle pointers to insert
 *@param[in]        particle_list_end       Iterator to one past end of range of particle pointers to insert
 *@param[in]        statistical_weight      Number of physical particles represented by each computational particle
 *
 *@pre  The length of the input sequence is at most m_ensemble.capacity()
 */
void Cell::SetParticles(
        std::list<Particle*>::iterator particle_list_begin,
        std::list<Particle*>::iterator particle_list_end,
        double statistical_weight)
{
    assert(statistical_weight > 0);
    // This puts the particles into the ensemble and clears any scaling
    // stored inside the ensemble
    if(std::distance(particle_list_begin, particle_list_end) > m_ensemble.Capacity())
        throw std::runtime_error("Attempt to set too many particles in Sweep::Cell::SetParticles");

    // The rng will not be used, so just provide a default constructed object
    rng_type rng;
    const rng_type rngCopy(rng);
    m_ensemble.SetParticles(particle_list_begin, particle_list_end, rng);
    assert(rngCopy == rng);

    m_smpvol = 1.0 / statistical_weight;
    assert(isValid());
}

// Returns particle statistics.
void Cell::GetVitalStats(Stats::EnsembleStats &stats) const
{
    stats.Calculate(m_ensemble, 1.0/SampleVolume());
}

// SCALING ROUTINES INCL. SAMPLE VOLUME.

// Returns the double system to stochastic system scaling factor.
double Cell::SampleVolume() const
{
    return m_smpvol * m_ensemble.Scaling();
}

/*!
 * Set the number density which the ensemble represents including the
 * effects of ensemble doublings and contractions.
 *
 *@param[in]    scale_factor    Ratio of new to old sample volumes
 *
 *@pre      scale_factor > 0
 */
void Cell::AdjustSampleVolume(double scale_factor)
{
    assert(scale_factor > 0);
    assert(scale_factor <= std::numeric_limits<double>::max());
    m_smpvol = SampleVolume() * scale_factor;

    // The effects of ensemble rescalings are now incorporated in this sample
    // volume.
    m_ensemble.ResetScaling();
    assert(isValid());
}

unsigned int Cell::NumOfStartingSpecies(const int index) const
{
    // figure out how many starting PAHs are supposed in the particle ensemble
    // N = NA*Vsmpl*molar density*volume fraction of starting PAH.
    unsigned int m_amount = NA * SampleVolume() * GasPhase().SpeciesConcentration(index);
    return m_amount;
}

/**
 * Clear any particles and set the sample volume so that a full ensemble
 * (of m_ensemble.Capacity() particles) has the specified m0.
 *
 *@param[in]    m0              Particle number density for full ensemble (units \f$\mathrm{m}^{-3}\f$)
 */
void Cell::Reset(const double m0)
{
    assert(m0 > 0.0);
    m_ensemble.Clear();
    m_ensemble.ResetScaling();

    if ((m_ensemble.Capacity() > 0) && (m0 > 0.0)) {
        m_smpvol = m_ensemble.Capacity() / m0;
    }
    else {
        // The ensemble has not yet been initialised
        m_smpvol = 1.0;
    }
    assert(isValid());

}

// FIXED/VARIABLE CHEMISTRY.

// Returns whether or not the chemical conditions are fixed.
bool Cell::FixedChem() const {return m_fixed_chem;}

// Sets whether or not the chemical conditions are fixed.
void Cell::SetFixedChem(bool fixed) {m_fixed_chem = fixed;}

// Set the chemical conditions to be variable.
void Cell::SetVariableChem(bool vari) {m_fixed_chem = !vari;}

// PARTICLE INFLOW PROCESSES.

// Returns the number of inflow processes defined
// for this Cell.
unsigned int Cell::InflowCount(void) const {return m_inflow.size();}

// Returns the vector of particle inflow processes.
const Processes::BirthPtrVector &Cell::Inflows(void) const {return m_inflow;}

// Returns the ith particle inflow process.
Processes::BirthProcess *const Cell::Inflows(unsigned int i) const
{
    if (i < m_inflow.size()) {
        return m_inflow[i];
    } else {
        return NULL;
    }
}

// Add an inflow process to the Cell. The process is copied.
void Cell::AddInflow(Processes::BirthProcess &inf)
{
    m_inflow.push_back(inf.Clone());
}

// PARTICLE OUTFLOW PROCESSES.

// Returns the number of outflow processes defined
// for this Cell.
unsigned int Cell::OutflowCount(void) const {return m_outflow.size();}

// Returns the vector of particle outflow processes.
const Processes::DeathPtrVector &Cell::Outflows(void) const {return m_outflow;}

// Returns the ith particle outflow process.
Processes::DeathProcess *const Cell::Outflows(unsigned int i) const
{
    if (i < m_outflow.size()) {
        return m_outflow[i];
    } else {
        return NULL;
    }
}

// Add an outflow process to the Cell. The process is copied.
void Cell::AddOutflow(Processes::DeathProcess &out)
{
    m_outflow.push_back(out.Clone());
}

// Add an outflow process with the given rate to the Cell.
void Cell::AddOutflow(double rate, const Sweep::Mechanism &mech)
{
    Processes::DeathProcess *death = new Processes::DeathProcess(mech);
    death->SetA(rate);
    m_outflow.push_back(death);
}

// Store temperature properties to be used in particle step
void Cell::setGasPhaseProperties(double C_bulk, double C_particle, double rhop, fvector enthalpies)
{
    m_bulk_heat_capacity = C_bulk;
    m_particle_heat_capacity = C_particle;
    m_particle_density = rhop;
    m_enthalpies = enthalpies;
}

// READ/WRITE/COPY.

/*!
 * Writes the object to a binary stream.
 *
 *@param[in,out]    out     output stream
 */
void Cell::Serialize(std::ostream &out) const
{
    assert(isValid());
    if (out.good()) {
        // Output the gas mixture
        // First check how the mixture is stored
        const SprogIdealGasWrapper *pSprogWrapper = dynamic_cast<const SprogIdealGasWrapper*>(m_gas);
        const FixedMixture *pFixedMix = dynamic_cast<const FixedMixture*>(m_gas);

        // Now serialise with a label to say what type of object has been serialised
        // so that it can be correctly deserialised.
        if(NULL != pSprogWrapper) {
            const int sprogWrapperID = 1;
            out.write(reinterpret_cast<const char *>(&sprogWrapperID), sizeof(sprogWrapperID));
            pSprogWrapper->Serialize(out);
        }
        else if(NULL != pFixedMix) {
            const int FixedMixID = 2;
            out.write(reinterpret_cast<const char *>(&FixedMixID), sizeof(FixedMixID));
            throw std::logic_error("Serialisation of Sweep::FixedMixture not supported in Sweep::Cell::Serialize");
        }
        else {
            throw std::logic_error("Unsupported gas phase mixture type in Sweep::Cell::Serialize");
        }

        // Output the sample volume.
        double v = (double)m_smpvol;
        out.write((char*)&v, sizeof(v));

        // Output if fixed chem.
        out.write((char*)&m_fixed_chem, sizeof(m_fixed_chem));

		// Output if constant volume.
		out.write((char*)&m_constv, sizeof(m_constv));

		// Output if fully adiabatic.
		out.write((char*)&m_adiabatic_flag, sizeof(m_adiabatic_flag));

        // Output the ensemble.
        m_ensemble.Serialize(out);
    } else {
        throw invalid_argument("Output stream not ready "
                               "(Sweep, Cell::Serialize).");
    }
}

/*!
 * Read object state from a binary stream.
 *
 *@param[in,out]        in      Stream from which to read
 *@param[in]            model   Model for interpreting the particles in the cell
 *
 *@pre  m_gas is initialised
 */
void Cell::Deserialize(std::istream &in, const Sweep::ParticleModel &model)
{
    if (in.good()) {

        // Find out what kind of mixture to read, see Serialize() method for possible values
        int mixID;
        in.read(reinterpret_cast<char *>(&mixID), sizeof(mixID));

        switch(mixID) {
            case 1:
            {
                //SprogIdealGasWrapper
                Sweep::SprogIdealGasWrapper *pGas = new Sweep::SprogIdealGasWrapper(*model.Species());
                pGas->Implementation()->Deserialize(in);
                // Set the species, because the pointer gets reset in Sprog::Mixture::Deserialize
                pGas->Implementation()->SetSpecies(*model.Species());
                m_gas = pGas;
                break;
            }
            case 2:
                // FixedMixture - not currently supported
                throw std::logic_error("Deserialisation of Sweep::FixedMixture not supported in Sweep::Cell::Deserialize");
                break;
            default:
                throw std::logic_error("Unrecognised gas-phase mixture type in Sweep::Cell::Deserialize");
                break;
        }

        // Read the sample volume.
        in.read(reinterpret_cast<char*>(&m_smpvol), sizeof(m_smpvol));

        // Read if fixed chem.
        in.read(reinterpret_cast<char*>(&m_fixed_chem), sizeof(m_fixed_chem));

		// Read if constant volume.
		in.read(reinterpret_cast<char*>(&m_constv), sizeof(m_constv));

		// Read if fully adiabatic.
		in.read(reinterpret_cast<char*>(&m_adiabatic_flag), sizeof(m_adiabatic_flag));

        // Read the ensemble.
        m_ensemble.Deserialize(in, model);

    } else {
        throw invalid_argument("Input stream not ready "
                               "(Sweep, Cell::Deserialize).");
    }
}

// Check cell consistency, ensure gas phase mixture pointers are never null
bool Cell::isValid() const {
    return m_gas != NULL;
}

} // Sweep namespace


/*
 * Copyright (c) 2009-2021, Albertas Vyšniauskas
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution.
 *     * Neither the name of the software author nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
 * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS
 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
 * IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 * OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "Color.h"
#include <cmath>
#include <string>
#include <tuple>
#include <stdexcept>
#include <boost/math/special_functions/relative_difference.hpp>

// Constant used for lab->xyz transform. Should be calculated with maximum accuracy possible.
const double Epsilon = 216.0 / 24389.0;
const double Kk = 24389.0 / 27.0;
const Color Color::white = { 1.0f, 1.0f, 1.0f };
const Color Color::black = { 0.0f, 0.0f, 0.0f };
static Color::Matrix3d sRGBTransformation;
static Color::Matrix3d sRGBTransformationInverted;
static Color::Matrix3d d65d50AdaptationMatrixValue;
static Color::Matrix3d d50d65AdaptationMatrixValue;
const Color::Matrix3d &Color::sRGBMatrix = sRGBTransformation;
const Color::Matrix3d &Color::sRGBInvertedMatrix = sRGBTransformationInverted;
const Color::Matrix3d &Color::d65d50AdaptationMatrix = d65d50AdaptationMatrixValue;
const Color::Matrix3d &Color::d50d65AdaptationMatrix = d50d65AdaptationMatrixValue;
static Color::Vector3f references[][2] = {
	{ { 109.850f, 100.000f, 35.585f }, { 111.144f, 100.000f, 35.200f } },
	{ { 98.074f, 100.000f, 118.232f }, { 97.285f, 100.000f, 116.145f } },
	{ { 96.422f, 100.000f, 82.521f }, { 96.720f, 100.000f, 81.427f } },
	{ { 95.682f, 100.000f, 92.149f }, { 95.799f, 100.000f, 90.926f } },
	{ { 95.047f, 100.000f, 108.883f }, { 94.811f, 100.000f, 107.304f } },
	{ { 94.972f, 100.000f, 122.638f }, { 94.416f, 100.000f, 120.641f } },
	{ { 99.187f, 100.000f, 67.395f }, { 103.280f, 100.000f, 69.026f } },
	{ { 95.044f, 100.000f, 108.755f }, { 95.792f, 100.000f, 107.687f } },
	{ { 100.966f, 100.000f, 64.370f }, { 103.866f, 100.000f, 65.627f } },
};
void Color::initialize() {
	// constants used below are sRGB working space red, green and blue primaries for D65 reference white
	sRGBTransformation = getWorkingSpaceMatrix(0.6400f, 0.3300f, 0.3000f, 0.6000f, 0.1500f, 0.0600f, getReference(ReferenceIlluminant::D65, ReferenceObserver::_2));
	sRGBTransformationInverted = sRGBTransformation.inverse().value();
	d65d50AdaptationMatrixValue = getChromaticAdaptationMatrix(getReference(ReferenceIlluminant::D65, ReferenceObserver::_2), getReference(ReferenceIlluminant::D50, ReferenceObserver::_2));
	d50d65AdaptationMatrixValue = getChromaticAdaptationMatrix(getReference(ReferenceIlluminant::D50, ReferenceObserver::_2), getReference(ReferenceIlluminant::D65, ReferenceObserver::_2));
}
namespace util {
template<typename T>
inline void copy(const T *source, T *destination, size_t count = 1) {
	for (size_t i = 0; i < count; i++) {
		destination[i] = source[i];
	}
}
template<typename T, typename TSource>
inline void castCopy(const TSource *source, T *destination, size_t count = 1) {
	for (size_t i = 0; i < count; i++) {
		destination[i] = static_cast<T>(source[i]);
	}
}
template<typename T>
inline void fill(T *destination, T value, size_t count = 1) {
	for (size_t i = 0; i < count; i++) {
		destination[i] = value;
	}
}
}
static Color::Vector3d xyzToLab(const Color::Vector3d &xyzVector, const Color::Vector3f &referenceWhite);
static Color::Vector3d labToXyz(const Color::Vector3d &labVector, const Color::Vector3f &referenceWhite);
static Color::Vector3d xyzChromaticAdaptation(Color::Vector3d xyzVector, const Color::Matrix3d &adaptation);
Color &Color::zero() {
	for (auto i = 0; i < Color::MemberCount; i++)
		data[i] = 0;
	return *this;
}
Color Color::zero() const {
	return Color();
}
Color::Color() {
	util::fill(data, 0.0f, ChannelCount);
}
Color::Color(float value) {
	util::fill(data, value, ChannelCount);
}
Color::Color(int value) {
	util::fill(data, value / 255.0f, ChannelCount);
}
Color::Color(float red, float green, float blue) {
	rgb.red = red;
	rgb.green = green;
	rgb.blue = blue;
	util::fill(data + 3, 0.0f, ChannelCount - 3);
}
Color::Color(float value1, float value2, float value3, float value4) {
	data[0] = value1;
	data[1] = value2;
	data[2] = value3;
	data[3] = value4;
}
Color::Color(int red, int green, int blue) {
	rgb.red = static_cast<float>(red / 255.0f);
	rgb.green = static_cast<float>(green / 255.0f);
	rgb.blue = static_cast<float>(blue / 255.0f);
	util::fill(data + 3, 0.0f, ChannelCount - 3);
}
Color::Color(const Color &color) {
	util::copy(color.data, data, MemberCount);
}
Color::Color(const Vector3f &value) {
	util::copy(value.data, data, 3);
	util::fill(data + 3, 0.0f, ChannelCount - 3);
}
Color::Color(const Vector3d &value) {
	util::castCopy(value.data, data, 3);
	util::fill(data + 3, 0.0f, ChannelCount - 3);
}
template<>
Color::Vector3f Color::rgbVector() const {
	return Vector3f(rgb.red, rgb.green, rgb.blue);
}
template<>
Color::Vector3d Color::rgbVector() const {
	return Vector3d(rgb.red, rgb.green, rgb.blue);
}
Color Color::rgbToHsv() const {
	float min, max;
	std::tie(min, max) = math::minMax(rgb.red, rgb.green, rgb.blue);
	float delta = max - min;
	Color result;
	result.hsv.value = max;
	if (max != 0.0f)
		result.hsv.saturation = delta / max;
	else
		result.hsv.saturation = 0.0f;
	if (result.hsv.saturation == 0.0f) {
		result.hsv.hue = 0.0f;
	} else {
		if (rgb.red == max)
			result.hsv.hue = (rgb.green - rgb.blue) / delta;
		else if (rgb.green == max)
			result.hsv.hue = 2.0f + (rgb.blue - rgb.red) / delta;
		else if (rgb.blue == max)
			result.hsv.hue = 4.0f + (rgb.red - rgb.green) / delta;
		result.hsv.hue /= 6.0f;
		result.hsv.hue = math::wrap(result.hsv.hue);
	}
	return result;
}
Color Color::hsvToRgb() const {
	float v = hsv.value;
	Color result;
	if (hsv.saturation == 0.0f) {
		result.rgb.red = result.rgb.green = result.rgb.blue = hsv.value;
	} else {
		float h, f, x, y, z;
		h = (hsv.hue - std::floor(hsv.hue)) * 6.0f;
		int i = int(h);
		f = h - std::floor(h);
		x = v * (1.0f - hsv.saturation);
		y = v * (1.0f - (hsv.saturation * f));
		z = v * (1.0f - (hsv.saturation * (1.0f - f)));
		if (i == 0) {
			result.rgb.red = v;
			result.rgb.green = z;
			result.rgb.blue = x;
		} else if (i == 1) {
			result.rgb.red = y;
			result.rgb.green = v;
			result.rgb.blue = x;
		} else if (i == 2) {
			result.rgb.red = x;
			result.rgb.green = v;
			result.rgb.blue = z;
		} else if (i == 3) {
			result.rgb.red = x;
			result.rgb.green = y;
			result.rgb.blue = v;
		} else if (i == 4) {
			result.rgb.red = z;
			result.rgb.green = x;
			result.rgb.blue = v;
		} else {
			result.rgb.red = v;
			result.rgb.green = x;
			result.rgb.blue = y;
		}
	}
	return result;
}
Color Color::rgbToHsl() const {
	float min, max;
	std::tie(min, max) = math::minMax(rgb.red, rgb.green, rgb.blue);
	float delta = max - min;
	Color result;
	result.hsl.lightness = (max + min) / 2;
	if (delta == 0) {
		result.hsl.hue = 0;
		result.hsl.saturation = 0;
	} else {
		if (result.hsl.lightness < 0.5f) {
			result.hsl.saturation = delta / (max + min);
		} else {
			result.hsl.saturation = delta / (2 - max - min);
		}
		if (rgb.red == max)
			result.hsl.hue = (rgb.green - rgb.blue) / delta;
		else if (rgb.green == max)
			result.hsl.hue = 2.0f + (rgb.blue - rgb.red) / delta;
		else if (rgb.blue == max)
			result.hsl.hue = 4.0f + (rgb.red - rgb.green) / delta;
		result.hsl.hue /= 6.0f;
		result.hsl.hue = math::wrap(result.hsl.hue);
	}
	return result;
}
Color Color::hslToRgb() const {
	Color result;
	if (hsl.saturation == 0) {
		result.rgb.red = result.rgb.green = result.rgb.blue = hsl.lightness;
	} else {
		float q;
		if (hsl.lightness < 0.5f)
			q = hsl.lightness * (1.0f + hsl.saturation);
		else
			q = hsl.lightness + hsl.saturation - hsl.lightness * hsl.saturation;
		float p = 2 * hsl.lightness - q;
		float R = hsl.hue + 1 / 3.0f;
		float G = hsl.hue;
		float B = hsl.hue - 1 / 3.0f;
		if (R > 1) R -= 1;
		if (B < 0) B += 1;
		if (6.0f * R < 1)
			result.rgb.red = p + (q - p) * 6.0f * R;
		else if (2.0 * R < 1)
			result.rgb.red = q;
		else if (3.0 * R < 2)
			result.rgb.red = p + (q - p) * ((2.0f / 3.0f) - R) * 6.0f;
		else
			result.rgb.red = p;
		if (6.0f * G < 1)
			result.rgb.green = p + (q - p) * 6.0f * G;
		else if (2.0f * G < 1)
			result.rgb.green = q;
		else if (3.0f * G < 2)
			result.rgb.green = p + (q - p) * ((2.0f / 3.0f) - G) * 6.0f;
		else
			result.rgb.green = p;
		if (6.0f * B < 1)
			result.rgb.blue = p + (q - p) * 6.0f * B;
		else if (2.0f * B < 1)
			result.rgb.blue = q;
		else if (3.0f * B < 2)
			result.rgb.blue = p + (q - p) * ((2.0f / 3.0f) - B) * 6.0f;
		else
			result.rgb.blue = p;
	}
	return result;
}
Color Color::rgbToXyz(const Matrix3d &transformation) const {
	return transformation * linearRgb().rgbVector<double>();
}
static Color::Vector3d rgbToXyz(const Color::Vector3d &rgbVector, const Color::Matrix3d &transformation) {
	return transformation * rgbVector;
}
Color Color::xyzToRgb(const Matrix3d &transformationInverted) const {
	return Color(transformationInverted * rgbVector<double>()).nonLinearRgbInplace();
}
static Color::Vector3d xyzToRgb(const Color::Vector3d &xyzVector, const Color::Matrix3d &transformationInverted) {
	return transformationInverted * xyzVector;
}
Color Color::rgbToLab(const Vector3f &referenceWhite, const Matrix3d &transformation, const Matrix3d &adaptationMatrix) const {
	return ::xyzToLab(::xyzChromaticAdaptation(::rgbToXyz(linearRgb().rgbVector<double>(), transformation), adaptationMatrix), referenceWhite);
}
Color Color::labToRgb(const Vector3f &referenceWhite, const Matrix3d &transformationInverted, const Matrix3d &adaptationMatrixInverted) const {
	return Color(::xyzToRgb(::xyzChromaticAdaptation(::labToXyz(rgbVector<double>(), referenceWhite), adaptationMatrixInverted), transformationInverted)).nonLinearRgbInplace();
}
Color Color::labToLch() const {
	double H;
	if (lab.a == 0 && lab.b == 0) {
		H = 0;
	} else {
		H = std::atan2(lab.b, lab.a);
	}
	H *= 180.0f / math::PI;
	if (H < 0) H += 360;
	if (H >= 360) H -= 360;
	return Color(lab.L, static_cast<float>(std::sqrt(lab.a * lab.a + lab.b * lab.b)), static_cast<float>(H));
}
Color Color::lchToLab() const {
	return Color(lch.L, static_cast<float>(lch.C * std::cos(lch.h * math::PI / 180.0f)), static_cast<float>(lch.C * std::sin(lch.h * math::PI / 180.0f)));
}
Color Color::hslToHsv() const {
	float l = hsl.lightness * 2.0f;
	float s = hsl.saturation * ((l <= 1.0f) ? (l) : (2.0f - l));
	Color result;
	result.hsv.hue = hsl.hue;
	if (l + s == 0) {
		result.hsv.value = 0;
		result.hsv.saturation = 1;
	} else {
		result.hsv.value = (l + s) / 2.0f;
		result.hsv.saturation = (2.0f * s) / (l + s);
	}
	return result;
}
Color Color::hsvToHsl() const {
	float l = (2.0f - hsv.saturation) * hsv.value;
	float s = (hsv.saturation * hsv.value) / ((l <= 1.0f) ? (l) : (2 - l));
	if (l == 0) s = 0;
	return Color(hsv.hue, s, l / 2.0f);
}
Color &Color::linearRgbInplace() {
	if (rgb.red > 0.04045f)
		rgb.red = std::pow((rgb.red + 0.055f) / 1.055f, 2.4f);
	else
		rgb.red = rgb.red / 12.92f;
	if (rgb.green > 0.04045f)
		rgb.green = std::pow((rgb.green + 0.055f) / 1.055f, 2.4f);
	else
		rgb.green = rgb.green / 12.92f;
	if (rgb.blue > 0.04045f)
		rgb.blue = std::pow((rgb.blue + 0.055f) / 1.055f, 2.4f);
	else
		rgb.blue = rgb.blue / 12.92f;
	return *this;
}
Color Color::linearRgb() const {
	Color result;
	if (rgb.red > 0.04045f)
		result.rgb.red = std::pow((rgb.red + 0.055f) / 1.055f, 2.4f);
	else
		result.rgb.red = rgb.red / 12.92f;
	if (rgb.green > 0.04045f)
		result.rgb.green = std::pow((rgb.green + 0.055f) / 1.055f, 2.4f);
	else
		result.rgb.green = rgb.green / 12.92f;
	if (rgb.blue > 0.04045f)
		result.rgb.blue = std::pow((rgb.blue + 0.055f) / 1.055f, 2.4f);
	else
		result.rgb.blue = rgb.blue / 12.92f;
	return result;
}
Color Color::nonLinearRgb() const {
	Color result;
	if (rgb.red > 0.0031308f)
		result.rgb.red = (1.055f * std::pow(rgb.red, 1.0f / 2.4f)) - 0.055f;
	else
		result.rgb.red = rgb.red * 12.92f;
	if (rgb.green > 0.0031308f)
		result.rgb.green = (1.055f * std::pow(rgb.green, 1.0f / 2.4f)) - 0.055f;
	else
		result.rgb.green = rgb.green * 12.92f;
	if (rgb.blue > 0.0031308f)
		result.rgb.blue = (1.055f * std::pow(rgb.blue, 1.0f / 2.4f)) - 0.055f;
	else
		result.rgb.blue = rgb.blue * 12.92f;
	return result;
}
Color &Color::nonLinearRgbInplace() {
	if (rgb.red > 0.0031308f)
		rgb.red = (1.055f * std::pow(rgb.red, 1.0f / 2.4f)) - 0.055f;
	else
		rgb.red = rgb.red * 12.92f;
	if (rgb.green > 0.0031308f)
		rgb.green = (1.055f * std::pow(rgb.green, 1.0f / 2.4f)) - 0.055f;
	else
		rgb.green = rgb.green * 12.92f;
	if (rgb.blue > 0.0031308f)
		rgb.blue = (1.055f * std::pow(rgb.blue, 1.0f / 2.4f)) - 0.055f;
	else
		rgb.blue = rgb.blue * 12.92f;
	return *this;
}
Color Color::rgbToLch(const Vector3f &referenceWhite, const Matrix3d &transformation, const Matrix3d &adaptationMatrix) const {
	return rgbToLab(referenceWhite, transformation, adaptationMatrix).labToLch();
}
Color Color::lchToRgb(const Vector3f &referenceWhite, const Matrix3d &transformationInverted, const Matrix3d &adaptationMatrixInverted) const {
	return lchToLab().labToRgb(referenceWhite, transformationInverted, adaptationMatrixInverted);
}
Color Color::rgbToLchD50() const {
	return rgbToLab(getReference(ReferenceIlluminant::D50, ReferenceObserver::_2), sRGBMatrix, d65d50AdaptationMatrix).labToLch();
}
Color Color::lchToRgbD50() const {
	return lchToLab().labToRgb(getReference(ReferenceIlluminant::D50, ReferenceObserver::_2), sRGBInvertedMatrix, d50d65AdaptationMatrix);
}
Color Color::rgbToLabD50() const {
	return rgbToLab(getReference(ReferenceIlluminant::D50, ReferenceObserver::_2), sRGBMatrix, d65d50AdaptationMatrix);
}
Color Color::labToRgbD50() const {
	return labToRgb(getReference(ReferenceIlluminant::D50, ReferenceObserver::_2), sRGBInvertedMatrix, d50d65AdaptationMatrix);
}
Color Color::rgbToCmy() const {
	return Color(1 - rgb.red, 1 - rgb.green, 1 - rgb.blue);
}
Color Color::cmyToRgb() const {
	return Color(1 - cmy.c, 1 - cmy.m, 1 - cmy.y);
}
Color Color::cmyToCmyk() const {
	float k = 1;
	if (cmy.c < k) k = cmy.c;
	if (cmy.m < k) k = cmy.m;
	if (cmy.y < k) k = cmy.y;
	Color result;
	if (k == 1) {
		result.cmyk.c = result.cmyk.m = result.cmyk.y = 0;
	} else {
		result.cmyk.c = (cmy.c - k) / (1 - k);
		result.cmyk.m = (cmy.m - k) / (1 - k);
		result.cmyk.y = (cmy.y - k) / (1 - k);
	}
	result.cmyk.k = k;
	return result;
}
Color Color::cmykToCmy() const {
	return Color(cmyk.c * (1 - cmyk.k) + cmyk.k, cmyk.m * (1 - cmyk.k) + cmyk.k, cmyk.y * (1 - cmyk.k) + cmyk.k);
}
Color Color::rgbToCmyk() const {
	return rgbToCmy().cmyToCmyk();
}
Color Color::cmykToRgb() const {
	return cmykToCmy().cmyToRgb();
}
Color Color::xyzToLab(const Vector3f &referenceWhite) const {
	float X = xyz.x / referenceWhite.x;
	float Y = xyz.y / referenceWhite.y;
	float Z = xyz.z / referenceWhite.z;
	if (X > Epsilon) {
		X = static_cast<float>(std::pow(X, 1.0f / 3.0f));
	} else {
		X = static_cast<float>((Kk * X + 16.0f) / 116.0f);
	}
	if (Y > Epsilon) {
		Y = static_cast<float>(std::pow(Y, 1.0f / 3.0f));
	} else {
		Y = static_cast<float>((Kk * Y + 16.0f) / 116.0f);
	}
	if (Z > Epsilon) {
		Z = static_cast<float>(std::pow(Z, 1.0f / 3.0f));
	} else {
		Z = static_cast<float>((Kk * Z + 16.0f) / 116.0f);
	}
	return Color((116 * Y) - 16, 500 * (X - Y), 200 * (Y - Z));
}
static Color::Vector3d xyzToLab(const Color::Vector3d &xyzVector, const Color::Vector3f &referenceWhite) {
	double X = xyzVector.x / referenceWhite.x;
	double Y = xyzVector.y / referenceWhite.y;
	double Z = xyzVector.z / referenceWhite.z;
	if (X > Epsilon) {
		X = std::pow(X, 1.0 / 3.0);
	} else {
		X = (Kk * X + 16.0) / 116.0;
	}
	if (Y > Epsilon) {
		Y = std::pow(Y, 1.0 / 3.0);
	} else {
		Y = (Kk * Y + 16.0) / 116.0;
	}
	if (Z > Epsilon) {
		Z = std::pow(Z, 1.0 / 3.0);
	} else {
		Z = (Kk * Z + 16.0) / 116.0;
	}
	return Color::Vector3d((116 * Y) - 16, 500 * (X - Y), 200 * (Y - Z));
}
Color Color::labToXyz(const Vector3f &referenceWhite) const {
	float x, y, z;
	float fy = (lab.L + 16) / 116;
	float fx = lab.a / 500 + fy;
	float fz = fy - lab.b / 200;
	const double K = (24389.0 / 27.0);
	if (std::pow(fx, 3) > Epsilon) {
		x = static_cast<float>(std::pow(fx, 3));
	} else {
		x = static_cast<float>((116 * fx - 16) / K);
	}
	if (lab.L > K * Epsilon) {
		y = static_cast<float>(std::pow((lab.L + 16) / 116, 3));
	} else {
		y = static_cast<float>(lab.L / K);
	}
	if (std::pow(fz, 3) > Epsilon) {
		z = static_cast<float>(std::pow(fz, 3));
	} else {
		z = static_cast<float>((116 * fz - 16) / K);
	}
	return Color(x * referenceWhite.x, y * referenceWhite.y, z * referenceWhite.z);
}
static Color::Vector3d labToXyz(const Color::Vector3d &labVector, const Color::Vector3f &referenceWhite) {
	double x, y, z;
	double fy = (labVector.x + 16) / 116;
	double fx = labVector.y / 500 + fy;
	double fz = fy - labVector.z / 200;
	const double K = (24389.0 / 27.0);
	if (std::pow(fx, 3) > Epsilon) {
		x = std::pow(fx, 3);
	} else {
		x = (116 * fx - 16) / K;
	}
	if (labVector.x > K * Epsilon) {
		y = std::pow((labVector.x + 16) / 116, 3);
	} else {
		y = labVector.x / K;
	}
	if (std::pow(fz, 3) > Epsilon) {
		z = std::pow(fz, 3);
	} else {
		z = (116 * fz - 16) / K;
	}
	return Color::Vector3d(x * referenceWhite.x, y * referenceWhite.y, z * referenceWhite.z);
}
Color::Matrix3d Color::getWorkingSpaceMatrix(float xr, float yr, float xg, float yg, float xb, float yb, const Vector3f &referenceWhite) {
	float Xr = xr / yr;
	float Yr = 1;
	float Zr = (1 - xr - yr) / yr;
	float Xg = xg / yg;
	float Yg = 1;
	float Zg = (1 - xg - yg) / yg;
	float Xb = xb / yb;
	float Yb = 1;
	float Zb = (1 - xb - yb) / yb;
	auto s = Matrix3d { Xr, Xg, Xb, Yr, Yg, Yb, Zr, Zg, Zb }.inverse().value() * math::vectorCast<double>(referenceWhite);
	return Matrix3d { Xr * s.r, Xg * s.g, Xb * s.b, Yr * s.r, Yg * s.g, Yb * s.b, Zr * s.r, Zg * s.g, Zb * s.b };
}
Color::Matrix3d Color::getChromaticAdaptationMatrix(const Vector3f &sourceReferenceWhite, const Vector3f &destinationReferenceWhite) {
	const Matrix3d bradfordMatrix { 0.8951, 0.2664, -0.1614, -0.7502, 1.7135, 0.0367, 0.0389, -0.0685, 1.0296 };
	const Matrix3d bradfordInvertedMatrix = *bradfordMatrix.inverse();
	auto source = bradfordMatrix * math::vectorCast<double>(sourceReferenceWhite);
	auto destination = bradfordMatrix * math::vectorCast<double>(destinationReferenceWhite);
	Matrix3d matrix;
	matrix[0] = destination.x / source.x;
	matrix[4] = destination.y / source.y;
	matrix[8] = destination.z / source.z;
	return bradfordMatrix * matrix * bradfordInvertedMatrix;
}
Color Color::xyzChromaticAdaptation(const Matrix3d &adaptation) const {
	return adaptation * rgbVector<double>();
}
static Color::Vector3d xyzChromaticAdaptation(Color::Vector3d xyzVector, const Color::Matrix3d &adaptation) {
	return adaptation * xyzVector;
}
const Color::Vector3f &Color::getReference(ReferenceIlluminant illuminant, ReferenceObserver observer) {
	return references[static_cast<uint8_t>(illuminant)][static_cast<uint8_t>(observer)];
}
ReferenceIlluminant Color::getIlluminant(const std::string &illuminant) {
	const struct {
		const char *label;
		ReferenceIlluminant illuminant;
	} illuminants[] = {
		{ "A", ReferenceIlluminant::A },
		{ "C", ReferenceIlluminant::C },
		{ "D50", ReferenceIlluminant::D50 },
		{ "D55", ReferenceIlluminant::D55 },
		{ "D65", ReferenceIlluminant::D65 },
		{ "D75", ReferenceIlluminant::D75 },
		{ "F2", ReferenceIlluminant::F2 },
		{ "F7", ReferenceIlluminant::F7 },
		{ "F11", ReferenceIlluminant::F11 },
		{ nullptr, ReferenceIlluminant::D50 },
	};
	for (int i = 0; illuminants[i].label; i++) {
		if (illuminant == illuminants[i].label) {
			return illuminants[i].illuminant;
		}
	}
	return ReferenceIlluminant::D50;
};
ReferenceObserver Color::getObserver(const std::string &observer) {
	const struct {
		const char *label;
		ReferenceObserver observer;
	} observers[] = {
		{ "2", ReferenceObserver::_2 },
		{ "10", ReferenceObserver::_10 },
		{ nullptr, ReferenceObserver::_2 },
	};
	for (int i = 0; observers[i].label; i++) {
		if (observer == observers[i].label) {
			return observers[i].observer;
		}
	}
	return ReferenceObserver::_2;
}
bool Color::isOutOfRgbGamut() const {
	if (rgb.red < 0 || rgb.red > 1)
		return true;
	if (rgb.green < 0 || rgb.green > 1)
		return true;
	if (rgb.blue < 0 || rgb.blue > 1)
		return true;
	return false;
}
float Color::distance(const Color &a, const Color &b) {
	auto al = a.linearRgb();
	auto bl = b.linearRgb();
	return static_cast<float>(std::sqrt(
		std::pow(bl.rgb.red - al.rgb.red, 2) +
		std::pow(bl.rgb.green - al.rgb.green, 2) +
		std::pow(bl.rgb.blue - al.rgb.blue, 2)));
}
float Color::distanceLch(const Color &a, const Color &b) {
	auto al = a.labToLch();
	auto bl = b.labToLch();
	return static_cast<float>(std::sqrt(
		std::pow((bl.lch.L - al.lch.L) / 1, 2) +
		std::pow((bl.lch.C - al.lch.C) / (1 + 0.045 * al.lch.C), 2) +
		std::pow((std::pow(a.lab.a - b.lab.a, 2) + std::pow(a.lab.b - b.lab.b, 2) - (bl.lch.C - al.lch.C)) / (1 + 0.015 * al.lch.C), 2)));
}
Color mix(const Color &a, const Color &b, float ratio) {
	return (a.linearRgb() * (1.0f - ratio) + b.linearRgb() * ratio).nonLinearRgb();
}
bool Color::operator==(const Color &color) const {
	for (auto i = 0; i < MemberCount; i++) {
		if (data[i] == 0.0f || color.data[i] == 0.0f) {
			if (math::abs(data[i] - color.data[i]) > 1e-10)
				return false;
		} else if (boost::math::epsilon_difference(data[i], color.data[i]) > 50.0f)
			return false;
	}
	return true;
}
bool Color::operator!=(const Color &color) const {
	for (auto i = 0; i < MemberCount; i++) {
		if (data[i] == 0.0f || color.data[i] == 0.0f) {
			if (math::abs(data[i] - color.data[i]) > 1e-10)
				return false;
		} else if (boost::math::epsilon_difference(data[i], color.data[i]) > 50.0f)
			return true;
	}
	return false;
}
Color Color::operator*(float value) const {
	return Color(data[0] * value, data[1] * value, data[2] * value, data[3] * value);
}
Color Color::operator/(float value) const {
	return Color(data[0] / value, data[1] / value, data[2] / value, data[3] / value);
}
Color Color::operator+(const Color &color) const {
	Color result;
	for (auto i = 0; i < Color::MemberCount; i++)
		result.data[i] = data[i] + color.data[i];
	return result;
}
Color Color::operator-(const Color &color) const {
	Color result;
	for (auto i = 0; i < Color::MemberCount; i++)
		result.data[i] = data[i] - color.data[i];
	return result;
}
Color Color::operator*(const Color &color) const {
	Color result;
	for (auto i = 0; i < Color::MemberCount; i++)
		result.data[i] = data[i] * color.data[i];
	return result;
}
Color &Color::operator*=(const Color &color) {
	for (auto i = 0; i < Color::MemberCount; i++)
		data[i] *= color.data[i];
	return *this;
}
Color &Color::operator+=(const Color &color) {
	for (auto i = 0; i < Color::MemberCount; i++)
		data[i] += color.data[i];
	return *this;
}
float &Color::operator[](int index) {
	if (index < 0 || index >= MemberCount)
		throw std::invalid_argument("index");
	return data[index];
}
float Color::operator[](int index) const {
	if (index < 0 || index >= MemberCount)
		throw std::invalid_argument("index");
	return data[index];
}
Color Color::normalizeRgb() const {
	Color result(*this);
	result.rgb.red = math::clamp(rgb.red, 0.0f, 1.0f);
	result.rgb.green = math::clamp(rgb.green, 0.0f, 1.0f);
	result.rgb.blue = math::clamp(rgb.blue, 0.0f, 1.0f);
	return result;
}
Color &Color::normalizeRgbInplace() {
	rgb.red = math::clamp(rgb.red, 0.0f, 1.0f);
	rgb.green = math::clamp(rgb.green, 0.0f, 1.0f);
	rgb.blue = math::clamp(rgb.blue, 0.0f, 1.0f);
	return *this;
}
Color Color::absolute() const {
	Color result(*this);
	for (auto i = 0; i < Color::MemberCount; i++)
		result.data[i] = math::abs(data[i]);
	return result;
}
Color &Color::absoluteInplace() {
	for (auto i = 0; i < Color::MemberCount; i++)
		data[i] = math::abs(data[i]);
	return *this;
}
const Color &Color::getContrasting() const {
	auto t = rgbToLab(getReference(ReferenceIlluminant::D50, ReferenceObserver::_2), sRGBMatrix, d65d50AdaptationMatrix);
	return t.lab.L > 50 ? black : white;
}


#ifndef DART_NEURAL_DIFF_CONSTRAINT_HPP_
#define DART_NEURAL_DIFF_CONSTRAINT_HPP_

#include <memory>
#include <string>
#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

#include "dart/collision/Contact.hpp"
#include "dart/neural/NeuralConstants.hpp"
#include "dart/neural/NeuralUtils.hpp"
#include "dart/simulation/World.hpp"

namespace dart {

namespace constraint {
class ConstrainedGroup;
class ConstraintBase;
class ContactConstraint;
} // namespace constraint

namespace dynamics {
class Skeleton;
}

namespace simulation {
class World;
}

namespace neural {
class BackpropSnapshot;

enum DofContactType
{
  UNSUPPORTED = 0,
  NONE = 1,
  VERTEX = 2,
  FACE = 3,
  EDGE_A = 4,
  EDGE_B = 5,
  SELF_COLLISION = 7,
  SPHERE_TO_BOX = 8,
  BOX_TO_SPHERE = 9,
  SPHERE_A = 10,
  SPHERE_B = 11,
  SPHERE_TO_FACE = 12,
  FACE_TO_SPHERE = 13,
  SPHERE_TO_EDGE = 14,
  EDGE_TO_SPHERE = 15,
  SPHERE_TO_VERTEX = 16,
  VERTEX_TO_SPHERE = 17,
  PIPE_TO_SPHERE = 18,
  SPHERE_TO_PIPE = 19,
  PIPE_A = 20,
  PIPE_B = 21,
  PIPE_TO_VERTEX = 22,
  VERTEX_TO_PIPE = 23,
  PIPE_TO_EDGE = 24,
  EDGE_TO_PIPE = 25
};

struct EdgeData
{
  Eigen::Vector3s edgeAPos;
  Eigen::Vector3s edgeADir;
  Eigen::Vector3s edgeBPos;
  Eigen::Vector3s edgeBDir;
};

class DifferentiableContactConstraint
{

public:
  DifferentiableContactConstraint(
      std::shared_ptr<constraint::ConstraintBase> constraint,
      int index,
      s_t constraintForce);

  Eigen::Vector3s getContactWorldPosition();

  /// This returns the normal of the contact, pointing from A to B. This IS NOT
  /// NECESSARILY THE DIRECTION OF FORCE! If this contact constraint is a
  /// friction constraint, then this returns the normal of the contact, and
  /// getContactWorldForceDirection() returns the direction of the force.
  Eigen::Vector3s getContactWorldNormal();

  Eigen::Vector3s getContactWorldForceDirection();

  /// This computes the force, in world space exponential coordinates, that this
  /// contact generates
  Eigen::Vector6s getWorldForce();

  /// This returns the nature of the contact, whether it's a face-vertex, or a
  /// vertex-face, or an edge-edge, or something else. This is relevant because
  /// we need to know in order to accurately estimate how the contact position
  /// and normal will change as we perturb skeletons by small amounts.
  collision::ContactType getContactType();

  collision::Contact& getContact();

  /// This figures out what type of contact this skeleton is involved in.
  DofContactType getDofContactType(dynamics::DegreeOfFreedom* dof);

  /// This analytically computes a column of the A_c matrix just for this
  /// skeleton.
  Eigen::VectorXs getConstraintForces(std::shared_ptr<dynamics::Skeleton> skel);

  /// This analytically computes a column of the A_c matrix for this set of
  /// skeletons.
  Eigen::VectorXs getConstraintForces(
      simulation::World* world, std::vector<std::string> skelNames);

  /// This analytically computes a column of the A_c matrix, for this contact
  /// constraint, across the whole world by concatenating the result for each
  /// skeleton together into a single vector.
  Eigen::VectorXs getConstraintForces(simulation::World* world);

  /// Returns the gradient of the contact position with respect to the
  /// specified dof of this skeleton
  Eigen::Vector3s getContactPositionGradient(dynamics::DegreeOfFreedom* dof);

  /// Returns the gradient of the contact normal with respect to the
  /// specified dof of this skeleton
  Eigen::Vector3s getContactNormalGradient(dynamics::DegreeOfFreedom* dof);

  /// Returns the gradient of the contact force with respect to the
  /// specified dof of this skeleton
  Eigen::Vector3s getContactForceGradient(dynamics::DegreeOfFreedom* dof);

  /// Returns the gradient of the full 6d twist force
  Eigen::Vector6s getContactWorldForceGradient(dynamics::DegreeOfFreedom* dof);

  /// Returns the gradient of the screw axis with respect to the rotate dof
  Eigen::Vector6s getScrewAxisForPositionGradient(
      dynamics::DegreeOfFreedom* screwDof,
      dynamics::DegreeOfFreedom* rotateDof);

  /// Returns the gradient of the screw axis with respect to the rotate dof
  Eigen::Vector6s getScrewAxisForForceGradient(
      dynamics::DegreeOfFreedom* screwDof,
      dynamics::DegreeOfFreedom* rotateDof);

  /// Returns the gradient of the screw axis with respect to the rotate dof
  ///
  /// Unlike its sibling, getScrewAxisForForceGradient(), this allows passing
  /// in values that are otherwise repeatedly computed.
  Eigen::Vector6s getScrewAxisForForceGradient_Optimized(
      dynamics::DegreeOfFreedom* screwDof,
      dynamics::DegreeOfFreedom* rotateDof,
      const Eigen::Vector6s& axisWorldTwist);

  /// This is the analytical Jacobian for the contact position
  math::LinearJacobian getContactPositionJacobian(
      std::shared_ptr<simulation::World> world);

  /// This is the analytical Jacobian for the contact position
  math::LinearJacobian getContactPositionJacobian(
      std::shared_ptr<dynamics::Skeleton> skel);

  /// This is the analytical Jacobian for the contact normal
  math::LinearJacobian getContactForceDirectionJacobian(
      std::shared_ptr<simulation::World> world);

  /// This is the analytical Jacobian for the contact normal
  math::LinearJacobian getContactForceDirectionJacobian(
      std::shared_ptr<dynamics::Skeleton> skel);

  /// This is the analytical Jacobian for the force (in exponential coordinates,
  /// in the world frame) generated by this contact. This is measuring how the
  /// force (in world space) changes as a result of moving the contact position
  /// and normal as a result of moving the joints of the skeletons in the world.
  math::Jacobian getContactForceJacobian(
      std::shared_ptr<simulation::World> world);

  /// This is the analytical Jacobian for the force (in exponential coordinates,
  /// in the world frame) generated by this contact. This is measuring how the
  /// force (in world space) changes as a result of moving the contact position
  /// and normal as a result of moving the joints of the skeletons passed in.
  math::Jacobian getContactForceJacobian(
      std::shared_ptr<dynamics::Skeleton> skel);

  /// This gets the constraint force for a given DOF
  s_t getConstraintForce(dynamics::DegreeOfFreedom* dof);

  /// This gets the gradient of constraint force at this joint with respect to
  /// another joint
  s_t getConstraintForceDerivative(
      dynamics::DegreeOfFreedom* dof, dynamics::DegreeOfFreedom* wrt);

  /// This returns an analytical Jacobian relating the skeletons that this
  /// contact touches.
  const Eigen::MatrixXs& getConstraintForcesJacobian(
      std::shared_ptr<simulation::World> world);

  /// This computes and returns the analytical Jacobian relating how changes in
  /// the positions of wrt's DOFs changes the constraint forces on skel.
  Eigen::MatrixXs getConstraintForcesJacobian(
      std::shared_ptr<dynamics::Skeleton> skel,
      std::shared_ptr<dynamics::Skeleton> wrt);

  /// This computes and returns the analytical Jacobian relating how changes in
  /// the positions of wrt's DOFs changes the constraint forces on all the
  /// skels.
  Eigen::MatrixXs getConstraintForcesJacobian(
      std::vector<std::shared_ptr<dynamics::Skeleton>> skels,
      std::shared_ptr<dynamics::Skeleton> wrt);

  /// This computes and returns the analytical Jacobian relating how changes in
  /// the positions of any of the DOFs changes the constraint forces on all the
  /// skels.
  Eigen::MatrixXs getConstraintForcesJacobian(
      std::shared_ptr<simulation::World> world,
      std::vector<std::shared_ptr<dynamics::Skeleton>> skels);

  /// This returns the skeletons that this contact constraint interacts with.
  const std::vector<std::shared_ptr<dynamics::Skeleton>>& getSkeletons();

  /////////////////////////////////////////////////////////////////////////////////////
  // Testing
  /////////////////////////////////////////////////////////////////////////////////////

  /// The linear Jacobian for the contact position
  math::LinearJacobian bruteForceContactPositionJacobian(
      std::shared_ptr<simulation::World> world);

  /// The linear Jacobian for the contact normal
  math::LinearJacobian bruteForceContactForceDirectionJacobian(
      std::shared_ptr<simulation::World> world);

  /// This is the brute force version of getWorldForceJacobian()
  math::Jacobian bruteForceContactForceJacobian(
      std::shared_ptr<simulation::World> world);

  /// This is the brute force version of getConstraintForcesJacobian()
  Eigen::MatrixXs bruteForceConstraintForcesJacobian(
      std::shared_ptr<simulation::World> world);

  /// Just for testing: This analytically estimates the way the contact position
  /// will change if we perturb the `dofIndex`'th DOF of `skel` by `eps`.
  Eigen::Vector3s estimatePerturbedContactPosition(
      std::shared_ptr<dynamics::Skeleton> skel, int dofIndex, s_t eps);

  /// Just for testing: This analytically estimates the way the contact normal
  /// will change if we perturb the `dofIndex`'th DOF of `skel` by `eps`.
  Eigen::Vector3s estimatePerturbedContactNormal(
      std::shared_ptr<dynamics::Skeleton> skel, int dofIndex, s_t eps);

  /// Just for testing: This analytically estimates the way the contact normal
  /// will change if we perturb the `dofIndex`'th DOF of `skel` by `eps`.
  Eigen::Vector3s estimatePerturbedContactForceDirection(
      std::shared_ptr<dynamics::Skeleton> skel, int dofIndex, s_t eps);

  /// Just for testing: This analytically estimates how edges will move under a
  /// perturbation
  EdgeData estimatePerturbedEdges(
      std::shared_ptr<dynamics::Skeleton> skel, int dofIndex, s_t eps);

  /// Just for testing: returns the edges, if this is an edge-edge collision,
  /// otherwise 0s
  EdgeData getEdges();

  /// Just for testing: Returns the gradient of the edge data for this collision
  /// (0s if this isn't an edge-edge collision)
  EdgeData getEdgeGradient(dynamics::DegreeOfFreedom* dof);

  /// Just for testing: This analytically estimates how a screw axis will move
  /// when rotated by another screw.
  Eigen::Vector6s estimatePerturbedScrewAxisForPosition(
      dynamics::DegreeOfFreedom* axis,
      dynamics::DegreeOfFreedom* rotate,
      s_t eps);

  /// Just for testing: This analytically estimates how a screw axis will move
  /// when rotated by another screw.
  Eigen::Vector6s estimatePerturbedScrewAxisForForce(
      dynamics::DegreeOfFreedom* axis,
      dynamics::DegreeOfFreedom* rotate,
      s_t eps);

  /// Just for testing: This lets the world record what index this
  /// constraint is at, so that we can recover the analagous constraint from
  /// another forward pass for finite-differencing.
  void setOffsetIntoWorld(int offset, bool isUpperBoundConstraint);

  /// Just for testing: This runs a full timestep to get the way the contact
  /// position will change if we perturb the `dofIndex`'th DOF of `skel` by
  /// `eps`.
  Eigen::Vector3s bruteForcePerturbedContactPosition(
      std::shared_ptr<simulation::World> world,
      std::shared_ptr<dynamics::Skeleton> skel,
      int dofIndex,
      s_t eps);

  /// Just for testing: This runs a full timestep to get the way the contact
  /// normal will change if we perturb the `dofIndex`'th DOF of `skel` by
  /// `eps`.
  Eigen::Vector3s bruteForcePerturbedContactNormal(
      std::shared_ptr<simulation::World> world,
      std::shared_ptr<dynamics::Skeleton> skel,
      int dofIndex,
      s_t eps);

  /// Just for testing: This runs a full timestep to get the way the contact
  /// force direction will change if we perturb the `dofIndex`'th DOF of `skel`
  /// by `eps`.
  Eigen::Vector3s bruteForcePerturbedContactForceDirection(
      std::shared_ptr<simulation::World> world,
      std::shared_ptr<dynamics::Skeleton> skel,
      int dofIndex,
      s_t eps);

  /// Just for testing: This perturbs the world position of a skeleton to read a
  /// screw axis will move when rotated by another screw.
  Eigen::Vector6s bruteForceScrewAxisForPosition(
      dynamics::DegreeOfFreedom* axis,
      dynamics::DegreeOfFreedom* rotate,
      s_t eps);

  /// Just for testing: This perturbs the world position of a skeleton to read a
  /// screw axis will move when rotated by another screw.
  Eigen::Vector6s bruteForceScrewAxisForForce(
      dynamics::DegreeOfFreedom* axis,
      dynamics::DegreeOfFreedom* rotate,
      s_t eps);

  /// Just for testing: This perturbs the world position of a skeleton  to read
  /// how edges will move.
  EdgeData bruteForceEdges(
      std::shared_ptr<simulation::World> world,
      std::shared_ptr<dynamics::Skeleton> skel,
      int dofIndex,
      s_t eps);

  /// Return the index into the contact that this constraint represents. If it's
  /// >0, then this is a frictional constraint.
  int getIndexInConstraint();

  /// This returns the axis for the specified dof index
  static Eigen::Vector6s getWorldScrewAxisForPosition(
      std::shared_ptr<dynamics::Skeleton> skel, int dofIndex);

  /// This returns the axis for the specified dof index
  static Eigen::Vector6s getWorldScrewAxisForPosition(
      dynamics::DegreeOfFreedom* dof);

  /// This returns the axis for the specified dof index which, when dotted with
  /// force, calculates the torque on this joint. THIS IS NOT THE SAME AS
  /// getWorldScrewAxisForPosition(), because of FreeJoints and BallJoints.
  static Eigen::Vector6s getWorldScrewAxisForForce(
      std::shared_ptr<dynamics::Skeleton> skel, int dofIndex);

  /// This returns the axis for the specified dof index which, when dotted with
  /// force, calculates the torque on this joint. THIS IS NOT THE SAME AS
  /// getWorldScrewAxisForPosition(), because of FreeJoints and BallJoints.
  static Eigen::Vector6s getWorldScrewAxisForForce(
      dynamics::DegreeOfFreedom* dof);

  /// This returns the constraint that's at our same location in the snapshot.
  /// This assumes that `mOffsetIntoWorld` and `mIsUpperBoundConstraint` are
  /// set.
  std::shared_ptr<DifferentiableContactConstraint> getPeerConstraint(
      std::shared_ptr<neural::BackpropSnapshot> snapshot);

  /// This returns 1.0 by default, 0.0 if this constraint doesn't effect the
  /// specified DOF, and -1.0 if the constraint effects this dof negatively.
  /// Pretty much only public for testing
  s_t getControlForceMultiple(dynamics::DegreeOfFreedom* dof);

public:
  /// Returns true if this dof moves this body node
  bool isParent(
      const dynamics::DegreeOfFreedom* dof, const dynamics::BodyNode* node);

  /// Returns true if this dof moves the other dof's screw axis
  bool isParent(
      const dynamics::DegreeOfFreedom* parent,
      const dynamics::DegreeOfFreedom* child);

  friend class BackpropSnapshot;

protected:
  std::shared_ptr<constraint::ConstraintBase> mConstraint;
  std::shared_ptr<constraint::ContactConstraint> mContactConstraint;
  std::shared_ptr<collision::Contact> mContact;
  std::vector<std::string> mSkeletons;
  std::vector<Eigen::VectorXs> mSkeletonOriginalPositions;
  s_t mConstraintForce;

  bool mWorldConstraintJacCacheDirty;
  Eigen::MatrixXs mWorldConstraintJacCache;

  int mIndex;

  /// This allows us to locate this constraint in the world arrays. This value
  /// is not guaranteed to be set, but must be set before calling any of the
  /// brute force methods!
  int mOffsetIntoWorld;
  /// This allows us to locate this constraint in the world arrays. If true,
  /// we're in the upper bound array. Otherwise, we're clamping. This value
  /// is not guaranteed to be set, but must be set before calling any of the
  /// brute force methods!
  bool mIsUpperBoundConstraint;
};
} // namespace neural
} // namespace dart

#endif


//| This file is a part of the sferes2 framework.
//| Copyright 2009, ISIR / Universite Pierre et Marie Curie (UPMC)
//| Main contributor(s): Jean-Baptiste Mouret, mouret@isir.fr
//|
//| This software is a computer program whose purpose is to facilitate
//| experiments in evolutionary computation and evolutionary robotics.
//|
//| This software is governed by the CeCILL license under French law
//| and abiding by the rules of distribution of free software.  You
//| can use, modify and/ or redistribute the software under the terms
//| of the CeCILL license as circulated by CEA, CNRS and INRIA at the
//| following URL "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and rights to
//| copy, modify and redistribute granted by the license, users are
//| provided only with a limited warranty and the software's author,
//| the holder of the economic rights, and the successive licensors
//| have only limited liability.
//|
//| In this respect, the user's attention is drawn to the risks
//| associated with loading, using, modifying and/or developing or
//| reproducing the software by the user in light of its specific
//| status of free software, that may mean that it is complicated to
//| manipulate, and that also therefore means that it is reserved for
//| developers and experienced professionals having in-depth computer
//| knowledge. Users are therefore encouraged to load and test the
//| software's suitability as regards their requirements in conditions
//| enabling the security of their systems and/or data to be ensured
//| and, more generally, to use and operate it in the same conditions
//| as regards security.
//|
//| The fact that you are presently reading this means that you have
//| had knowledge of the CeCILL license and that you accept its terms.

#ifndef NSGA2_HPP_
#define NSGA2_HPP_

#include <algorithm>
#include <limits>

#include <boost/foreach.hpp>

#include <sferes/stc.hpp>
#include <sferes/parallel.hpp>
#include <sferes/ea/ea.hpp>
#include <sferes/fit/fitness.hpp>
#include <sferes/ea/dom_sort.hpp>
#include <sferes/ea/common.hpp>
#include <sferes/ea/crowd.hpp>

namespace sferes {
  namespace ea {
    // Main class
    SFERES_EA(Nsga2, Ea) {
    public:
      typedef boost::shared_ptr<crowd::Indiv<Phen> > indiv_t;
      typedef typename std::vector<indiv_t> pop_t;
      typedef typename pop_t::iterator it_t;
      typedef typename std::vector<std::vector<indiv_t> > front_t;

      void random_pop() {
        parallel::init();

        _parent_pop.resize(Params::pop::size);
        assert(Params::pop::size % 4 == 0);

        pop_t init_pop((size_t)(Params::pop::size * Params::pop::initial_aleat));
        parallel::p_for(parallel::range_t(0, init_pop.size()),
                        random<crowd::Indiv<Phen> >(init_pop));
        _eval_pop(init_pop);
        _apply_modifier(init_pop);
        front_t fronts;
        _rank_crowd(init_pop, fronts);
        _fill_nondominated_sort(init_pop, _parent_pop);
      }

      void epoch() {
        this->_pop.clear();
        _pareto_front.clear();
        _selection (_parent_pop, _child_pop);
        parallel::p_for(parallel::range_t(0, _child_pop.size()),
                        mutate<crowd::Indiv<Phen> >(_child_pop));
#ifndef EA_EVAL_ALL
        _eval_pop(_child_pop);
        _merge(_parent_pop, _child_pop, _mixed_pop);
#else
        _merge(_parent_pop, _child_pop, _mixed_pop);
        _eval_pop(_mixed_pop);
#endif
        _apply_modifier(_mixed_pop);
#ifndef NDEBUG
        BOOST_FOREACH(indiv_t& ind, _mixed_pop)
        for (size_t i = 0; i < ind->fit().objs().size(); ++i) {
          assert(!std::isnan(ind->fit().objs()[i]));
        }
#endif
        _fill_nondominated_sort(_mixed_pop, _parent_pop);
        _mixed_pop.clear();
        _child_pop.clear();

        _convert_pop(_parent_pop, this->_pop);

        assert(_parent_pop.size() == Params::pop::size);
        assert(_pareto_front.size() <= Params::pop::size * 2);
        assert(_mixed_pop.size() == 0);
        //	assert(_child_pop.size() == 0);
        assert(this->_pop.size() == Params::pop::size);
      }
      const std::vector<boost::shared_ptr<Phen> >& pareto_front() const {
        return _pareto_front;
      }
      const pop_t& mixed_pop() {
        return _mixed_pop;
      }
      const pop_t& parent_pop() {
        return _parent_pop;
      }
      const pop_t& child_pop() {
        return _child_pop;
      }
    protected:

      std::vector<boost::shared_ptr<Phen> > _pareto_front;

      pop_t _parent_pop;
      pop_t _child_pop;
      pop_t _mixed_pop;

      void _update_pareto_front(const front_t& fronts) {
        _convert_pop(fronts.front(), _pareto_front);
      }

      void _convert_pop(const pop_t& pop1,
                        std::vector<boost::shared_ptr<Phen> > & pop2) {
        pop2.resize(pop1.size());
        for (size_t i = 0; i < pop1.size(); ++i)
          pop2[i] = pop1[i];
      }

      void _eval_pop(pop_t& pop) {
        this->_eval.eval(pop, 0, pop.size());
      }

      void _apply_modifier(pop_t& pop) {
        _convert_pop(pop, this->_pop);
        this->apply_modifier();
      }
      void _fill_nondominated_sort(pop_t& mixed_pop, pop_t& new_pop) {
        assert(mixed_pop.size());
        front_t fronts;
#ifndef NDEBUG
        BOOST_FOREACH(indiv_t& ind, mixed_pop)
        for (size_t i = 0; i < ind->fit().objs().size(); ++i) {
          assert(!std::isnan(ind->fit().objs()[i]));
        }
#endif
        _rank_crowd(mixed_pop, fronts);
        new_pop.clear();

        // fill the i first layers
        size_t i;
        for (i = 0; i < fronts.size(); ++i)
          if (fronts[i].size() + new_pop.size() < Params::pop::size)
            new_pop.insert(new_pop.end(), fronts[i].begin(), fronts[i].end());
          else
            break;

        size_t size = Params::pop::size - new_pop.size();
        // sort the last layer
        if (new_pop.size() < Params::pop::size) {
          std::sort(fronts[i].begin(), fronts[i].end(), crowd::compare_crowd());
          for (size_t k = 0; k < size ; ++k) {
            assert(i < fronts.size());
            new_pop.push_back(fronts[i][k]);
          }
        }
        assert(new_pop.size() == Params::pop::size);
      }

      //
      void _merge(const pop_t& pop1, const pop_t& pop2, pop_t& pop3) {
        assert(pop1.size());
        assert(pop2.size());
        pop3.clear();
        pop3.insert(pop3.end(), pop1.begin(), pop1.end());
        pop3.insert(pop3.end(), pop2.begin(), pop2.end());
        assert(pop3.size() == pop1.size() + pop2.size());
      }

      // --- tournament selection ---
      void _selection(pop_t& old_pop, pop_t& new_pop) {
        new_pop.resize(old_pop.size());
        std::vector<size_t> a1, a2;
        misc::rand_ind(a1, old_pop.size());
        misc::rand_ind(a2, old_pop.size());
        // todo : this loop could be parallelized
        for (size_t i = 0; i < old_pop.size(); i += 4) {
          const indiv_t& p1 = _tournament(old_pop[a1[i]], old_pop[a1[i + 1]]);
          const indiv_t& p2 = _tournament(old_pop[a1[i + 2]], old_pop[a1[i + 3]]);
          const indiv_t& p3 = _tournament(old_pop[a2[i]], old_pop[a2[i + 1]]);
          const indiv_t& p4 = _tournament(old_pop[a2[i + 2]], old_pop[a2[i + 3]]);
          assert(i + 3 < new_pop.size());
          p1->cross(p2, new_pop[i], new_pop[i + 1]);
          p3->cross(p4, new_pop[i + 2], new_pop[i + 3]);
        }
      }

      const indiv_t& _tournament(const indiv_t& i1, const indiv_t& i2) {
        // if (i1->rank() < i2->rank())
        //   return i1;
        // else if (i2->rank() > i1->rank())
        //   return i2;
        // else if (misc::flip_coin())
        //   return i1;
        // else
        //   return i2;

        int flag = fit::dominate_flag(i1, i2);
        if (flag == 1)
          return i1;
        if (flag == -1)
          return i2;
        if (i1->crowd() > i2->crowd())
          return i1;
        if (i1->crowd() < i2->crowd())
          return i2;
        if (misc::flip_coin())
          return i1;
        else
          return i2;
      }

      // --- rank & crowd ---

      void _rank_crowd(pop_t& pop, front_t& fronts) {
        std::vector<size_t> ranks;
#ifndef NDEBUG
        BOOST_FOREACH(indiv_t& ind, pop)
        for (size_t i = 0; i < ind->fit().objs().size(); ++i) {
          assert(!std::isnan(ind->fit().objs()[i]));
        }
#endif
        dom_sort(pop, fronts, ranks);
        _update_pareto_front(fronts);
        parallel::p_for(parallel::range_t(0, fronts.size()),
                        crowd::assign_crowd<indiv_t >(fronts));

        for (size_t i = 0; i < ranks.size(); ++i)
          pop[i]->set_rank(ranks[i]);
        parallel::sort(pop.begin(), pop.end(), crowd::compare_ranks());;
      }

      void _assign_rank(pop_t& pop) {
        int rank = 0;
        fit::compare_pareto comp;
        assert(pop.size());
        std::sort(pop.begin(), pop.end(), comp);
        pop[0]->set_rank(0);
        for (unsigned i = 1; i < pop.size(); ++i) {
          assert(comp(pop[i-1], pop[i]) || comp.eq(pop[i -1], pop[i]));
          if (comp(pop[i-1], pop[i]))
            ++rank;
          pop[i]->set_rank(rank);
        }
      }

    };
  }
}
#endif