/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     lev_mar_optimizer.cpp
 * \author   Collin Johnson
 *
 * Definition of LevMarOptimizer.
 */

#include "hssh/global_topological/mapping/lev_mar_optimizer.h"
#include "hssh/global_topological/chi.h"
#include "hssh/global_topological/global_path.h"
#include "hssh/global_topological/global_place.h"
#include "hssh/global_topological/topological_map.h"
#include <boost/range/adaptor/map.hpp>
#include <boost/range/iterator_range.hpp>
#include <cassert>
#include <iostream>
#include <levmar.h>

// #define DEBUG_INITIAL_P
// #define DEBUG_X
// #define DEBUG_JACOBIAN
// #define DEBUG_INCREMENTAL_P
// #define DEBUG_LAMBDA
// #define DEBUG_COVARIANCE
// #define DEBUG_FINAL_P
// #define DEBUG_LEV_MAR

namespace vulcan
{
namespace hssh
{

// Callbacks for talking with levmar
void levmar_func(double* p, double* hx, int m, int n, void* adata);
void levmar_jacobian(double* p, double* j, int m, int n, void* adata);

LevMarOptimizer::LevMarOptimizer(const lev_mar_optimizer_params_t& params) : params_(params)
{
}

Chi LevMarOptimizer::optimizeMap(const TopologicalMap& map)
{
    // Clear out any previous results
    idToPIndex.clear();
    xLambdaIndices.clear();

    p.clear();
    x.clear();
    variance.clear();
    xError.clear();
    covariance.clear();

    countEdges(map);

    // Only run the optimization if there are at least as many edges as places. Otherwise,
    // the optimization is actually unconstrained and thus won't produce different results
    // from just laying out the places using the lambda values.
    // The first place is fixed at (0, 0, 0), then all places afterward are relative to this initial place.
    if ((numEdges + 1 < map.numPlaces()) || (map.numPlaces() == 0)) {
        return Chi(map);
    }

    //     if(numEdges >= map.numPlaces())
    //     {
    setupPVector(map);
    setupXVector(map);
    //         calculateJacobian();
    runLevmar();

    return createChiFromP();
    //     }
    //     else
    //     {
    //         return Chi(map);
    //     }
}

void LevMarOptimizer::setupPVector(const TopologicalMap& map)
{
    assert(map.numPlaces() > 0);
    numPlaces = map.numPlaces();

    p.resize(numPlaces * 3);
    covariance.resize(p.size() * p.size());
    std::fill(covariance.begin(), covariance.end(), 0.0);
    std::fill(p.begin(), p.end(), 0.0);

    int pIndex = 0;
    // Skip the first place because it is the free parameter that we fix to be (0, 0, 0).
    auto beginIt = map.places().begin();
    initialId_ = beginIt->first;
    ++beginIt;
    for (auto& place : boost::make_iterator_range(beginIt, map.places().end())) {
        pose_t pose = map.referenceFrame(place.second->id());

        p[pIndex * 3] = pose.x;
        p[pIndex * 3 + 1] = pose.y;
        p[pIndex * 3 + 2] = pose.theta;

        idToPIndex[place.first] = pIndex;

        ++pIndex;
    }

    // Save the initial, fixed at the end of the pVector, so the optimization can safely refer to it without actually
    // trying to overwrite it. The value is always 0, so it should work okay.
    idToPIndex[initialId_] = pIndex;
    p[pIndex * 3] = 0.0;
    p[pIndex * 3 + 1] = 0.0;
    p[pIndex * 3 + 2] = 0.0;

#ifdef DEBUG_INITIAL_P
    std::cout << "DEBUG:LevMar:Initial p-vector (initial mean poses):\n";
    for (std::size_t n = 0; n < numPlaces; ++n) {
        std::cout << '(' << p[n * 3] << ',' << p[n * 3 + 1] << ',' << p[n * 3 + 2] << ")\n";
    }
#endif
}

void LevMarOptimizer::setupXVector(const TopologicalMap& map)
{
    const std::size_t kXMinSize = numEdges * 3;

    if (x.capacity() < kXMinSize) {
        x.reserve(kXMinSize);
        variance.reserve(kXMinSize);
        xError.reserve(kXMinSize);
    }

    std::size_t xIndex = 0;

    for (auto& segment : boost::adaptors::values(map.segments())) {
        // Ignore frontiers the front half of a loop, as the constraint will be added for the end segment
        if (segment->isFrontier()) {
            continue;
        }

        // Check the indices to see if this is a well-constrained segment, which means both ends have been visited.
        // Don't add lambdas for frontier segments because they aren't actually useful information for constraining
        // the Chi value

        std::pair<int, int> pIndices(idToPIndex[segment->plusPlace().id()], idToPIndex[segment->minusPlace().id()]);

        assert(xIndex == xLambdaIndices.size());
        assert(xIndex * 3 == variance.size());

#ifdef DEBUG_X
        std::cout << "Segment from " << segment->plusPlace().id() << " to " << segment->minusPlace().id()
                  << " idx:" << pIndices.first << " to " << pIndices.second << '\n';
#endif

        const std::vector<Lambda>& lambdas = segment->getAllLambdas();
        for (auto lambdaIt = lambdas.begin(), lambdaEnd = lambdas.end(); lambdaIt != lambdaEnd; ++lambdaIt) {
            const Lambda& segmentLambda = *lambdaIt;

            double xVar = segmentLambda.xVariance;
            double yVar = segmentLambda.yVariance;
            double thetaVar = segmentLambda.thetaVariance;

            variance.push_back(xVar);
            variance.push_back(yVar);
            variance.push_back(thetaVar);

            x.push_back(segmentLambda.x / xVar);
            x.push_back(segmentLambda.y / yVar);
            x.push_back(segmentLambda.theta / thetaVar);

            xLambdaIndices.push_back(pIndices);
            ++xIndex;

#ifdef DEBUG_X
            std::cout << '(' << segmentLambda.x << ',' << segmentLambda.y << ',' << segmentLambda.theta << ")\n";
#endif
        }
    }

#ifdef DEBUG_X
    std::cout << "DEBUG:LevMar:x-vector (measurements):\n";
    for (std::size_t n = 0; n < numEdges; ++n) {
        std::cout << '(' << x[n * 3] << ',' << x[n * 3 + 1] << ',' << x[n * 3 + 2] << ")\n";
    }

    std::cout << "DEBUG:LevMar:Measurement variance:\n";
    for (std::size_t i = 0; i < numEdges; ++i) {
        for (std::size_t j = 0; j < 3; ++j) {
            std::cout << variance[i * 3 + j] << ' ';
        }
        std::cout << '\n';
    }
#endif
}

void LevMarOptimizer::countEdges(const TopologicalMap& map)
{
    numEdges = 0;

    for (auto& segment : boost::adaptors::values(map.segments())) {
        if (!segment->isFrontier()) {
            numEdges += segment->getAllLambdas().size();
        }
    }
}

void LevMarOptimizer::runLevmar(void)
{
    // Need to have at least as many edges as places in order to optimizer, otherwise graph is tree-structured
    //     if(numEdges < numPlaces)
    //     {
    //         return;
    //     }

    assert(xLambdaIndices.size() == numEdges);

    if (work.size() < LM_DIF_WORKSZ(p.size(), x.size())) {
        work.resize(LM_DIF_WORKSZ(p.size(), x.size()));
        std::fill(work.begin(), work.end(), 0.0);
    }

    double opts[5];
    double info[LM_INFO_SZ];

    opts[0] = params_.initialMu;
    opts[1] = params_.stopThreshold;
    opts[2] = params_.stopThreshold;
    opts[3] = params_.stopThreshold;
    opts[4] = 1.0;

    //     dlevmar_der(levmar_func, levmar_jacobian, p, x, numPlaces*3, numEdges*3, params_.maxIterations, opts, info,
    //     work, covariance, this); dlevmar_dif(levmar_func, p, x, numPlaces*3, numEdges*3, params_.maxIterations, opts,
    //     info, work, covariance, this);
    dlevmar_dif(
      levmar_func,
      p.data(),
      x.data(),
      // handle the case where there's a single area, so it needs to have the error computed, which will be large
      (p.size() > 3) ? p.size() - 3 : p.size(),   // ignoring the final place, which will always be (0,0,0)
      x.size(),
      params_.maxIterations,
      opts,
      info,
      work.data(),
      covariance.data(),
      this);

    finalError = info[1];

#ifdef DEBUG_JACOBIAN
    std::vector<double> error(x.size());
    dlevmar_chkjac(levmar_func, levmar_jacobian, p.data(), p.size() - 3, x.size(), this, error.data());

    std::cout << "DEBUG:LevMar:Jacobian check:\n";
    for (std::size_t n = 0; n < error.size(); ++n) {
        std::cout << error[n] << '\n';
    }
#endif

#ifdef DEBUG_LEV_MAR
    std::cout << "DEBUG:LevMar:Optimization info:\n"
              << "Initial error:" << info[0] << '\n'
              << "Final error:  " << info[1] << '\n'
              << "Iterations:   " << info[5] << '\n'
              << "Termination:  " << info[6] << '\n'
              << "Func evals:   " << info[7] << '\n'
              << "Jacob evals:  " << info[8] << '\n'
              << "Sys solved:   " << info[9] << '\n';
#endif
}

Chi LevMarOptimizer::createChiFromP(void)
{
    std::unordered_map<Id, pose_distribution_t> finalPoses;

    for (auto& index : idToPIndex) {
        int idx = index.second * 3;

        pose_distribution_t pose;
        pose.x = p[idx];
        pose.y = p[idx + 1];
        pose.theta = wrap_to_pi(p[idx + 2]);
        pose.uncertainty[0] = pose.x;
        pose.uncertainty[1] = pose.y;
        pose.uncertainty[2] = pose.theta;

        // Copy out the covariance off the block diagonal -- don't care about the other cross-correlation terms
        for (int y = 0; y < 3; ++y) {
            for (int x = 0; x < 3; ++x) {
                pose.uncertainty(x, y) = covariance[((idx + y) * numPlaces * 3) + idx + x];
            }
        }

        finalPoses[index.first] = pose;
    }

#ifdef DEBUG_FINAL_P
    std::cout << "DEBUG::LevMar: Mean poses:\n";
    for (auto& idToPose : finalPoses) {
        std::cout << idToPose.first << ":\n"
                  << idToPose.second.uncertainty.getMean() << '\n'
                  << idToPose.second.uncertainty.getCovariance();
    }
#endif

#ifdef DEBUG_COVARIANCE
    //     std::cout<<"DEBUG:LevMar:Covariance:\n";
    //     for(std::size_t i = 0; i < numPlaces*3; ++i)
    //     {
    //         for(std::size_t j = 0; j < numPlaces*3; ++j)
    //         {
    //             printf("%.2f ", covariance[i*numPlaces*3 + j]);
    // //             std::cout<<covariance[i*numPlaces*3 + j]<<' ';
    //         }
    //         std::cout<<'\n';
    //     }
    std::cout << "DEBUG:LevMar:Covariance diag:\n";
    for (std::size_t i = 0; i < numPlaces * 3; ++i) {
        printf("%.2f ", dlevmar_stddev(covariance.data(), numPlaces * 3, i));   // covariance[i*numPlaces*3 + i*3]);
        std::cout << '\n';
    }
#endif

    // Error minimizes (lambda_chi - lambda_obs)^T cov_obs^-1 (lambda_chi - lambda_obs)
    // To get log-likelihood, just multiply by -0.5, which gives the Gaussian log-likelihood
    return Chi(finalPoses, -0.5 * finalError);
}

void LevMarOptimizer::calculateLambdas(double* newP, double* hx, int m, int n)
{
    pose_t start;
    pose_t end;

    for (std::size_t i = 0; i < xLambdaIndices.size(); ++i) {
        // Only consider the new poses if they have valid indices. If out of range, that means it should be 0,0,0
        // per the design of the p matrix
        if (xLambdaIndices[i].first * 3 < m) {
            int startIdx = xLambdaIndices[i].first * 3;
            start.x = newP[startIdx];
            start.y = newP[startIdx + 1];
            start.theta = wrap_to_pi(newP[startIdx + 2]);
        }
        // At the origin
        else {
            start.x = 0.0f;
            start.y = 0.0f;
            start.theta = 0.0f;
        }

        if (xLambdaIndices[i].second * 3 < m) {
            int endIdx = xLambdaIndices[i].second * 3;
            end.x = newP[endIdx];
            end.y = newP[endIdx + 1];
            end.theta = wrap_to_pi(newP[endIdx + 2]);
        }
        // At the origin
        else {
            end.x = 0.0f;
            end.y = 0.0f;
            end.theta = 0.0f;
        }

        auto diff = end.transformToNewFrame(start);

        int xIdx = i * 3;
        int yIdx = xIdx + 1;
        int thetaIdx = xIdx + 2;

        hx[xIdx] = diff.x / variance[xIdx];
        hx[yIdx] = diff.y / variance[yIdx];
        hx[thetaIdx] = diff.theta / variance[thetaIdx];

        xError[xIdx] = x[xIdx] - hx[xIdx];
        xError[yIdx] = x[yIdx] - hx[yIdx];
        xError[thetaIdx] = wrap_to_pi(x[thetaIdx] - hx[thetaIdx]);
    }

#ifdef DEBUG_INCREMENTAL_P
    std::cout << "DEBUG:LevMar:Incremental p-vector:\n";
    for (int i = 0; i * 3 < m; ++i) {
        std::cout << '(' << newP[i * 3] << ',' << newP[i * 3 + 1] << ',' << newP[i * 3 + 2] << ")\n";
    }
#endif

#ifdef DEBUG_LAMBDA
    std::cout << "DEBUG:LevMar:Incremental lambdas (hx-vector):\n";
    for (std::size_t i = 0; i < numEdges; ++i) {
        std::cout << '(' << hx[i * 3] << ',' << hx[i * 3 + 1] << ',' << hx[i * 3 + 2] << ")\n";
    }

    std::cout << "DEBUG:LevMar:Residuals (x-hx):\n";
    for (std::size_t i = 0; i < numEdges; ++i) {
        std::cout << '(' << (x[i * 3] - hx[i * 3]) << ',' << (x[i * 3 + 1] - hx[i * 3 + 1]) << ','
                  << (x[i * 3 + 2] - hx[i * 3 + 2]) << ")\n";
    }
#endif
}

void LevMarOptimizer::calculateJacobian(double* j, int m, int n)
{
    // The residuals are (p_i - p_j - x_n)^2. Thus the Jacobian is +1 for J_n_i and -1 for J_n_j.

    std::size_t rowStart = 0;
    std::size_t rowStep = numPlaces * 3;

    memset(j, 0, m * m * sizeof(double));

    // The Jacobian is two diagonal blocks for (x,y,theta).  second = end place, first = start place. so lambda = p_end
    // - p_start
    for (std::size_t i = 0; i < numEdges; ++i) {
        if (xLambdaIndices[i].second * 3 < n) {
            j[rowStart + xLambdaIndices[i].second * 3] = xError[i * 3];
        }
        if (xLambdaIndices[i].first * 3 < n) {
            j[rowStart + xLambdaIndices[i].first * 3] = -xError[i * 3];
        }

        rowStart += rowStep;

        if (xLambdaIndices[i].second * 3 < n) {
            j[rowStart + xLambdaIndices[i].second * 3 + 1] = xError[i * 3 + 1];
        }
        if (xLambdaIndices[i].first * 3 < n) {
            j[rowStart + xLambdaIndices[i].first * 3 + 1] = -xError[i * 3 + 1];
        }

        rowStart += rowStep;

        if (xLambdaIndices[i].second * 3 < n) {
            j[rowStart + xLambdaIndices[i].second * 3 + 2] = xError[i * 3 + 2];
        }
        if (xLambdaIndices[i].first * 3 < n) {
            j[rowStart + xLambdaIndices[i].first * 3 + 2] = -xError[i * 3 + 2];
        }

        rowStart += rowStep;
    }

#ifdef DEBUG_JACOBIAN
    rowStart = 0;
    std::cout << "DEBUG:LevMar:Jacobian:\n";
    for (std::size_t n = 0; n < numEdges * 3; ++n) {
        for (std::size_t i = 0; i < numPlaces * 3; ++i) {
            std::cout << j[rowStart + i] << ' ';
        }
        std::cout << '\n';
        rowStart += rowStep;
    }
#endif
}

// Create friend functions to issue as the callbacks used by levmar. The adata will point to this, which will then
// be used to call the
void levmar_func(double* p, double* hx, int m, int n, void* adata)
{
    LevMarOptimizer* optimizer = static_cast<LevMarOptimizer*>(adata);
    optimizer->calculateLambdas(p, hx, m, n);
}

void levmar_jacobian(double* p, double* j, int m, int n, void* adata)
{
    LevMarOptimizer* optimizer = static_cast<LevMarOptimizer*>(adata);
    optimizer->calculateJacobian(j, m, n);
}

}   // namespace hssh
}   // namespace vulcan


//
//=======================================================================
// Copyright 2002 Marc Wintermantel (wintermantel@even-ag.ch)
// ETH Zurich, Center of Structure Technologies
// (https://web.archive.org/web/20050307090307/http://www.structures.ethz.ch/)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
//

#ifndef BOOST_GRAPH_SLOAN_HPP
#define BOOST_GRAPH_SLOAN_HPP

#define WEIGHT1 1 // default weight for the distance in the Sloan algorithm
#define WEIGHT2 2 // default weight for the degree in the Sloan algorithm

#include <boost/config.hpp>
#include <vector>
#include <queue>
#include <algorithm>
#include <limits>
#include <boost/pending/queue.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/graph/properties.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/cuthill_mckee_ordering.hpp>

////////////////////////////////////////////////////////////
//
// Sloan-Algorithm for graph reordering
//(optimzes profile and wavefront, not primiraly bandwidth
//
////////////////////////////////////////////////////////////

namespace boost
{

/////////////////////////////////////////////////////////////////////////
// Function that returns the maximum depth of
// a rooted level strucutre (RLS)
//
/////////////////////////////////////////////////////////////////////////
template < class Distance > typename Distance::value_type RLS_depth(Distance& d)
{
    typename Distance::value_type h_s = 0;
    typename Distance::iterator iter;

    for (iter = d.begin(); iter != d.end(); ++iter)
    {
        if (*iter > h_s)
        {
            h_s = *iter;
        }
    }

    return h_s;
}

/////////////////////////////////////////////////////////////////////////
// Function that returns the width of the largest level of
// a rooted level strucutre (RLS)
//
/////////////////////////////////////////////////////////////////////////
template < class Distance, class my_int >
typename Distance::value_type RLS_max_width(Distance& d, my_int depth)
{

    typedef typename Distance::value_type Degree;

    // Searching for the maximum width of a level
    std::vector< Degree > dummy_width(depth + 1, 0);
    typename std::vector< Degree >::iterator my_it;
    typename Distance::iterator iter;
    Degree w_max = 0;

    for (iter = d.begin(); iter != d.end(); ++iter)
    {
        dummy_width[*iter]++;
    }

    for (my_it = dummy_width.begin(); my_it != dummy_width.end(); ++my_it)
    {
        if (*my_it > w_max)
            w_max = *my_it;
    }

    return w_max;
}

/////////////////////////////////////////////////////////////////////////
// Function for finding a good starting node for Sloan algorithm
//
// This is to find a good starting node. "good" is in the sense
// of the ordering generated.
/////////////////////////////////////////////////////////////////////////
template < class Graph, class ColorMap, class DegreeMap >
typename graph_traits< Graph >::vertex_descriptor sloan_start_end_vertices(
    Graph& G, typename graph_traits< Graph >::vertex_descriptor& s,
    ColorMap color, DegreeMap degree)
{
    typedef typename property_traits< DegreeMap >::value_type Degree;
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename std::vector<
        typename graph_traits< Graph >::vertices_size_type >::iterator vec_iter;
    typedef typename graph_traits< Graph >::vertices_size_type size_type;

    typedef typename property_map< Graph, vertex_index_t >::const_type VertexID;

    s = *(vertices(G).first);
    Vertex e = s;
    Vertex i;
    Degree my_degree = get(degree, s);
    Degree dummy, h_i, h_s, w_i, w_e;
    bool new_start = true;
    Degree maximum_degree = 0;

    // Creating a std-vector for storing the distance from the start vertex in
    // dist
    std::vector< typename graph_traits< Graph >::vertices_size_type > dist(
        num_vertices(G), 0);

    // Wrap a property_map_iterator around the std::iterator
    boost::iterator_property_map< vec_iter, VertexID, size_type, size_type& >
        dist_pmap(dist.begin(), get(vertex_index, G));

    // Creating a property_map for the indices of a vertex
    typename property_map< Graph, vertex_index_t >::type index_map
        = get(vertex_index, G);

    // Creating a priority queue
    typedef indirect_cmp< DegreeMap, std::greater< Degree > > Compare;
    Compare comp(degree);
    std::priority_queue< Vertex, std::vector< Vertex >, Compare > degree_queue(
        comp);

    // step 1
    // Scan for the vertex with the smallest degree and the maximum degree
    typename graph_traits< Graph >::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui)
    {
        dummy = get(degree, *ui);

        if (dummy < my_degree)
        {
            my_degree = dummy;
            s = *ui;
        }

        if (dummy > maximum_degree)
        {
            maximum_degree = dummy;
        }
    }
    // end 1

    do
    {
        new_start = false; // Setting the loop repetition status to false

        // step 2
        // initialize the the disance std-vector with 0
        for (typename std::vector< typename graph_traits<
                 Graph >::vertices_size_type >::iterator iter
             = dist.begin();
             iter != dist.end(); ++iter)
            *iter = 0;

        // generating the RLS (rooted level structure)
        breadth_first_search(G, s,
            visitor(
                make_bfs_visitor(record_distances(dist_pmap, on_tree_edge()))));

        // end 2

        // step 3
        // calculating the depth of the RLS
        h_s = RLS_depth(dist);

        // step 4
        // pushing one node of each degree in an ascending manner into
        // degree_queue
        std::vector< bool > shrink_trace(maximum_degree, false);
        for (boost::tie(ui, ui_end) = vertices(G); ui != ui_end; ++ui)
        {
            dummy = get(degree, *ui);

            if ((dist[index_map[*ui]] == h_s) && (!shrink_trace[dummy]))
            {
                degree_queue.push(*ui);
                shrink_trace[dummy] = true;
            }
        }

        // end 3 & 4

        // step 5
        // Initializing w
        w_e = (std::numeric_limits< Degree >::max)();
        // end 5

        // step 6
        // Testing for termination
        while (!degree_queue.empty())
        {
            i = degree_queue.top(); // getting the node with the lowest degree
                                    // from the degree queue
            degree_queue.pop(); // ereasing the node with the lowest degree from
                                // the degree queue

            // generating a RLS
            for (typename std::vector< typename graph_traits<
                     Graph >::vertices_size_type >::iterator iter
                 = dist.begin();
                 iter != dist.end(); ++iter)
                *iter = 0;

            breadth_first_search(G, i,
                boost::visitor(make_bfs_visitor(
                    record_distances(dist_pmap, on_tree_edge()))));

            // Calculating depth and width of the rooted level
            h_i = RLS_depth(dist);
            w_i = RLS_max_width(dist, h_i);

            // Testing for termination
            if ((h_i > h_s) && (w_i < w_e))
            {
                h_s = h_i;
                s = i;
                while (!degree_queue.empty())
                    degree_queue.pop();
                new_start = true;
            }
            else if (w_i < w_e)
            {
                w_e = w_i;
                e = i;
            }
        }
        // end 6

    } while (new_start);

    return e;
}

//////////////////////////////////////////////////////////////////////////
// Sloan algorithm with a given starting Vertex.
//
// This algorithm requires user to provide a starting vertex to
// compute Sloan ordering.
//////////////////////////////////////////////////////////////////////////
template < class Graph, class OutputIterator, class ColorMap, class DegreeMap,
    class PriorityMap, class Weight >
OutputIterator sloan_ordering(Graph& g,
    typename graph_traits< Graph >::vertex_descriptor s,
    typename graph_traits< Graph >::vertex_descriptor e,
    OutputIterator permutation, ColorMap color, DegreeMap degree,
    PriorityMap priority, Weight W1, Weight W2)
{
    // typedef typename property_traits<DegreeMap>::value_type Degree;
    typedef typename property_traits< PriorityMap >::value_type Degree;
    typedef typename property_traits< ColorMap >::value_type ColorValue;
    typedef color_traits< ColorValue > Color;
    typedef typename graph_traits< Graph >::vertex_descriptor Vertex;
    typedef typename std::vector<
        typename graph_traits< Graph >::vertices_size_type >::iterator vec_iter;
    typedef typename graph_traits< Graph >::vertices_size_type size_type;

    typedef typename property_map< Graph, vertex_index_t >::const_type VertexID;

    // Creating a std-vector for storing the distance from the end vertex in it
    typename std::vector< typename graph_traits< Graph >::vertices_size_type >
        dist(num_vertices(g), 0);

    // Wrap a property_map_iterator around the std::iterator
    boost::iterator_property_map< vec_iter, VertexID, size_type, size_type& >
        dist_pmap(dist.begin(), get(vertex_index, g));

    breadth_first_search(g, e,
        visitor(make_bfs_visitor(record_distances(dist_pmap, on_tree_edge()))));

    // Creating a property_map for the indices of a vertex
    typename property_map< Graph, vertex_index_t >::type index_map
        = get(vertex_index, g);

    // Sets the color and priority to their initial status
    Degree cdeg;
    typename graph_traits< Graph >::vertex_iterator ui, ui_end;
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui)
    {
        put(color, *ui, Color::white());
        cdeg = get(degree, *ui) + 1;
        put(priority, *ui, W1 * dist[index_map[*ui]] - W2 * cdeg);
    }

    // Priority list
    typedef indirect_cmp< PriorityMap, std::greater< Degree > > Compare;
    Compare comp(priority);
    std::list< Vertex > priority_list;

    // Some more declarations
    typename graph_traits< Graph >::out_edge_iterator ei, ei_end, ei2, ei2_end;
    Vertex u, v, w;

    put(color, s,
        Color::green()); // Sets the color of the starting vertex to gray
    priority_list.push_front(s); // Puts s into the priority_list

    while (!priority_list.empty())
    {
        priority_list.sort(comp); // Orders the elements in the priority list in
                                  // an ascending manner

        u = priority_list
                .front(); // Accesses the last element in the priority list
        priority_list
            .pop_front(); // Removes the last element in the priority list

        if (get(color, u) == Color::green())
        {
            // for-loop over all out-edges of vertex u
            for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei)
            {
                v = target(*ei, g);

                put(priority, v, get(priority, v) + W2); // updates the priority

                if (get(color, v)
                    == Color::white()) // test if the vertex is inactive
                {
                    put(color, v,
                        Color::green()); // giving the vertex a preactive status
                    priority_list.push_front(
                        v); // writing the vertex in the priority_queue
                }
            }
        }

        // Here starts step 8
        *permutation++
            = u; // Puts u to the first position in the permutation-vector
        put(color, u, Color::black()); // Gives u an inactive status

        // for loop over all the adjacent vertices of u
        for (boost::tie(ei, ei_end) = out_edges(u, g); ei != ei_end; ++ei)
        {

            v = target(*ei, g);

            if (get(color, v) == Color::green())
            { // tests if the vertex is inactive

                put(color, v,
                    Color::red()); // giving the vertex an active status
                put(priority, v, get(priority, v) + W2); // updates the priority

                // for loop over alll adjacent vertices of v
                for (boost::tie(ei2, ei2_end) = out_edges(v, g); ei2 != ei2_end;
                     ++ei2)
                {
                    w = target(*ei2, g);

                    if (get(color, w) != Color::black())
                    { // tests if vertex is postactive

                        put(priority, w,
                            get(priority, w) + W2); // updates the priority

                        if (get(color, w) == Color::white())
                        {

                            put(color, w, Color::green()); // gives the vertex a
                                                           // preactive status
                            priority_list.push_front(
                                w); // puts the vertex into the priority queue

                        } // end if

                    } // end if

                } // end for

            } // end if

        } // end for

    } // end while

    return permutation;
}

/////////////////////////////////////////////////////////////////////////////////////////
// Same algorithm as before, but without the weights given (taking default
// weights
template < class Graph, class OutputIterator, class ColorMap, class DegreeMap,
    class PriorityMap >
OutputIterator sloan_ordering(Graph& g,
    typename graph_traits< Graph >::vertex_descriptor s,
    typename graph_traits< Graph >::vertex_descriptor e,
    OutputIterator permutation, ColorMap color, DegreeMap degree,
    PriorityMap priority)
{
    return sloan_ordering(
        g, s, e, permutation, color, degree, priority, WEIGHT1, WEIGHT2);
}

//////////////////////////////////////////////////////////////////////////
// Sloan algorithm without a given starting Vertex.
//
// This algorithm finds a good starting vertex itself to
// compute Sloan-ordering.
//////////////////////////////////////////////////////////////////////////

template < class Graph, class OutputIterator, class Color, class Degree,
    class Priority, class Weight >
inline OutputIterator sloan_ordering(Graph& G, OutputIterator permutation,
    Color color, Degree degree, Priority priority, Weight W1, Weight W2)
{
    typedef typename boost::graph_traits< Graph >::vertex_descriptor Vertex;

    Vertex s, e;
    e = sloan_start_end_vertices(G, s, color, degree);

    return sloan_ordering(
        G, s, e, permutation, color, degree, priority, W1, W2);
}

/////////////////////////////////////////////////////////////////////////////////////////
// Same as before, but without given weights (default weights are taken instead)
template < class Graph, class OutputIterator, class Color, class Degree,
    class Priority >
inline OutputIterator sloan_ordering(Graph& G, OutputIterator permutation,
    Color color, Degree degree, Priority priority)
{
    return sloan_ordering(
        G, permutation, color, degree, priority, WEIGHT1, WEIGHT2);
}

} // namespace boost

#endif // BOOST_GRAPH_SLOAN_HPP


// Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.

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

#ifndef UUID_20D98340A3EB11DEB2180CD156D89593
#define UUID_20D98340A3EB11DEB2180CD156D89593

#include <boost/qvm/assert.hpp>
#include <boost/qvm/deduce_mat.hpp>
#include <boost/qvm/detail/transp_impl.hpp>
#include <boost/qvm/enable_if.hpp>
#include <boost/qvm/inline.hpp>

namespace boost {
namespace qvm {
////////////////////////////////////////////////

namespace qvm_detail {
template <int Row, class OriginalMatrix> class del_row_ {
  del_row_(del_row_ const &);
  del_row_ &operator=(del_row_ const &);
  ~del_row_();

public:
  template <class T> BOOST_QVM_INLINE_TRIVIAL del_row_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int I, class OriginalMatrix>
struct mat_traits<qvm_detail::del_row_<I, OriginalMatrix>> {
  typedef qvm_detail::del_row_<I, OriginalMatrix> this_matrix;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const rows = mat_traits<OriginalMatrix>::rows - 1;
  static int const cols = mat_traits<OriginalMatrix>::cols;

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_matrix const &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template read_element<Row + (Row >= I),
                                                             Col>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_matrix &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template write_element<Row + (Row >= I),
                                                              Col>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int row, int col, this_matrix const &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::read_element_idx(
        row + (row >= I), col, reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int row, int col, this_matrix &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::write_element_idx(
        row + (row >= I), col, reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat<qvm_detail::del_row_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat2<qvm_detail::del_row_<J, OriginalMatrix>,
                   qvm_detail::del_row_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int Row, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::del_row_<Row, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    del_row(A const &a) {
  return reinterpret_cast<typename qvm_detail::del_row_<Row, A> const &>(a);
}

template <int Row, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::del_row_<Row, A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    del_row(A &a) {
  return reinterpret_cast<typename qvm_detail::del_row_<Row, A> &>(a);
}

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

namespace qvm_detail {
template <int Col, class OriginalMatrix> class del_col_ {
  del_col_(del_col_ const &);
  del_col_ &operator=(del_col_ const &);
  ~del_col_();

public:
  template <class T> BOOST_QVM_INLINE_TRIVIAL del_col_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int J, class OriginalMatrix>
struct mat_traits<qvm_detail::del_col_<J, OriginalMatrix>> {
  typedef qvm_detail::del_col_<J, OriginalMatrix> this_matrix;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const rows = mat_traits<OriginalMatrix>::rows;
  static int const cols = mat_traits<OriginalMatrix>::cols - 1;

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_matrix const &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template read_element<Row,
                                                             Col + (Col >= J)>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_matrix &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template write_element<Row,
                                                              Col + (Col >= J)>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int row, int col, this_matrix const &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::read_element_idx(
        row, col + (col >= J), reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int row, int col, this_matrix &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::write_element_idx(
        row, col + (col >= J), reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat<qvm_detail::del_col_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat2<qvm_detail::del_col_<J, OriginalMatrix>,
                   qvm_detail::del_col_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int Col, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::del_col_<Col, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    del_col(A const &a) {
  return reinterpret_cast<typename qvm_detail::del_col_<Col, A> const &>(a);
}

template <int Col, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::del_col_<Col, A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    del_col(A &a) {
  return reinterpret_cast<typename qvm_detail::del_col_<Col, A> &>(a);
}

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

namespace qvm_detail {
template <int Row, int Col, class OriginalMatrix> class del_row_col_ {
  del_row_col_(del_row_col_ const &);
  ~del_row_col_();

public:
  BOOST_QVM_INLINE_TRIVIAL
  del_row_col_ &operator=(del_row_col_ const &x) {
    assign(*this, x);
    return *this;
  }

  template <class T>
  BOOST_QVM_INLINE_TRIVIAL del_row_col_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int I, int J, class OriginalMatrix>
struct mat_traits<qvm_detail::del_row_col_<I, J, OriginalMatrix>> {
  typedef qvm_detail::del_row_col_<I, J, OriginalMatrix> this_matrix;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const rows = mat_traits<OriginalMatrix>::rows - 1;
  static int const cols = mat_traits<OriginalMatrix>::cols - 1;

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_matrix const &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template read_element<Row + (Row >= I),
                                                             Col + (Col >= J)>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_matrix &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template write_element<Row + (Row >= I),
                                                              Col + (Col >= J)>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int row, int col, this_matrix const &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::read_element_idx(
        row + (row >= I), col + (col >= J),
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int row, int col, this_matrix &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::write_element_idx(
        row + (row >= I), col + (col >= J),
        reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <int I, int J, class OriginalMatrix, int R, int C>
struct deduce_mat<qvm_detail::del_row_col_<I, J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int I, int J, class OriginalMatrix, int R, int C>
struct deduce_mat2<qvm_detail::del_row_col_<I, J, OriginalMatrix>,
                   qvm_detail::del_row_col_<I, J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int Row, int Col, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::del_row_col_<Row, Col, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    del_row_col(A const &a) {
  return reinterpret_cast<
      typename qvm_detail::del_row_col_<Row, Col, A> const &>(a);
}

template <int Row, int Col, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::del_row_col_<Row, Col, A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    del_row_col(A &a) {
  return reinterpret_cast<typename qvm_detail::del_row_col_<Row, Col, A> &>(a);
}

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

namespace qvm_detail {
template <int Row, class OriginalMatrix> class neg_row_ {
  neg_row_(neg_row_ const &);
  neg_row_ &operator=(neg_row_ const &);
  ~neg_row_();

public:
  template <class T> BOOST_QVM_INLINE_TRIVIAL neg_row_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int I, class OriginalMatrix>
struct mat_traits<qvm_detail::neg_row_<I, OriginalMatrix>> {
  typedef qvm_detail::neg_row_<I, OriginalMatrix> this_matrix;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const rows = mat_traits<OriginalMatrix>::rows;
  static int const cols = mat_traits<OriginalMatrix>::cols;

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_matrix const &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return Row == I
               ? -mat_traits<OriginalMatrix>::template read_element<Row, Col>(
                     reinterpret_cast<OriginalMatrix const &>(x))
               : mat_traits<OriginalMatrix>::template read_element<Row, Col>(
                     reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int row, int col, this_matrix const &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return row == I
               ? -mat_traits<OriginalMatrix>::read_element_idx(
                     row, col, reinterpret_cast<OriginalMatrix const &>(x))
               : mat_traits<OriginalMatrix>::read_element_idx(
                     row, col, reinterpret_cast<OriginalMatrix const &>(x));
  }
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat<qvm_detail::neg_row_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat2<qvm_detail::neg_row_<J, OriginalMatrix>,
                   qvm_detail::neg_row_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int Row, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::neg_row_<Row, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    neg_row(A const &a) {
  return reinterpret_cast<typename qvm_detail::neg_row_<Row, A> const &>(a);
}

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

namespace qvm_detail {
template <int Col, class OriginalMatrix> class neg_col_ {
  neg_col_(neg_col_ const &);
  neg_col_ &operator=(neg_col_ const &);
  ~neg_col_();

public:
  template <class T> BOOST_QVM_INLINE_TRIVIAL neg_col_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int J, class OriginalMatrix>
struct mat_traits<qvm_detail::neg_col_<J, OriginalMatrix>> {
  typedef qvm_detail::neg_col_<J, OriginalMatrix> this_matrix;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const rows = mat_traits<OriginalMatrix>::rows;
  static int const cols = mat_traits<OriginalMatrix>::cols;

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_matrix const &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return Col == J
               ? -mat_traits<OriginalMatrix>::template read_element<Row, Col>(
                     reinterpret_cast<OriginalMatrix const &>(x))
               : mat_traits<OriginalMatrix>::template read_element<Row, Col>(
                     reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int row, int col, this_matrix const &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return col == J
               ? -mat_traits<OriginalMatrix>::read_element_idx(
                     row, col, reinterpret_cast<OriginalMatrix const &>(x))
               : mat_traits<OriginalMatrix>::read_element_idx(
                     row, col, reinterpret_cast<OriginalMatrix const &>(x));
  }
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat<qvm_detail::neg_col_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int J, class OriginalMatrix, int R, int C>
struct deduce_mat2<qvm_detail::neg_col_<J, OriginalMatrix>,
                   qvm_detail::neg_col_<J, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int Col, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::neg_col_<Col, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    neg_col(A const &a) {
  return reinterpret_cast<typename qvm_detail::neg_col_<Col, A> const &>(a);
}

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

template <class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::transposed_<A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    transposed(A const &a) {
  return reinterpret_cast<typename qvm_detail::transposed_<A> const &>(a);
}

template <class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::transposed_<A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    transposed(A &a) {
  return reinterpret_cast<typename qvm_detail::transposed_<A> &>(a);
}

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

namespace qvm_detail {
template <int Row1, int Row2, class OriginalMatrix> class swap_rows_ {
  swap_rows_(swap_rows_ const &);
  swap_rows_ &operator=(swap_rows_ const &);
  ~swap_rows_();

public:
  template <class T>
  BOOST_QVM_INLINE_TRIVIAL swap_rows_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int R1, int R2, class OriginalMatrix>
struct mat_traits<qvm_detail::swap_rows_<R1, R2, OriginalMatrix>> {
  typedef qvm_detail::swap_rows_<R1, R2, OriginalMatrix> this_matrix;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const rows = mat_traits<OriginalMatrix>::rows;
  static int const cols = mat_traits<OriginalMatrix>::cols;

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_matrix const &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template read_element<
        (Row == R1 && R1 != R2) * R2 + (Row == R2 && R1 != R2) * R1 +
            ((Row != R1 && Row != R2) || R1 == R2) * Row,
        Col>(reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_matrix &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template write_element<
        (Row == R1 && R1 != R2) * R2 + (Row == R2 && R1 != R2) * R1 +
            ((Row != R1 && Row != R2) || R1 == R2) * Row,
        Col>(reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int row, int col, this_matrix const &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::read_element_idx(
        row == R1 ? R2 : row == R2 ? R1 : row, col,
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int row, int col, this_matrix &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::write_element_idx(
        row == R1 ? R2 : row == R2 ? R1 : row, col,
        reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <int R1, int R2, class OriginalMatrix, int R, int C>
struct deduce_mat<qvm_detail::swap_rows_<R1, R2, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int R1, int R2, class OriginalMatrix, int R, int C>
struct deduce_mat2<qvm_detail::swap_rows_<R1, R2, OriginalMatrix>,
                   qvm_detail::swap_rows_<R1, R2, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int R1, int R2, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::swap_rows_<R1, R2, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    swap_rows(A const &a) {
  return reinterpret_cast<typename qvm_detail::swap_rows_<R1, R2, A> const &>(
      a);
}

template <int R1, int R2, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::swap_rows_<R1, R2, A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    swap_rows(A &a) {
  return reinterpret_cast<typename qvm_detail::swap_rows_<R1, R2, A> &>(a);
}

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

namespace qvm_detail {
template <int Row1, int Row2, class OriginalMatrix> class swap_cols_ {
  swap_cols_(swap_cols_ const &);
  swap_cols_ &operator=(swap_cols_ const &);
  ~swap_cols_();

public:
  template <class T>
  BOOST_QVM_INLINE_TRIVIAL swap_cols_ &operator=(T const &x) {
    assign(*this, x);
    return *this;
  }

  template <class R> BOOST_QVM_INLINE_TRIVIAL operator R() const {
    R r;
    assign(r, *this);
    return r;
  }
};
} // namespace qvm_detail

template <int C1, int C2, class OriginalMatrix>
struct mat_traits<qvm_detail::swap_cols_<C1, C2, OriginalMatrix>> {
  typedef qvm_detail::swap_cols_<C1, C2, OriginalMatrix> this_matrix;
  typedef typename mat_traits<OriginalMatrix>::scalar_type scalar_type;
  static int const rows = mat_traits<OriginalMatrix>::rows;
  static int const cols = mat_traits<OriginalMatrix>::cols;

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element(this_matrix const &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template read_element<
        Row, (Col == C1 && C1 != C2) * C2 + (Col == C2 && C1 != C2) * C1 +
                 ((Col != C1 && Col != C2) || C1 == C2) * Col>(
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  template <int Row, int Col>
  static BOOST_QVM_INLINE_CRITICAL scalar_type &write_element(this_matrix &x) {
    BOOST_QVM_STATIC_ASSERT(Row >= 0);
    BOOST_QVM_STATIC_ASSERT(Row < rows);
    BOOST_QVM_STATIC_ASSERT(Col >= 0);
    BOOST_QVM_STATIC_ASSERT(Col < cols);
    return mat_traits<OriginalMatrix>::template write_element<
        Row, (Col == C1 && C1 != C2) * C2 + (Col == C2 && C1 != C2) * C1 +
                 ((Col != C1 && Col != C2) || C1 == C2) * Col>(
        reinterpret_cast<OriginalMatrix &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type
  read_element_idx(int row, int col, this_matrix const &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::read_element_idx(
        row, col == C1 ? C2 : col == C2 ? C1 : col,
        reinterpret_cast<OriginalMatrix const &>(x));
  }

  static BOOST_QVM_INLINE_CRITICAL scalar_type &
  write_element_idx(int row, int col, this_matrix &x) {
    BOOST_QVM_ASSERT(row >= 0);
    BOOST_QVM_ASSERT(row < rows);
    BOOST_QVM_ASSERT(col >= 0);
    BOOST_QVM_ASSERT(col < cols);
    return mat_traits<OriginalMatrix>::write_element_idx(
        row, col == C1 ? C2 : col == C2 ? C1 : col,
        reinterpret_cast<OriginalMatrix &>(x));
  }
};

template <int C1, int C2, class OriginalMatrix, int R, int C>
struct deduce_mat<qvm_detail::swap_cols_<C1, C2, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int C1, int C2, class OriginalMatrix, int R, int C>
struct deduce_mat2<qvm_detail::swap_cols_<C1, C2, OriginalMatrix>,
                   qvm_detail::swap_cols_<C1, C2, OriginalMatrix>, R, C> {
  typedef mat<typename mat_traits<OriginalMatrix>::scalar_type, R, C> type;
};

template <int C1, int C2, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::swap_cols_<C1, C2, A> const &>::type
    BOOST_QVM_INLINE_TRIVIAL
    swap_cols(A const &a) {
  return reinterpret_cast<typename qvm_detail::swap_cols_<C1, C2, A> const &>(
      a);
}

template <int C1, int C2, class A>
typename boost::enable_if_c<is_mat<A>::value,
                            qvm_detail::swap_cols_<C1, C2, A> &>::type
    BOOST_QVM_INLINE_TRIVIAL
    swap_cols(A &a) {
  return reinterpret_cast<typename qvm_detail::swap_cols_<C1, C2, A> &>(a);
}

////////////////////////////////////////////////
} // namespace qvm
} // namespace boost

#endif


/*
 * GloveVLBL.cpp
 *
 *  Created on: 2014/09/13
 *      Author: miwa
 */
#include "GloveVLBL.h"
#include <gzstream.h>
#include <iostream>
#include <fstream>
#include <chrono>
#include <boost/filesystem.hpp>
#include <algorithm>
#include <unordered_set>
#include <cmath>
using namespace std;
using namespace boost::filesystem;

template<class T>
coin::GloveVLBL<T>::GloveVLBL(const string& freq_file, const string& coocc_file, int dim): coocc_file_(coocc_file), dim_(dim) {
	total_ = load_word_counts(freq_file, word_indices_, word_strings_, word_counts_);
	function<double()> rand(bind(uniform_real_distribution<double>(-.5/dim, .5/dim), default_random_engine(0)));
	words_.resize(word_indices_.size());
	int nwords = 0;
	for(pair<string, int> word:word_indices_){
		words_[word.second] = new T(word.second, dim_, rand);
		nwords++;
	}
	assert(nwords == word_indices_.size());
	init_table();
}

template<class T>
coin::GloveVLBL<T>::GloveVLBL(const string& freq_file, const string& model_file) : coocc_file_("") {
	if(freq_file == "-g"){
		load_glove_vector(model_file, false);
	}else if(freq_file == "-l"){
		// log-bilinear
		load_glove_vector(model_file, true);
	}else{
		total_ = load_word_counts(freq_file, word_indices_, word_strings_, word_counts_);
		load_model(model_file);
	}
}

template<class T>
coin::GloveVLBL<T>::GloveVLBL(const string& freq_file, const string& coocc_file, const string& model_file): coocc_file_(coocc_file) {
	total_ = load_word_counts(freq_file, word_indices_, word_strings_, word_counts_);
	load_model(model_file);
	init_table();
}

template<class T>
bool more(const pair<T, double>& left, const pair<T, double>& right ) {
    if( left.second < right.second ){
    	return false;
    }else if(right.second < left.second){
    	return true;
    }else{
    	return left.first < right.first;
    }
}

template<class T>
string coin::GloveVLBL<T>::filename(int index) const{
	ostringstream oss;
	oss << tmpdir_ << "/" << index;
	return oss.str();
}

string current(){
	ostringstream oss;
	chrono::system_clock::time_point p = chrono::system_clock::now();
	time_t t = chrono::system_clock::to_time_t(p);
  char time_string[100];
  struct tm *tm = localtime(&t);
  strftime(time_string,sizeof(time_string),"%F_%T",tm);
  oss << time_string;
	return oss.str();
}

template<class T>
void coin::GloveVLBL<T>::load_dict(const std::string& dict, std::unordered_map<int, set<int>>& map){
	ifstream in(dict.c_str());
	if(!in){
		cerr << dict << " not found!!" << endl;
		exit(0);
	}
	string line;
	string word;
	while(in && getline(in, line)){
		preprocess(line);
		istringstream iss(line);
		iss >> word;
		if(word_indices_.find(word) != word_indices_.end()){
			int target_id = word_indices_[word];
			while(iss >> word){
				if(word_indices_.find(word) != word_indices_.end()){
					int ref_id = word_indices_[word];
					if(target_id == ref_id)continue;
					if(target_id < ref_id){
						if(map.find(target_id) == map.end()){
							map[target_id] = set<int>();
						}
						map[target_id].insert(ref_id);
					}else{
						if(map.find(ref_id) == map.end()){
							map[ref_id] = set<int>();
						}
						map[ref_id].insert(target_id);
					}
				}
			}
		}
	}
}

template<class T>
void coin::GloveVLBL<T>::load_dict(){
	load_dict("data/dict/synonym.txt", synonyms_);
	load_dict("data/dict/antonym.txt", antonyms_);
	cerr << "synonyms:" << synonyms_.size() << ", " << "antonyms:" << antonyms_.size() << endl;
}

template<class T>
bool coin::GloveVLBL<T>::includes_pair(unordered_map<int, set<int> >& map, int w1, int w2){
	if(w1 < w2){
		if(map.find(w1) != map.end() && map[w1].find(w2) != map[w1].end()){
			return true;
		}
	}else{
		if(map.find(w2) != map.end() && map[w2].find(w1) != map[w2].end()){
			return true;
		}
	}
	return false;
}

template<class T>
void coin::GloveVLBL<T>::init_table(){
	load_dict();
	nwords_ = words_.size();
    tmpdir_ = "tmp/"+current();
	if(!is_directory(tmpdir_)){
		if(!create_directories(tmpdir_)){
			cerr << "cannot create directory: " << tmpdir_ << endl;
			exit(-1);
		}
	}
	vector<pair<int32_t, count_t>> neg_sample_word_counts;
	neg_sample_word_counts.reserve(nwords_);
	double neg_sample_total = 0.;
	vector<double> subsampling;
	subsampling.resize(nwords_);
	int index = 0;
	for(double wc:word_counts_){
		//subsampling (word2vec)
		double s = wc / (SUBSAMPLING * total_);
		double p = min((sqrt(s) + 1.) / s, 1.);
		//double p = min(sqrt(total_ * SUBSAMPLING / wc), 1.); // mikolov's paper
		subsampling[index] = p;
		//neg_sampling
		count_t pc = pow(wc, .75);
		neg_sample_word_counts.push_back(pair<int32_t, count_t>(index, pc));
		neg_sample_total += pc;
		index++;
	}
	vector<count_t> subsampled_counts;
	subsampled_counts.resize(nwords_, 0);
	{
		ifstream coocc_in(coocc_file_, ios::in | ios::binary);
		if(!coocc_in){
			cerr << coocc_file_ << " not found!!" << endl;
			exit(0);
		}
		int32_t w1, w2;
		double count;
		double subsampled_count_total = 0.;
		int64_t count_total = 0;
		int64_t total = 0;
		while(!coocc_in.eof()){
			coocc_in.read((char *)&w1, sizeof(int32_t));
			if(coocc_in.eof())break;
			coocc_in.read((char *)&w2, sizeof(int32_t));
			coocc_in.read((char *)&count, sizeof(double));

			double subsampled_count = count * subsampling[w1] * subsampling[w2];
			subsampled_counts[w1] += subsampled_count;
			subsampled_count_total += subsampled_count;
			count_total += count;
			total++;
		}
		coocc_in.close();
		cerr << count_total << " (" << (int64_t) subsampled_count_total << " sampled) cooccurrences, " << total << " unique cooccurrences" << endl;
	}
	set<int64_t> antonym_set;
	for(pair<int, std::set<int> > pairs:antonyms_){
		int w1 = pairs.first;
		for(int w2: pairs.second){
			antonym_set.insert(to_scalar(w2, w1));
			antonym_set.insert(to_scalar(w1, w2));
		}
	}
	set<int64_t> synonym_set;
	for(pair<int, std::set<int> > pairs:synonyms_){
		int w1 = pairs.first;
		for(int w2: pairs.second){
			synonym_set.insert(to_scalar(w2, w1));
			synonym_set.insert(to_scalar(w1, w2));
		}
	}
	sort(neg_sample_word_counts.begin(), neg_sample_word_counts.end(), more<int32_t>);
	{
		char *buf[FILES];
		ofstream *tmp_files[FILES];
		for(int i = 0;i < FILES;++i){
			buf[i] = new char[BUFSIZE];
			tmp_files[i] = new ofstream(filename(i), ios::out | ios::binary | ios::trunc);
			tmp_files[i]->rdbuf()->pubsetbuf(buf[i], BUFSIZE);
		}

		unsigned long long next_random = 0.;

		int64_t updates = 0;
		double total_counts = 0., total_neg_counts = 0.;
		int64_t antonym_matches = 0, synonym_matches = 0;
		int file_idx = 0;
		vector<int32_t> *cooccurrences = new vector<int32_t>[nwords_];
		{
			ifstream coocc_in(coocc_file_, ios::in | ios::binary);
			if(!coocc_in){
				cerr << coocc_file_ << " not found!!" << endl;
				exit(0);
			}
			int32_t w1, w2;
			double coocc_count;
			while(!coocc_in.eof()){
				coocc_in.read((char *)&w1, sizeof(int32_t));
				if(coocc_in.eof())break;
				coocc_in.read((char *)&w2, sizeof(int32_t));
				coocc_in.read((char *)&coocc_count, sizeof(double));

				assert(w1 < nwords_);
				cooccurrences[w1].push_back(w2);

				count_t count = coocc_count * subsampling[w1] * subsampling[w2];
				count_t cnt = (subsampled_counts[w1] * K) / neg_sample_total;
				count_t neg_count = cnt * pow(word_counts_[w2], .75);
				if(count < MIN_COUNT){
					count = 0.;
				}else if(count < 1.){
					next_random = next_random * (unsigned long long)25214903917 + 11;
					count = ((next_random & 0xFFFF) / (count_t)65536 < count) ? 1. : 0.;
				}
				if(neg_count < MIN_COUNT){
					neg_count = 0.;
				}else if(neg_count < 1.){
					next_random = next_random * (unsigned long long)25214903917 + 11;
					neg_count = ((next_random & 0xFFFF) / (count_t)65536 < neg_count) ? 1. : 0.;
				}
				if(count > 0. || neg_count > 0.){
					if(synonym_set.find(to_scalar(w1, w2)) != synonym_set.end()){
						continue;
					}
					if(antonym_set.find(to_scalar(w1, w2)) != antonym_set.end()){
						continue;
					}
					total_counts += count;
					total_neg_counts += neg_count;
					tmp_files[file_idx]->write((char *)&w1, sizeof(int32_t));
					tmp_files[file_idx]->write((char *)&w2, sizeof(int32_t));
					tmp_files[file_idx]->write((char *)&count, sizeof(count_t));
					tmp_files[file_idx]->write((char *)&neg_count, sizeof(count_t));
					updates++;
					file_idx = (file_idx+1) % FILES;
				}
			}
			coocc_in.close();
		}
		for(int i = 0;i < nwords_;++i){
			pair<int32_t, count_t>& w1cnt = neg_sample_word_counts[i];

			vector<int32_t>& coocc = cooccurrences[w1cnt.first];
			sort(coocc.begin(), coocc.end());

			int l = 0, r = nwords_ - 1, m = (l+r) / 2;
			count_t cnt = (subsampled_counts[w1cnt.first] * K) / neg_sample_total;
			count_t icnt = MIN_COUNT / cnt;
			while(l < r){
				if(neg_sample_word_counts[m].second >= icnt){
					l = m + 1;
				}else{
					r = m;
				}
				m = (l + r) / 2;
			}
			assert(m == 0 || neg_sample_word_counts[m - 1].second >= icnt);
			assert(m == nwords_ - 1 || neg_sample_word_counts[m].second < icnt);
			for(int j = 0;j < m;++j){
				pair<int32_t, count_t>& w2cnt = neg_sample_word_counts[j];
				if(binary_search(coocc.begin(), coocc.end(), w2cnt.first)){
					continue;
				}
				count_t count = 0.;
				count_t neg_count = cnt * w2cnt.second;

				assert(neg_count >= MIN_COUNT);
				if(neg_count < 1.){
					next_random = next_random * (unsigned long long)25214903917 + 11;
					neg_count = ((next_random & 0xFFFF) / (count_t)65536 < neg_count) ? 1. : 0.;
				}
				if(neg_count > 0.){
					if(synonym_set.find(to_scalar(w1cnt.first, w2cnt.first)) != synonym_set.end()){
						continue;
					}
					if(antonym_set.find(to_scalar(w1cnt.first, w2cnt.first)) != antonym_set.end()){
						continue;
					}
					total_counts += count;
					total_neg_counts += neg_count;
					tmp_files[file_idx]->write((char *)&w1cnt.first, sizeof(int32_t));
					tmp_files[file_idx]->write((char *)&w2cnt.first, sizeof(int32_t));
					tmp_files[file_idx]->write((char *)&count, sizeof(count_t));
					tmp_files[file_idx]->write((char *)&neg_count, sizeof(count_t));
					updates++;
					file_idx = (file_idx+1) % FILES;
				}
			}
		}
		for(int64_t synonym:synonym_set){
			pair<int32_t, int32_t> p = to_pair(synonym);
			count_t count = SYN_WEIGHT;
			count_t neg_count = 0.;
			synonym_matches++;
			if(antonym_set.find(synonym) != antonym_set.end()){
				continue;
			}
			tmp_files[file_idx]->write((char *)&p.first, sizeof(int32_t));
			tmp_files[file_idx]->write((char *)&p.second, sizeof(int32_t));
			tmp_files[file_idx]->write((char *)&count, sizeof(count_t));
			tmp_files[file_idx]->write((char *)&neg_count, sizeof(count_t));
			file_idx = (file_idx+1) % FILES;
		}
		for(int64_t antonym:antonym_set){
			pair<int32_t, int32_t> p = to_pair(antonym);
			count_t count = 0.;
			count_t neg_count = ANT_WEIGHT;
			antonym_matches++;
			tmp_files[file_idx]->write((char *)&p.first, sizeof(int32_t));
			tmp_files[file_idx]->write((char *)&p.second, sizeof(int32_t));
			tmp_files[file_idx]->write((char *)&count, sizeof(count_t));
			tmp_files[file_idx]->write((char *)&neg_count, sizeof(count_t));
			file_idx = (file_idx+1) % FILES;
		}
		print_mem();
		cerr << "Total updates: " << updates << ", counts: " << total_counts << ", neg_counts: " << total_neg_counts << endl;
		cerr << "Total synonym matches: " << synonym_matches << ", antonym matches: " << antonym_matches << endl;
		for(int i = 0;i < FILES;++i){
			tmp_files[i]->close();
			delete tmp_files[i];
			delete[] buf[i];
		}
		delete[] cooccurrences;
	}
	merge_files();
}

template<class T>
void coin::GloveVLBL<T>::merge_files(){
#ifdef _OPENMP
#pragma omp parallel num_threads(N_THREADS)
#endif
	{
		int size = FILES / N_THREADS;
		int start = omp_get_thread_num() * size;
		char *buf_in = new char[BUFSIZE];
		char *buf_out = new char[BUFSIZE];
		string tmp_filename = filename(omp_get_thread_num())+".tmp";
		ofstream out_file(tmp_filename, ios::out | ios::binary);
		default_random_engine rand(omp_get_thread_num());
		out_file.rdbuf()->pubsetbuf(buf_out, BUFSIZE);
		int nupdates = 0;
		vector<UpdateEntry> updates;
		updates.reserve(SHUFFLE_SIZE+1);
		for(int i = 0;i < size;++i){
			string in_filename = filename(start+i);
			ifstream in_file(in_filename, ios::in | ios::binary);
			in_file.rdbuf()->pubsetbuf(buf_in, BUFSIZE);
			while(true){
				int32_t w1, w2;
				count_t count, neg_count;
				in_file.read((char *)&w1, sizeof(int32_t));
				if(in_file.eof())break;
				in_file.read((char *)&w2, sizeof(int32_t));
				in_file.read((char *)&count, sizeof(count_t));
				in_file.read((char *)&neg_count, sizeof(count_t));
				updates.push_back(UpdateEntry(w1, w2, count, neg_count));
				nupdates++;
				if(nupdates > SHUFFLE_SIZE){
					shuffle(updates.begin(), updates.end(), rand);
					for(UpdateEntry entry:updates){
						out_file.write((char*)&entry.w1, sizeof(int32_t));
						out_file.write((char*)&entry.w2, sizeof(int32_t));
						out_file.write((char*)&entry.count, sizeof(count_t));
						out_file.write((char*)&entry.neg_count, sizeof(count_t));
					}
					updates.clear();
					nupdates = 0;
				}
			}
			in_file.close();
			try{
				remove(path(in_filename));
			}catch(filesystem_error& ex) {
				cerr << ex.what() << endl;
				exit(-1);
			}
		}
		if(nupdates > 0){
			shuffle(updates.begin(), updates.end(), rand);
			for(UpdateEntry entry:updates){
				out_file.write((char*)&entry.w1, sizeof(int32_t));
				out_file.write((char*)&entry.w2, sizeof(int32_t));
				out_file.write((char*)&entry.count, sizeof(count_t));
				out_file.write((char*)&entry.neg_count, sizeof(count_t));
			}
		}
		out_file.close();
		delete[] buf_in;
		delete[] buf_out;
	}
	for(int i = 0;i < N_THREADS;i++){
		rename(path(filename(i)+".tmp"), path(filename(i)));
	}
}

template<class T>
void coin::GloveVLBL<T>::load_glove_vector(const std::string& glove_file, const bool skip_id){
	ifstream glove_in(glove_file, ios::in);
	if(!glove_in){
		cerr << glove_file << " not found!!" << endl;
	}
	string line;
	int windex = 0;
	if(skip_id){
		getline(glove_in, line); // skip header
	}
	while(glove_in && getline(glove_in, line)){
		int dim = 0;
		istringstream iss(line);
		string word;
		iss >> word;
		vector<double> embed;
		double bias = 0.;
		if(skip_id){
			int id; double gram;
			iss >> id >> bias >> gram;
		}
		while(iss){
			double v;
			iss >> v;
			if(iss){
				embed.push_back(v);
				dim++;
			}
		}
		dim_ = dim;
		preprocess(word);
		if(word_indices_.find(word) == word_indices_.end()){
			word_indices_[word] = windex;
			word_strings_.push_back(word);
			Vector embeddings(dim);
			for(int i = 0;i < dim;i++){
				embeddings[i] = embed[i];
			}
			words_.push_back(new T(windex, embeddings, bias));
			windex++;
		}else{
			//TODO: lowercase
		}
	}
	cerr << windex << " words are loaded." << endl;
}

template<class T>
void coin::GloveVLBL<T>::save_model(const string& model_file){
	ofstream os(model_file, ios::binary | ios::out | ios::trunc);
	os.write((char *)&niters_, sizeof(int));
	os.write((char *)&dim_, sizeof(int));
	int size = words_.size();
	os.write((char *)&size, sizeof(int));
	for(T* word:words_){
		word->save(os);
	}
	assert(words_.size() == word_indices_.size());
	os.close();
}

template<class T>
void coin::GloveVLBL<T>::save_text_model(const string& model_file){
	ofstream os(model_file, ios::out | ios::trunc);
	os << dim_ << " " << words_.size() << endl;
	for(T* word:words_){
        os << word_strings_[word->id()];
		word->save_text(os);
        os << endl;
	}
	assert(words_.size() == word_indices_.size());
	os.close();
}

template<class T>
void coin::GloveVLBL<T>::load_model(const string& model_file){
	ifstream is(model_file, ios::binary | ios::in );
	if(!is){
		cerr << "error in opening model file: " << model_file << endl;
		exit(0);
	}
	is.read((char *)&niters_, sizeof(int));
	is.read((char *)&dim_, sizeof(int));
	for(T* word:words_){
		delete word;
	}
	int size = 0;
	is.read((char *)&size, sizeof(int));
	words_.resize(size);
	for(int i = 0;i < size;i++){
		T* word = new T(dim_, is);
		words_[word->id()] = word;
	}
	is.close();
}

template<class T>
coin::GloveVLBL<T>::~GloveVLBL(){
	for(T* word:words_){
		delete word;
	}
	if(is_directory(tmpdir_)){
		remove_all(tmpdir_);
	}
}

template<class T>
void coin::GloveVLBL<T>::similar_words(const string& word, int limit, bool reverse){
	if(word_indices_.find(word) == word_indices_.end()){
		cout << "no word vector for " << word << endl;
    int i = 0;
    while(true){
      if(++i > limit)break;
      cout << "unknown" << "\t" << 0.0000 << endl;
    }
		return;
	}
	T *w = words_[word_indices_[word]];
	list<pair<int, double>> sim_words;
	for(T* comp:words_){
		if(w == comp)continue;
    if(reverse){
      sim_words.push_back(pair<int, double>(comp->id(), -w->similarity(*comp)));
    }else{
      sim_words.push_back(pair<int, double>(comp->id(), w->similarity(*comp)));
    }
	}
	sim_words.sort(more<int>);
	int i = 0;
	for(pair<int, double> sim_word:sim_words){
		if(++i > limit)break;
    if(reverse){
      cout << word_strings_[sim_word.first] << "\t" << -sim_word.second << endl;
    }else{
      cout << word_strings_[sim_word.first] << "\t" << sim_word.second << endl;
    }
	}
}

template<class T>
void coin::GloveVLBL<T>::antonym_score(const std::string& file){
	ifstream in(file.c_str());
	if(!in){
		cerr << file << " not found!!" << endl;
		return;
	}
	load_dict();
	string line;
	string target, word, ans_word;
	int w1idx, w2idx;
	T* w1;
	T* w2;
	int correct = 0, total = 0, answer = 0;
	while(in && getline(in, line)){
		total++;
		preprocess(line);
		istringstream iss(line);
		iss >> target;
		target = target.substr(0, target.size() - 1);
		if(word_indices_.find(target) != word_indices_.end()){
			w1idx = word_indices_[target];
			w1 = words_[w1idx];
		}else{
			w1idx = -1;
			continue;
		}
		ans_word = "";
		double min_sim = 1000000.;
		bool skip = false;
		for(int i = 0;i < 5;++i){
			iss >> word;
			double sim = 100.;
			if(word_indices_.find(word) != word_indices_.end()){
				w2idx = word_indices_[word];
				w2 = words_[w2idx];
				sim = w1->similarity(*w2);
			}else{
				w2idx = -1;
				skip = true;
			}
			if(sim < min_sim){
				min_sim = sim;
				ans_word = word;
			}
			if(includes_pair(antonyms_, w1idx, w2idx)){
				cerr << target << ":" << word << ":" << sim << "*" << endl;
			}else{
				cerr << target << ":" << word << ":" << sim << endl;
			}
		}
		iss >> word;
		assert(word == "::");
		iss >> word;
		cerr << "====" << target << ":" << word << " => " << ans_word << endl;
		if(skip){
			continue;
		}
		if(ans_word == word){
			correct++;
		}
		answer++;
	}
	double p = correct/(double)answer;
	double r = correct/(double)total;
	double f = 2. * p * r / (p + r);
	cerr << "P/R/F: " << p << " / " << r << " / " << f << endl;
}

template<class T>
void coin::GloveVLBL<T>::conv(const string& gzfile){
	igzstream in(gzfile.c_str());
	if(!in){
		cerr << gzfile << " not found!!" << endl;
		return;
	}
	string line;
	string word;
	while(in && getline(in, line)){
		preprocess(line);
		istringstream iss(line);
		Vector v(Vector::Zero(dim_));
		while(iss >> word){
			if(word_indices_.find(word) != word_indices_.end()){
				v += words_[word_indices_[word]]->embeddings();
			}
		}
		for(int i = 0;i < dim_;++i){
			cout << v[i] << " ";
		}
		cout << endl;
	}
	in.close();
}

template<class T>
void coin::GloveVLBL<T>::iterate(){
	double cost = 0.;
	long nupdates = 0;
	Timer t;
	omp_lock_t lock[nwords_];
	for (int i = 0;i < nwords_;++i){
	  omp_init_lock(&(lock[i]));
	}

#ifdef _OPENMP
#pragma omp parallel num_threads(N_THREADS) reduction(+:nupdates) reduction(+:cost) default(shared)
#endif
	{
		//unsigned long long next_random = niters_ * omp_get_thread_num();
		char *buf = new char[BUFSIZE];
		ifstream in_file(filename(omp_get_thread_num()), ios::in | ios::binary);
		in_file.rdbuf()->pubsetbuf(buf, BUFSIZE);
		while(true){
			int32_t w1idx, w2idx;
			count_t count, neg_count;
			in_file.read((char *)&w1idx, sizeof(int32_t));
			if(in_file.eof())break;
			in_file.read((char *)&w2idx, sizeof(int32_t));
			in_file.read((char *)&count, sizeof(count_t));
			in_file.read((char *)&neg_count, sizeof(count_t));

			// if(count < 1. && count > 0.){
			// 	next_random = next_random * (unsigned long long)25214903917 + 11;
			// 	count = ((next_random & 0xFFFF) / (count_t)65536 < count) ? 1. : 0.;
			// }
			// if(neg_count < 1. && neg_count > 0.){
			// 	next_random = next_random * (unsigned long long)25214903917 + 11;
			// 	neg_count = ((next_random & 0xFFFF) / (count_t)65536 < neg_count) ? 1. : 0.;
			// }

			T* w1 = words_[w1idx];
			T* w2 = words_[w2idx];

			// ivlbl
			double q = sigmoid(w1->t_embeddings().dot(w2->c_embeddings()) + w2->c_bias());
			//double q = sigmoid(w1->t_embeddings().dot(w2->c_embeddings())); // no bias
			double diff = count * (1. - q) - neg_count * q;
			if(q > 0. && q < 1.){
				cost -= count * log(q) + neg_count * log(1. - q);
			}
			if(diff == 0.){
			    continue;
			}
			Vector w2update(w1->t_embeddings());

			omp_set_lock(&(lock[w1idx]));
			w1->update_t(w2->c_embeddings(), 0., diff);
			omp_unset_lock(&(lock[w1idx]));

			omp_set_lock(&(lock[w2idx]));
			w2->update_c(w2update, 1., diff);
			//w2->update_c(w2update, 0., diff); // no bias
			omp_unset_lock(&(lock[w2idx]));
			// #pragma omp critical
			// {
			// 	cerr << q << ":" << count << ":" << neg_count << ":" << sigmoid(w1->t_embeddings().dot(w2->c_embeddings()) + w2->c_bias()) << endl;
			//}
			nupdates++;
		}
		in_file.close();
		delete[] buf;
	}
	niters_++;
	//reset_history();

    for(int i = 0; i < nwords_;++i){
        omp_destroy_lock(&(lock[i]));
	}
	double time = (double)t.seconds();
	cerr << "cost: " << cost << ", updates: " << nupdates << ", time: " << time << " seconds, speed:" << nupdates / (N_THREADS * time) << " [words/threads/sec]" << endl;
}

template<class T>
void coin::GloveVLBL<T>::reset_history(){
	for(T* w:words_){
		w->reset_history();
	}
}

template<class T>
T coin::GloveVLBL<T>::calc_mean_word(){
	T mean(dim_);
	for(T* word:words_){
		mean.add(*word);
	}
	mean.mult(1. / words_.size());
	return mean;
}

coin::SymmetricWord::SymmetricWord(const SymmetricWord& word):
	bias_(word.bias()), grad_(word.grad_), embeddings_(word.embeddings()){};

coin::SymmetricWord& coin::SymmetricWord::operator=(const SymmetricWord& word) {
	bias_ = word.bias();
	grad_ = word.grad_;
	embeddings_ = word.embeddings();
	return *this;
}

coin::SymmetricWord::SymmetricWord(int dim):
	Word(0), bias_(0.), grad_(0.), embeddings_(Vector::Zero(dim)){
}

coin::SymmetricWord::SymmetricWord(int id, int dim, function<double()> &rand):
	Word(id), bias_(0.), grad_(0.), embeddings_(Vector(dim)){
	for(int i = 0;i < dim;++i){
		embeddings_[i] = rand();
	}
	assert(dim == embeddings_.size());
}

coin::SymmetricWord::SymmetricWord(int id, const Vector &v, double bias):
	Word(id), bias_(bias), grad_(0.), embeddings_(v){
}

inline void coin::SymmetricWord::update(const Vector& vector_grad, const double bias_grad, const double scale){
	grad_ += (vector_grad.squaredNorm() + bias_grad * bias_grad) / (vector_grad.size() + 1.) * scale * scale;
 	double alpha = RHO * scale / sqrt(grad_);
 	embeddings_ += alpha * vector_grad;
 	bias_ += alpha * bias_grad;
}

void coin::SymmetricWord::save(ostream& os){
	os.write((char *)&id_, sizeof(int));
	os.write((char *)&bias_, sizeof(double));
	os.write((char *)&grad_, sizeof(double));
	for(int i = 0;i < embeddings_.size();++i){
		os.write((char*)&embeddings_[i], sizeof(double));
	}
}

void coin::SymmetricWord::save_text(ostream& os){
	for(int i = 0;i < embeddings_.size();++i){
		os << " " << embeddings_[i];
	}
    os << " " << bias_;
}

void coin::SymmetricWord::load(istream& is){
	is.read((char *)&id_, sizeof(int));
	is.read((char *)&bias_, sizeof(double));
	is.read((char *)&grad_, sizeof(double));
	for(int i = 0;i < embeddings_.size();++i){
		is.read((char*)&embeddings_[i], sizeof(double));
	}
}

void coin::SymmetricWord::add(const SymmetricWord& word){
	embeddings_ += word.embeddings();
	bias_ += word.bias();
}

void coin::SymmetricWord::mult(const double mult){
	embeddings_ *= mult;
	bias_ *= mult;
}

// explicit instantiation of a template
namespace coin{
	template class GloveVLBL<SymmetricWord>;
};