/* 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     hypothesis_features.cpp
 * \author   Collin Johnson
 *
 * Definition of FeatureVector.
 */

#include "utils/feature_vector.h"
#include "core/float_comparison.h"
#include "utils/float_io.h"
#include <boost/range/iterator_range.hpp>
#include <cassert>
#include <cmath>
#include <functional>

namespace vulcan
{
namespace utils
{

////////////////////  Operators ///////////////////////////

std::ostream& operator<<(std::ostream& out, const FeatureVector& features)
{
    out << features.version() << ' ' << features.numFeatures() << ' ';
    for (auto f : features) {
        utils::save_floating_point(out, f);
    }

    return out;
}

std::istream& operator>>(std::istream& in, FeatureVector& features)
{
    in >> features.version_;

    int numFeatures = 0;
    in >> numFeatures;

    features.features_.resize(numFeatures);
    for (int n = 0; n < numFeatures; ++n) {
        features.features_[n] = utils::load_floating_point(in);
    }

    return in;
}

bool operator==(const FeatureVector& lhs, const FeatureVector& rhs)
{
    if (lhs.numFeatures() != rhs.numFeatures()) {
        return false;
    }

    for (std::size_t n = 0; n < lhs.numFeatures(); ++n) {
        if (!absolute_fuzzy_equal(lhs.featureAt(n), rhs.featureAt(n))) {
            return false;
        }
    }

    return true;
}

bool operator!=(const FeatureVector& lhs, const FeatureVector& rhs)
{
    return !(lhs == rhs);
}

//////////////////// FeatureVector /////////////////////////////

FeatureVector::FeatureVector(const Features& features, int version) : version_(version), features_(features)
{
}

FeatureVector::FeatureVector(std::size_t numFeatures, int version) : version_(version), features_(numFeatures)
{
}

FeatureVector FeatureVector::sample(const Vector& stddev) const
{
    // Add zero-mean noise for the sampling
    return FeatureVector(features_ + (arma::randn(numFeatures()) % stddev), version());
}

}   // namespace utils
}   // namespace vulcan


/*
	This file is part of the implementation for the technical paper

		Field-Aligned Online Surface Reconstruction
		Nico Schertler, Marco Tarini, Wenzel Jakob, Misha Kazhdan, Stefan Gumhold, Daniele Panozzo
		ACM TOG 36, 4, July 2017 (Proceedings of SIGGRAPH 2017)

	Use of this source code is granted via a BSD-style license, which can be found
	in License.txt in the repository root.

	@author Wenzel Jakob
*/

#include "osr/common.h"

#include <tbb/tbb.h>
#include <Eigen/Geometry>

#include <iomanip>

using namespace osr;

std::string osr::memString(size_t size, bool precise)
{
	double value = (double)size;
	const char *suffixes[] = {
		"B", "KiB", "MiB", "GiB", "TiB", "PiB"
	};
	int suffix = 0;
	while (suffix < 5 && value > 1024.0f) {
		value /= 1024.0f; ++suffix;
	}

	std::ostringstream os;
	os << std::setprecision(suffix == 0 ? 0 : (precise ? 4 : 1))
		<< std::fixed << value << " " << suffixes[suffix];

	return os.str();
}

void osr::coordinate_system(const Vector3f &a, Vector3f &b, Vector3f &c)
{
	if (std::abs(a.x()) > std::abs(a.y())) {
		Float invLen = 1.0f / std::sqrt(a.x() * a.x() + a.z() * a.z());
		c = Vector3f(a.z() * invLen, 0.0f, -a.x() * invLen);
	}
	else {
		Float invLen = 1.0f / std::sqrt(a.y() * a.y() + a.z() * a.z());
		c = Vector3f(0.0f, a.z() * invLen, -a.y() * invLen);
	}
	b = c.cross(a);
}

Float osr::signum(Float value)
{
	return std::copysign((Float)1, value);
}

Float osr::modulo(Float a, Float b)
{
	Float r = std::fmod(a, b);
	return (r < 0.0) ? r + b : r;
}

float osr::fast_acos(float x)
{
	float negate = float(x < 0.0f);
	x = std::abs(x);
	float ret = -0.0187293f;
	ret *= x; ret = ret + 0.0742610f;
	ret *= x; ret = ret - 0.2121144f;
	ret *= x; ret = ret + 1.5707288f;
	ret = ret * std::sqrt(1.0f - x);
	ret = ret - 2.0f * negate * ret;
	return negate * (float)M_PI + ret;
}


// CHAP - The Channel Annotation Package
//
// Copyright (c) 2016 - 2018 Gianni Klesse, Shanlin Rao, Mark S. P. Sansom, and
// Stephen J. Tucker
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <iostream>
#include <functional>

#include <boost/math/tools/minima.hpp>

#include "geometry/spline_curve_1D.hpp"

/*!
 * Constructor for creating a spline curve of given degree from a set of knots
 * and control points.
 */
SplineCurve1D::SplineCurve1D(int degree,
                             std::vector<real> knotVector,
                             std::vector<real> ctrlPoints)
{
    nCtrlPoints_ = ctrlPoints.size();
    nKnots_ = knotVector.size();
    degree_ = degree;

    // ensure minimal number of control points for given degree:
    if( nCtrlPoints_ < degree_ + 1 )
    {
        std::cerr<<"ERROR: Need at least d + 1 control points!"<<std::endl;
        std::cerr<<"d = "<<degree_
                 <<" and n = "<<nCtrlPoints_
                 <<std::endl;
        std::abort();
    }

    // ensure minimal number of knots:
    if( nKnots_ < nCtrlPoints_ + degree_ + 1 )
    {
        std::cerr<<"ERROR: Need at least n + d + 1 knots!"<<std::endl;
        std::cerr<<"d = "<<degree_
                 <<" and n = "<<nCtrlPoints_
                 <<" and k = "<<nKnots_
                 <<std::endl;
        std::abort();
    }

    // assign knot vector and control points:
    knots_ = knotVector;
    ctrlPoints_ = ctrlPoints;
}

/*!
 * Default constror for initialiser lists.
 */
SplineCurve1D::SplineCurve1D()
{

}

/*!
 * Public interface for evaluating the spline curve. In contrast to the method
 * of the same name in SplineCurve3D, this will use constant rather than linear
 * extrapolation when the evaluation point lies outside the range covered by
 * the knot vector.
 */
real
SplineCurve1D::evaluate(
        const real &eval,
        unsigned int deriv)
{
    // interpolation or extrapolation:
    if( eval < knots_.front() || eval > knots_.back() )
    {
        return evaluateExternal(eval, deriv);
    }
    else
    {
        return evaluateInternal(eval, deriv);
    }
}

/*!
 * Public interface for evaluating the spline curve at mutliple points. Uses
 * constant extrapolation.
 */
std::vector<real>
SplineCurve1D::evaluateMultiple(
        const std::vector<real> &eval,
        unsigned int deriv)
{
    // evaluate spline at each point:
    std::vector<real> values;
    for(auto e : eval)
    {
        values.push_back( evaluate(e, deriv) );
    }

    return values;
}

/*!
 * Helper function for evaluating the spline curve at points inside the range
 * covered by the knot vector.
 */
real
SplineCurve1D::evaluateInternal(const real &eval, unsigned int deriv)
{
    // container for basis functions or derivatives:
    SparseBasis basis;

    // derivative required?
    if( deriv == 0 )
    {
        // evaluate B-spline basis:
        basis = B_(eval, knots_, degree_);
    }
    else
    {
        // evaluate B-spline basis derivatives:
        basis = B_(eval, knots_, degree_, deriv);
    }

    // return value of spline curve (derivative) at given evalaution point:
    return computeLinearCombination(basis);

}

/*!
 * Helper function for evaluating the spline curve at points outside the range
 * covered by the knot vector. Constant extrapolation is used here!
 */
real
SplineCurve1D::evaluateExternal(const real &eval, unsigned int deriv)
{
    // which boundary is extrapolation based on?
    real boundary;
    if( eval < knots_.front() )
    {
        boundary = knots_.front();
    }
    else
    {
        boundary = knots_.back();
    }

    // derivative required?
    if( deriv == 0 )
    {
        // return value of curve at boundary:
        SparseBasis basis = B_(boundary, knots_, degree_);
        return computeLinearCombination(basis);
    }
    else
    {
        // for linear extrapolation, second and higher order deriv are zero:
        return 0.0;
    }
}

/*!
 * Auxiliary function for computing the linear combination of basis functions
 * weighted by control points.
 */
real
SplineCurve1D::computeLinearCombination(const SparseBasis &basis)
{
    real value = 0.0;
    for(auto b : basis)
    {
        value += b.second * ctrlPoints_[b.first];
    }

    return value;
}

/*!
 * Getter function for access to the spline curves control points.
 */
std::vector<real>
SplineCurve1D::ctrlPoints() const
{
    return ctrlPoints_;
}

/*!
 * Returns length of curve between first and last (unique) knot.
 */
real
SplineCurve1D::length() const
{
    real sLo = uniqueKnots().front();
    real sHi = uniqueKnots().back();
    return sHi - sLo;
}

/*!
 * Returns a pair struct of argmin and min of function value.
 */
std::pair<real, real>
SplineCurve1D::minimum(const std::pair<real, real> &lim)
{
    // internal parameters:
    real maxSampleDist = 0.1;
    boost::uintmax_t maxIter = 100;

    // draw sample of values and find minimum:
    real length = lim.second - lim.first;
    int nSamples = std::ceil(length/maxSampleDist);
    real sampleDist = length/nSamples;
    std::vector<real> par;
    std::vector<real> val;
    for(int i = 0; i < nSamples; i++)
    {
        par.push_back( lim.first + i*sampleDist );
        val.push_back( evaluate(par.back(), 0) );
    }

    // find smallest sample radius:
    auto itMin = std::min_element(val.begin(), val.end());
    int idxMin = std::distance(val.begin(), itMin);

    // determine bracketing interval:
    real sMin = par[idxMin - 1];
    real sMax = par[idxMin + 1];
    if( itMin == val.begin() )
    {
        sMin = par[idxMin];
        sMax = par[idxMin + 1];
    }
    else if( itMin == val.end() )
    {
        sMin = par[idxMin - 1];
        sMax = par[idxMin];
    }

    // find exact location of minimum through Brent's method:
    return boost::math::tools::brent_find_minima(
            std::bind(&SplineCurve1D::evaluate, this, std::placeholders::_1, 0),
            sMin,
            sMax,
            std::numeric_limits<real>::digits,
            maxIter);
}


#ifndef UQ_CELLS_HASH_TABLE_HPP
#define UQ_CELLS_HASH_TABLE_HPP

#include <bits/stdc++.h>
#include <Eigen/Dense>
#include "mfem.hpp"

using namespace mfem;

//! Defines an element type of the hash list
struct HashTableElement
{
    HashTableElement () {
        velocity.setZero();
    }

    void display() {
        std::cout << id << ", "
                  << weight << ", "
                  << coords.transpose() << ", "
                  << velocity.transpose();
    }

    int id;
    double weight;
    Eigen::Vector2d coords;
    Eigen::Vector2d velocity;
};
typedef struct HashTableElement tabEl;

//! Defines the hash table
class CellsHashTable
{
public:
    //! Constructor
    CellsHashTable(int Nx, double xl, double xr,
                   int Ny, double yl, double yr);

    //! Destructor
    ~ CellsHashTable() { destroy(); }

    //! Hash function to map key to values
    int hash_function(Eigen::Vector2d&);

    //! Searches key in the hash table
    //! returns the cell indices
    std::pair<int, int> search(Eigen::Vector2d&);

    //! Inserts a key in the hash table
    void insert(int, double, Eigen::Vector4d&);

    //! Modifies a key in the hash table
    int modify(int, Eigen::Vector4d&);

    //! Gives cell number from coords
    inline int get_cell_number(int i, int j)
    {
        return std::move(i + j*m_Nx);
    }

    //! Gives cell coords from number
    inline std::pair<int, int> get_cell_indices(int index)
    {
        int i = index%m_Nx;
        int j = index/m_Nx;
        return {std::move(i), std::move(j)};
    }

    //! Displays the hash table
    void display()
    {
        for (int i = 0; i < m_numCells; i++) {
            std::cout << i;
            for (tabEl data : table[i])
            {
                std::cout << " --> ";
                data.display();
            }
        std::cout << std::endl;
      }
    }

    //! Deletes the hash table
    void destroy()
    {
        for (int i=0; i<m_numCells; i++) {
            table[i].clear();
        }
    }

    //! Operators to access table
    std::list<tabEl> operator()(int i, int j) {
        return get_data(i,j);
    }

    std::list<tabEl> operator()(int index) {
        return get_data(index);
    }

    //! Returns table
    std::list<tabEl> *get_table() {
        return table;
    }

    //! Returns the list of data for cell i,j
    //! data = (coords, area)
    std::list<tabEl> get_data(int i, int j) {
        return table[get_cell_number(i,j)];
    }

    //! Returns the list of data for cell i,j
    //! data = (coords, area)
    std::list<tabEl> get_data(int index) {
        return table[index];
    }

    //! Returns the total number of elements
    inline int get_numElements() {
        return get_numElements(0, m_Nx-1, 0, m_Ny-1);
    }

    //! Returns the total number of elements
    inline int get_numElements(int iStart, int iEnd,
                               int jStart, int jEnd)
    {
        int numElements = 0;
        for (int i=iStart; i<=iEnd; i++)
            for (int j=jStart; j<=jEnd; j++) {
                numElements += get_data_size(i,j);
            }
        return numElements;
    }

    //! Returns the number of elements in the list
    //! for cell i,j
    inline int get_data_size(int i, int j) {
        return table[get_cell_number(i,j)].size();
    }

    //! Returns the number of cells along x-axis
    inline int get_numCellsInX() {
        return m_Nx;
    }

    //! Returns the number of cells along y-axis
    inline int get_numCellsInY() {
        return m_Ny;
    }

    //! Returns x-axis left bound
    inline double get_xLeft() {
        return m_xl;
    }

    //! Returns x-axis right bound
    inline double get_xRight() {
        return m_xr;
    }

    //! Returns y-axis left bound
    inline double get_yLeft() {
        return m_yl;
    }

    //! Returns y-axis right bound
    inline double get_yRight() {
        return m_yr;
    }

private:
    int m_Nx;
    double m_xl, m_xr;

    int m_Ny;
    double m_yl, m_yr;

    double m_tol;

    int m_numCells;
    std::list<tabEl> *table;
};

//! Makes the hash table for a given mesh
std::shared_ptr<CellsHashTable> make_hashTable
(std::shared_ptr<Mesh>& mesh,
 int Nx, double xl, double xr,
 int Ny, double yl, double yr);

//! Updates the velocity data in hash table
void update_hashTable
(std::shared_ptr<CellsHashTable>&,
 std::shared_ptr<Mesh>& mesh,
 GridFunction *vx, GridFunction *vy);

//! Updates the velocity fluctuations data in hash table
void update_hashTable
(std::shared_ptr<CellsHashTable>&,
 std::shared_ptr<Mesh>& mesh,
 GridFunction *vx, GridFunction *vxMean,
 GridFunction *vy, GridFunction *vyMean);

#endif /// UQ_CELLS_HASH_TABLE_HPP


/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing,
 *  software distributed under the License is distributed on an "AS
 *  IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either
 *  express or implied.  See the License for the specific language
 *  governing permissions and limitations under the License.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

#include <boost/unordered_set.hpp>
#include <graphlab.hpp>
#include <graphlab/ui/metrics_server.hpp>
#include <graphlab/util/cuckoo_set_pow2.hpp>
#include <graphlab/macros_def.hpp>
/**
 This implements the exact counting procedure described in

 Efficient Algorithms for Large-Scale Local Triangle Counting
 Luca Becchetti, Paolo Boldi, Carlos Castillo, Aristides Gioni

  */

// Radix sort implementation from https://github.com/gorset/radix
// Thanks to Erik Gorset
//
/*
Copyright 2011 Erik Gorset. All rights reserved.

Redistribution and use in source and binary forms, with or without modification, are
permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this list of
conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice, this list
of conditions and the following disclaimer in the documentation and/or other materials
provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Erik Gorset ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL Erik Gorset OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

The views and conclusions contained in the software and documentation are those of the
authors and should not be interpreted as representing official policies, either expressed
or implied, of Erik Gorset.
*/
void radix_sort(graphlab::vertex_id_type *array, int offset, int end, int shift) {
    int x, y;
    graphlab::vertex_id_type value, temp;
    int last[256] = { 0 }, pointer[256];

    for (x=offset; x<end; ++x) {
        ++last[(array[x] >> shift) & 0xFF];
    }

    last[0] += offset;
    pointer[0] = offset;
    for (x=1; x<256; ++x) {
        pointer[x] = last[x-1];
        last[x] += last[x-1];
    }

    for (x=0; x<256; ++x) {
        while (pointer[x] != last[x]) {
            value = array[pointer[x]];
            y = (value >> shift) & 0xFF;
            while (x != y) {
                temp = array[pointer[y]];
                array[pointer[y]++] = value;
                value = temp;
                y = (value >> shift) & 0xFF;
            }
            array[pointer[x]++] = value;
        }
    }

    if (shift > 0) {
        shift -= 8;
        for (x=0; x<256; ++x) {
            temp = x > 0 ? pointer[x] - pointer[x-1] : pointer[0] - offset;
            if (temp > 64) {
                radix_sort(array, pointer[x] - temp, pointer[x], shift);
            } else if (temp > 1) {
                std::sort(array + (pointer[x] - temp), array + pointer[x]);
                //insertion_sort(array, pointer[x] - temp, pointer[x]);
            }
        }
    }
}

size_t HASH_THRESHOLD = 64;

// We on each vertex, either a vector of sorted VIDs
// or a hash set (cuckoo hash) of VIDs.
// If the number of elements is greater than HASH_THRESHOLD,
// the hash set is used. Otherwise the vector is used.
struct vid_vector{
  std::vector<graphlab::vertex_id_type> vid_vec;
  graphlab::cuckoo_set_pow2<graphlab::vertex_id_type, 3> *cset;
  vid_vector(): cset(NULL) { }
  vid_vector(const vid_vector& v):cset(NULL) {
    (*this) = v;
  }

  vid_vector& operator=(const vid_vector& v) {
    if (this == &v) return *this;
    vid_vec = v.vid_vec;
    if (v.cset != NULL) {
      // allocate the cuckoo set if the other side is using a cuckoo set
      // or clear if I alrady have one
      if (cset == NULL) {
        cset = new graphlab::cuckoo_set_pow2<graphlab::vertex_id_type, 3>(-1, 0, 2 * v.cset->size());
      }
      else {
        cset->clear();
      }
      (*cset) = *(v.cset);
    }
    else {
      // if the other side is not using a cuckoo set, lets not use a cuckoo set
      // either
      if (cset != NULL) {
        delete cset;
        cset = NULL;
      }
    }
    return *this;
  }

  ~vid_vector() {
    if (cset != NULL) delete cset;
  }

  // assigns a vector of vertex IDs to this storage.
  // this function will clear the contents of the vid_vector
  // and reconstruct it.
  // If the assigned values has length >= HASH_THRESHOLD,
  // we will allocate a cuckoo set to store it. Otherwise,
  // we just store a sorted vector
  void assign(const std::vector<graphlab::vertex_id_type>& vec) {
    clear();
    if (vec.size() >= HASH_THRESHOLD) {
        // move to cset
        cset = new graphlab::cuckoo_set_pow2<graphlab::vertex_id_type, 3>(-1, 0, 2 * vec.size());
        foreach (graphlab::vertex_id_type v, vec) {
          cset->insert(v);
        }
    }
    else {
      vid_vec = vec;
      if (vid_vec.size() > 64) {
        radix_sort(&(vid_vec[0]), 0, vid_vec.size(), 24);
      }
      else {
        std::sort(vid_vec.begin(), vid_vec.end());
      }
      std::vector<graphlab::vertex_id_type>::iterator new_end = std::unique(vid_vec.begin(),
                                               vid_vec.end());
      vid_vec.erase(new_end, vid_vec.end());
    }
  }

  void save(graphlab::oarchive& oarc) const {
    oarc << (cset != NULL);
    if (cset == NULL) oarc << vid_vec;
    else oarc << (*cset);
  }

  void clear() {
    vid_vec.clear();
    if (cset != NULL) {
      delete cset;
      cset = NULL;
    }
  }

  size_t size() const {
    return cset == NULL ? vid_vec.size() : cset->size();
  }

  void load(graphlab::iarchive& iarc) {
    clear();
    bool hascset;
    iarc >> hascset;
    if (!hascset) iarc >> vid_vec;
    else {
      cset = new graphlab::cuckoo_set_pow2<graphlab::vertex_id_type, 3>(-1, 0, 2);
      iarc >> (*cset);
    }
  }
};

/*
  A simple counting iterator which can be used as an insert iterator.
  but only counts the number of elements inserted. Useful for
  use with counting the size of an intersection using std::set_intersection
*/
template <typename T>
struct counting_inserter {
  size_t* i;
  counting_inserter(size_t* i):i(i) { }
  counting_inserter& operator++() {
    ++(*i);
    return *this;
  }
  void operator++(int) {
    ++(*i);
  }

  struct empty_val {
    empty_val operator=(const T&) { return empty_val(); }
  };

  empty_val operator*() {
    return empty_val();
  }

  typedef empty_val reference;
};

/*
 * Computes the size of the intersection of two vid_vector's
 */
static uint32_t count_set_intersect(
             const vid_vector& smaller_set,
             const vid_vector& larger_set) {
  if (smaller_set.size() > larger_set.size()) {
    return count_set_intersect(larger_set, smaller_set);
  }
  if (smaller_set.cset == NULL && larger_set.cset == NULL) {
    size_t i = 0;
    counting_inserter<graphlab::vertex_id_type> iter(&i);
    std::set_intersection(smaller_set.vid_vec.begin(), smaller_set.vid_vec.end(),
                          larger_set.vid_vec.begin(), larger_set.vid_vec.end(),
                          iter);
    return i;
  }
  else if (smaller_set.cset == NULL && larger_set.cset != NULL) {
    size_t i = 0;
    foreach(graphlab::vertex_id_type vid, smaller_set.vid_vec) {
      i += larger_set.cset->count(vid);
    }
    return i;
  }
  else if (smaller_set.cset != NULL && larger_set.cset == NULL) {
    size_t i = 0;
    foreach(graphlab::vertex_id_type vid, larger_set.vid_vec) {
      i += smaller_set.cset->count(vid);
    }
    return i;
  }
  else {
    size_t i = 0;
    foreach(graphlab::vertex_id_type vid, *(smaller_set.cset)) {
      i += larger_set.cset->count(vid);
    }
    return i;

  }
}

// This structure is used to hold the final triangle counts
// on each vertex
struct triangle_count: public graphlab::IS_POD_TYPE {
  triangle_count(): out_triangles(0), in_triangles(0),
                      through_triangles(0), cycle_triangles(0) { }
  // A is the example below
  /*
    A ---> B
    |   /
    |  /
    v /
    C
           diagonal edge direction does not matter
  */
  uint32_t out_triangles;

/*
    A<--- B
    ^   /
    |  /
    | /
    C
          diagonal edge direction does not matter
  */
  uint32_t in_triangles;

  /*
    A---> B
    ^   ^
    |  /
    | /
    C
  */
  uint32_t through_triangles;

  /*
    A---> B
    ^   /
    |  /
    | v
    C
  */
  uint32_t cycle_triangles;

  triangle_count& operator+=(const triangle_count& other) {
    out_triangles += other.out_triangles;
    in_triangles += other.in_triangles;
    through_triangles += other.through_triangles;
    cycle_triangles += other.cycle_triangles;
    return *this;
  }
};

/*
 * Each vertex maintains a list of all its neighbors.
 * and a final count for the number of triangles it is involved in
 */
struct vertex_data_type {
  vertex_data_type(){ }
  // A list of all its neighbors
  vid_vector in_vid_set;
  vid_vector out_vid_set;
  triangle_count count;
 // The number of triangles this vertex is involved it.
  // only used if "per vertex counting" is used
  void save(graphlab::oarchive &oarc) const {
    oarc << in_vid_set << out_vid_set << count;
  }
  void load(graphlab::iarchive &iarc) {
    iarc >> in_vid_set >> out_vid_set >> count;
  }
};

// This structure is used to hold the final triangle counts
// on each edge
struct edge_triangle_count: public graphlab::IS_POD_TYPE {
  edge_triangle_count(): s_s(0), st_st(0),
                      st_s(0) { }
  // using notation from the paper
  // s_s is the intersection between outgoing of source and outgoing of target
  // st_st is intersection between incoming of source and incoming of target
  // st_s is intersectino between incoming of source and outgoing of target
  uint32_t s_s, st_st, st_s;

  edge_triangle_count & operator+=(const edge_triangle_count& other) {
    s_s += other.s_s;
    st_st += other.st_st;
    st_s += other.st_s;
    return *this;
  }
};

/*
 * Each edge is simply a counter of triangles
 */
typedef edge_triangle_count edge_data_type;

/*
 * This is the gathering type which accumulates an array of
 * all neighboring vertices.
 * It is a simple wrapper around a vector with
 * an operator+= which simply performs a  +=
 */
struct set_union_gather {
  graphlab::vertex_id_type v;
  std::vector<graphlab::vertex_id_type> vid_vec;

  set_union_gather():v(-1) {
  }

  size_t size() const {
    if (v == (graphlab::vertex_id_type)-1) return vid_vec.size();
    else return 1;
  }
  /*
   * Combining with another collection of vertices.
   * Union it into the current set.
   */
  set_union_gather& operator+=(const set_union_gather& other) {
    if (size() == 0) {
      (*this) = other;
      return (*this);
    }
    else if (other.size() == 0) {
      return *this;
    }

    if (vid_vec.size() == 0) {
      vid_vec.push_back(v);
      v = (graphlab::vertex_id_type)(-1);
    }
    if (other.vid_vec.size() > 0) {
      size_t ct = vid_vec.size();
      vid_vec.resize(vid_vec.size() + other.vid_vec.size());
      for (size_t i = 0; i < other.vid_vec.size(); ++i) {
        vid_vec[ct + i] = other.vid_vec[i];
      }
    }
    else if (other.v != (graphlab::vertex_id_type)-1) {
      vid_vec.push_back(other.v);
    }
    return *this;
  }

  // serialize
  void save(graphlab::oarchive& oarc) const {
    oarc << bool(vid_vec.size() == 0);
    if (vid_vec.size() == 0) oarc << v;
    else oarc << vid_vec;
  }

  // deserialize
  void load(graphlab::iarchive& iarc) {
    bool novvec;
    v = (graphlab::vertex_id_type)(-1);
    vid_vec.clear();
    iarc >> novvec;
    if (novvec) iarc >> v;
    else iarc >> vid_vec;
  }
};

struct set_union_pair{
  set_union_gather in_set;
  set_union_gather out_set;

  set_union_pair& operator+=(const set_union_pair& other) {
    in_set += other.in_set;
    out_set += other.out_set;
    return (*this);
  }

  void save(graphlab::oarchive& oarc) const {
    oarc << in_set << out_set;
  }

  // deserialize
  void load(graphlab::iarchive& iarc) {
    iarc >> in_set >> out_set;
  }

};

/*
 * Define the type of the graph
 */
typedef graphlab::distributed_graph<vertex_data_type,
                                    edge_data_type> graph_type;

/*
 * This class implements the triangle counting algorithm as described in
 * the header. On gather, we accumulate a set of all adjacent vertices.
 * If per_vertex output is not necessary, we can use the optimization
 * where each vertex only accumulates neighbors with greater vertex IDs.
 */
class triangle_count_program :
      public graphlab::ivertex_program<graph_type,
                                      set_union_pair>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  bool do_not_scatter;

  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }

  /*
   * For each edge, figure out the ID of the "other" vertex
   * and accumulate a set of the neighborhood vertex IDs.
   */
  gather_type gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    set_union_pair gather;
    // check the edge direction
    if (edge.source().id() == vertex.id()) {
      // this is an out_edge
      graphlab::vertex_id_type otherid = edge.target().id();
      gather.out_set.v = otherid;
    }
    else {
      // this is an in_edge
      graphlab::vertex_id_type otherid = edge.source().id();
      gather.in_set.v = otherid;
    }
    return gather;
  }

  /*
   * the gather result now contains the vertex IDs in the neighborhood.
   * store it on the vertex.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& neighborhood) {
   do_not_scatter = false;
   if (neighborhood.in_set.vid_vec.size() == 0) {
     // neighborhood set may be empty or has only 1 element
     vertex.data().in_vid_set.clear();
     if (neighborhood.in_set.v != (graphlab::vertex_id_type(-1))) {
       vertex.data().in_vid_set.vid_vec.push_back(neighborhood.in_set.v);
     }
   }
   else {
     vertex.data().in_vid_set.assign(neighborhood.in_set.vid_vec);
   }

   if (neighborhood.out_set.vid_vec.size() == 0) {
     // neighborhood set may be empty or has only 1 element
     vertex.data().out_vid_set.clear();
     if (neighborhood.out_set.v != (graphlab::vertex_id_type(-1))) {
       vertex.data().out_vid_set.vid_vec.push_back(neighborhood.out_set.v);
     }
   }
   else {
     vertex.data().out_vid_set.assign(neighborhood.out_set.vid_vec);
   }

   do_not_scatter = vertex.data().in_vid_set.size() == 0 &&
                    vertex.data().out_vid_set.size() == 0 ;
  } // end of apply

  /*
   * Scatter over all edges to compute the intersection.
   * I only need to touch each edge once, so if I scatter just on the
   * out edges, that is sufficient.
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    if (do_not_scatter) return graphlab::NO_EDGES;
    else return graphlab::OUT_EDGES;
  }

  /*
   * For each edge, count the intersection of the neighborhood of the
   * adjacent vertices. This is the number of triangles this edge is involved
   * in.
   */
  void scatter(icontext_type& context,
              const vertex_type& vertex,
              edge_type& edge) const {

    //vertex_type othervtx = edge.target();
    // ok. the real work happens here.

    const vertex_data_type& srclist = edge.source().data();
    const vertex_data_type& targetlist = edge.target().data();

    edge.data().s_s = count_set_intersect(srclist.out_vid_set,
                                    targetlist.out_vid_set);
    edge.data().st_st += count_set_intersect(srclist.in_vid_set,
                                    targetlist.in_vid_set);
    edge.data().st_s += count_set_intersect(srclist.in_vid_set,
                                    targetlist.out_vid_set);

  }
};

/*
 * This class is used in a second engine call if per vertex counts are needed.
 * The number of triangles a vertex is involved in can be computed easily
 * by summing over the number of triangles each adjacent edge is involved in
 * and dividing by 2.
 */
class get_per_vertex_count :
      public graphlab::ivertex_program<graph_type, triangle_count>,
      /* I have no data. Just force it to POD */
      public graphlab::IS_POD_TYPE  {
public:
  // Gather on all edges
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }
  // We gather the number of triangles each edge is involved in
  triangle_count gather(icontext_type& context,
                     const vertex_type& vertex,
                     edge_type& edge) const {
    triangle_count ret;
    if (edge.source().id() == vertex.id()) {
      ret.out_triangles += edge.data().s_s;
      ret.through_triangles += edge.data().st_st;
      ret.cycle_triangles += edge.data().st_s;
    }
    else {
      ret.in_triangles += edge.data().st_st;
    }
    return ret;
  }

  /* the gather result is the total sum of the number of triangles
   * each adjacent edge is involved in . Dividing by 2 gives the
   * desired result.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const gather_type& tc) {
    vertex.data().in_vid_set.clear();
    vertex.data().out_vid_set.clear();
    vertex.data().count = tc;
  }

  // No scatter
  edge_dir_type scatter_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::NO_EDGES;
  }

};

typedef graphlab::synchronous_engine<triangle_count_program> engine_type;

/* Used to sum over all the vertices in the graph in a
 * map_reduce_vertices call
 * to get the total number of triangles
 */
triangle_count get_vertex_counts(const graph_type::vertex_type& v) {
  return v.data().count;
}

/*
 * A saver which saves a file where each line is a vid / # triangles pair
 */
struct save_triangle_count{
  std::string save_vertex(graph_type::vertex_type v) {
    triangle_count tc = v.data().count;
    double n_followed = v.num_out_edges();
    double n_following = v.num_in_edges();

    return graphlab::tostr(v.id()) + "\t" +
           graphlab::tostr(tc.in_triangles) + "\t" +
           graphlab::tostr(tc.out_triangles) + "\t" +
           graphlab::tostr(tc.through_triangles) + "\t" +
           graphlab::tostr(tc.cycle_triangles) + "\t" +
           graphlab::tostr(n_followed) + "\t" +
           graphlab::tostr(n_following) + "\n";
  }
  std::string save_edge(graph_type::edge_type e) {
    return "";
  }
};

int main(int argc, char** argv) {
  std::cout << "This program counts the exact number of triangles in the "
            "provided graph.\n\n";

  graphlab::command_line_options clopts("Exact Triangle Counting. "
    "Given a graph, this program computes the total number of triangles "
    "in the graph. An option (per_vertex) is also provided which "
    "computes for each vertex, the number of triangles it is involved in."
    "The algorithm assumes that each undirected edge appears exactly once "
    "in the graph input. If edges may appear more than once, this procedure "
    "will over count.");
  std::string prefix, format;
  std::string per_vertex;
  bool PER_VERTEX_COUNT = false;
  clopts.attach_option("graph", prefix,
                       "Graph input. reads all graphs matching prefix*");
  clopts.attach_option("format", format,
                       "The graph format");
  clopts.attach_option("ht", HASH_THRESHOLD,
                       "Above this size, hash tables are used");
  clopts.attach_option("per_vertex", per_vertex,
                       "If not empty, will count the number of "
                       "triangles each vertex belongs to and "
                       "save to file with prefix \"[per_vertex]\". "
                       "The algorithm used is slightly different "
                       "and thus will be a little slower");
  if(!clopts.parse(argc, argv)) return EXIT_FAILURE;
  if (prefix == "") {
    std::cout << "--graph is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }
  else if (format == "") {
    std::cout << "--format is not optional\n";
    clopts.print_description();
    return EXIT_FAILURE;
  }

  if (per_vertex != "") PER_VERTEX_COUNT = true;
  // Initialize control plane using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;

  graphlab::launch_metric_server();
  // load graph
  graph_type graph(dc, clopts);
  graph.load_format(prefix, format);
  graph.finalize();
  dc.cout() << "Number of vertices: " << graph.num_vertices() << std::endl
            << "Number of edges:    " << graph.num_edges() << std::endl;

  graphlab::timer ti;

  // create engine to count the number of triangles
  dc.cout() << "Counting Triangles..." << std::endl;
  engine_type engine(dc, graph, clopts);
  engine.signal_all();
  engine.start();

  dc.cout() << "Counted in " << ti.current_time() << " seconds" << std::endl;
  dc.cout() << "Collecting results ... " << std::endl;
  graphlab::synchronous_engine<get_per_vertex_count> engine2(dc, graph, clopts);
  engine2.signal_all();
  engine2.start();

  if (PER_VERTEX_COUNT == false) {
    triangle_count count = graph.map_reduce_vertices<triangle_count>(get_vertex_counts);
    dc.cout() << count.in_triangles << " In triangles\n";
    dc.cout() << count.out_triangles << " Out triangles\n";
    dc.cout() << count.through_triangles << " Through triangles\n";
    dc.cout() << count.cycle_triangles << " Cycle triangles\n";
  }
  else {
   dc.cout() << "Saving Results...\n";
   dc.cout() << "Format is \n";
   dc.cout() << "   [vid]  [in triangles]  [out triangles]   [through triangles]  [cycle_triangles]  [#out edges] [#in edges]" << std::endl;
   graph.save(per_vertex,
            save_triangle_count(),
            false, /* no compression */
            true, /* save vertex */
            false, /* do not save edge */
            clopts.get_ncpus()); /* one file per machine */

  }

  graphlab::stop_metric_server();

  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;
} // End of main


#ifndef utils_utils_hpp
#define utils_utils_hpp

#include <vector>
#include <opencv2/opencv.hpp>
#include <fstream>
#include <string>
#include <regex>

#include <stdio.h>      /* printf, scanf, puts, NULL */
#include <stdlib.h>     /* srand, rand */
#include <time.h>       /* time */
#include <math.h>

#include "PerceptualHash.h"

#include "Triangle.h"
#include "mainImageProcessingFunctions.hpp"
#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <iostream>
#include "utils.hpp"
#include <tuple>

using boost::property_tree::ptree;
using boost::property_tree::read_json;
using boost::property_tree::write_json;

namespace pt = boost::property_tree;
using namespace std;

static unsigned long x=123456789, y=362436069, z=521288629;

unsigned long xorshf96(void) {          //period 2^96-1
    unsigned long t;
    x ^= x << 16;
    x ^= x >> 5;
    x ^= x << 1;

    t = x;
    x = y;
    y = z;
    z = t ^ x ^ y;

    return z;
}

cv::Mat convertKeypointsVectorToMat(vector<Keypoint> kps)
{
	cv::Mat ret = cv::Mat::zeros(3, kps.size(), CV_64F);

	for (unsigned int i = 0; i < kps.size(); i++)
	{
		auto k = kps[i];
		ret.at<double>(0, i) = k.x;
		ret.at<double>(1, i) = k.y;
		ret.at<double>(2, i) = 1;
	}
	return ret;
}

vector<Keypoint> convertMatToKeypointsVector(cv::Mat inputPoints)
{
	vector<Keypoint> ret;
	for (unsigned int i = 0; i < (unsigned int) inputPoints.cols; i++)
	{
		double x = inputPoints.at<double>(0, i);
		double y = inputPoints.at<double>(1, i);
		Keypoint temp(x, y);
		ret.push_back(temp);
	}
	return ret;
}

void drawSingleTriangleOntoImage(Triangle tri, cv::Mat inputImage, bool setColour = false, cv::Scalar colourInput = cv::Scalar(0,0,0)){
    auto keypoints = tri.toKeypoints();
    auto prevPoint = keypoints.back();

    int r = (int) xorshf96();
    int g = (int) xorshf96();
    int b = (int) xorshf96();
    for (int i = 0; i < 3; i++){
        auto currentPoint = keypoints[i];
        auto colour = (setColour)? colourInput: cv::Scalar(b,g,r);

        cv::line(inputImage, cv::Point(prevPoint.x, prevPoint.y), cv::Point(currentPoint.x, currentPoint.y),
                 colour);
        //cv::imshow("something", inputImage);
        //cv::waitKey(10);
        prevPoint = currentPoint;
    }
}

void drawTrianglesOntoImage(vector<Triangle> tris, cv::Mat inputImage, bool randomColours = true)
{
    for (auto tri: tris){
        drawSingleTriangleOntoImage(tri, inputImage, !randomColours);
    }
}

vector<Keypoint> applyTransformationMatrixToKeypointVector(vector<Keypoint> keypoints, cv::Mat transformationMat)
{
	cv::Mat keypointMat = convertKeypointsVectorToMat(keypoints);
	cv::Mat transKeypointMat = transformationMat*keypointMat;
	return convertMatToKeypointsVector(transKeypointMat);
}

vector<Keypoint> readKeypointsFromJsonFile(string filename);

//TODO: explain why this sucks
vector<Keypoint> getKeypoints(cv::Mat inputImage)
{
	cv::imwrite("tempImage.jpg", inputImage);
	FILE* file = popen("python ./src/dumpKeypointsToJson.py ./tempImage.jpg ./tempOutputKeypoints.json", "r");
	pclose(file);
	return readKeypointsFromJsonFile("./tempOutputKeypoints.json");
}

cv::Size calcBoundingRectangleOfShape(cv::Mat shape)
{
	//convert Mat to vector of 2d Points
	vector<cv::Point> convertedMat;
	for (int i = 0; i < shape.cols; i++) {
		double x = shape.at<double>(i);
		double y = shape.at<double>(shape.cols + i);
		cv::Point tempPt(x, y);
		convertedMat.push_back(tempPt);
	}

	auto resultRect = cv::boundingRect(cv::Mat(convertedMat));
	return resultRect.size();
}

Triangle getTriangleFromRedisEntry(string redisEntry)
{
	pt::ptree root;
	std::stringstream ss;
	ss << redisEntry;
	pt::read_json(ss, root);

	vector<Keypoint> keypoints;
	for (auto pt_j : root.get_child("triangle"))
	{
		double x = pt_j.second.get<double>("x");
		double y = pt_j.second.get<double>("y");
		keypoints.push_back(Keypoint(x, y));
	}
	return Triangle(keypoints);
}

string getImageNameFromRedisEntry(string redisEntry)
{
	pt::ptree root;
	std::stringstream ss;
	ss << redisEntry;
	pt::read_json(ss, root);
	return root.get<string>("imageName");
}

string convertToRedisEntryJson(string imageName, Triangle tri)
{
	pt::ptree root;
	root.put("imageName", imageName);

	pt::ptree points;
	for (auto pt : tri.toKeypoints())
	{
		pt::ptree point;
		point.put("x", pt.x);
		point.put("y", pt.y);
		points.push_back(std::make_pair("", point));
	}
	root.add_child("triangle", points);

	std::ostringstream buf;
	write_json(buf, root, false);
	return buf.str();
}

double getKeypointDistance(Keypoint one, Keypoint two)
{
	return sqrt(pow(one.x - two.x, 2.0) + pow(one.y - two.y, 2.0));
}

vector<Keypoint> findKeypointsWithInRangeFromTwoPoints(Keypoint one, Keypoint two, vector<Keypoint> otherKeypoints, double lowerThreshold, double upperThreshold)
{
	vector<Keypoint> result;
	for (auto cmpKp : otherKeypoints)
	{
		double distanceFromPointOne = getKeypointDistance(one, cmpKp);
		double distanceFromPointTwo = getKeypointDistance(two, cmpKp);
		if (distanceFromPointOne > lowerThreshold && distanceFromPointOne < upperThreshold
			&& distanceFromPointTwo > lowerThreshold && distanceFromPointTwo < upperThreshold)
		{
			result.push_back(cmpKp);
		}
	}
	return result;
}

bool isInKeypointExcludeList(Keypoint keypoint, vector<Keypoint> excludeList) {
	for (auto kp : excludeList)
	{
		if (kp.x == keypoint.x && kp.y == keypoint.y){
			return true;
		}
	}
	return false;
}

bool shouldPointBeExcluded(Keypoint pt, vector<Keypoint> previouslyProcessedPoints, vector<Keypoint> currentProcessedPoints, Keypoint currentTopLevelPoint, Keypoint currentSecondLevelPoint)
{
	return isInKeypointExcludeList(pt, previouslyProcessedPoints)
		|| isInKeypointExcludeList(pt, currentProcessedPoints)
		|| currentTopLevelPoint == pt
		|| currentSecondLevelPoint == pt
		;
}

//TODO: explain why this sucks
vector<Triangle> buildTrianglesForSingleKeypoint(Keypoint centerKeypoint, vector<Keypoint> otherKeypoints, vector<Keypoint> previouslyProcessedPoints, double lowerThreshold, double upperThreshold)
{
	vector<Triangle> result;
	vector<Keypoint> currentProcessedPoints;//a collection of points we have processed since entering this function
	for (auto iterKeypoint : otherKeypoints)
	{
		if (isInKeypointExcludeList(iterKeypoint, previouslyProcessedPoints) || iterKeypoint == centerKeypoint) {
			continue;
		}

		double distance = getKeypointDistance(iterKeypoint, centerKeypoint);
		if (distance > lowerThreshold && distance < upperThreshold)
		{
			vector<Keypoint> finalKeypoints = findKeypointsWithInRangeFromTwoPoints(iterKeypoint, centerKeypoint, otherKeypoints, lowerThreshold, upperThreshold);
			for (auto finKp : finalKeypoints)
			{
				//check if this combination of points will make a triangle we have already created
				if (shouldPointBeExcluded(finKp, previouslyProcessedPoints, currentProcessedPoints, centerKeypoint, iterKeypoint)){
					continue;
				}
				Triangle testingTri(centerKeypoint, iterKeypoint, finKp);
				if (testingTri.calcArea() > 1300){
					result.push_back(testingTri);
				}
			}
		}
		currentProcessedPoints.push_back(iterKeypoint);
	}
	return result;
}

vector<Triangle> buildTrianglesFromKeypoints(vector<Keypoint> keypoints, double lowerThreshold = 150, double upperThreshold = 500)
{
	vector<Triangle> outputTriangles;
	//	for (auto keypoint: keypoints)
	//	{
	//FIXME: this multi-threading needs to be improved
#pragma omp parallel
	{
		vector<Triangle> vec_private;
#pragma omp for nowait schedule(static)
		for (unsigned int i = 0; i < keypoints.size(); i++)
		{
			vector<Keypoint>::const_iterator first = keypoints.begin();
			vector<Keypoint>::const_iterator last = keypoints.begin() + i;
			vector<Keypoint> processedPoints(first, last);
			auto keypoint = keypoints[i];
			auto triangles = buildTrianglesForSingleKeypoint(keypoint, keypoints, processedPoints, lowerThreshold, upperThreshold);
			for (auto tri : triangles){
				vec_private.push_back(tri);
			}
			processedPoints.push_back(keypoint);
		}
#pragma omp critical
		outputTriangles.insert(outputTriangles.end(), vec_private.begin(), vec_private.end());
	}
	return outputTriangles;
}

vector<Triangle> buildTrianglesFromKeypointJsonFile(string filename)
{
	vector<Keypoint> output = readKeypointsFromJsonFile(filename);
	vector<Triangle> ret = buildTrianglesFromKeypoints(output, 50, 400);
	return ret;
}

//TODO: see if we can remove this
vector<Triangle> getTriangles(string filename)
{
	return buildTrianglesFromKeypointJsonFile(filename);
}

ShapeAndPositionInvariantImage getLoadedImage(string imageFullPath)
{
	cout << "Loading image: " << imageFullPath;
	cv::Mat img = cv::imread(imageFullPath);
	cout << " ... done" << endl;
	return ShapeAndPositionInvariantImage("", img, std::vector<Keypoint>(), "");
}

template<typename T>
const vector<T> readJsonHashesFile(std::ifstream *file)
{
	vector<T> ret;
	vector<Triangle> image1OutputTriangles;
	vector<Triangle> image2OutputTriangles;
	try {
		boost::property_tree::ptree pt;
		boost::property_tree::read_json(*file, pt);

		for (auto label0 : pt) {
			if (label0.first == "output") {
				for (auto label1 : label0.second) {
					if (label1.first == "imageName") {
						//TODO: process the imageName
					}
					else if (label1.first == "hashes") {
						for (auto hash_item : label1.second) {
							ret.push_back(T(hash_item.second.get_value<std::string>()));
						}
					}
				}
			}
		}

	}
	catch (std::exception const &e) {
		std::cerr << e.what() << std::endl;
	}

	return ret;
}

template<typename T>
const vector<T> readJsonHashesFile(const string filename)
{
	std::ifstream file(filename);
	return readJsonHashesFile<T>(&file);
}

vector<Keypoint> readKeypointsFromJsonFile(std::ifstream *file)
{
	vector<Keypoint> result;
	try {
		boost::property_tree::ptree pt;
		boost::property_tree::read_json(*file, pt);

		for (auto label0 : pt) {
			if (label0.first == "output") {
				for (auto label1 : label0.second) {
					if (label1.first == "keypoints") {
						for (auto kp : label1.second){
							double x, y;
							for (auto pt : kp.second){
								if (pt.first == "x"){
									x = pt.second.get_value<double>();
								}
								else{
									y = pt.second.get_value<double>();
								}
							}
							result.push_back(Keypoint(x, y));
						}
					}
				}
			}
		}
	}
	catch (std::exception const &e) {
		std::cerr << e.what() << std::endl;
	}
	return result;
}

vector<Keypoint> readKeypointsFromJsonFile(string filename)
{
	std::ifstream file(filename);
	return readKeypointsFromJsonFile(&file);
}

#endif//utils_utils_hpp


#include<bits/stdc++.h>

//#define USE_GRAPHVIZ

#ifdef USE_GRAPHVIZ
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#endif

enum {
    NOTFOUND = 0xFFFFFFFFFFFFFFFFLLU
};

uint64_t NODE_NO = 0;

class Node {
public:
    uint64_t no;    // node番号

    // internal nodeのときに使用
    uint64_t num;       // 左の子の部分木のもつbitの数
    uint64_t ones;      // 左の子の部分木のもつ1の数
    Node *left;
    Node *right;
    int64_t balance;    // 右の子の高さ - 左の子の高さ．+なら右の子の方が高い，-なら左の子の方が高い

    // leafのときに使用
    uint64_t bits;       // bit
    uint64_t bits_size;  // bitのサイズ(debug用)

    bool is_leaf;

    Node(uint64_t bits, uint64_t bits_size, bool is_leaf) : no(NODE_NO++), num(0), ones(0), bits(bits), bits_size(bits_size), is_leaf(is_leaf), left(nullptr), right(nullptr), balance(0) {}

    bool is_valid_node() {
        if (is_leaf) {
            if (num != 0) { return false; }
            if (ones != 0) { return false; }
            if (left != nullptr) { return false; }
            if (right != nullptr) { return false; }
        }
        else {
            if (num == 0) { return false; }
            if (left == nullptr) { return false; }
            if (right == nullptr) { return false; }
            if (bits != 0) { return false; }
            if (bits_size != 0) { return false; }
            if (ones > num) {return false; }
        }
        return true;
    }

    std::string info() {
        std::string str = "No:" + std::to_string(this->no) + "\n";
        if (is_leaf) {
            str += "size:" + std::to_string(this->bits_size) + "\n";
            for (int i = 0; i < bits_size; ++i) {
                str += std::to_string((bits >> (uint64_t)i) & (uint64_t)1);
            }
        }
        else {
            str += "num:" + std::to_string(this->num) + " ones:" + std::to_string(this->ones) + "\n";
        }

        return str;
    }
};

class DynamicBitVector {
public:
    Node *root;
    uint64_t size;                         // 全体のbitの数
    uint64_t num_one;                      // 全体の1の数
    const uint64_t bits_size_limit = 32;   // 各ノードが管理するbitの長さ制限．2 * bits_size_limit以上になったらノードを分割し， 1/2*bits_size_limit以下になったらノードを結合する

    DynamicBitVector(): root(nullptr), size(0), num_one(0) {}

    DynamicBitVector(std::vector<uint64_t> &v): root(nullptr), size(0), num_one(0) {
        if (v.size() == 0) {
            return;
        }

        std::deque<std::pair<Node*, uint64_t>> leaves;
        for (int i = 0; i < v.size(); i += this->bits_size_limit) {
            uint64_t bits = 0;
            const uint64_t bits_size = std::min(this->bits_size_limit, v.size() - i);
            for (int j = 0; j < bits_size; ++j) {
                assert(v[i + j] == 0 or v[i + j] == 1);
                if (v[i + j] == 1) {
                    bits |= (uint64_t)1 << j;
                }
            }

            leaves.emplace_back(std::make_pair(new Node(bits, bits_size, true), bits_size));
        }

        std::deque<std::tuple<Node*, uint64_t, uint64_t, uint64_t>> nodes;   // (node, 全体のbit数, 全体の1の数, 高さ)
        while (not leaves.empty()) {
            const auto node = leaves.front().first;
            const auto bits_size = leaves.front().second;
            leaves.pop_front();
            nodes.emplace_back(std::make_tuple(node, bits_size, popCount(node->bits), 0));
        }

        while (nodes.size() > 1) {

            std::deque<std::tuple<Node*, uint64_t, uint64_t, uint64_t>> next_nodes;
            while (not nodes.empty()) {
                // あまりがでたときは，最後に作った中間ノードと結合させるためにnodesに戻す
                if (nodes.size() == 1) {
                    nodes.push_front(next_nodes.back());
                    next_nodes.pop_back();
                }

                Node *left_node;
                uint64_t left_num, left_ones, left_height;
                std::tie(left_node, left_num, left_ones, left_height) = nodes.front(); nodes.pop_front();

                Node *right_node;
                uint64_t right_num, right_ones, right_height;
                std::tie(right_node, right_num, right_ones, right_height) = nodes.front(); nodes.pop_front();

                const auto internal_node = new Node(0, 0, false);
                internal_node->num = left_num;
                internal_node->ones = left_ones;
                internal_node->left = left_node;
                internal_node->right = right_node;
                internal_node->balance = right_height - left_height;

                next_nodes.emplace_back(std::make_tuple(internal_node, left_num + right_num, left_ones + right_ones, std::max(left_height, right_height) + 1));
            }

            nodes = next_nodes;
        }

        uint64_t height;
        std::tie(this->root, this->size, this->num_one, height) = nodes.front(); nodes.pop_front();
        assert(this->size == v.size());
    }

    // B[pos]
    uint64_t access(uint64_t pos) {
        assert(pos < this->size);

        return access(this->root, pos);
    }

    // B[0, pos)にある指定されたbitの数
    uint64_t rank(uint64_t bit, uint64_t pos) {
        assert(bit == 0 or bit == 1);
        assert(pos <= this->size);

        if (bit) {
            return rank1(this->root, pos, 0);
        }
        else {
            return pos - rank1(this->root, pos, 0);
        }
    }

    // rank番目の指定されたbitの位置 + 1(rankは1-origin)
    uint64_t select(uint64_t bit, uint64_t rank) {
        assert(bit == 0 or bit == 1);
        assert(rank > 0);

        if (bit == 0 and rank > this->size - this-> num_one) { return NOTFOUND; }
        if (bit == 1 and rank > this-> num_one)              { return NOTFOUND; }

        if (bit) {
            return select1(this->root, 0, rank);
        }
        else {
            return select0(this->root, 0, rank);
        }
    }

    // posにbitを挿入する
    void insert(uint64_t pos, uint64_t bit) {
        assert(bit == 0 or bit == 1);
        assert(pos <= this->size);  // 現在もってるbitsの末尾には挿入できる

        if (root == nullptr) {
            root = new Node(bit, 1, true);
        } else {
            insert(this->root, nullptr, bit, pos, this->size);
        }
        this->size++;
        this->num_one += (bit == 1);
    }

    // 末尾にbitを追加する
    void push_back(uint64_t bit) {
        assert(bit == 0 or bit == 1);

        this->insert(this->size, bit);
    }

    // posを削除する
    void erase(uint64_t pos) {
        assert(pos < this->size);

        uint64_t bit = this->remove(this->root, nullptr, pos, this->size, 0, true).first;
        this->size--;
        this->num_one -= (bit == 1);
    }

    // posにbitをセットする
    void update(uint64_t pos, uint64_t bit) {
        assert(bit == 0 or bit == 1);
        assert(pos < this->size);

        if (bit == 1) {
            this->bitset(pos);
        }
        else {
            this->bitclear(pos);
        }
    }

    // posのbitを1にする
    void bitset(uint64_t pos) {
        assert(pos < this->size);

        bool flip = bitset(this->root, pos);
        this->num_one += flip;
    }

    // posのbitを0にする
    void bitclear(uint64_t pos) {
        assert(pos < this->size);

        bool flip = bitclear(this->root, pos);
        this->num_one -= flip;
    }

    // dotファイルを作成する(debug用)
    void graphviz(const std::string &file_path) {

#ifdef USE_GRAPHVIZ
        boost::adjacency_list<> graph;
        std::vector<std::string> labels;

        auto root = boost::add_vertex(graph);
        labels.emplace_back(this->root->info());

        std::queue<std::pair<Node*, boost::adjacency_list<>::vertex_descriptor>> que;
        que.emplace(std::make_pair(this->root, root));

        while (not que.empty()) {
            Node *node = que.front().first;
            auto parent = que.front().second;
            que.pop();

            if (not node->is_leaf) {
                // left
                auto left = boost::add_vertex(graph);
                boost::add_edge(parent, left, graph);
                labels.emplace_back("L\n" + node->left->info());
                que.emplace(std::make_pair(node->left, left));

                // right
                auto right = boost::add_vertex(graph);
                boost::add_edge(parent, right, graph);
                labels.emplace_back("R\n" + node->right->info());
                que.emplace(std::make_pair(node->right, right));
            }
        }
        std::ofstream file(file_path);
        boost::write_graphviz(file, graph, boost::make_label_writer(&labels[0]));
#else
        std::cerr << "please define USE_GRAPHVIZ" << std::endl;
#endif
    }

    // 木の状態が正しいかチェックする(debug用)
    // 各ノードのbalanceの値が正しいか．AVL木の制約を守っているかのチェック
    bool is_valid_tree(bool verbose) {
        if (this->root == nullptr) {
            if (this->size == 0 and this->num_one == 0) {
                return true;
            }
            std::cerr << "root is nullptr but size is " << this->size << " and num_one is " << this->num_one << std::endl;
            return false;
        }
        std::map<uint64_t, uint64_t> height;
        get_height(this->root, height);

        std::queue<Node*> que;
        que.emplace(this->root);

        while (not que.empty()) {
            Node *node = que.front(); que.pop();

            if (not node->is_valid_node()) {
                if (verbose) {
                    std::cerr << "node " << node->no << " is invalid node" << std::endl;
                    std::cerr << node->info() << std::endl;
                }
                return false;
            }

            if (not node->is_leaf) {
                auto left_height = height[node->left->no];
                auto right_height = height[node->right->no];
                // バランスの値が正しいかチェック
                if (node->balance != right_height - left_height) {
                    if (verbose) {
                        std::cerr << "node" << node->no << "'s balance is " << node->balance << "(left height:" << left_height << ", right height:" << right_height << ")" << std::endl;
                    }
                    return false;
                }

                // AVL木の制約を満たしていない
                if (node->balance < -1 or 1 < node->balance) {
                    if (verbose) {
                        std::cerr << "node" << node->no << "is not balanced." << "(balance:" << node->balance << ", left height:" << left_height << ", right height:" << right_height << ")" << std::endl;
                    }
                    return false;
                }

                que.emplace(node->left);
                que.emplace(node->right);
            }
            else {
                // バランスの値が正しいかチェック
                if (node->balance != 0) {
                    if (verbose) {
                        std::cerr << "node " << node->no << "'s balance is not 0" << std::endl;
                    }
                    return false;
                }
            }
        }
        return true;
    }

private:
    uint64_t access(const Node *node, uint64_t pos) {
        if (node->is_leaf) {
            assert(pos <= 2 * this->bits_size_limit);
            return (node->bits >> pos) & (uint64_t)1;
        }

        if (pos < node->num) {
            return this->access(node->left, pos);
        } else {
            return this->access(node->right, pos - node->num);
        }
    }

    uint64_t rank1(const Node *node, uint64_t pos, uint64_t ones) {
        if (node->is_leaf) {
            assert(node->bits_size >= pos);
            const uint64_t mask = ((uint64_t)1 << pos) - 1;
            return ones + popCount(node->bits & mask);
        }

        if (pos < node->num) {
            return this->rank1(node->left, pos, ones);
        } else {
            return this->rank1(node->right, pos - node->num, ones + node->ones);
        }
    }

    uint64_t select1(const Node *node, uint64_t pos, uint64_t rank) {
        if (node->is_leaf) {
            return pos + this->selectInBlock(node->bits, rank) + 1;
        }

        if (rank <= node->ones) {
            return this->select1(node->left, pos, rank);
        } else {
            return this->select1(node->right, pos + node->num, rank - node->ones);
        }
    }

    uint64_t select0(const Node *node, uint64_t pos, uint64_t rank) {
        if (node->is_leaf) {
            return pos + this->selectInBlock(~node->bits, rank) + 1;
        }

        if (rank <= (node->num - node->ones)) {
            return this->select0(node->left, pos, rank);
        } else {
            return this->select0(node->right, pos + node->num, rank - (node->num - node->ones));
        }
    }

    // bits(64bit)のrank番目(0-index)の1の数
    uint64_t selectInBlock(uint64_t bits, uint64_t rank) {
        const uint64_t x1 = bits - ((bits & 0xAAAAAAAAAAAAAAAALLU) >> (uint64_t)1);
        const uint64_t x2 = (x1 & 0x3333333333333333LLU) + ((x1 >> (uint64_t)2) & 0x3333333333333333LLU);
        const uint64_t x3 = (x2 + (x2 >> (uint64_t)4)) & 0x0F0F0F0F0F0F0F0FLLU;

        uint64_t pos = 0;
        for (;;  pos += 8){
            const uint64_t rank_next = (x3 >> pos) & 0xFFLLU;
            if (rank <= rank_next) break;
            rank -= rank_next;
        }

        const uint64_t v2 = (x2 >> pos) & 0xFLLU;
        if (rank > v2) {
            rank -= v2;
            pos += 4;
        }

        const uint64_t v1 = (x1 >> pos) & 0x3LLU;
        if (rank > v1){
            rank -= v1;
            pos += 2;
        }

        const uint64_t v0  = (bits >> pos) & 0x1LLU;
        if (v0 < rank){
            pos += 1;
        }

        return pos;
    }

    // nodeから辿れる葉のpos番目にbitをいれる(葉のbitの長さはlen)
    // 高さの変化を返す
    int64_t insert(Node *node, Node *parent, uint64_t bit, uint64_t pos, uint64_t len) {
        assert(bit == 0 or bit == 1);
        if (node->is_leaf) {
            assert(pos <= 2 * this->bits_size_limit);

            // posより左をとりだす
            const uint64_t up_mask = (((uint64_t)1 << (len - pos)) - 1) << pos;
            const uint64_t up = (node->bits & up_mask);

            // posより右をとりだす
            const uint64_t down_mask = ((uint64_t)1 << pos) - 1;
            const uint64_t down = node->bits & down_mask;

            node->bits = (up << (uint64_t)1) | (bit << pos) | down;
            node->bits_size++;
            assert(node->bits_size == len + 1);

            // bitsのサイズが大きくなったので分割する
            if (len + 1 >= 2 * bits_size_limit) {
                this->splitNode(node, len + 1); // 引数のlenはinsert後の長さなので+1する
                return 1;
            }

            return 0;
        }

        if (pos < node->num) {
            const int64_t diff = this->insert(node->left, node, bit, pos, node->num);
            node->num += 1;
            node->ones += (bit == 1);
            return achieveBalance(parent, node, diff, 0);
        } else {
            const int64_t diff = this->insert(node->right, node, bit, pos - node->num, len - node->num);
            return achieveBalance(parent, node, 0, diff);
        }
    }

    // nodeのpos番目のbitを削除する
    // 消したbitと高さの変化のpairを返す
    std::pair<uint64_t, int64_t> remove(Node *node, Node *parent, uint64_t pos, uint64_t len, uint64_t ones, bool allow_under_flow) {
        if (node->is_leaf) {
            // 消すとunder flowになるので消さない
            if (node != this->root and len <= bits_size_limit / 2 and not allow_under_flow) {
                return std::make_pair(NOTFOUND, 0);
            }

            assert(pos <= 2 * this->bits_size_limit);
            assert(pos < len);
            const uint64_t bit = (node->bits >> pos) & (uint64_t)1;

            // posより左をとりだす(posを含まないようにする)
            const uint64_t up_mask = (((uint64_t)1 << (len - pos - 1)) - 1) << (pos + 1);
            const uint64_t up = (node->bits & up_mask);

            // posより右をとりだす
            const uint64_t down_mask = ((uint64_t)1 << pos) - 1;
            const uint64_t down = node->bits & down_mask;

            node->bits = (up >> (uint64_t)1) | down;
            node->bits_size--;
            assert(node->bits_size == len - 1);

            return std::make_pair(bit, 0);
        }

        if (pos < node->num) {
            const auto bit_diff = this->remove(node->left, node, pos, node->num, node->ones, allow_under_flow);
            if (bit_diff.first == NOTFOUND) {
                return bit_diff;
            }

            node->ones -= (bit_diff.first == 1);
            // 左の子の葉のbitを1つ消したのでunder flowが発生している
            if (node->num == bits_size_limit / 2) {
                const auto b_d = remove(node->right, node, 0, len - bits_size_limit / 2, 0, false);  // 右の葉の先頭bitを削る

                // 右の葉もunder flowになって消せない場合は2つの葉を統合する
                if (b_d.first == NOTFOUND) {
                    assert(node->left->is_leaf);
                    assert(node->left->bits_size == bits_size_limit / 2 - 1);
                    // 右の子から辿れる一番左の葉の先頭にleftのbitsを追加する
                    mergeNodes(node->right, 0, len - bits_size_limit / 2, node->left->bits, bits_size_limit / 2 - 1, node->ones, true);
                    this->replace(parent, node, node->right);    // parentの子のnodeをnode->rightに置き換える
                    return std::make_pair(bit_diff.first, -1);
                }

                // 右の葉からとった先頭bitを左の葉の末尾にいれる
                assert(node->left->bits_size == bits_size_limit / 2 - 1);
                insert(node->left, node, b_d.first, bits_size_limit / 2 - 1, bits_size_limit / 2 - 1);
                node->ones += (b_d.first == 1);
            }
            else {
                node->num -= 1;
            }

            const int64_t diff = achieveBalance(parent, node, bit_diff.second, 0);
            return std::make_pair(bit_diff.first, diff);

        } else {
            const auto bit_diff = this->remove(node->right, node, pos - node->num, len - node->num, ones - node->ones, allow_under_flow);
            if (bit_diff.first == NOTFOUND) {
                return bit_diff;
            }

            ones -= (bit_diff.first == 1);
            // 右の子の葉のbitを1つ消したのでunder flowが発生する
            if ((len - node->num) == bits_size_limit / 2) {
                const auto b_d = remove(node->left, node, node->num - 1, node->num, 0, false);    // 左の葉の末尾をbitを削る

                // 左の葉もunder flowになって消せない場合は2つの葉を統合する
                if (b_d.first == NOTFOUND) {
                    assert(node->right->is_leaf);
                    assert(node->right->bits_size == bits_size_limit / 2 - 1);
                    // 左の子から辿れる一番右の葉の末尾にleftのbitsを追加する
                    mergeNodes(node->left, node->num, node->num, node->right->bits, bits_size_limit / 2 - 1, ones - node->ones, false);
                    this->replace(parent, node, node->left);    // parentの子のnodeをnode->leftに置き換える
                    return std::make_pair(bit_diff.first, -1);
                }

                // 左の葉からとった最後尾bitを右の葉の先頭にいれる
                insert(node->right, node, b_d.first, 0, bits_size_limit / 2 - 1);
                node->num -= 1;
                node->ones -= (b_d.first == 1);
            }

            const int64_t diff = achieveBalance(parent, node, 0, bit_diff.second);
            return std::make_pair(bit_diff.first, diff);
        }
    }

    // pos番目のbitを1にする．0から1への反転が起きたらtrueを返す
    bool bitset(Node *node, uint64_t pos) {
        if (node->is_leaf) {
            assert(pos <= 2 * this->bits_size_limit);
            const uint64_t bit = (node->bits >> pos) & 1;
            if (bit == 1) {
                return false;
            }

            node->bits |= ((uint64_t)1 << pos);
            return true;
        }

        if (pos < node->num) {
            bool flip = this->bitset(node->left, pos);
            node->ones += flip;
            return flip;
        } else {
            return this->bitset(node->right, pos - node->num);
        }
    }

    // pos番目のbitを0にする．1から0への反転が起きたらtrueを返す
    bool bitclear(Node *node, uint64_t pos) {
        if (node->is_leaf) {
            assert(pos <= 2 * this->bits_size_limit);

            const uint64_t bit = (node->bits >> pos) & 1;
            if (bit == 0) {
                return false;
            }
            node->bits &= ~((uint64_t)1 << pos);
            return true;
        }

        if (pos < node->num) {
            const bool flip = this->bitclear(node->left, pos);
            node->ones -= flip;
            return flip;
        } else {
            return this->bitclear(node->right, pos - node->num);
        }
    }

    // nodeを2つの葉に分割する
    void splitNode(Node *node, uint64_t len) {
        assert(node->is_leaf);
        assert(node->bits_size == len);

        // 左の葉
        const uint64_t left_size = len / 2;
        const uint64_t left_bits = node->bits & (((uint64_t)1 << left_size) - 1);
        node->left = new Node(left_bits, left_size, true);

        // 右の葉
        const uint64_t right_size = len - left_size;
        const uint64_t right_bits = node->bits >> left_size;
        node->right = new Node(right_bits, right_size, true);

        // nodeを内部ノードにする
        node->is_leaf = false;
        node->num = left_size;
        node->ones = this->popCount(left_bits);
        node->bits = 0;
        node->bits_size = 0;
    }

    // nodeから辿れる葉のpos番目にbitsを格納する
    void mergeNodes(Node *node, uint64_t pos, uint64_t len, uint64_t bits, uint64_t s, uint64_t ones, bool left) {
        if (node->is_leaf) {
            if (left) {
                node->bits = (node->bits << s) | bits;
            }
            else {
                assert(len == node->bits_size);
                node->bits = node->bits | (bits << len);
            }
            node->bits_size += s;
            return;
        }

        if (pos < node->num) {
            node->num += s;
            node->ones += ones;
            mergeNodes(node->left, pos, node->num, bits, s, ones, left);
        }
        else {
            mergeNodes(node->right, pos, len - node->num, bits, s, ones, left);
        }
    }

    // nodeの左の高さがleftHeightDiffだけ変わり，右の高さがrightHeightDiffだけ変わったときにnodeを中心に回転させる
    // 高さの変化を返す
    int64_t achieveBalance(Node *parent, Node *node, int64_t leftHeightDiff, int64_t rightHeightDiff) {
        assert(-1 <= node->balance and node->balance <= 1);
        assert(-1 <= leftHeightDiff and leftHeightDiff <= 1);
        assert(-1 <= rightHeightDiff and rightHeightDiff <= 1);

        if (leftHeightDiff == 0 and rightHeightDiff == 0) {
            return 0;
        }

        int64_t heightDiff = 0;
        // 左が高いときに，左が高くなる or 右が高いときに右が高くなる
        if ((node->balance <= 0 and leftHeightDiff > 0) or (node->balance >= 0 and rightHeightDiff > 0)) {
            ++heightDiff;
        }
        // 左が高いときに左が低くなる or 右が高いときに右が低くなる
        if ((node->balance < 0 and leftHeightDiff < 0) or (node->balance > 0 and rightHeightDiff < 0)) {
            --heightDiff;
        }

        node->balance += -leftHeightDiff + rightHeightDiff;
        assert(-2 <= node->balance and node->balance <= 2);

        // 左が2高い
        if (node->balance == -2) {
            assert(-1 <= node->left->balance and node->left->balance <= 1);
            if (node->left->balance != 0) {
                heightDiff--;
            }

            if (node->left->balance == 1) {
                replace(node, node->left, rotateLeft(node->left));
            }
            replace(parent, node, rotateRight(node));
        }
            // 右が2高い
        else if (node->balance == 2) {
            assert(-1 <= node->right->balance and node->right->balance <= 1);
            if (node->right->balance != 0) {
                heightDiff--;
            }

            if (node->right->balance == -1) {
                replace(node, node->right, rotateRight(node->right));
            }
            replace(parent, node, rotateLeft(node));
        }

        return heightDiff;
    }

    // node Bを中心に左回転する．新しい親を返す
    Node *rotateLeft(Node *B) {
        Node *D = B->right;

        const int64_t heightC = 0;
        const int64_t heightE = heightC + D->balance;
        const int64_t heightA = std::max(heightC, heightE) + 1 - B->balance;

        B->right = D->left;
        D->left = B;

        B->balance = heightC - heightA;
        D->num += B->num;
        D->ones += B->ones;
        D->balance = heightE - (std::max(heightA, heightC) + 1);

        assert(-2 <= B->balance and B->balance <= 2);
        assert(-2 <= D->balance and D->balance <= 2);

        return D;
    }

    // node Dを中心に右回転する．新しい親を返す
    Node *rotateRight(Node *D) {
        Node *B = D->left;

        const int64_t heightC = 0;
        const int64_t heightA = heightC - B->balance;
        const int64_t heightE = std::max(heightA, heightC) + 1 + D->balance;

        D->left = B->right;
        B->right = D;

        D->num -= B->num;
        D->ones -= B->ones;
        D->balance = heightE - heightC;
        B->balance = std::max(heightC, heightE) + 1 - heightA;

        assert(-2 <= B->balance and B->balance <= 2);
        assert(-2 <= D->balance and D->balance <= 2);

        return B;
    }

    // parentの子のoldNodeをnewNodeに置き換える
    void replace(Node *parent, Node *oldNode, Node *newNode) {
        if (parent == nullptr) {
            this->root = newNode;
            return;
        }

        if (parent->left == oldNode) {
            parent->left = newNode;
        }
        else if (parent->right == oldNode) {
            parent->right = newNode;
        }
        else {
            throw "old node is not child";
        }
    }

    uint64_t popCount(uint64_t x) {
        x = (x & 0x5555555555555555ULL) + ((x >> (uint64_t)1) & 0x5555555555555555ULL);
        x = (x & 0x3333333333333333ULL) + ((x >> (uint64_t)2) & 0x3333333333333333ULL);
        x = (x + (x >> (uint64_t)4)) & 0x0f0f0f0f0f0f0f0fULL;
        x = x + (x >>  (uint64_t)8);
        x = x + (x >> (uint64_t)16);
        x = x + (x >> (uint64_t)32);
        return x & 0x7FLLU;
    }

    // 各ノードの高さ(一番遠い葉からの距離)を取得(debug用)
    uint64_t get_height(Node *node, std::map<uint64_t, uint64_t> &height) {
        if (node->is_leaf) {
            return 0;
        }

        if (height.find(node->no) != height.end()) {
            return height[node->no];
        }

        auto left_height = get_height(node->left, height);
        auto right_height = get_height(node->right, height);
        return height[node->no] = std::max(left_height, right_height) + 1;
    }
};


#include <iostream>
#include <vector>
#include <random>
#include <fstream>
#include <numeric>
#include <chrono>
#include <math.h>
#include <assert.h>
#include <random>
#include <boost/beast/core.hpp>
#include <boost/beast/http.hpp>
#include <boost/beast/version.hpp>
#include <boost/asio.hpp>

namespace beast = boost::beast;     // from <boost/beast.hpp>
namespace http = beast::http;       // from <boost/beast/http.hpp>
namespace net = boost::asio;        // from <boost/asio.hpp>
using tcp = net::ip::tcp;           // from <boost/asio/ip/tcp.hpp>

class Key_Generator
{
    private:
    const int SEED = 101;
    const double LOCATION = 30.7984;
    const double SCALE = 8.20449;

    std::vector<std::string> key_cache;

    std::mt19937 gen{SEED};
    std::extreme_value_distribution<double> dist {LOCATION, SCALE};
    std::uniform_int_distribution<> key_cache_dist {0, static_cast<int> (key_cache.size())};

    const std::string ALPHANUMERIC ="0123456789abcdefghijklmnopqrstuwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
    std::uniform_int_distribution<> a_dist {0, int(ALPHANUMERIC.size() - 1)};
    //change this to use modulus instead of using so many distribution
    std::uniform_int_distribution<> distribution1 {0, int(KEY_POOL_SIZE - 1)};

    unsigned KEY_POOL_SIZE = 10000;
    std::vector<std::string> key_pool = generate_key_pool_();

    unsigned generate_key_length_();

    std::string generate_key_(unsigned length);

    std::vector<std::string> generate_key_pool_();

    std::string get_key_from_pool_();

    public:
    std::string generate_key();
    unsigned get_key_cache_length();
    std::string get_key_from_cache();
    void delete_key_from_cache(std::string key);
    void add_key_to_cache(std::string key);
    bool is_key_in_cache(std::string key);
};

class Value_Generator
{
    private:
    const int SEED = 101;
    const double LOCATION = 214.476;
    const double SCALE = 200;

    std::mt19937 gen{SEED};
    std::extreme_value_distribution<double> dist {LOCATION, SCALE};

    const std::string ALPHANUMERIC ="0123456789abcdefghijklmnopqrstuwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ";
    std::uniform_int_distribution<> a_dist {0, int(ALPHANUMERIC.size() - 1)};

    unsigned generate_value_length_();

    std::string generate_value_(unsigned length);

    public:
    std::string generate_value();
};

class Method_Generator
{
    private:
    const int SEED = 101;

    double PERCENT_GET = 0.60;
    double PERCENT_SET = 0.30;
    double PERCENT_DELETE = 0.10;

    double sum = PERCENT_GET + PERCENT_SET + PERCENT_DELETE;

    double get_cutoff = PERCENT_GET;
    double set_cutoff = get_cutoff + PERCENT_SET;
    double delete_cutoff = set_cutoff + PERCENT_DELETE;

    std::mt19937 gen{SEED};
    std::uniform_real_distribution<double> dist{0, 1};
    double generate_probability();

    public:

    std::string generate_method();

    void set_percents(double get_percent, double set_percent, double delete_percent)
    {
        PERCENT_GET = get_percent;
        PERCENT_SET = set_percent;
        PERCENT_DELETE = delete_percent;
        get_cutoff = PERCENT_GET;
        set_cutoff = get_cutoff + PERCENT_SET;
        delete_cutoff = set_cutoff + PERCENT_DELETE;
    }

    void print_percents()
    {
        std::cout << "percent get "  << PERCENT_GET << "\n";
        std::cout << "percent set " << PERCENT_SET << "\n";
        std::cout << "percent delete " << PERCENT_DELETE << "\n";
    }

};

class Request_Generator
{
    private:

    Method_Generator method_generator_{};
    Key_Generator key_generator_{};
    Value_Generator value_generator_{};
    const int SEED = 101;
    std::mt19937 gen{SEED};
    std::uniform_real_distribution<double> dist{0, 1};
    const double PERCENT_KNOWN = 0.95;
    double known_cutoff = PERCENT_KNOWN;
    double generate_probability();

    public:
    http::request<http::string_body> generate_request();

    Request_Generator(double percent_get, double percent_set, double percent_delete)
    {
        method_generator_.set_percents(percent_get, percent_set, percent_delete);
    }
    void print_params()
    {
        method_generator_.print_percents();
    }
};


#include <boost/math/special_functions/expm1.hpp>