/*
 * author: Sergei Belousov aka BeS
 * email: belbes122@yandex.ru
 */
#include "ESOINN.h"

#include <fstream>
#include <boost/foreach.hpp>
#include <boost/utility.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/iteration_macros.hpp>

#define E1(t) 1./t
#define E2(t) 1./(100*t)

using namespace soinn;
using namespace boost::numeric;

ESOINN::ESOINN(int dim, int ageMax, int iterationThreshold, double c1, double c2):
    dim(dim),
    ageMax(ageMax),
    iterationThreshold(iterationThreshold),
    c1(c1),
    c2(c2)
{
}

ESOINN::~ESOINN()
{
}

Graph ESOINN::getGraph()
{
    return graph;
}

void ESOINN::process(const boost::numeric::ublas::vector<double> &inputSignal)
{
    if(inputSignal.size() != dim)
    {
        throw ESOINNException(std::string("Incorrect dimension of input signal in ESOINN::addSignal()."));
    }
    else
    {
        addSignal(inputSignal);
    }
}

void ESOINN::addSignal(const boost::numeric::ublas::vector<double> &inputSignal)
{
    if(boost::num_vertices(graph) < 2)
    {
        Vertex vertex = boost::add_vertex(graph);
        graph[vertex].weight = ublas::vector<double>(inputSignal);
        graph[vertex].classId = -1;
        graph[vertex].density = 0.;
        graph[vertex].numberOfSignals = 0;
        graph[vertex].S = 0;
        return;
    }
    Vertex firstWinner, secondWinner;
    boost::tie(firstWinner, secondWinner) = findWinners(inputSignal);
    if(!isWithinThreshold(inputSignal, firstWinner, secondWinner))
    {
        Vertex vertex = boost::add_vertex(graph);
        graph[vertex].weight = ublas::vector<double>(inputSignal);
        graph[vertex].classId = -1;
        graph[vertex].density = 0.;
        graph[vertex].numberOfSignals = 0;
        graph[vertex].S = 0;
        return;
    }
    incrementEdgesAge(firstWinner);
    if(needAddEdge(firstWinner, secondWinner))
    {
        Edge e = boost::add_edge(firstWinner, secondWinner, graph).first;
        graph[e].age = 0;
    }
    else
    {
        boost::remove_edge(firstWinner, secondWinner, graph);
    }
    updateDensity(firstWinner);
    updateWeights(firstWinner, inputSignal);
    deleteOldEdges();
    if(iterationCount % iterationThreshold == 0)
    {
        updateClassLabels();
    }
    iterationCount++;
}

double ESOINN::distance(const boost::numeric::ublas::vector<double> &x, const boost::numeric::ublas::vector<double> &y)
{
    return ublas::norm_2( x - y );
}

std::pair<Vertex,Vertex> ESOINN::findWinners(const boost::numeric::ublas::vector<double> &inputSignal)
{
    Vertex firstWinner = NULL;
    Vertex secondWinner = NULL;
    double firstWinnerDistance = std::numeric_limits<double>::max();
    double secondWinnerDistance = std::numeric_limits<double>::max();
    VertexIterator current, end;
    boost::tie(current, end) = boost::vertices(graph);
    for(; current != end; current++)
    {
        double dist = distance(inputSignal, graph[*current].weight);
        if(dist < firstWinnerDistance)
        {
            secondWinner = firstWinner;
            secondWinnerDistance = firstWinnerDistance;
            firstWinner = *current;
            firstWinnerDistance = dist;
        }
        else if(dist < secondWinnerDistance)
        {
            secondWinner = *current;
            secondWinnerDistance = dist;
        }
    }
    return std::pair<Vertex,Vertex>(firstWinner, secondWinner);
}

bool ESOINN::isWithinThreshold(const boost::numeric::ublas::vector<double>& inputSignal, Vertex& firstWinner, Vertex& secondWinner)
{
    if(distance(inputSignal, graph[firstWinner].weight) > getSimilarityThreshold(firstWinner))
    {
        return false;
    }
    if(distance(inputSignal, graph[secondWinner].weight) > getSimilarityThreshold(secondWinner))
    {
        return false;
    }
    return true;
}

double ESOINN::getSimilarityThreshold(const Vertex& vertex)
{
    double dist = 0.0;
    if(!boost::out_degree(vertex, graph))
    {
        dist = std::numeric_limits<double>::max();
        VertexIterator current, end;
        boost::tie(current, end) = boost::vertices(graph);
        for(; current != end; current++)
        {
            if(*current != vertex)
            {
                double distCurrent = distance(graph[vertex].weight, graph[*current].weight);
                if(distCurrent < dist)
                {
                    dist = distCurrent;
                }
            }
        }
    }
    else
    {
        dist = std::numeric_limits<double>::min();
        AdjacencyIterator current, end;
        boost::tie(current, end) = boost::adjacent_vertices(vertex, graph);
        for(; current != end; current++)
        {
            double distCurrent = distance(graph[vertex].weight, graph[*current].weight);
            if(distCurrent > dist)
            {
                dist = distCurrent;
            }
        }
    }
    return dist;
}

void ESOINN::incrementEdgesAge(Vertex& vertex)
{
    OutEdgeIterator current, end;
    boost::tie(current, end) = boost::out_edges(vertex, graph);
    for(; current != end; current++)
    {
        graph[*current].age++;
    }
}

bool ESOINN::needAddEdge(Vertex& firstWinner, Vertex &secondWinner)
{
    if(graph[firstWinner].classId == -1 || graph[secondWinner].classId == -1)
    {
        return true;
    }
    else if(graph[firstWinner].classId == graph[secondWinner].classId)
    {
        return true;
    }
    else if(graph[firstWinner].classId != graph[secondWinner].classId && needMergeClasses(firstWinner, secondWinner))
    {
        return true;
    }
    return false;
}

bool ESOINN::needMergeClasses(Vertex &a, Vertex &b)
{
    int A = graph[a].classId;
    double meanA = meanDensity(A);
    double maxA = maxDensity(A);
    double thresholdA = densityThershold(meanA, maxA);
    int B = graph[b].classId;
    double meanB = meanDensity(B);
    double maxB = maxDensity(B);
    double thresholdB = densityThershold(meanB, maxB);
    double minAB = std::min(graph[a].density, graph[b].density);
    if(minAB > thresholdA * maxA && minAB > thresholdB * maxB)
    {
        return true;
    }
    return false;
}

void ESOINN::mergeClasses(int A, int B)
{
    int classId = std::min(A, B);
    VertexIterator current, end;
    boost::tie(current, end) = boost::vertices(graph);
    for(; current != end; current++)
    {
        if(graph[*current].classId == A || graph[*current].classId == B)
        {
            graph[*current].classId = classId;
        }
    }
}

double ESOINN::meanDensity(int classId)
{
    if(classId == -1) return 0.0;
    int n = 0;
    double density = 0.0;
    VertexIterator current, end;
    boost::tie(current, end) = boost::vertices(graph);
    for(; current != end; current++)
    {
        if(graph[*current].classId == classId)
        {
            n++;
            density += graph[*current].density;
        }
    }
    density *= 1./double(n);
    return density;
}

double ESOINN::maxDensity(int classId)
{
    double density = std::numeric_limits<double>::min();
    VertexIterator current, end;
    boost::tie(current, end) = boost::vertices(graph);
    for(; current != end; current++)
    {
        if(graph[*current].density > density && graph[*current].classId == classId)
        {
            density = graph[*current].density;
        }
    }
    return density;
}

double ESOINN::densityThershold(double mean, double max)
{
    double threshold;
    if(2.0 * mean >= max)
    {
        threshold = 0.0;
    }
    else if(3.0 * mean >= max && max > 2.0 * mean)
    {
        threshold = 0.5;
    }
    else
    {
        threshold = 1.0;
    }
    return threshold;
}

void ESOINN::updateDensity(Vertex& vertex)
{
    double mDistance = meanDistance(vertex);
    graph[vertex].numberOfSignals++;
    graph[vertex].S += 1./((1 + mDistance)*(1 + mDistance));
    graph[vertex].density = graph[vertex].S/double(graph[vertex].numberOfSignals);
}

void ESOINN::updateWeights(Vertex& firstWinner, const boost::numeric::ublas::vector<double> &inputSignal)
{
    graph[firstWinner].weight += E1(graph[firstWinner].numberOfSignals) * (inputSignal - graph[firstWinner].weight);
    AdjacencyIterator current, end;
    boost::tie(current, end) = boost::adjacent_vertices(firstWinner, graph);
    for(; current != end; current++)
    {
        graph[*current].weight += E2(graph[firstWinner].numberOfSignals) * (inputSignal - graph[*current].weight);
    }
}

double ESOINN::meanDistance(Vertex& vertex)
{
    double mDistance = 0.0;
    int m = 0;
    VertexIterator current, end;
    boost::tie(current, end) = boost::vertices(graph);
    for(; current != end; current++)
    {
        if(graph[vertex].classId == graph[*current].classId)
        {
            mDistance += distance(graph[vertex].weight, graph[*current].weight);
            m++;
        }
    }
    mDistance *= 1./double(m);
    return mDistance;
}

void ESOINN::deleteOldEdges()
{
    EdgeIterator current, end;
    boost::tie(current, end) = boost::edges(graph);
    EdgeIterator next = current;
    for(;current != end; current = next)
    {
        next ++;
        if(graph[*current].age > ageMax)
        {
            Vertex vertexS = boost::source(*current, graph);
            Vertex vertexT = boost::target(*current, graph);
            boost::remove_edge(*current, graph);
        }
    }
}

void ESOINN::updateClassLabels()
{
    markClasses();
    partitionClasses();
    deleteNoiseVertex();
}

void ESOINN::markClasses()
{
    std::list<VertexIterator> vertexList;
    VertexIterator begin, end;
    boost::tie(begin, end) = boost::vertices(graph);
    for(VertexIterator current = begin; current != end; current++)
    {
        graph[*current].classId = -1;
        vertexList.push_back(current);
    }
    vertexList.sort([&](VertexIterator &a, VertexIterator &b) -> bool
                    {
                        if(graph[*a].density > graph[*b].density) return true;
                        return false;
                    });
    int classCount = 0;
    for(std::list<VertexIterator>::iterator current = vertexList.begin(); current != vertexList.end(); current++)
    {
        if(graph[**current].classId == -1)
        {
            graph[**current].classId = classCount;
            markAdjacentVertices(**current, classCount++);
        }
    }
}

void ESOINN::partitionClasses()
{
    EdgeIterator current, end;
    boost::tie(current, end) = boost::edges(graph);
    EdgeIterator next = current;
    for(;current != end; current = next) {
        next ++;
        Vertex vertexS = boost::source(*current, graph);
        Vertex vertexT = boost::target(*current, graph);
        if(graph[vertexS].classId != graph[vertexT].classId)
        {
            if(needMergeClasses(vertexS, vertexT))
            {
                mergeClasses(graph[vertexS].classId, graph[vertexT].classId);
            }
            else
            {
                boost::remove_edge(*current, graph);
            }
        }
    }
}

void ESOINN::markAdjacentVertices(Vertex &vertex, int cID)
{
    AdjacencyIterator current, end;
    boost::tie(current, end) = boost::adjacent_vertices(vertex, graph);
    for(; current != end; current++){
        if(graph[*current].classId == -1 && graph[*current].density < graph[vertex].density)
        {
            graph[*current].classId = cID;
            Vertex v = *current;
            markAdjacentVertices(v, cID);
        }
    }
}

void ESOINN::deleteNoiseVertex()
{
    VertexIterator begin, end;
    boost::tie(begin, end) = boost::vertices(graph);
    VertexIterator next = begin;
    for(VertexIterator current = begin; current != end; current = next)
    {
        next++;
        double mean = meanDensity(graph[*current].classId);
        if((boost::out_degree(*current, graph) == 2 && graph[*current].density < c1* mean) ||
                (boost::out_degree(*current, graph) == 1 && graph[*current].density < c2* mean) ||
                (boost::out_degree(*current, graph) == 0)) {
            boost::clear_vertex(*current, graph);
            boost::remove_vertex(*current, graph);
        }
    }
}

void ESOINN::classify()
{
    deleteNoiseVertex();
    size_t index = 0;
    BGL_FORALL_VERTICES(v, graph, Graph)
    {
        boost::put(boost::vertex_index, graph, v, index++);
    }
    ComponentMap component;
    boost::associative_property_map<ComponentMap> componentMap(component);
    numberOfClasses = connected_components(graph, componentMap);
    BGL_FORALL_VERTICES(v, graph, Graph)
    {
        graph[v].classId = boost::get(componentMap, v);
    }
}

void ESOINN::save(std::string filename)
{
    std::ofstream ofs(filename.c_str());
    boost::archive::xml_oarchive oa(ofs);
    oa << BOOST_SERIALIZATION_NVP(*this);
}

void ESOINN::load(std::string filename)
{
    clear();
    std::ifstream ifs(filename.c_str());
    boost::archive::xml_iarchive ia(ifs);
    ia >> BOOST_SERIALIZATION_NVP(*const_cast<ESOINN*>(this));
}

void ESOINN::clear()
{
    graph.clear();
    numberOfClasses = 0;
}

void ESOINN::setParams(int dim, int ageMax, int iterationThreshold, double c1, double c2)
{
    this->dim = dim;
    this->ageMax = ageMax;
    this->iterationThreshold = iterationThreshold;
    this->c1 = c1;
    this->c2 = c2;
}

int ESOINN::getNumberOfClasses()
{
    return numberOfClasses;
}

int ESOINN::getNumberOfVertices()
{
    return boost::num_vertices(graph);
}

boost::numeric::ublas::vector<double> ESOINN::getCenterOfCluster(int classId)
{
    double density = -1;
    Vertex center;
    BGL_FORALL_VERTICES(v, graph, Graph) {
        if(graph[v].classId == classId && graph[v].density > density) {
            center = v;
            density = graph[center].density;
        }
    }
    return graph[center].weight;
}

VertexProperties ESOINN::getBestMatch(boost::numeric::ublas::vector<double>& inputSignal)
{
    Vertex firstWinner = NULL;
    double firstWinnerDistance = std::numeric_limits<double>::max();
    VertexIterator current, end;
    boost::tie(current, end) = boost::vertices(graph);
    for(; current != end; current++)
    {
        double dist = distance(inputSignal, graph[*current].weight);
        if(dist < firstWinnerDistance)
        {
            firstWinner = *current;
            firstWinnerDistance = dist;
        }
    }
    return graph[firstWinner];
}


#include <boost/detail/algorithm.hpp>
#include <boost/graph/adjacency_iterator.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/adjacency_list_io.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/adj_list_serialize.hpp>
#include <boost/graph/astar_search.hpp>
#include <boost/graph/bandwidth.hpp>
#include <boost/graph/bc_clustering.hpp>
#include <boost/graph/bellman_ford_shortest_paths.hpp>
#include <boost/graph/betweenness_centrality.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/bipartite.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/boykov_kolmogorov_max_flow.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/bron_kerbosch_all_cliques.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/graph/chrobak_payne_drawing.hpp>
#include <boost/graph/circle_layout.hpp>
#include <boost/graph/closeness_centrality.hpp>
#include <boost/graph/clustering_coefficient.hpp>
#include <boost/graph/compressed_sparse_row_graph.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/graph/copy.hpp>
#include <boost/graph/core_numbers.hpp>
#include <boost/graph/create_condensation_graph.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>
#include <boost/graph/cycle_canceling.hpp>
#include <boost/graph/dag_shortest_paths.hpp>
#include <boost/graph/degree_centrality.hpp>
#include <boost/graph/depth_first_search.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/dijkstra_shortest_paths_no_color_map.hpp>
#include <boost/graph/dimacs.hpp>
#include <boost/graph/directed_graph.hpp>
#include <boost/graph/dll_import_export.hpp>
#include <boost/graph/dominator_tree.hpp>
#include <boost/graph/eccentricity.hpp>
#include <boost/graph/edge_coloring.hpp>
#include <boost/graph/edge_connectivity.hpp>
#include <boost/graph/edge_list.hpp>
#include <boost/graph/edmonds_karp_max_flow.hpp>
#include <boost/graph/erdos_renyi_generator.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/exterior_property.hpp>
#include <boost/graph/filtered_graph.hpp>
#include <boost/graph/find_flow_cost.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>
#include <boost/graph/fruchterman_reingold.hpp>
#include <boost/graph/geodesic_distance.hpp>
#include <boost/graph/graph_archetypes.hpp>
#include <boost/graph/graph_as_tree.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/graphml.hpp>
#include <boost/graph/graph_mutability_traits.hpp>
#include <boost/graph/graph_selectors.hpp>
#include <boost/graph/graph_stats.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/grid_graph.hpp>
#include <boost/graph/gursoy_atun_layout.hpp>
#include <boost/graph/hawick_circuits.hpp>
#include <boost/graph/howard_cycle_ratio.hpp>
#include <boost/graph/incremental_components.hpp>
#include <boost/graph/is_kuratowski_subgraph.hpp>
#include <boost/graph/isomorphism.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/iteration_macros_undef.hpp>
#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/kamada_kawai_spring_layout.hpp>
#include <boost/graph/king_ordering.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <boost/graph/labeled_graph.hpp>
#include <boost/graph/lookup_edge.hpp>
#include <boost/graph/loop_erased_random_walk.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_connected.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/matrix_as_graph.hpp>
#include <boost/graph/max_cardinality_matching.hpp>
#include <boost/graph/maximum_adjacency_search.hpp>
#include <boost/graph/maximum_weighted_matching.hpp>
#include <boost/graph/mcgregor_common_subgraphs.hpp>
#include <boost/graph/mesh_graph_generator.hpp>
#include <boost/graph/metis.hpp>
#include <boost/graph/metric_tsp_approx.hpp>
#include <boost/graph/minimum_degree_ordering.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/named_graph.hpp>
#include <boost/graph/neighbor_bfs.hpp>
#include <boost/graph/numeric_values.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/overloading.hpp>
#include <boost/graph/page_rank.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/planar_face_traversal.hpp>
#include <boost/graph/plod_generator.hpp>
#include <boost/graph/point_traits.hpp>
#include <boost/graph/prim_minimum_spanning_tree.hpp>
#include <boost/graph/profile.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>
#include <boost/graph/random.hpp>
#include <boost/graph/random_layout.hpp>
#include <boost/graph/random_spanning_tree.hpp>
#include <boost/graph/r_c_shortest_paths.hpp>
#include <boost/graph/read_dimacs.hpp>
#include <boost/graph/relax.hpp>
#include <boost/graph/reverse_graph.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/graph/sequential_vertex_coloring.hpp>
#include <boost/graph/simple_point.hpp>
#include <boost/graph/sloan_ordering.hpp>
#include <boost/graph/smallest_last_ordering.hpp>
#include <boost/graph/small_world_generator.hpp>
#include <boost/graph/ssca_graph_generator.hpp>
#include <boost/graph/st_connected.hpp>
#include <boost/graph/stoer_wagner_min_cut.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/subgraph.hpp>
#include <boost/graph/successive_shortest_path_nonnegative_weights.hpp>
#include <boost/graph/tiernan_all_cycles.hpp>
#include <boost/graph/topological_sort.hpp>
#include <boost/graph/topology.hpp>
#include <boost/graph/transitive_closure.hpp>
#include <boost/graph/transitive_reduction.hpp>
#include <boost/graph/transpose_graph.hpp>
#include <boost/graph/tree_traits.hpp>
#include <boost/graph/two_bit_color_map.hpp>
#include <boost/graph/two_graphs_common_spanning_trees.hpp>
#include <boost/graph/undirected_dfs.hpp>
#include <boost/graph/undirected_graph.hpp>
#include <boost/graph/use_mpi.hpp>
#include <boost/graph/vector_as_graph.hpp>
#include <boost/graph/vertex_and_edge_range.hpp>
#include <boost/graph/vf2_sub_graph_iso.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/wavefront.hpp>
#include <boost/graph/write_dimacs.hpp>
#include <boost/pending/bucket_sorter.hpp>
#include <boost/pending/container_traits.hpp>
#include <boost/pending/disjoint_sets.hpp>
#include <boost/pending/fenced_priority_queue.hpp>
#include <boost/pending/fibonacci_heap.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/pending/is_heap.hpp>
#include <boost/pending/mutable_heap.hpp>
#include <boost/pending/mutable_queue.hpp>
#include <boost/pending/property.hpp>
#include <boost/pending/property_serialize.hpp>
#include <boost/pending/queue.hpp>
#include <boost/pending/stringtok.hpp>

#include <sstream>

using namespace std;
using namespace boost;

int
main ()
{
  // This code uses the graphviz parser which is one of the few compiled parts
  // of Boost.Graph.
  //

  // Vertex properties.
  //
  typedef property<vertex_name_t, string, property<vertex_color_t, float>>
      vertex_p;

  // Edge properties.
  //
  typedef property<edge_weight_t, double> edge_p;

  // Graph properties.
  //
  typedef property<graph_name_t, string> graph_p;

  // adjacency_list-based type.
  //
  typedef adjacency_list<vecS, vecS, directedS, vertex_p, edge_p, graph_p>
      graph_t;

  // Construct an empty graph and prepare the dynamic_property_maps.
  //
  graph_t graph (0);
  dynamic_properties dp;

  property_map<graph_t, vertex_name_t>::type name (get (vertex_name, graph));
  dp.property ("node_id", name);

  property_map<graph_t, vertex_color_t>::type mass (get (vertex_color, graph));
  dp.property ("mass", mass);

  property_map<graph_t, edge_weight_t>::type weight (get (edge_weight, graph));
  dp.property ("weight", weight);

  // Use ref_property_map to turn a graph property into a property map.
  //
  boost::ref_property_map<graph_t*, string> gname (
      get_property (graph, graph_name));
  dp.property ("name", gname);

  // Sample graph as an istream.
  //
  istringstream gvgraph (
      "digraph { graph [name=\"graphname\"]  a  c e [mass = 6.66] }");

  return read_graphviz (gvgraph, graph, dp, "node_id") ? 0 : 1;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2008 Roland Lichters
 Copyright (C) 2009, 2014 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

#include <boost/make_shared.hpp>

#include <ql/experimental/credit/basket.hpp>
#include <ql/experimental/credit/loss.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <ql/experimental/credit/defaultlossmodel.hpp>

using namespace std;

namespace QuantLib {

    Basket::Basket(const Date& refDate,
        const vector<string>& names,
        const vector<Real>& notionals,
        const boost::shared_ptr<Pool> pool,
        Real attachment,
        Real detachment,
        const boost::shared_ptr<Claim>& claim
        )
    : notionals_(notionals),
      pool_(pool),
      claim_(claim),
      attachmentRatio_(attachment),
      detachmentRatio_(detachment),
      basketNotional_(0.0),
      attachmentAmount_(0.0),
      detachmentAmount_(0.0),
      trancheNotional_(0.0),
      refDate_(refDate)
    {
        QL_REQUIRE(!notionals_.empty(), "notionals empty");
        QL_REQUIRE (attachmentRatio_ >= 0 &&
                    attachmentRatio_ <= detachmentRatio_ &&
                    detachmentRatio_ <= 1,
                    "invalid attachment/detachment ratio");
        QL_REQUIRE(pool_, "Empty pool pointer.");
        QL_REQUIRE(notionals_.size() == pool_->size(),
                   "unmatched data entry sizes in basket");

        // registrations relevant to the loss status, not to the expected
        // loss values; those are through models.
        registerWith(Settings::instance().evaluationDate());
        registerWith(claim_);

        computeBasket();

        // At this point Issuers in the pool might or might not have
        //   probability term structures for the defultKeys(eventType+
        //   currency+seniority) entering in this basket. This is not
        //   neccessarily a problem.
        for (Size i = 0; i < notionals_.size(); i++) {
            basketNotional_ += notionals_[i];
            attachmentAmount_ += notionals_[i] * attachmentRatio_;
            detachmentAmount_ += notionals_[i] * detachmentRatio_;
        }
        trancheNotional_ = detachmentAmount_ - attachmentAmount_;
    }

    /*\todo Alternatively send a relinkable handle so it can be changed from
    the outside. In that case reconsider the observability chain.
    */
    void Basket::setLossModel(
        const boost::shared_ptr<DefaultLossModel>& lossModel) {

        if (lossModel_)
            unregisterWith(lossModel_);
        lossModel_ = lossModel;
        if (lossModel_) {
            //recovery quotes, defaults(once Issuer is observable)etc might
            //  trigger us:
            registerWith(lossModel_);
        }
        LazyObject::update(); //<- just set calc=false
    }

    void Basket::performCalculations() const {
        // Calculations for status
        computeBasket();// or we might be called from an statistic member
                        // without being intialized yet (first called)
        QL_REQUIRE(lossModel_, "Basket has no default loss model assigned.");

        /* The model must notify us if the another basket calls it for
        reasignment. The basket works as an argument to the deafult loss models
        so, even if the models dont cache anything, they will be using the wrong
        defautl TS. \todo: This has a possible optimization: the basket
        incorporates trancheability and many models do their compuations
        independently of that (some do but do it inefficiently when asked for
        two tranches on the same basket; e,g, recursive model) so it might be
        more efficient sending the pool only; however the modtionals and other
        basket info are still used.*/
        lossModel_->setBasket(const_cast<Basket*>(this));
    }

    Disposable<vector<Real> > Basket::probabilities(const Date& d) const {
        vector<Real> prob(size());
        vector<DefaultProbKey> defKeys = defaultKeys();
        for (Size j = 0; j < size(); j++)
            prob[j] = pool_->get(pool_->names()[j]).defaultProbability(
                defKeys[j])->defaultProbability(d);
        return prob;
    }

    Real Basket::cumulatedLoss(const Date& endDate) const {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");
        Real loss = 0.0;
        for (Size i = 0; i < size(); i++) {
            boost::shared_ptr<DefaultEvent> credEvent =
                pool_->get(pool_->names()[i]).defaultedBetween(refDate_,
                    endDate, pool_->defaultKeys()[i]);
            if (credEvent) {
                /* \todo If the event has not settled one would need to
                introduce some model recovery rate (independently of a loss
                model) This remains to be done.
                */
                if(credEvent->hasSettled())
                    loss += claim_->amount(credEvent->date(),
                            // notionals_[i],
                            exposure(pool_->names()[i], credEvent->date()),
                            credEvent->settlement().recoveryRate(
                                pool_->defaultKeys()[i].seniority()));
            }
        }
        return loss;
    }

    Real Basket::settledLoss(const Date& endDate) const {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        Real loss = 0.0;
        for (Size i = 0; i < size(); i++) {
            boost::shared_ptr<DefaultEvent> credEvent =
                pool_->get(pool_->names()[i]).defaultedBetween(refDate_,
                    endDate, pool_->defaultKeys()[i]);
            if (credEvent) {
                if(credEvent->hasSettled()) {
                    loss += claim_->amount(credEvent->date(),
                            //notionals_[i],
                            exposure(pool_->names()[i], credEvent->date()),
                            //NOtice I am requesting an exposure in the past...
                            /* also the seniority does not belong to the
                            counterparty anymore but to the position.....*/
                            credEvent->settlement().recoveryRate(
                                pool_->defaultKeys()[i].seniority()));
                }
            }
        }
        return loss;
    }

    Real Basket::remainingNotional() const {
        return evalDateRemainingNot_;
    }

    Disposable<std::vector<Size> >
        Basket::liveList(const Date& endDate) const {
        std::vector<Size> calcBufferLiveList;
        for (Size i = 0; i < size(); i++)
            if (!pool_->get(pool_->names()[i]).defaultedBetween(
                    refDate_,
                    endDate,
                    pool_->defaultKeys()[i]))
                calcBufferLiveList.push_back(i);

        return calcBufferLiveList;
    }

    Real Basket::remainingNotional(const Date& endDate) const {
        Real notional = 0;
        vector<DefaultProbKey> defKeys = defaultKeys();
        for (Size i = 0; i < size(); i++) {
            if (!pool_->get(pool_->names()[i]).defaultedBetween(refDate_,
                                                        endDate,
                                                        defKeys[i]))
                notional += notionals_[i];
        }
        return notional;
    }

    Disposable<vector<Real> >
        Basket::remainingNotionals(const Date& endDate) const
    {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        std::vector<Real> calcBufferNotionals;
        const std::vector<Size>& alive = liveList(endDate);
        for(Size i=0; i<alive.size(); i++)
            calcBufferNotionals.push_back(
                exposure(pool_->names()[i], endDate)
                );// some better way to trim it?
        return calcBufferNotionals;
    }

    Disposable<std::vector<Probability> >
        Basket::remainingProbabilities(const Date& d) const
    {
        QL_REQUIRE(d >= refDate_, "Target date lies before basket inception");
        vector<Real> prob;
        const std::vector<Size>& alive = liveList();

        for(Size i=0; i<alive.size(); i++)
            prob.push_back(pool_->get(pool_->names()[i]).defaultProbability(
                pool_->defaultKeys()[i])->defaultProbability(d, true));
        return prob;
    }

    /* It is supossed to return the addition of ALL notionals from the
    requested ctpty......*/
    Real Basket::exposure(const std::string& name, const Date& d) const {
        //'this->names_' contains duplicates, contrary to 'pool->names'
        std::vector<std::string>::const_iterator match =
            std::find(pool_->names().begin(), pool_->names().end(), name);
        QL_REQUIRE(match != pool_->names().end(), "Name not in basket.");
        Real totalNotional = 0.;
        do{
            totalNotional +=
             // NOT IMPLEMENTED YET:
    //positions_[std::distance(names_.begin(), match)]->expectedExposure(d);
                notionals_[std::distance(pool_->names().begin(), match)];
            match++;
            match = std::find(match, pool_->names().end(), name);
        }while(match != pool_->names().end());

        return totalNotional;
        //Size position = std::distance(poolNames.begin(),
        //    std::find(poolNames.begin(), poolNames.end(), name));
        //QL_REQUIRE(position < pool_->size(), "Name not in pool list");

        //return positions_[position]->expectedExposure(d);
    }

    Disposable<std::vector<std::string> >
        Basket::remainingNames(const Date& endDate) const
    {
        // maybe return zero directly instead?:
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        const std::vector<Size>& alive = liveList(endDate);
        std::vector<std::string> calcBufferNames;
        for(Size i=0; i<alive.size(); i++)
            calcBufferNames.push_back(pool_->names()[alive[i]]);
        return calcBufferNames;
    }

    Disposable<vector<DefaultProbKey> >
        Basket::remainingDefaultKeys(const Date& endDate) const
    {
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");

        const std::vector<Size>& alive = liveList(endDate);
        vector<DefaultProbKey> defKeys;
        for(Size i=0; i<alive.size(); i++)
            defKeys.push_back(pool_->defaultKeys()[alive[i]]);
        return defKeys;
    }

    Size Basket::remainingSize() const {
        return evalDateLiveList_.size();
    }

    /* computed on the inception values, notice the positions might have
    amortized or changed in value and the total outstanding notional might
    differ from the inception one.*/
    Real Basket::remainingDetachmentAmount(const Date& endDate) const {
        return detachmentAmount_;
    }

    Real Basket::remainingAttachmentAmount(const Date& endDate) const {
        // maybe return zero directly instead?:
        QL_REQUIRE(endDate >= refDate_,
            "Target date lies before basket inception");
        Real loss = settledLoss(endDate);
        return std::min(detachmentAmount_, attachmentAmount_ +
            std::max(0.0, loss - attachmentAmount_));
    }

    Probability Basket::probOverLoss(const Date& d, Real lossFraction) const {
        // convert initial basket fraction to remaining basket fraction
        calculate();
        // if eaten up all the tranche the prob of losing any amount is 1
        //  (we have already lost it)
        if(evalDateRemainingNot_ == 0.) return 1.;

        // Turn to live (remaining) tranche units to feed into the model request
        Real xPtfl = attachmentAmount_ +
            (detachmentAmount_-attachmentAmount_)*lossFraction;
        Real xPrim = (xPtfl- evalDateAttachAmount_)/
            (detachmentAmount_-evalDateAttachAmount_);
        // in live tranche fractional units
        // if the level falls within realized losses the prob is 1.
        if(xPtfl < 0.) return 1.;

        return lossModel_->probOverLoss(d, xPrim);
    }

    Real Basket::percentile(const Date& d, Probability prob) const {
        calculate();
        return lossModel_->percentile(d, prob);

        Real percLiveFract = lossModel_->percentile(d, prob);
        return (percLiveFract*(detachmentAmount_-evalDateAttachAmount_)
            + attachmentAmount_ - evalDateAttachAmount_)
            /(detachmentAmount_-attachmentAmount_);
    }

    Real Basket::expectedTrancheLoss(const Date& d) const {
        calculate();
        return cumulatedLoss() + lossModel_->expectedTrancheLoss(d);
    }

    Disposable<std::vector<Real> >
        Basket::splitVaRLevel(const Date& date, Real loss) const {
        calculate();
        return lossModel_->splitVaRLevel(date, loss);
    }

    Real Basket::expectedShortfall(const Date& d, Probability prob) const {
        calculate();
        return lossModel_->expectedShortfall(d, prob);
    }

    Disposable<std::map<Real, Probability> >
        Basket::lossDistribution(const Date& d) const {
        calculate();
        return lossModel_->lossDistribution(d);
    }

    std::vector<Probability>
        Basket::probsBeingNthEvent(Size n, const Date& d) const {

        Size alreadyDefaulted = pool_->size() - remainingNames().size();
        if(alreadyDefaulted >=n)
            return std::vector<Probability>(remainingNames().size(), 0.);

        calculate();
        return lossModel_->probsBeingNthEvent(n-alreadyDefaulted, d);
    }

    Real Basket::defaultCorrelation(const Date& d, Size iName, Size jName) const{
        calculate();
        return lossModel_->defaultCorrelation(d, iName, jName);

    }

    /*! Returns the probaility of having a given or larger number of
    defaults in the basket portfolio at a given time.
    */
    Probability Basket::probAtLeastNEvents(Size n, const Date& d) const{
        calculate();
        return lossModel_->probAtLeastNEvents(n, d);

    }

    Real Basket::recoveryRate(const Date& d, Size iName) const {
        calculate();
        return
            lossModel_->expectedRecovery(d, iName, pool_->defaultKeys()[iName]);
    }

}


#include <string>
#include <array>
#include <vector>
#include <set>
#include <map>
#include <queue>
#include <stack>
#include <sstream>
#include <cstdint>
#include <ostream>
#include <bitset>
#include <algorithm>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "Action.hpp"
#include "UnionFind.hpp"
#include "DirectedGraph.hpp"
#include "Partition.hpp"

#ifndef STRATEGY_N3M5_HPP
#define STRATEGY_N3M5_HPP

class StrategyN3M5;

typedef std::bitset<5> HistoM5;

class StateN3M5 {
 public:
  StateN3M5(const HistoM5 &a_histo, const HistoM5 &b_histo, const HistoM5 &c_histo) :
      ha(a_histo), hb(b_histo), hc(c_histo) {};
  StateN3M5(const std::string &str) { // format "ccccc_dddcc_cdccd" (a0,a1,a2,...c3,c4) last action comes first
    assert(str.size() == 17 && str[5] == '_' && str[11] == '_');
    std::string sa = str.substr(0, 5), sb = str.substr(6, 5), sc = str.substr(12, 5);
    auto s_to_binary = [](std::string s)->HistoM5 {
      std::reverse(s.begin(), s.end());
      std::replace(s.begin(), s.end(), 'c', '0');
      std::replace(s.begin(), s.end(), 'd', '1');
      return HistoM5(s);
    };
    ha = s_to_binary(sa); hb = s_to_binary(sb); hc = s_to_binary(sc);
  }
  StateN3M5(uint64_t i) { // (c4,c3,c2,....,a1,a0) the lowest bit is a0
    uint64_t mask = 31ull;
    ha = HistoM5(i & mask); hb = HistoM5((i>>5ul)&mask); hc = HistoM5((i>>10)&mask);
  }
  HistoM5 ha, hb, hc;

  bool operator==(const StateN3M5 &rhs) const {
    return (ha == rhs.ha && hb == rhs.hb && hc == rhs.hc);
  }
  friend std::ostream &operator<<(std::ostream &os, const StateN3M5 &s) {
    auto to_s = [](const HistoM5 &h)->std::string {
      std::string s = h.to_string();
      std::reverse(s.begin(), s.end());
      std::replace(s.begin(), s.end(), '0', 'c');
      std::replace(s.begin(), s.end(), '1', 'd');
      return s;
    };
    os << to_s(s.ha) << '_' << to_s(s.hb) << '_' << to_s(s.hc);
    return os;
  };

  StateN3M5 NextState(Action act_a, Action act_b, Action act_c) const {
    HistoM5 nha = ha << 1;
    HistoM5 nhb = hb << 1;
    HistoM5 nhc = hc << 1;
    if (act_a == D) { nha.set(0); }
    if (act_b == D) { nhb.set(0); }
    if (act_c == D) { nhc.set(0); }
    return StateN3M5(nha, nhb, nhc);
  };

  std::vector<StateN3M5> PossiblePrevStates() const {
    std::vector<StateN3M5> ans;
    for (size_t i = 0; i < 2; i++) {
      for (size_t j = 0; j < 2; j++) {
        for (size_t k = 0; k < 2; k++) {
          HistoM5 lha = ha >> 1;
          HistoM5 lhb = hb >> 1;
          HistoM5 lhc = hc >> 1;
          if (i == 1) { lha.set(4); }
          if (j == 1) { lhb.set(4); }
          if (k == 1) { lhc.set(4); }
          ans.emplace_back(lha, lhb, lhc);
        }
      }
    }
    return std::move(ans);
  }

  int RelativePayoff(bool against_B = true) const {
    bool a0 = ha[0];
    bool b0 = against_B ? hb[0] : hc[0];
    if (a0 == false && b0 == true) { return -1; } // C,D
    else if (a0 == true && b0 == false) { return 1; } // D,C
    else if (a0 == b0) { return 0; }  // C,C or D,D
    else {
      assert(false);
      return -10000;
    }
  }

  StateN3M5 StateFromB() const { return StateN3M5(hb, hc, ha); } // state from B's viewpoint
  StateN3M5 StateFromC() const { return StateN3M5(hc, ha, hb); } // state from C's viewpoint

  std::array<StateN3M5, 3> NoisedStates() const {
    HistoM5 ha_n = ha, hb_n = hb, hc_n = hc;
    ha_n ^= HistoM5(1ull);
    hb_n ^= HistoM5(1ull);
    hc_n ^= HistoM5(1ull);
    std::array<StateN3M5, 3> ans = {StateN3M5(ha_n, hb, hc), StateN3M5(ha, hb_n, hc), StateN3M5(ha, hb, hc_n)};
    return ans;
  }

  int NumDiffInT1(const StateN3M5 &other) const {
    const auto b1 = ToBits();
    const auto b2 = other.ToBits();

    std::bitset<15> mask("111101111011110");
    if( (b1 & mask) != (b2 & mask) ) { // inconsistent bit is found
      return -1;
    } else {
      return ((b1 & ~mask) ^ (b2 & ~mask)).count(); // number of different bits
    }
  }

  std::string ToString() const {
    std::ostringstream os;
    os << *this;
    return os.str();
  }

  std::bitset<15> ToBits() const {
    std::bitset<15> bits(0ull);
    bits ^= ha.to_ullong();
    bits ^= (hb.to_ullong() << 5);
    bits ^= (hc.to_ullong() <<10);
    return bits;
  }

  uint64_t ID() const {
    return ToBits().to_ullong();
  }

  bool operator<(const StateN3M5 &rhs) const {
    return (ID() < rhs.ID());
  }
};

class StrategyN3M5 {
 public:
  static const size_t N = 1ull << 15ull; // == 32768
  StrategyN3M5(const std::bitset<N> &actions); // construct a strategy from a list of actions. 0=>c,1=>d
  StrategyN3M5 &operator=(const StrategyN3M5 & rhs) = default;

  std::string ToString() const;
  friend std::ostream &operator<<(std::ostream &os, const StrategyN3M5 &strategy);
  bool operator==(const StrategyN3M5 &rhs) const {
    for (size_t i = 0; i < 64; i++) { if (actions[i] != rhs.actions[i]) return false; }
    return true;
  }

  Action ActionAt(const StateN3M5 &s) const { return actions[s.ID()] ? D : C; }
  bool IsDefensible() const;  // check defensibility. Not computationally feasible.
  bool IsDefensibleDFA() const; // check defensibility using DFA minimization
  // get stationary state. When coplayer is nullptr, it is set to self
  std::array<double, N> StationaryState(double e = 0.0001, const StrategyN3M5 *B = nullptr, const StrategyN3M5 *C = nullptr) const;
  // std::array<double, N> StationaryState2(double e = 0.0001, const StrategyN3M5 *B = nullptr, const StrategyN3M5 *C = nullptr) const;
  // check efficiency. all actions must be fixed
  bool IsEfficient(double e = 0.00001, double th = 0.95) const { return (StationaryState(e)[0] > th); }
  bool IsEfficientTopo() const; // check efficiency using ITG
  bool IsDistinguishable(double e = 0.00001, double th = 0.95) const {
    const StrategyN3M5 allc(std::bitset<N>(0ull));
    return (StationaryState(e, &allc, &allc)[0] < th);
    // return (StationaryState(e, this, &allc)[0] < th);
  };  // check distinguishability against AllC
  bool IsDistinguishableTopo() const; // check distinguishability using the transition graph
  DirectedGraph ITG() const;  // construct g(S,S).
  std::array<uint64_t , StrategyN3M5::N> DestsOfITG() const; // Trace g(S,S) from node i. Destination is stored in i'th element.
  uint64_t NextITGState(const StateN3M5 &s) const; // Trace the intra-transition graph by one step
  std::vector<uint64_t> TraceStates(uint64_t start, const StrategyN3M5 *B = nullptr, const StrategyN3M5 *C = nullptr) const;
  UnionFind MinimizeDFA(bool noisy = false) const; // DFA minimization using brute-force method. Computationally demanding
  Partition MinimizeDFAHopcroft(bool noisy = false) const;

  static StrategyN3M5 AllC();
  static StrategyN3M5 AllD();
  static StrategyN3M5 TFT();
  static StrategyN3M5 WSLS();
  static StrategyN3M5 AON(size_t m);
  static StrategyN3M5 FUSS_m3();
  static StrategyN3M5 CAPRI3();
  static StrategyN3M5 sCAPRI3();
 private:
  std::bitset<N> actions;
  std::vector<StateN3M5> NextPossibleStates(StateN3M5 current) const;
  bool _Equivalent(size_t i, size_t j, UnionFind &uf_0, bool noisy) const;
  typedef std::pair<size_t, int> splitter_t;
  std::array<std::set<size_t>,2> _SplitBySplitter(const Partition &partition, size_t org, const std::set<size_t> &Q, int b, bool noisy) const;
};

#endif //STRATEGY_N3M5_HPP


// All content Copyright (C) 2018 Genomics plc
#include "utils/NeedlemanWunsch.hpp"
//#include <boost/numeric/ublas/io.hpp>
#include <stack>
#include "utils/interval.hpp"

namespace wecall
{
namespace utils
{
    std::vector< NWVariant > NeedlemanWunsch::traceBack() const
    {
        std::stack< Backtrace > currentItems;

        const auto lastRefIndex = m_scoreMatrix.size1() - 1;
        const auto lastAltIndex = m_scoreMatrix.size2() - 1;
        currentItems.push(
            Backtrace{{}, lastRefIndex, lastAltIndex, m_traceMatrix( lastRefIndex, lastAltIndex ), false} );

        std::vector< std::vector< NWVariant > > allVariantCombinations;

        while ( not currentItems.empty() )
        {
            bool deadEnd = false;

            auto current = currentItems.top();
            currentItems.pop();

            while ( ( current.refIndex > 0 or current.altIndex > 0 ) and not deadEnd )
            {
                auto cache = current;
                if ( current.trace == 0 )
                {
                    // Reached the left side or the top
                    if ( current.refIndex and current.previousIsDeletion )
                    {
                        deadEnd = true;
                    }
                    else
                    {
                        if ( current.altIndex != 0 )
                        {
                            // insertion of the rest of the alt sequence
                            const auto alt = m_altString.substr( 0, current.altIndex );
                            current.variants.push_back( NWVariant( 0, 0, alt ) );
                        }
                        if ( current.refIndex != 0 )
                        {
                            // deletion of the rest of ref sequence
                            current.variants.push_back( NWVariant( 0, current.refIndex, "" ) );
                        }
                    }
                    break;
                }
                else if ( current.trace & m_matchBit )
                {
                    // No indel
                    current.trace ^= m_matchBit;
                    current.previousIsDeletion = false;
                    current.altIndex -= 1;
                    current.refIndex -= 1;
                }
                else if ( current.trace & m_snpBit )
                {
                    // SNP
                    current.trace ^= m_snpBit;
                    current.previousIsDeletion = false;
                    current.altIndex -= 1;
                    current.refIndex -= 1;
                    const auto alt = m_altString.substr( current.altIndex, 1 );
                    current.variants.push_back( NWVariant( current.refIndex, current.refIndex + 1, alt ) );
                }
                else if ( current.trace & m_insertSingleBit )
                {
                    // insertion of size 1
                    current.trace ^= m_insertSingleBit;
                    if ( current.previousIsDeletion )
                    {
                        deadEnd = true;
                    }
                    else
                    {
                        current.previousIsDeletion = false;
                        current.altIndex -= 1;
                        const auto alt = m_altString.substr( current.altIndex, 1 );
                        current.variants.push_back( NWVariant( current.refIndex, current.refIndex, alt ) );
                    }
                }
                else if ( current.trace & m_deleteSingleBit )
                {
                    // deletion of size 1
                    current.trace ^= m_deleteSingleBit;
                    current.previousIsDeletion = true;
                    current.refIndex -= 1;
                    current.variants.push_back( NWVariant( current.refIndex, current.refIndex + 1, "" ) );
                }
                else if ( current.trace & m_insertMultiBit )
                {
                    // insertion of size > 1
                    current.trace ^= m_insertMultiBit;
                    if ( current.previousIsDeletion )
                    {
                        deadEnd = true;
                    }
                    else
                    {
                        current.previousIsDeletion = false;
                        auto targetScore = m_scoreMatrix( current.refIndex, current.altIndex );
                        auto startingAltIndex = current.altIndex;

                        for ( std::size_t len = 2; len <= startingAltIndex; ++len )
                        {
                            current.altIndex = startingAltIndex - len;
                            auto actualScore = m_scoreMatrix( current.refIndex, current.altIndex ) +
                                               m_nWPenalties.insertionFunction( current.altIndex, len );

                            if ( targetScore == actualScore )
                            {
                                const auto alt = m_altString.substr( current.altIndex, len );
                                const auto variant = NWVariant( current.refIndex, current.refIndex, alt );
                                const auto newTrace = m_traceMatrix( current.refIndex, current.altIndex );

                                if ( newTrace == 0 )
                                {
                                    // We are at the beginning of the alt sequence, so there can't be any more starting
                                    // points that achieve the required score after this. Add the new variant to the
                                    // current variants and continue.
                                    current.variants.push_back( variant );
                                    break;
                                }
                                else
                                {
                                    // There may be more starting points earlier in the alt sequence, so we save the
                                    // information about starting at this point on the stack rather than continuing with
                                    // it straight away.
                                    auto newVariants = current.variants;
                                    newVariants.push_back( variant );
                                    currentItems.push( Backtrace{newVariants,
                                                                 current.refIndex,
                                                                 current.altIndex,
                                                                 newTrace,
                                                                 current.previousIsDeletion} );
                                }
                            }
                            if ( m_traceMatrix( current.refIndex, current.altIndex ) == 0 )
                            {
                                // If we get here then we got to the beginning of the alt sequence, and it wasn't a
                                // possible starting point for the insertion (if it was then we would have broken out of
                                // the for loop), so we shouldn't continue from the current position. We make this
                                // happen by setting deadEnd to true.
                                deadEnd = true;
                            }
                        }
                    }
                }
                else if ( current.trace & m_deleteMultiBit )
                {
                    // deletion of size > 1
                    current.trace ^= m_deleteMultiBit;
                    current.previousIsDeletion = true;

                    auto targetScore = m_scoreMatrix( current.refIndex, current.altIndex );
                    auto startingRefIndex = current.refIndex;

                    for ( std::size_t len = 2; len <= startingRefIndex; ++len )
                    {
                        current.refIndex = startingRefIndex - len;
                        auto actualScore = m_scoreMatrix( current.refIndex, current.altIndex ) +
                                           m_nWPenalties.deletionFunction( current.refIndex, len );

                        if ( targetScore == actualScore )
                        {
                            const auto variant = NWVariant( current.refIndex, startingRefIndex, "" );
                            const auto newTrace = m_traceMatrix( current.refIndex, current.altIndex );

                            if ( newTrace == 0 )
                            {
                                // We are at the beginning of the ref sequence, so there can't be any more starting
                                // points that achieve the required score after this. Add the new variant to the
                                // current variants and continue.
                                current.variants.push_back( variant );
                                break;
                            }
                            else
                            {
                                // There may be more starting points earlier in the ref sequence, so we save the
                                // information about starting at this point on the stack rather than continuing with
                                // it straight away.
                                auto newVariants = current.variants;
                                newVariants.push_back( variant );
                                currentItems.push( Backtrace{newVariants,
                                                             current.refIndex,
                                                             current.altIndex,
                                                             newTrace,
                                                             current.previousIsDeletion} );
                            }
                        }
                        if ( m_traceMatrix( current.refIndex, current.altIndex ) == 0 )
                        {
                            // If we get here then we got to the beginning of the ref sequence, and it wasn't a
                            // possible starting point for the insertion (if it was then we would have broken out of
                            // the for loop), so we shouldn't continue from the current position. We make this
                            // happen by setting deadEnd to true.
                            deadEnd = true;
                        }
                    }
                }
                if ( current.trace != 0 )
                {
                    cache.trace = current.trace;
                    currentItems.push( cache );
                }
                current.trace = m_traceMatrix( current.refIndex, current.altIndex );
            }
            if ( not deadEnd )
            {
                allVariantCombinations.push_back( current.variants );
            }
        }
        if ( allVariantCombinations.size() > 1 )
        {
            WECALL_LOG( DEBUG, "Warning: while normalizing variants we found "
                                    << allVariantCombinations.size() << " possible normalizations" << std::endl );
        }

        return allVariantCombinations.back();
    }

    int32_t NeedlemanWunsch::getScoreMatrix()
    {
        const std::size_t refDim = m_referenceString.size() + 1;
        const std::size_t altDim = m_altString.size() + 1;

        //        scoreMatrix_t m_scoreMatrix( refDim, altDim );
        //        traceMatrix_t m_traceMatrix( refDim, altDim );
        m_scoreMatrix = scoreMatrix_t( refDim, altDim );
        m_traceMatrix = traceMatrix_t( refDim, altDim );

        m_scoreMatrix( 0, 0 ) = 0;

        // Initialise the first column of the matrix
        //        for ( std::size_t rowIndex = 1; rowIndex < refDim; ++rowIndex )
        //        {
        //            m_scoreMatrix( rowIndex, 0 ) = m_nWPenalties.insertionFunction(0, rowIndex);
        //            m_traceMatrix( rowIndex, 0 ) = 0;
        //        }

        if ( refDim > 1 )
        {
            m_scoreMatrix( 1, 0 ) = m_nWPenalties.insertionOpen();
            m_traceMatrix( 1, 0 ) = 0;
        }

        for ( std::size_t rowIndex = 2; rowIndex < refDim; ++rowIndex )
        {
            m_scoreMatrix( rowIndex, 0 ) = m_scoreMatrix( rowIndex - 1, 0 ) + m_nWPenalties.extendInsertion();
            m_traceMatrix( rowIndex, 0 ) = 0;
        }

        // Initialise the first row of the matrix
        //        for ( std::size_t colIndex = 1; colIndex < refDim; ++colIndex )
        //        {
        //            m_scoreMatrix( 0, colIndex ) = m_nWPenalties.deletionFunction(0, colIndex);
        //            m_traceMatrix( 0, colIndex ) = 0;
        //        }

        if ( altDim > 1 )
        {
            m_scoreMatrix( 0, 1 ) = m_nWPenalties.deletionOpen();
            m_traceMatrix( 0, 1 ) = 0;
        }

        for ( std::size_t colIndex = 2; colIndex < altDim; ++colIndex )
        {
            m_scoreMatrix( 0, colIndex ) = m_scoreMatrix( 0, colIndex - 1 ) + m_nWPenalties.extendDeletion();
            m_traceMatrix( 0, colIndex ) = 0;
        }

        // Create the colScore matrix we will use when filling in the matrix
        // It took me a while to work out why we use these values. When we add
        std::vector< int32_t > colScore( altDim );
        // Note that colScore[0] is never used, so there's no need to initialise it
        //        for ( std::size_t colIndex = 1; colIndex < altDim; ++colIndex )
        //        {
        //            // When we add n*m_nWPenalties.gapLinear to colScore[colIndex] we want to get the score for having
        //            // an insertion of size colIndex followed by a deletion of size n
        //            colScore[colIndex] = m_nWPenalties.insertionFunction(0, colIndex) +
        //                                 m_nWPenalties.deletionFunction(0, 1) - m_nWPenalties.gapLinear;
        //        }
        if ( altDim > 1 )
        {
            colScore[1] = m_nWPenalties.insertionOpen() + m_nWPenalties.deletionOpen() - m_nWPenalties.extendDeletion();
        }

        for ( std::size_t colIndex = 2; colIndex < altDim; ++colIndex )
        {
            colScore[colIndex] = colScore[colIndex - 1] + m_nWPenalties.extendDeletion();
        }

        // Now go through the rest of the matrix, row by row, calculating the correct values
        for ( int32_t rowIndex = 1; rowIndex < static_cast< int32_t >( refDim ); ++rowIndex )
        {
            //            // When we add m_nWPenalties.gapLinear to rowScore we want to get the score for having a
            //            // deletion of size rowIndex followed by an insertion of size 1
            auto rowScore = m_nWPenalties.deletionFunction( 0, rowIndex ) + m_nWPenalties.insertionFunction( 0, 1 ) -
                            m_nWPenalties.extendInsertion();
            //            auto rowScore = 2 * m_nWPenalties.gapConstant + rowIndex * m_nWPenalties.extendDeletion();

            for ( int32_t colIndex = 1; colIndex < static_cast< int32_t >( altDim ); ++colIndex )
            {
                // Calculate score for not having a gap, i.e. sequences match or there is a SNP
                const auto refBase = m_referenceString.at( rowIndex - 1 );
                const auto altBase = m_altString.at( colIndex - 1 );
                const int32_t noGapScore =
                    m_scoreMatrix( rowIndex - 1, colIndex - 1 ) + m_nWPenalties.matchFunction( refBase, altBase );

                const auto insertSingleScore =
                    m_scoreMatrix( rowIndex, colIndex - 1 ) + m_nWPenalties.insertionFunction( colIndex - 1, 1 );
                const auto insertMultiScore = rowScore + m_nWPenalties.extendInsertion();
                rowScore = std::max( insertSingleScore, insertMultiScore );

                const auto deleteSingleScore =
                    m_scoreMatrix( rowIndex - 1, colIndex ) + m_nWPenalties.deletionFunction( rowIndex - 1, 1 );
                const auto deleteMultiScore = colScore[colIndex] + m_nWPenalties.extendDeletion();
                colScore[colIndex] = std::max( deleteSingleScore, deleteMultiScore );

                const auto bestScore =
                    std::max( {noGapScore, insertSingleScore, insertMultiScore, deleteSingleScore, deleteMultiScore} );

                m_scoreMatrix( rowIndex, colIndex ) = bestScore;

                bitType_t trace = 0;

                if ( bestScore == noGapScore )
                {
                    if ( refBase == altBase )
                    {
                        trace |= m_matchBit;
                    }
                    else
                    {
                        trace |= m_snpBit;
                    }
                }
                if ( bestScore == insertSingleScore )
                {
                    trace |= m_insertSingleBit;
                }
                if ( bestScore == deleteSingleScore )
                {
                    trace |= m_deleteSingleBit;
                }
                if ( bestScore == insertMultiScore )
                {
                    trace |= m_insertMultiBit;
                }
                if ( bestScore == deleteMultiScore )
                {
                    trace |= m_deleteMultiBit;
                }

                m_traceMatrix( rowIndex, colIndex ) = trace;
            }
        }

        return m_scoreMatrix( refDim - 1, altDim - 1 );
    }

    std::ostream & operator<<( std::ostream & out, const NWPenalties & nWPenalties )
    {
        out << "NWPenalties(" << nWPenalties.m_baseMatch << ", " << nWPenalties.m_baseMismatch << ", "
            << nWPenalties.m_insertionConstant << ", " << nWPenalties.m_insertionLinear << ", "
            << nWPenalties.m_deletionConstant << ", " << nWPenalties.m_deletionLinear << ", "
            << nWPenalties.m_indelPosition << ")";
        return out;
    }

    std::ostream & operator<<( std::ostream & out, const NWVariant & nWVariant )
    {
        out << "NWVariant(" << nWVariant.m_start << ", " << nWVariant.m_end << ", " << nWVariant.m_alt << ")";
        return out;
    }
}
}


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

#include <nt2/linalg/functions/geev_wvrvl.hpp>
#include <boost/assert.hpp>
#include <nt2/linalg/details/lapack/declare/geev.hpp>
#include <boost/dispatch/attributes.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/scalar/max.hpp>
#include <nt2/include/functions/of_size.hpp>
#include <nt2/include/functions/resize.hpp>
#include <nt2/include/functions/sqr.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/linalg/details/utility/envblocksize.hpp>
#include <nt2/linalg/details/utility/f77_wrapper.hpp>
#include <nt2/linalg/details/utility/workspace.hpp>
#include <nt2/linalg/functions/details/combine.hpp>
#include <nt2/core/container/table/table.hpp>

namespace nt2 { namespace ext
{

  // the routines here compute only the eigenvalues and always in a complex colon vector
  //---------------------------------------------Real-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  double_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                             )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A2&w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvr = n;
      nt2_la_int ldvl = n;
      NT2_F77NAME(dgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0/*wr*/, 0/*wi*/
                         , 0/*vl*/, &ldvl
                         , 0/*vr*/, &ldvr
                         , wk.main(), details::query()
                         , &info);
      info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
    }
  };

  //---------------------------------------------Real-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  single_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvr = n;
      nt2_la_int ldvl = n;
      NT2_F77NAME(sgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0/*wr*/, 0/*wi*/
                         , 0/*vl*/, &ldvl
                         , 0/*vr*/, &ldvr
                         , wk.main(), details::query()
                         , &info);
      info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
     }
  };

//---------------------------------------------Complex-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  complex_<single_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvl = n;
      nt2_la_int ldvr = n;
      NT2_F77NAME(cgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0 /*w*/
                         , 0 /*vl*/, &ldvl
                         , 0 /*vr*/, &ldvr
                         , wk.main(), details::query()
                         , wk.reals()
                         , &info);
     info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
     }
  };

  //---------------------------------------------Complex-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Compute the workspace
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)
                            , ((container_<nt2::tag::table_,  complex_<double_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                           )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info = 0;
      details::workspace<typename A0::value_type> wk;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2_la_int lda  = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int ldvl = n;
      nt2_la_int ldvr = n;
      NT2_F77NAME(zgeev) ( &jobvl, &jobvr
                         , &n
                         , 0 /*a*/, &lda
                         , 0 /*w*/
                         , 0 /*vl*/, &ldvl
                         , 0 /*vr*/, &ldvr
                         , wk.main(), details::query()
                         , wk.reals()
                         , &info);
      info = nt2::geev_wvrvl(a, w, vr, vl, wk);
      return info;
     }
  };

  //---------------------------------------------Real-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  double_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                              (unspecified_<WK>)                                              //workspace
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2::container::table<double> wr(of_size(n, 1)),  wi(of_size(n, 1))
                                  , rvr(of_size(n, n)), rvl(of_size(n, n));
      nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int wn = wk.main_need();
      wk.resize_main(wn);
      nt2_la_int ldvl = rvl.leading_size();
      nt2_la_int ldvr = rvr.leading_size();
      NT2_F77NAME(dgeev) ( &jobvl, &jobvr
                         , &n
                         , a.data() , &lda
                         , wr.data(), wi.data()
                         , rvl.data(), &ldvl
                         , rvr.data(), &ldvr
                         , wk.main(), &wn
                         , &info);
      details::combine_eigens(wr, wi, w);
      details::combine_vects(rvr, wi, vr);
      details::combine_vects(rvl, wi, vl);
      return info;
    }
  };

  //---------------------------------------------Real-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  single_<A0>, S0 >))             //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                               (unspecified_<WK>)                                              //workspace
                            )
  {
    typedef nt2_la_int result_type;

    BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
    {
      char jobvl = 'V';
      char jobvr = 'V';
      result_type info;
      nt2_la_int n = nt2::width(a);
      BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
      nt2::container::table<float> wr(of_size(n, 1)),  wi(of_size(n, 1))
                                 , rvr(of_size(n, n)), rvl(of_size(n, n));
      nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
      nt2_la_int wn = wk.main_need();
      wk.resize_main(wn);
      nt2_la_int ldvl = rvl.leading_size();
      nt2_la_int ldvr = rvr.leading_size();
      NT2_F77NAME(sgeev) ( &jobvl, &jobvr
                         , &n
                         , a.data() , &lda
                         , wr.data(), wi.data()
                         , rvl.data(), &ldvl
                         , rvr.data(), &ldvr
                         , wk.main(), &wn
                         , &info);
      details::combine_eigens(wr, wi, w);
      details::combine_vects(rvr, wi, vr);
      details::combine_vects(rvl, wi, vl);
      return info;
    }
  };

  //---------------------------------------------Complex-single- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  complex_<single_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<single_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<single_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<single_<A3> >, S3 >))  //vl
                              (unspecified_<WK>)                                              //workspace
                            )
  {
     typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
     {
       char jobvl = 'V';
       char jobvr = 'V';
       result_type info;
       nt2_la_int n = nt2::width(a);
       BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
       nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
       vl.resize(of_size(n, n));
       vr.resize(of_size(n, n));
       nt2_la_int ldvl = vl.leading_size();
       nt2_la_int ldvr = vr.leading_size();
       nt2_la_int wn = wk.main_need();
       wk.resize_main(wn);
       wk.resize_reals(2*n);
       w.resize(of_size(n, 1));
       NT2_F77NAME(cgeev) ( &jobvl, &jobvr
                          , &n
                          , a.data() , &lda
                          , w.data()
                          , vl.data(), &ldvl
                          , vr.data(), &ldvr
                          , wk.main(), &wn
                          , wk.reals()
                          , &info);
        return info;
     }
  };

  //---------------------------------------------Complex-double- wvlvr------------------------------------------------//
  /// INTERNAL ONLY - Workspace is ready
  BOOST_DISPATCH_IMPLEMENT  ( geev_wvrvl_, tag::cpu_
                            , (A0)(S0)(A1)(S1)(A2)(S2)(A3)(S3)(WK)
                            , ((container_<nt2::tag::table_,  complex_<double_<A0> >, S0 >))  //a
                              ((container_<nt2::tag::table_,  complex_<double_<A1> >, S1 >))  //w
                              ((container_<nt2::tag::table_,  complex_<double_<A2> >, S2 >))  //vr
                              ((container_<nt2::tag::table_,  complex_<double_<A3> >, S3 >))  //vl
                              (unspecified_<WK>)                                              //workspace
                            )
  {
     typedef nt2_la_int result_type;

     BOOST_FORCEINLINE result_type operator()( A0& a, A1& w, A2& vr, A3& vl, WK& wk) const
     {
       char jobvl = 'V';
       char jobvr = 'V';
       result_type info;
       nt2_la_int n = nt2::width(a);
       BOOST_ASSERT_MSG(n == nt2_la_int(nt2::height(a)), "input must be square");
       nt2_la_int lda = nt2::max(a.leading_size(), One<size_t>());
       vl.resize(of_size(n, n));
       vr.resize(of_size(n, n));
       nt2_la_int ldvl = vl.leading_size();
       nt2_la_int ldvr = vr.leading_size();
       nt2_la_int wn = wk.main_need();
       wk.resize_main(wn);
       wk.resize_reals(2*n);
       w.resize(of_size(n, 1));
       NT2_F77NAME(zgeev) ( &jobvl, &jobvr
                          , &n
                          , a.data() , &lda
                          , w.data()
                          , vl.data(), &ldvl
                          , vr.data(), &ldvr
                          , wk.main(), &wn
                          , wk.reals()
                          , &info);
        return info;
     }
  };
} }

#endif


// Copyright (c) 2013 Vasili Baranau
// Distributed under the MIT software license
// See the accompanying file License.txt or http://opensource.org/licenses/MIT

#include <stdio.h>
#include <boost/numeric/odeint.hpp>
#include "../Headers/ClosestJammingStep.h"
#include "Core/Headers/Exceptions.h"
#include "Core/Headers/Constants.h"
#include "Generation/PackingServices/DistanceServices/Headers/INeighborProvider.h"
#include "Generation/PackingServices/DistanceServices/Headers/IClosestPairProvider.h"
#include "Generation/Model/Headers/Config.h"
#include "Generation/Geometries/Headers/IGeometry.h"

using namespace PackingServices;
using namespace Core;
using namespace Model;
using namespace std;
using namespace boost::numeric::odeint;

namespace PackingGenerators
{
    ClosestJammingStep::ClosestJammingStep(GeometryService* geometryService,
            INeighborProvider* neighborProvider,
            PackingServices::IClosestPairProvider* closestPairProvider,
            MathService* mathService) :
            BasePackingStep(geometryService, neighborProvider, closestPairProvider, mathService),
            closestJammingVelocityProvider(mathService)
    {
        isOuterDiameterChanging = false;
        canOvercomeTheoreticalDensity = true;

        maxTimeStep = -1.0;
        integrationTimeStep = 1e-9;
    }

    ClosestJammingStep::~ClosestJammingStep()
    {

    }

    const FLOAT_TYPE ClosestJammingStep::GetBondThreshold() const
    {
        return bondsProvider.GetBondThreshold();
    }

    void ClosestJammingStep::SetBondThreshold(Core::FLOAT_TYPE value)
    {
        bondsProvider.SetBondThreshold(value);
    }

    void ClosestJammingStep::SetParticles(Packing* particles)
    {
        BasePackingStep::SetParticles(particles);
        ParticlePair closestPair = closestPairProvider->FindClosestPair();
        innerDiameterRatio = sqrt(closestPair.normalizedDistanceSquare);

        bondsProvider.Reset(config->particlesCount);

        particleVelocities.clear();
        particleVelocities.resize(config->particlesCount);

        bondsProvider.UpdateBonds(*neighborProvider, *mathService, *particles, innerDiameterRatio, false);
        startBondsCountForIntegrationTimeStep = bondsProvider.GetBonds().size();
    }

    void ClosestJammingStep::DisplaceParticles()
    {
        initialInnerDiameterRatio = innerDiameterRatio;
        clock_t linearSystemSolutionTime = FillVelocities();
        FLOAT_TYPE timeStep = FindBestMovementTime();

        int odeCyclesCount = 0;
        if (timeStep <= integrationTimeStep)
        {
            MoveParticles(timeStep);
            innerDiameterRatio += timeStep;
        }
        else
        {
            odeCyclesCount = DisplaceParticlesForLongTime();
        }

        // Remove intersections at each iteration
        innerDiameterRatio = sqrt(closestPairProvider->FindClosestPair().normalizedDistanceSquare);

        // Search for new contacts, update bonds and bonds per particle
        BondsProvider::Statistics statistics = bondsProvider.UpdateBonds(*neighborProvider, *mathService, *particles, innerDiameterRatio, false);
        if (statistics.gapsCount > 0)
        {
            printf("BROKEN: broken bonds count is %d, mean gap length is %g\n", statistics.gapsCount, statistics.meanGapLength);
        }
        if (statistics.intersectionsCount > 0)
        {
            printf("INTERSECTIONS: intersections count is %d, mean intersection length is %g\n", statistics.intersectionsCount, statistics.meanIntersectionLength);
        }

        printf("Bonds count: %d; bond pairs count: %d; time step: %g; slae solution time: %g (s); ode cycles count: %d; integration time step: %g\n",
                bondsProvider.GetBonds().size(), bondsProvider.GetBondPairsCount(), timeStep, linearSystemSolutionTime / static_cast<FLOAT_TYPE>(CLOCKS_PER_SEC), odeCyclesCount, integrationTimeStep);

        FixIntersections(statistics);
    }

    // TODO: merge somehow with DoBinarySearchForCollision
    int ClosestJammingStep::DisplaceParticlesForLongTime()
    {
        // OdeObserver is passed by value, that's why need to store reference to odeCyclesCount.
        int odeCyclesCount = 0;
        OdeObserver observer(&odeCyclesCount, this);

        ParticleSystemForODE odeSystem(this);
        vector<FLOAT_TYPE> combinedParticleCoordinates(DIMENSIONS * config->particlesCount);
        UpdateCombinedCoordinates(&combinedParticleCoordinates);

        FLOAT_TYPE maxTime = maxTimeStep > 0 ? innerDiameterRatio + maxTimeStep : MAX_FLOAT_VALUE;

        try
        {
            // Solve ODE to reach the nextCollisionTime
//            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
//            result_of::make_controlled<TStepper>::type stepper = make_controlled(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold(), TStepper());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, maxTime, integrationTimeStep, observer);

//            bulirsch_stoer< vector<FLOAT_TYPE> > stepper(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, maxTime, integrationTimeStep, observer);

            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
            integrate_const(TStepper(), odeSystem, combinedParticleCoordinates, innerDiameterRatio, maxTime, integrationTimeStep, observer);

            // Integration may end normally only if maxTimeStep is specified and reached.
            // Observer is called even for the very last time, therefore coordinates and velocities are correctly updated.
            // In case observer is not called for the last time.
            if (maxTime != MAX_FLOAT_VALUE && maxTime > innerDiameterRatio)
            {
                observer(combinedParticleCoordinates, maxTime);
            }
        }
        catch (const CollisionMissedException& e)
        {
            printf((e.GetMessage() + "\n").c_str());
            DoBinarySearchForCollision(e.nextCollisionTimeEstimate, &observer);
        }
        catch (const CollisionIsNearException& e)
        {
            printf((e.GetMessage() + "\n").c_str());
            // All the velocities are set in the observer and are correct.
            MoveParticles(e.nextCollisionTime - innerDiameterRatio);
            innerDiameterRatio = e.nextCollisionTime;
        }
        return odeCyclesCount;
    }

    void ClosestJammingStep::DoBinarySearchForCollision(FLOAT_TYPE nextCollisionTimeEstimate, OdeObserver* observer)
    {
        printf("DoBinarySearchForCollision; innerDiameterRatio: %17.15g, nextCollisionTimeEstimate: %17.15g\n", innerDiameterRatio, nextCollisionTimeEstimate);
        if ((nextCollisionTimeEstimate - innerDiameterRatio) < integrationTimeStep)
        {
            printf("(nextCollisionTimeEstimate - innerDiameterRatio) < integrationTimeStep\n");
            MoveParticles(nextCollisionTimeEstimate - innerDiameterRatio);
            innerDiameterRatio = nextCollisionTimeEstimate;
            return;
        }

        // Do binary search for collision with support for missing collision detection in the course of integration as well.
        ParticleSystemForODE odeSystem(this);
        vector<FLOAT_TYPE> combinedParticleCoordinates(DIMENSIONS * config->particlesCount);
        UpdateCombinedCoordinates(&combinedParticleCoordinates);

        // Integrate from last valid time (always innerDiameterRatio) to half between valid and invalid times
        FLOAT_TYPE middleTime = (innerDiameterRatio + nextCollisionTimeEstimate) * 0.5;
        try
        {
            // Solve ODE to reach the middleTime
//            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
//            result_of::make_controlled<TStepper>::type stepper = make_controlled(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold(), TStepper());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, middleTime, integrationTimeStep, observer);

//            bulirsch_stoer< vector<FLOAT_TYPE> > stepper(bondsProvider.GetBondThreshold(), bondsProvider.GetBondThreshold());
//            integrate_adaptive(stepper, odeSystem, combinedParticleCoordinates, innerDiameterRatio, middleTime, integrationTimeStep, observer);

            typedef runge_kutta_dopri5< vector<FLOAT_TYPE> > TStepper;
            integrate_const(TStepper(), odeSystem, combinedParticleCoordinates, innerDiameterRatio, middleTime, integrationTimeStep, *observer);

            // Observer was not called for the last time
            if (middleTime > innerDiameterRatio)
            {
                (*observer)(combinedParticleCoordinates, middleTime);
            }

            printf("innerDiameterRatio after successful integration: %17.15g\n", innerDiameterRatio);
            // Integrated successfully. Observer was also called for the last step,
            // so innerDiameterRatio, particle coordinates, bonds, and particle velocities have correct values (innerDiameterRatio = middleTime),
            // collision has not happened in the middle time.
            // Last valid time is the current time, nextCollisionTimeEstimate is the same.
            DoBinarySearchForCollision(nextCollisionTimeEstimate, observer);
        }

        // If integrated not successfully:
        // a. if collision missed, it can not be too close to the last valid time (otherwise the near exception would be fired).
        catch (const CollisionMissedException& e)
        {
            // innerDiameterRatio, particle coordinates, bonds, and particle velocities have correct values (innerDiameterRatio = last time before collision)
            printf((e.GetMessage() + "\n").c_str());
            DoBinarySearchForCollision(e.nextCollisionTimeEstimate, observer);
        }
        // b. if collision is near, move particles
        catch (const CollisionIsNearException& e)
        {
            printf((e.GetMessage() + "\n").c_str());
            // All the velocities are set in the observer and are correct.
            MoveParticles(e.nextCollisionTime - innerDiameterRatio);
            innerDiameterRatio = e.nextCollisionTime;
        }
    }

    void ClosestJammingStep::FixIntersections(BondsProvider::Statistics statistics)
    {
        ParticlePair closestPair = closestPairProvider->FindClosestPair();
        const FLOAT_TYPE tolerance = 1.0 - 5.0 * bondsProvider.GetBondThreshold();
        bool intersectionsExist = closestPair.normalizedDistanceSquare < innerDiameterRatio * innerDiameterRatio * tolerance * tolerance;
        if (intersectionsExist)
        {
            printf("WARNING: min normalized distance %f is lower than inner diameter ratio %f. Particles pair: %d and %d.\n",
                    std::sqrt(closestPair.normalizedDistanceSquare),
                    innerDiameterRatio,
                    closestPair.firstParticleIndex,
                    closestPair.secondParticleIndex);
        }

        bool errorIsLarge =
                statistics.meanGapLength > bondsProvider.GetBondThreshold() * 5.0 ||
                statistics.meanIntersectionLength > bondsProvider.GetBondThreshold() * 5.0 ||
                intersectionsExist;
        if (!errorIsLarge)
        {
            return;
        }

        printf("WARNING: meanGapLength or meanIntersectionLength are too large. Updating innerDiameterRatio and bonds.\n");
        // The last bond is added with errors. The last but one is not, because otherwise bonds would be recalculated.
        int endBondsCountForIntegrationTimeStep = bondsProvider.GetBonds().size() - 1;
        innerDiameterRatio = sqrt(closestPair.normalizedDistanceSquare);
        bondsProvider.UpdateBonds(*neighborProvider, *mathService, *particles, innerDiameterRatio, true);

        UpdateIntegrationTimeStep(endBondsCountForIntegrationTimeStep);
    }

    void ClosestJammingStep::UpdateIntegrationTimeStep(int endBondsCountForIntegrationTimeStep)
    {
        FLOAT_TYPE addedBondsCount = endBondsCountForIntegrationTimeStep - startBondsCountForIntegrationTimeStep;
        bool errorGrowsTooQuickly = addedBondsCount < 10;

        startBondsCountForIntegrationTimeStep = endBondsCountForIntegrationTimeStep;

        // After division by 2 integrationTimeStep should be at least 1e-14, as final particle diameter is about 1
        const FLOAT_TYPE minIntegrationTimeStep = 2e-14;
        bool shouldUpdateIntegrationTimeStep = errorGrowsTooQuickly && integrationTimeStep > minIntegrationTimeStep;
        if (shouldUpdateIntegrationTimeStep)
        {
            integrationTimeStep *= 0.5;
            printf("WARNING: error grows too quickly. Updated integrationTimeStep to %g.\n", integrationTimeStep);
        }

        if (addedBondsCount <= 0 && integrationTimeStep <= minIntegrationTimeStep)
        {
            throw InvalidOperationException("Particles do not grow during integration, integrationTimeStep is too low to be decreased further.");
        }
    }

    clock_t ClosestJammingStep::FillVelocities()
    {
        clock_t solutionTime = closestJammingVelocityProvider.FillVelocities(*neighborProvider, bondsProvider, *particles, innerDiameterRatio, &particleVelocities);
        return solutionTime;
    }

    void ClosestJammingStep::UpdateCombinedCoordinates(vector<FLOAT_TYPE>* combinedParticleCoordinates)
    {
        vector<FLOAT_TYPE>& combinedParticleCoordinatesRef = *combinedParticleCoordinates;
        const Packing& particlesRef = *particles;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            const DomainParticle& particle = particlesRef[particleIndex];
            for (int i = 0; i < DIMENSIONS; ++i)
            {
                combinedParticleCoordinatesRef[particleIndex * DIMENSIONS + i] = particle.coordinates[i];
            }
        }
    }

    void ClosestJammingStep::UpdateCombinedVelocities(vector<FLOAT_TYPE>* combinedVelocities)
    {
        vector<FLOAT_TYPE>& combinedVelocitiesRef = *combinedVelocities;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            const Core::SpatialVector& velocity = particleVelocities[particleIndex];
            for (int i = 0; i < DIMENSIONS; ++i)
            {
                combinedVelocitiesRef[particleIndex * DIMENSIONS + i] = velocity[i];
            }
        }
    }

    void ClosestJammingStep::UpdateParticleCoordinates(const vector<FLOAT_TYPE>& combinedParticleCoordinates)
    {
        Model::Packing& particlesRef = *particles;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            DomainParticle& particle = particlesRef[particleIndex];

            closestPairProvider->StartMove(particleIndex);
            neighborProvider->StartMove(particleIndex);

            for (int i = 0; i < DIMENSIONS; ++i)
            {
                particle.coordinates[i] = combinedParticleCoordinates[particleIndex * DIMENSIONS + i];
            }
            geometry->EnsureBoundaries(particle, &particle, innerDiameterRatio);

            neighborProvider->EndMove();
            closestPairProvider->EndMove();
        }
    }

    void ClosestJammingStep::ProvideODEDerivative(const vector<FLOAT_TYPE>& x, vector<FLOAT_TYPE>* dxdt, const FLOAT_TYPE t)
    {
        // copy x into particles, update innerDiameterRatio
        UpdateParticleCoordinates(x);
//        FLOAT_TYPE previousTime = innerDiameterRatio;
        innerDiameterRatio = t;

        FillVelocities();
        UpdateCombinedVelocities(dxdt);

//        ////////////////
//        // Search for new contacts, update bonds and bonds per particle
//        Nullable<FLOAT_TYPE> meanGapLength = UpdateContacts();
//
//        ParticlePair closestPair = closestPairProvider->FindClosestPair();
//        const FLOAT_TYPE tolerance = 1.0 - bondThreshold;
//        if (closestPair.normalizedDistanceSquare < innerDiameterRatio * innerDiameterRatio * tolerance * tolerance)
//        {
//            printf("ODE: WARNING: min normalized distance %f is lower than inner diameter ratio %f. Particles pair: %d and %d; bond existed: %d\n",
//                    std::sqrt(closestPair.normalizedDistanceSquare),
//                    innerDiameterRatio,
//                    closestPair.firstParticleIndex,
//                    closestPair.secondParticleIndex,
//                    ParticlesShareBond(closestPair.firstParticleIndex, closestPair.secondParticleIndex));
//        }
//
//        printf("ODE: Bonds count: %d; bond pairs count: %d; time step: %g; slae solution time: %g (s); ode delta t: %g\n",
//                bonds.size(), bondPairs.size(), timeStep, linearSystemSolutionTime / static_cast<FLOAT_TYPE>(CLOCKS_PER_SEC), t - previousTime);
//        ////////////////
    }

    FLOAT_TYPE ClosestJammingStep::FindBestMovementTime()
    {
        FLOAT_TYPE movementTime = closestJammingVelocityProvider.FindBestMovementTime();
        if (maxTimeStep > 0 && (movementTime + innerDiameterRatio - initialInnerDiameterRatio > maxTimeStep))
        {
            movementTime = initialInnerDiameterRatio + maxTimeStep - innerDiameterRatio;
        }
        return movementTime;
    }

    void ClosestJammingStep::MoveParticles(FLOAT_TYPE timeStep)
    {
        Model::Packing& particlesRef = *particles;
        for (ParticleIndex particleIndex = 0; particleIndex < config->particlesCount; ++particleIndex)
        {
            DomainParticle& particle = particlesRef[particleIndex];
            if (bondsProvider.GetBondIndexesPerParticle()[particleIndex].size() == 0)
            {
                continue;
            }

            closestPairProvider->StartMove(particleIndex);
            neighborProvider->StartMove(particleIndex);

            SpatialVector displacement;
            VectorUtilities::MultiplyByValue(particleVelocities[particleIndex], timeStep, &displacement);
            VectorUtilities::Add(particle.coordinates, displacement, &particle.coordinates);
            geometry->EnsureBoundaries(particle, &particle, innerDiameterRatio);

            neighborProvider->EndMove();
            closestPairProvider->EndMove();
        }
    }

    void ClosestJammingStep::ResetGeneration()
    {
        throw InvalidOperationException("ResetGeneration does nothing for ClosestJammingStep. Always set maxRunsCount = 1 for LS.");
    }

    bool ClosestJammingStep::ShouldContinue() const
    {
         // 3 degrees of freedom are diminished as one particle can always be considered fixed.
         return bondsProvider.GetBonds().size() < static_cast<size_t>(DIMENSIONS * (config->particlesCount - 1));

//        // Expected bonds count n = DIMENSIONS * (particlesCount - 1) = 30 000 - 3 = 29997
//        // Expected coordination number c = 2 * n / particlesCount = 2 * n - 1 / particlesCount = 5.9994 ~ 6 = 2 * n / particlesCount
//        // Acceptable coordination number = 5.98
//        // Acceptable bonds count n = 5.98 / 2 * particlesCount = 29900. This difference will matter a lot (each iteration may consume > 1 minute)
//        size_t acceptableBondsCount = DIMENSIONS * (5.98 / 6.0) * config->particlesCount;
//        return bondsProvider.GetBonds().size() < acceptableBondsCount;
    }
}