/***
Copyright 2017 Yaohai XU

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.
***/

/**
    FastLSH
    boostmpiCompute.cpp
    Purpose: This is the source file for FastLSH-MPI

    @author Peter Yaohai XU
    @version 1.0 4/07/17
*/

/**
 * Execution command
 * mpiexec -np 4 -host prj67,prj68,prj69,prj70 -mca btl ^openib ./boostmpiLSH

 * boost install command
 * sudo apt-get install libboost-all-dev
 * example cmd to run--
 * mpirun -np 10 --hosts slave1 ./boostmpiCompute

 * compliaiton cmd
 * mpic++ -std=c++11 -L/usr/lib -I/usr/include/boost/mpi -lboost_filesystem -lboost_system -lboost_mpi boostmpiCompute.cpp

 * send multi dimensional vector needs serialization, I will flaten the multidimwnsional vector and send them first, it
 * might drag down the speed.
 * this program stop at when the collision table finishs calculation on each process, but not gather in the mater node.

 * mpiexec -np 2 -host --allow-run-as-root -mca btl ^openib ./boostmpiLSH
 * mpiexec -np 8 --allow-run-as-root -host -mca btl ^openib ./boostmpiLSH
 */

#include <iostream>
#include <vector>
#include <fstream>
#include <chrono>
#include <random>
#include <boost/mpi.hpp>
#include <boost/serialization/vector.hpp>

namespace mpi = boost::mpi;

typedef std::vector<std::vector<std::vector<double> > > vector3D;
typedef std::vector<std::vector<double> > vector2D;
typedef std::vector<double> vector1D;

#define now() std::chrono::high_resolution_clock::now()
#define dcast std::chrono::duration_cast<std::chrono::microseconds>

/**
 * load file from Linux File System
 * @param filePath
 * @param row number of rows
 * @param col number of columns
 * @return 2D vector<double> dataset from csv
 */
vector2D loadDataFromLinuxSystem(char* filePath, size_t row, size_t col);

/**
 * flat a 2D vector into 1D vector
 * @param origVec the original 2D vector
 * @return 1D vector<double> of flatted vector
 */
vector1D flat2D(vector2D origVec);

/**
 * flat a 2D vector of candidate set (different length of each row)
 * @param origCandidate original candidate set
 * @return 1D vector<double> of flatted vector
 */
vector1D flat2DCandidate(vector2D origCandidate);

/**
 * flat a 3D vector into 1D vector
 * @param origVec original 3D vector
 * @return 1D vector of the flatten vector
 */
vector1D flat3D(vector3D origVec);

/**
 * reconstruct 1D vector to 2D vector
 * @param origVec the 1D VECTOR
 * @param d1 the length of first dimension
 * @param d2 the length of second dimension
 * @return 2D vector<double> of recontructed 2D vector
 */
vector2D reconstr2D(vector1D origVec, int d1, int d2);

/**
 * reconstruct 2D vector of candidate set (different length of each row)
 * @param candidateRecv the 1D vector
 * @param lineNum the number of lines
 * @return 2D reconstructed vector<double>
 */
vector2D reconstr2DCandidate(vector1D candidateRecv, int lineNum);

/**
 * reconstruct 3D vector
 * @param origVec the 3D vector
 * @param d1 number of index of first dimension
 * @param d2 number of index of second dimension
 * @param d3 number of index of third dimension
 * @return 3D vector<double>
 */
vector3D reconstr3D(vector1D origVec, int d1, int d2, int d3);

/**
 * compute the hash value
 * @param dataset the original dataset
 * @param randomLine random Lines generated
 * @param randomVector random numbers generated
 * @param pNum number of points
 * @param L # of group hash
 * @param K # the number of hash functions in each group hash
 * @param D # of dimensions
 * @param W bucket width
 * @return the 2D vector hash value
 */
vector2D computeHash(vector2D dataset, vector3D randomLine, vector1D randomVector, size_t pNum, size_t L, size_t K,
                     size_t D, size_t W);

/**
 * compute the collision matrix
 * @param hMatrixN 2D hash matrix N
 * @param hMatrixQ 2D hash matrix Q
 * @param Q # of vectors in the queryset
 * @param N # of vectors in the dataset
 * @param L # of group hash
 * @return 2D vector collision table
 */
vector2D computeCollision(vector2D hMatrixN, vector2D hMatrixQ, size_t Q, size_t N, size_t L);

/**
 * max-min normalization
 * @param dataset original dataset
 * @return 2D vector of normalized dataset
 */
vector2D normalize(vector2D dataset,vector1D normMaxs, vector1D normMins);

/**
 * compute the candidate set in normal style
 * @param Q # of vectors in the queryset
 * @param N # of vectors in the dataset
 * @param T threshold
 * @param collisionMatrix the collision matrixes
 * @return 2D vector<double> of candidate set
 */
vector2D computeCandidateNormal(size_t Q, size_t N, size_t T, vector2D collisionMatrix);

int main (int argc, char **argv) {

    //prepare timer
    std::chrono::high_resolution_clock::time_point t1;
    std::chrono::high_resolution_clock::time_point t2;
    std::chrono::high_resolution_clock::time_point t3;
    std::chrono::high_resolution_clock::time_point t4;

    //start first timing
    t1 = now();

    // initialize the mpi environment
    mpi::environment env;
    mpi::communicator world;

    //print identity to the screen
    std::cout<<"My name is "<<env.processor_name()<<" Processor id "<<world.rank()<<"\n";

    //parameter sets
    size_t N = 1000; //# of vectors in the dataset
    size_t Q = 1000; //# of vertors in the queryset
    size_t D = 57; //# of dimensions
    size_t L = 200; //# of group hash
    size_t K = 1; //# the number of hash functions in each group hash
    double W = 1.2; //bucket width
    size_t  T = 100; // threshold
    int root_process = 0;
    char * inputPathSetN = "../testData/dataset1000NoIndex.csv" ;
    char * inputPathSetQ = "../testData/dataset1000NoIndex.csv";
    char * outputPath = "../testData/candidate.csv";

    //1d structure for communication between process
    vector1D setQ1d;
    vector1D randomLine1d;
    vector1D partialSetN1d;

    //variables every process own
    vector1D UniformRandomVector;
    vector3D randomLine;
    vector2D setQ;
    vector2D setQNorm;
    vector2D hashValueQ;

    //the splited part of N for each thread
    vector2D partialSetN;
    vector2D partialHashValueN;
    vector2D partialCollisionTable;
    vector2D partialCandidateSet;

    //as the name indicates
    int numNperSlave;
    numNperSlave = ceil(N/(double)world.size());

    std::cout<<"start to calculate random \n";
    //the master node do the preparation
    if(world.rank() == root_process) {
        //generate random line
        std::random_device rd;
        std::mt19937 gen(rd());

        for (int i = 0; i < L; i++) {
            vector2D vK;
            for (int j = 0; j < K; ++j) {
                //generate random according normal distribution
                std::normal_distribution<double> distribution(0.5, 0.5);
                std::vector<double> vD;
                for (int k = 0; k < D; ++k) {
                    vD.push_back(distribution(gen));
                }
                vK.push_back(vD);
            }
            randomLine.push_back(vK);
        }

        //generate random vector
        std::uniform_real_distribution<> dis(0, 1.2);
        for (int i = 0; i < 1; ++i) {
            UniformRandomVector.push_back(dis(gen));
        }

        //flatten randomLine for transmission
        randomLine1d = flat3D(randomLine);
    }

    //broadcast the shared variables
    broadcast(world, randomLine1d, 0);
    broadcast(world, UniformRandomVector, 0);

    //let slaves reconstruct the multi-dimensional vector
    if(world.rank()!=root_process) {
        randomLine = reconstr3D(randomLine1d, L, K, D);
    }

    //deal with N
    if (world.rank()==root_process){
        vector2D setN;
        vector2D setNNorm;
        setN = loadDataFromLinuxSystem(inputPathSetN, N, D);

        size_t row = setN.size();
        size_t col = setN[0].size();

        vector1D normMaxs(col, -999999);
        vector1D normMins(col, 9999999);

        //find maximums and minimums of each column
        for (int i = 0; i < row; ++i) {
            for (int j = 0; j < col; ++j) {
                if(setN[i][j]>= normMaxs[j])
                    normMaxs[j]=setN[i][j];
                if(setN[i][j]<=normMins[j])
                    normMins[j]=setN[i][j];
            }
        }

        setNNorm = normalize(setN, normMaxs, normMins);

        //read in setQ
        setQ = loadDataFromLinuxSystem(inputPathSetQ, Q, D);

        setQNorm = normalize(setQ, normMaxs, normMins);
        //flatten setQ for transmission
        setQ1d = flat2D(setQNorm);

        {
            vector2D temp;
            setN.swap(temp);
        }
        //deal with sending splited parts of N
        {

            //the last part of N will be done by master node(it contains least num of N)
            for (int i = 0; i <world.size() ; ++i) {

                if(i!=(world.size()-1)) {
                    vector2D::const_iterator first = setNNorm.begin() +  numNperSlave * i;
                    vector2D::const_iterator last = setNNorm.begin() +  numNperSlave * (i + 1);
                    vector2D newVec(first, last);

                    vector1D vecToSend = flat2D(newVec);
                    world.send(i + 1, 0, vecToSend);
                }

                    // master node get last part of the job (usually less than other parts
                else{
                    vector2D::const_iterator first = setNNorm.begin() +  numNperSlave * i;
                    vector2D::const_iterator last = setNNorm.begin() + N;
                    vector2D newVec(first, last);
                    partialSetN = newVec;
                }
            }
            {
                vector2D temp;
                setNNorm.swap(temp);
            }       }

        t4=now();
        auto duration2 = dcast( t4 - t3 ).count();
        std::cout <<duration2 << " μ send out N set\n";
    }
        //slave nodes receive N
    else{
        world.recv(0, 0, partialSetN1d);
        partialSetN = reconstr2D(partialSetN1d, numNperSlave, D);
    }

    broadcast(world, setQ1d, 0);

    // calculate the hash and collision (separate master and slave operation because their num of N might be different)
    if(world.rank()==root_process){
        // FastLSH execution flow
        vector2D gatheredCandidateSet;
        hashValueQ = computeHash(setQNorm, randomLine, UniformRandomVector, Q, L, K, D ,W );
        partialHashValueN = computeHash(partialSetN, randomLine, UniformRandomVector,
                                        N- numNperSlave*(world.size()-1), L, K, D, W);
        partialCollisionTable = computeCollision(partialHashValueN,hashValueQ,Q,N- numNperSlave*(world.size()-1),L);
        partialCandidateSet = computeCandidateNormal(Q, N- numNperSlave*(world.size()-1), T, partialCollisionTable);
        for (int i = 1; i < world.size(); ++i) {
            vector1D candidateRecv;
            world.recv(i,0,candidateRecv);
            vector2D temp = reconstr2DCandidate(candidateRecv, Q);
            for (int j = 0; j < temp.size(); ++j) {
                gatheredCandidateSet.push_back(temp[0]);
            }
        }

        for (int k = 0; k < partialCandidateSet.size(); ++k) {
            gatheredCandidateSet.push_back(partialCandidateSet[k]);
        }

        std::ofstream out(outputPath);
        for (int i = 0; i < gatheredCandidateSet.size(); ++i) {
            for (int j = 0; j < gatheredCandidateSet[i].size(); ++j) {
                out << gatheredCandidateSet[i][j] << ',';
            }
            out<<'\n';
        }
        out.close();

    }

    else{
        setQNorm = reconstr2D(setQ1d,Q,D);
        // FastLSH execution flow
        hashValueQ = computeHash(setQNorm, randomLine, UniformRandomVector, Q, L, K, D ,W );
        partialHashValueN = computeHash(partialSetN, randomLine, UniformRandomVector, numNperSlave, L, K, D, W);
        partialCollisionTable = computeCollision(partialHashValueN,hashValueQ,Q,numNperSlave,L);
        partialCandidateSet = computeCandidateNormal(Q, numNperSlave, T, partialCollisionTable);
        vector1D candidate1D  = flat2DCandidate(partialCandidateSet);
        world.send(0,0,candidate1D);

    }

    //leave them for testing
//    std::cout<<"v1Process # " << world.rank() <<" "<<partialCollisionTable[0][0]<<"\n";

    t2 = now();
    auto duration = dcast( t2 - t1 ).count();
    std::cout <<"processor "<<world.rank()<< " uses "<<duration << " μs to compute\n";

    // use this to pause in IDE
//    int c;
//    std::cin>>c;

}

vector2D loadDataFromLinuxSystem(char* filePath, size_t row, size_t col) {
    std::ifstream file;// declare file stream:
    file.open(filePath);
    vector2D data;
    for (int i = 0; i < row; i++) {
        std::vector<double> temp(col, 0);
        data.push_back(temp);
    }
    std::string value;
    std::string num;
    //parse csv file
    for (int i = 0; i<row; i++) {
        for (int j = 0; j<col; j++) {
            if (j!=col-1)
                getline(file, value, ',');
            else
                getline(file,value,'\n');
            num = std::string(value, 0, value.length());
            data[i][j] = ::atof(num.c_str());
        }
    }

    file.close();
    return data;
}

vector1D flat2D(vector2D origVec){
    vector1D result;
    for (int i = 0; i < origVec.size(); ++i) {
        for (int j = 0; j < origVec[0].size(); ++j) {
            result.push_back(origVec[i][j]);
        }
    }
    return result;
}

vector1D flat2DCandidate(vector2D origCandidate){
    vector1D result;
    for (int i = 0; i < origCandidate.size(); ++i) {

        for (int j = 0; j < origCandidate[0].size(); ++j) {
            result.push_back(origCandidate[i][j]);
        }
        //-1 as flag to indicate new line
        result.push_back(-1);
    }
}

vector1D flat3D(vector3D origVec){
    vector1D result;
    for (int i = 0; i < origVec.size(); ++i) {
        for (int j = 0; j < origVec[0].size(); ++j) {
            for (int k = 0; k < origVec[0][0].size(); ++k) {
                result.push_back(origVec[i][j][k]);
            }

        }
    }
    return result;
}

vector2D reconstr2D(vector1D origVec, int d1, int d2){
    vector2D result;
    for (int i = 0; i < d1; ++i) {
        vector1D vL(d2,0);
        result.push_back(vL);
    }

    for (int i = 0; i < origVec.size(); ++i) {
        result[i/d2][i%d2] = origVec[i];
    }
    return result;
}

vector2D reconstr2DCandidate(vector1D candidateRecv, int lineNum){
    vector2D result;
    for (int i = 0; i < lineNum; ++i) {
        vector1D vL(0,0);
        result.push_back(vL);
    }
    int line = 0;
    for (int i = 0; i < candidateRecv.size(); ++i) {
        if(candidateRecv[i]==-1) {
            line++;
            continue;
        }
        result[line].push_back(candidateRecv[i]);
    }
    return result;
}

vector3D reconstr3D(vector1D origVec, int d1, int d2, int d3){
    vector3D result;
    for (int i = 0; i < d1; ++i) {
        vector2D vL;
        for (int j = 0; j < d2; ++j) {
            vector1D vD(d3,0);
            vL.push_back(vD);
        }
        result.push_back(vL);
    }

    int interV = d2*d3;

    for (int i = 0; i < origVec.size(); ++i) {
        result[i/interV][(i%(interV))/d3][(i%(interV))%d3] = origVec[i];
    }

    return result;
}

vector2D computeHash(vector2D dataset, vector3D randomLine, vector1D randomVector, size_t pNum, size_t L, size_t K,
                     size_t D, size_t W){
    vector2D hashMatrix;

    for (int i = 0; i < pNum; ++i) {
        vector1D vL(L,0);
        hashMatrix.push_back(vL);
    }

    //loop through # of data point
#pragma omp parallel for collapse(2)
    for (int n = 0; n < pNum; ++n) {
        //loop through # of hash function group
        for (int l = 0; l < L; ++l) {
            double hashFinalValue = 0;
            //loop through the inner of a hash function group
            for (int k = 0; k < K; ++k) {
                double dTemp = 0;
                //loop through all the dimensions
                for (int d = 0; d < D; ++d) {
                    //vector(math) multiply to make projection
                    dTemp += dataset[n][d]*randomLine[l][k][d];
                }
                //assign hashvalue into bucket
                double hashvalue = floor(dTemp/W);
                //merge hash group results **see documentation
                hashFinalValue = hashvalue*randomVector[k] + hashFinalValue;
            }
            hashMatrix[n][l]=hashFinalValue;
        }

    }
    return hashMatrix;
}

vector2D computeCollision(vector2D hMatrixN, vector2D hMatrixQ, size_t Q, size_t N, size_t L){
    vector2D collisionMatrix;

    for (int i = 0; i < Q ; ++i) {
        vector1D singleLine(N, 0);
        collisionMatrix.push_back(singleLine);
    }

#pragma omp parallel for collapse(2)
    for (int q = 0; q <Q ; ++q) {
        for (int n = 0; n <N ; ++n){
            for (int hash_id = 0; hash_id < L; ++hash_id) {
                if (hMatrixN[n][hash_id] == hMatrixQ[q][hash_id])
                    collisionMatrix[q][n]++;
            }
        }
    }
    return collisionMatrix;
}

vector2D normalize(vector2D dataset,vector1D normMaxs, vector1D normMins){
    size_t row = dataset.size();
    size_t col = dataset[0].size();

    for (int i = 0; i < row; ++i) {
        for (int j = 0; j < col; ++j) {
            //if max equals min, the value is set to 0.5
            if(normMaxs[j]==normMins[j])
                dataset[i][j] = 0.5;
            else
                //normalization formula -- (x-min(x))/(max(x)-min(x))
                dataset[i][j] = (dataset[i][j] - normMins[j])/(normMaxs[j]-normMins[j]);
        }
    }
    return dataset;
}

vector2D computeCandidateNormal(size_t Q, size_t N, size_t T, vector2D collisionMatrix){
    vector2D candidateSet;
    for (int i = 0; i < Q; ++i) {
        vector1D temp(0,0);
        candidateSet.push_back(temp);
    }

#pragma omp parallel for
    for (int i = 0; i < Q; ++i) {
        vector1D candidates;
        for (int j = 0; j < N; ++j) {
            if(collisionMatrix[i][j]>=T)
                candidates.push_back((double &&) j);
        }
        candidateSet[i] = candidates;
    }
    return candidateSet;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/slide.hpp>
#include <boost/simd/detail/unroll.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>

namespace bs = boost::simd;

template <typename T, int N, typename Env> void test(Env& runtime)
{
  std::array<T,4*N> data;
  for(std::size_t i = 0;i < data.size(); ++i) data[i] = T(0);
  for(int i =   N;i < 2*N; ++i) data[i] = T(i);
  for(int i = 2*N;i < 3*N; ++i) data[i] = T(i*2);

  using p_t = bs::pack<T,N>;
  p_t ref;
  p_t l = bs::load<p_t>(&data[  N]);
  p_t r = bs::load<p_t>(&data[2*N]);

  STF_EQUAL( bs::slide<-N>(l,r), bs::slide<0>(r,l) );
  if(N>1) STF_EQUAL( bs::slide<-N/2>(l,r), bs::slide<N/2>(r,l) );
  STF_EQUAL( bs::slide<-1>(l,r), bs::slide<N-1>(r,l) );

  ref = bs::load<p_t>(&data[N]);
  STF_EQUAL( bs::slide<0>(l,r), ref );

  ref = bs::load<p_t>(&data[N], +1);
  STF_EQUAL( bs::slide<+1>(l,r), ref );

  ref = bs::load<p_t>(&data[N], N/2);
  STF_EQUAL( bs::slide<N/2>(l,r), ref );

  ref = bs::load<p_t>(&data[N], N-1);
  STF_EQUAL( bs::slide<N-1>(l,r), ref );

  STF_EQUAL( bs::slide<N>(l,r), r );
}

STF_CASE_TPL( "Check binary slide behavior", STF_NUMERIC_TYPES )
{
  namespace bs = boost::simd;
  static const int N = bs::pack<T>::static_size;

  test<T, N  >(runtime);
  test<T, N/2>(runtime);
  test<T, N*2>(runtime);
}


/*
 * MIT License
 *
 * Copyright (c) 2020 Robert Grupp
 *
 * 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 "xregLineSearchOpt.h"

#include <boost/variant.hpp>

#include <fmt/printf.h>

#include "xregAssert.h"

namespace
{

using namespace xreg;

using Pt  = LineSearchOptimization::Pt;
using Mat = LineSearchOptimization::Mat;

using SearchDirFnFirstOrder = LineSearchOptimization::SearchDirFnFirstOrder;
using SearchDirFnSecOrder   = LineSearchOptimization::SearchDirFnSecOrder;

struct SearchDirNeedsHessian : public boost::static_visitor<bool>
{
  bool operator()(const SearchDirFnFirstOrder& search_fn) const
  {
    xregASSERT(bool(search_fn));
    return false;
  }

  bool operator()(const SearchDirFnSecOrder& search_fn) const
  {
    xregASSERT(bool(search_fn));
    return true;
  }
};

struct ComputeSearchDir : public boost::static_visitor<Pt>
{
  const Pt*  g;
  const Mat* H;

  Pt operator()(const SearchDirFnFirstOrder& search_fn) const
  {
    return search_fn(*g);
  }

  Pt operator()(const SearchDirFnSecOrder& search_fn) const
  {
    return search_fn(*g, *H);
  }
};

}  // un-named

std::tuple<xreg::LineSearchOptimization::TermStatus,
           xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Scalar,
           xreg::size_type>
xreg::LineSearchOptimization::solve(const Pt& init_x) const
{
  xregASSERT(bool(obj_fn));
  xregASSERT(bool(backtrack_fn));

  TermStatus status = kOTHER;

  Scalar cur_F;
  Pt cur_x = init_x;
  Pt cur_grad;
  Mat cur_H;

  // search direction
  Pt p;

  Pt prev_x;

  const bool requires_hessian = boost::apply_visitor(SearchDirNeedsHessian(), search_dir_fn);

  std::tie(cur_F,cur_grad,cur_H) = obj_fn(cur_x, true, requires_hessian);

  Scalar cur_grad_norm = cur_grad.norm();

  const Scalar grad_term_tol = std::max(Scalar(1), cur_grad_norm) * grad_tol;

  bool should_stop = false;

  size_type cur_it = 0;

  dout() << " Iter.      F       ||grad F||" << std::endl;

  while (!should_stop)
  {
    dout() << fmt::sprintf("%04lu   %+10.4f  %+10.4f", cur_it, cur_F, cur_grad_norm) << std::endl;

    if (cur_grad_norm <= grad_term_tol)
    {
      status = kGRAD_CONVERGED;
      should_stop = true;
    }
    else if ((cur_it > 0) && ((prev_x - cur_x).norm() < param_tol))
    {
      status = kNO_POS_CHANGE;
      should_stop = true;
    }
    else if (max_its && (cur_it >= max_its))
    {
      status = kMAX_ITS_PERFORMED;
      should_stop = true;
    }
    else
    {
      {
        ComputeSearchDir compute_search_dir_visitor;
        compute_search_dir_visitor.g = &cur_grad;
        compute_search_dir_visitor.H = &cur_H;

        p = boost::apply_visitor(compute_search_dir_visitor, search_dir_fn);
      }

      prev_x = cur_x;

      std::tie(cur_x,cur_F,cur_grad,cur_H) = backtrack_fn(obj_fn, p, prev_x, cur_F, cur_grad, requires_hessian);

      cur_grad_norm = cur_grad.norm();

      ++cur_it;
    }
  }

  dout() << "Termination Status: " << TermStatusString(status) << std::endl;

  return std::make_tuple(status, cur_x, cur_F, cur_it);
}

std::string xreg::LineSearchOptimization::TermStatusString(const TermStatus status)
{
  std::string s;

  switch (status)
  {
  case kGRAD_CONVERGED:
    s = "Gradient Converged";
    break;
  case kMAX_ITS_PERFORMED:
    s = "Maximum Iterations Performed";
    break;
  case kNO_POS_CHANGE:
    s = "Insufficient Parameter Change";
    break;
  case kOTHER:
  default:
    s = "Other/Unknown";
    break;
  }

  return s;
}

xreg::LineSearchOptimization::Pt
xreg::NegativeGradSearchDir(const LineSearchOptimization::Pt& grad,
                            const LineSearchOptimization::Mat& /*hessian*/)
{
  return -1 * grad;
}

xreg::LineSearchOptimization::Pt
xreg::NewtonSearchDir(const LineSearchOptimization::Pt& grad,
                      const LineSearchOptimization::Mat& hessian)
{
  using Mat = LineSearchOptimization::Mat;

  // solves hessian \ -grad
  return Eigen::JacobiSVD<Mat>(hessian, Eigen::ComputeThinU | Eigen::ComputeThinV).solve(-1 * grad);
}

xreg::ModNewtonSearchDir::Pt xreg::ModNewtonSearchDir::operator()(const Pt& grad, const Mat& hessian)
{
  spectral_dcomp.compute(hessian, Eigen::ComputeEigenvectors);

  const SpectralDecomp::RealVectorType& eig_vals = spectral_dcomp.eigenvalues();

  const Scalar H_L2_norm = eig_vals.array().abs().maxCoeff();

  const Scalar eps = (H_L2_norm > 1.0e-6) ? (H_L2_norm / beta) : Scalar(1);

  bool use_hessian = true;

  const unsigned long d = static_cast<unsigned long>(grad.size());

  mod_eigen_vals.resize(d);

  for (unsigned long i = 0; i < d; ++i)
  {
    if (eig_vals(i) >= eps)
    {
      mod_eigen_vals(i) = eig_vals(i);
    }
    else if (eig_vals(i) <= -eps)
    {
      mod_eigen_vals(i) = -eig_vals(i);
      use_hessian = false;
    }
    else
    {
      mod_eigen_vals(i) = eps;
      use_hessian = false;
    }
  }

  if (!use_hessian)
  {
    const Mat& eigen_vecs = spectral_dcomp.eigenvectors();

    B = eigen_vecs * mod_eigen_vals.asDiagonal() * eigen_vecs.transpose();
  }

  // solve B \ -grad
  return NewtonSearchDir(grad, use_hessian ? hessian : B);
}

xreg::LineSearchOptimization::SearchDirFn xreg::ModNewtonSearchDir::callback_fn()
{
  return [this] (const Pt& grad, const Mat& hessian) { return this->operator()(grad, hessian); };
}

std::tuple<xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Scalar,
           xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Mat>
xreg::FixedStepNoBacktracking(const LineSearchOptimization::ObjFn& obj_fn,
                              const LineSearchOptimization::Pt& p,
                              const LineSearchOptimization::Pt& x,
                              const bool compute_hessian,
                              const LineSearchOptimization::Scalar alpha)
{
  using Pt     = LineSearchOptimization::Pt;
  using Mat    = LineSearchOptimization::Mat;
  using Scalar = LineSearchOptimization::Scalar;

  Pt next_x = x + (alpha * p);

  Scalar next_F;
  Pt next_g;
  Mat next_H;

  std::tie(next_F, next_g, next_H) = obj_fn(next_x, true, compute_hessian);

  return std::make_tuple(next_x, next_F, next_g, next_H);
}

xreg::LineSearchOptimization::BacktrackFn
xreg::MakeFixedStepNoBacktrackingCallback(const LineSearchOptimization::Scalar alpha)
{
  return [alpha] (const LineSearchOptimization::ObjFn& obj_fn,
                  const LineSearchOptimization::Pt& p,
                  const LineSearchOptimization::Pt& x,
                  const LineSearchOptimization::Scalar /*F*/,
                  const LineSearchOptimization::Pt& /*g*/,
                  const bool compute_hessian)
  {
    return FixedStepNoBacktracking(obj_fn, p, x, compute_hessian, alpha);
  };
}

std::tuple<xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Scalar,
           xreg::LineSearchOptimization::Pt,
           xreg::LineSearchOptimization::Mat>
xreg::BacktrackingArmijo(const LineSearchOptimization::ObjFn& obj_fn,
                         const LineSearchOptimization::Pt& p,
                         const LineSearchOptimization::Pt& x,
                         const LineSearchOptimization::Scalar F,
                         const LineSearchOptimization::Pt& g,
                         const bool compute_hessian,
                         const LineSearchOptimization::Scalar init_alpha,
                         const LineSearchOptimization::Scalar eta,
                         const LineSearchOptimization::Scalar tau)
{
  using Pt     = LineSearchOptimization::Pt;
  using Mat    = LineSearchOptimization::Mat;
  using Scalar = LineSearchOptimization::Scalar;

  const Scalar eta_grad_dot_p = eta * g.dot(p);

  Scalar alpha = init_alpha;

  Pt next_x = x + (alpha * p);

  Scalar next_F;

  // passing false for gradient and hessian defers their computation until
  // after we stop backtracking - this really helps for expensive computations

  std::tie(next_F,std::ignore,std::ignore) = obj_fn(next_x, false, false);

  while (next_F > (F + (alpha * eta_grad_dot_p)))
  {
    alpha *= tau;
    next_x = x + (alpha * p);
    std::tie(next_F,std::ignore,std::ignore) = obj_fn(next_x, false, false);
  }

  Pt next_g;

  Mat next_H;

  std::tie(next_F,next_g,next_H) = obj_fn(next_x, true, compute_hessian);

  return std::make_tuple(next_x, next_F, next_g, next_H);
}

xreg::LineSearchOptimization::BacktrackFn
xreg::MakeBacktrackingArmijoCallback(const LineSearchOptimization::Scalar alpha,
                                     const LineSearchOptimization::Scalar eta,
                                     const LineSearchOptimization::Scalar tau)
{
  return [alpha,eta,tau] (const LineSearchOptimization::ObjFn& obj_fn,
                          const LineSearchOptimization::Pt& p,
                          const LineSearchOptimization::Pt& x,
                          const LineSearchOptimization::Scalar F,
                          const LineSearchOptimization::Pt& g,
                          const bool compute_hessian)
  {
    return BacktrackingArmijo(obj_fn, p, x, F, g, compute_hessian, alpha, eta, tau);
  };
}


////////////////////////////////////////////////////////////////////////////////
//  Copyright (c) 2015 Bryce Adelstein Lelbach aka wash
//
//  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)
////////////////////////////////////////////////////////////////////////////////

#include <mdspan>

#include <boost/core/lightweight_test.hpp>

using std::experimental::dyn;
using std::experimental::detail::count_dynamic_dims;

int main()
{
    BOOST_TEST_EQ((count_dynamic_dims<             >::value), 0);

    BOOST_TEST_EQ((count_dynamic_dims<3            >::value), 0);
    BOOST_TEST_EQ((count_dynamic_dims<3,   5       >::value), 0);
    BOOST_TEST_EQ((count_dynamic_dims<3,   5,   9  >::value), 0);

    BOOST_TEST_EQ((count_dynamic_dims<dyn          >::value), 1);
    BOOST_TEST_EQ((count_dynamic_dims<dyn, dyn     >::value), 2);
    BOOST_TEST_EQ((count_dynamic_dims<dyn, dyn, dyn>::value), 3);

    BOOST_TEST_EQ((count_dynamic_dims<dyn, 5,   9  >::value), 1);
    BOOST_TEST_EQ((count_dynamic_dims<3,   dyn, 9  >::value), 1);
    BOOST_TEST_EQ((count_dynamic_dims<3,   5,   dyn>::value), 1);

    BOOST_TEST_EQ((count_dynamic_dims<3,   dyn, dyn>::value), 2);
    BOOST_TEST_EQ((count_dynamic_dims<dyn, 5,   dyn>::value), 2);
    BOOST_TEST_EQ((count_dynamic_dims<dyn, dyn, 9  >::value), 2);

    return boost::report_errors();
}


#include <iostream>
#include <boost/program_options.hpp>
#include "instance/problem_instance.h"
#include "ip/optimal_solver.h"

namespace po = boost::program_options;

int main(const int argc, const char **argv)
{
  using namespace angularfreezetag;

  po::options_description desc("Allowed options");
  std::string filename;
  std::string output_file;
  size_t size;

  desc.add_options()
      ("help,h", "produce help message")
      ("i", po::value<std::string>(&filename), "Instance file (output if -g is used)")
      ("g", po::value<size_t>(&size), "Generate a random instance with n points.")
      ("output,o", po::value<std::string>(&output_file), "Output file");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  //print help
  if (vm.count("help") > 0) {
    std::cout << desc << std::endl;
    return 0;
  }

  ProblemInstance instance;
  if (vm.count("g") > 0) {
    //generate instance
    instance.FillRandomly(size);
    std::ofstream file;
    file.open(filename, std::ios::out | std::ios::trunc);
    if (file.is_open()) {
      file << instance;
      file.close();
    } else {
      std::cerr << "Could not write grid graph to file \"" << filename << "\"" << std::endl;
      std::exit(1);
    }
  } else {
    instance.LoadFromFile(filename);
  }

  std::ofstream log_file;
  log_file.open(output_file, std::ios::app);
  if (log_file.is_open()) {
    using clock = std::chrono::high_resolution_clock;
    using std::chrono::duration_cast;
    using time_unit = std::chrono::milliseconds;

    auto start = clock::now();
    OptimalSolver solver{instance};
    solver.Solve();
    auto end = clock::now();
    auto time = duration_cast<time_unit>(end - start).count();
    log_file << filename << "\t" << instance.points.size() << "\t" << time << "\t" << solver.GetLowerBound() << "\t"
             << solver.GetObjValue() << std::endl;
    return 0;
  }

  return 2;
}


#include<iostream>
#include<vector>
#include<cstdlib>
#include<mpi.h>
#include <sys/time.h>
#include <ctime>
#include <boost/multi_array.hpp>

double getTime() {
  struct timeval tv;
  gettimeofday(&tv, NULL);
  return double(tv.tv_sec)+double(tv.tv_usec)/1000000.0;
}

void mySum(void *in_, void *out_, int *len, MPI_Datatype *dtype) {
  double* in = reinterpret_cast<double*>(in_);
  double* out = reinterpret_cast<double*>(out_);
  MPI_Aint sz,lb;
  MPI_Type_get_extent(*dtype,&lb,&sz);
  for(int i=0, iend=sz/sizeof(double); i<iend; i++)
    out[i] += in[i];
}

using std::cout;
using std::cin;
using std::endl;
using boost::extents;

// run with 4 procs
int main(int argc,char* argv[])
{

  MPI_Init(&argc,&argv);;

  int rank, size;
  MPI_Comm_size(MPI_COMM_WORLD, &size);
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);

  int nw=16;
  int ni=2*624*624;
  double t1,t2;
  int blk=10000;
  if(argc>1) blk=atoi(argv[1]);

  boost::multi_array<std::complex<double>,2> A(extents[ni][nw*size]);  // 2x2 matrix
  boost::multi_array<std::complex<double>,2> B(extents[ni][nw]);  // 2x2 matrix
  boost::multi_array<std::complex<double>,2> C(extents[ni][nw]);  // 2x2 matrix

  std::vector<int> nr{10000,100000,250000,500000,ni};

  for(int n=0; n<nr.size(); n++) {

  ni = nr[n];
  if(blk > ni) continue;
  int nblk = ni/blk;
  ni = nblk*blk;

  MPI_Allreduce(MPI_IN_PLACE,A.data(),2*ni*nw*size,MPI_DOUBLE,MPI_SUM,MPI_COMM_WORLD);

  int nt=4;
  MPI_Barrier(MPI_COMM_WORLD);
  t1=getTime();
  for(int i=0; i<nt; i++)
    MPI_Allreduce(MPI_IN_PLACE,A.data(),2*ni*nw*size,MPI_DOUBLE,MPI_SUM,MPI_COMM_WORLD);
  t2=getTime();
  double tv1=(t2-t1)/nt;

  std::vector<MPI_Request> req(size*nblk);
  std::vector<MPI_Status> st(size*nblk);
  for(int r=0; r<size; r++) {
    if(rank==r)
      for(int b=0; b<nblk; b++)
        MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
    else
      for(int b=0; b<nblk; b++)
        MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
  }
  MPI_Waitall(size*nblk,req.data(),st.data());

  MPI_Barrier(MPI_COMM_WORLD);
  t1=getTime();
  for(int i=0; i<nt; i++) {
    for(int r=0; r<size; r++) {
      if(rank==r)
        for(int b=0; b<nblk; b++)
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
      else
        for(int b=0; b<nblk; b++)
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
    }
    MPI_Waitall(size*nblk,req.data(),st.data());
  }
  t2=getTime();
  double tv3=(t2-t1)/nt;

  for(int b=0; b<nblk; b++)
    for(int r=0; r<size; r++) {
      if(rank==r)
        MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
      else
        MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
    }
  MPI_Waitall(size*nblk,req.data(),st.data());

  MPI_Barrier(MPI_COMM_WORLD);
  t1=getTime();
  for(int i=0; i<nt; i++) {
    for(int b=0; b<nblk; b++)
      for(int r=0; r<size; r++) {
        if(rank==r)
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
        else
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[r*nblk+b]);
      }
    MPI_Waitall(size*nblk,req.data(),st.data());
  }
  t2=getTime();
  double tv4=(t2-t1)/nt;

  int nmmax=4096;
  req.resize(nmmax);
  st.resize(nmmax);
  MPI_Barrier(MPI_COMM_WORLD);
  t1=getTime();
  for(int i=0; i<nt; i++) {
    int cnt=0;
    for(int b=0; b<nblk; b++) {
      for(int r=0; r<size; r++) {
        if(rank==r)
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[cnt++]);
        else
          MPI_Ireduce(C.data()+b*blk,B.data()+b*blk,2*blk*nw,MPI_DOUBLE,MPI_SUM,r,MPI_COMM_WORLD,&req[cnt++]);
      }
      if(cnt==nmmax) {
        MPI_Waitall(cnt,req.data(),st.data());
        cnt=0;
      }
    }
    if(cnt>0) {
      MPI_Waitall(cnt,req.data(),st.data());
      cnt=0;
    }
  }
  t2=getTime();
  double tv5=(t2-t1)/nt;

  if(rank==0) cout<<" n, times (Allreduce, multiple IReduce 1, multiple IReduce 2): " <<ni <<" " <<tv1 <<" " <<tv3 <<" " <<tv4 <<" " <<tv5 <<endl;

  }

  MPI_Finalize();

  return 0;
}


#ifndef BNI_SERIALIZER_CSV_HPP
#define BNI_SERIALIZER_CSV_HPP

#include <string>
#include <vector>
#include <bayesian/graph.hpp>
#include <boost/algorithm/string.hpp>

namespace bn {
namespace serializer {

class csv {
public:
    template<class InputStream>
    InputStream& load(InputStream& ist, bn::graph_t& graph)
    {
	    auto const& node_list = graph.vertex_list();

	    std::string line;
	    std::vector<std::string> splited_line;

	    // 1行目は読み飛ばし
	    std::getline(ist, line);

	    // 2行目以降
	    for(int i = 0; i < node_list.size(); ++i)
	    {
		    std::getline(ist, line);
            boost::algorithm::split(splited_line, line, boost::is_any_of(","));
		    for(int j = 0; j < node_list.size(); ++j)
		    {
			    // i と jをつなぐ．見るところはiとj+1
                if(splited_line[j + 1] == "*")
                    graph.add_edge(node_list[i], node_list[j]);
		    }
	    }

        return ist;
    }

    template<class OutputStream>
    OutputStream& write(OutputStream& ost, bn::graph_t const& graph)
    {
        auto const& vertex_list = graph.vertex_list();
        auto const vertex_num = vertex_list.size();

        for(auto const& vertex : vertex_list) ost << "," << vertex->id;
        ost << "\n";

        for(auto const& looking_node : vertex_list)
        {
            ost << looking_node->id;

            auto const& child_nodes = graph.out_vertexes(looking_node);
            for(auto const& candidate_node : vertex_list)
            {
                auto const it = std::find(child_nodes.cbegin(), child_nodes.cend(), candidate_node);
                if(it == child_nodes.cend()) ost << ", ";
                else                         ost << ",*";
            }
            ost << "\n";
        }

        return ost;
    }
};

} // namespace serializer
} // namespace bn

#endif // #ifndef BNI_SERIALIZER_CSV_HPP


/**
 * \file gtpack/cooperative.hpp
 *
 * \brief Cooperative games.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright 2013 Marco Guazzone (marco.guazzone@gmail.com)
 *
 * 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.
 */

#ifndef GTPACK_COOPERATIVE_HPP
#define GTPACK_COOPERATIVE_HPP

#include <boost/dynamic_bitset.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/smart_ptr.hpp>
#include <cstddef>
#include <dcs/algorithm/combinatorics.hpp>
#include <dcs/assert.hpp>
#include <dcs/debug.hpp>
#include <dcs/exception.hpp>
#include <dcs/logging.hpp>
#include <dcs/math/traits/float.hpp>
#include <iostream>
#include <ilconcert/iloalg.h>
#include <ilconcert/iloenv.h>
#include <ilconcert/iloexpression.h>
#include <ilconcert/ilomodel.h>
#include <ilcplex/ilocplex.h>
#include <limits>
#include <map>
#include <set>
#include <sstream>
#include <stdexcept>

namespace gtpack {

typedef unsigned int pid_type;
typedef pid_type player_type; //XXX: DEPRECATED
typedef unsigned long cid_type;

static const cid_type empty_cid = 0;
static const cid_type empty_coalition_id = empty_cid; //XXX: DEPRECATED

/// Returns the identifier of the singleton coalition for the given player
inline
cid_type make_coalition_id(pid_type pid)
{
	const cid_type one = 1;

	return one << pid;
}

/// Returns the identifier of the coalition for the given players
template <typename IterT>
cid_type make_coalition_id(IterT first_player, IterT last_player)
{
	// Each coalition $S\subseteq\{1,2,...,N\}$ can be characterized
	// uniquely by the $\sum_{i \in S}{2^{i-1}}$, which is the sum of
	// integers associated to players belonging to a particular coalition.

	const cid_type one = 1;

	cid_type cid = 0;

	while (first_player != last_player)
	{
		const pid_type pid = *first_player;

		// Update only if the player in not already member of this coalition
		if (!(cid & (one << pid)))
		{
			cid += one << pid;
		}

		++first_player;
	}

	return cid;
}

/// Returns the identifier of the grand coalition for \a n players
inline
cid_type make_grand_coalition_id(std::size_t n)
{
	if (n == 0)
	{
		return empty_cid;
	}

	const std::size_t nb = sizeof(cid_type)*8; // # bits in cid_type
	return ~static_cast<cid_type>(0) >> (nb-n);
}

/// Returns the identifier of the complement of a given coalition
inline
cid_type make_complement_coalition_id(std::size_t n, cid_type cid)
{
    return make_grand_coalition_id(n) - cid;
}

/// Returns the identifier of the complement of a coalition for the given players
template <typename IterT>
cid_type make_complement_coalition_id(std::size_t n, IterT first_player, IterT last_player)
{
	return make_complement_coalition_id(n, make_coalition_id(first_player, last_player));
}

template <typename RealT>
struct characteristic_function
{
	typedef RealT real_type;

	public: real_type operator()(cid_type cid) const
	{
		if (cid == empty_coalition_id)
		{
			return 0;
		}

		return this->do_get(cid);
	}

	public: void operator()(cid_type cid, real_type v)
	{
		DCS_ASSERT(cid != empty_coalition_id,
				   DCS_EXCEPTION_THROW(::std::invalid_argument,
									   "Cannot change the value of the empty coalition"));

		this->do_set(cid, v);
	}

//	public: real_type& operator()(cid_type cid)
//	{
//		return this->do_get(cid);
//	}

	private: virtual real_type do_get(cid_type cid) const = 0;

	private: virtual void do_set(cid_type cid, real_type v) = 0;

//	private: virtual real_type& do_get(cid_type cid) = 0;
}; // characteristic_function

template <typename RealT>
class enumerated_characteristic_function: public characteristic_function<RealT>
{
	public: typedef RealT real_type;

	public: enumerated_characteristic_function()
	{
	}

	public: template <typename CidValIterT>
			enumerated_characteristic_function(CidValIterT first, CidValIterT last)
	: map_(first, last)
	{
	}

	private: real_type do_get(cid_type cid) const
	{
		return map_.count(cid) > 0 ? map_.at(cid) : ::std::numeric_limits<real_type>::quiet_NaN();
	}

	private: void do_set(cid_type cid, real_type v)
	{
		map_[cid] = v;
	}

//	private: real_type& do_get(cid_type cid)
//	{
//		return map_[cid];
//	}

	private: ::std::map<cid_type,real_type> map_;
}; // enumerated_characteristic_function

template <typename RealT>
class explicit_characteristic_function: public enumerated_characteristic_function<RealT> { };
/*
{
	public: typedef RealT real_type;

	public: explicit_characteristic_function()
	{
	}

	public: template <typename CidValIterT>
			explicit_characteristic_function(CidValIterT first, CidValIterT last)
	: map_(first, last)
	{
	}

	private: real_type do_get(cid_type cid) const
	{
		return map_.count(cid) > 0 ? map_.at(cid) : ::std::numeric_limits<real_type>::quiet_NaN();
	}

	private: void do_set(cid_type cid, real_type v)
	{
		map_[cid] = v;
	}

//	private: real_type& do_get(cid_type cid)
//	{
//		return map_[cid];
//	}

	private: ::std::map<cid_type,real_type> map_;
}; // explicit_characteristic_function
*/

template <typename RealT>
class players_coalition
{
	public: typedef unsigned long cid_type;
	public: typedef RealT real_type;
	private: typedef ::boost::dynamic_bitset<> bitset_container;
	private: typedef typename bitset_container::size_type size_type;

//	template <typename C, typename CT, typename R>
//	friend
//	::std::basic_ostream<C,CT>& operator<<(::std::basic_ostream<C,CT>&, players_coalition<R> const&);

	public: template <typename IterT>
			static cid_type make_id(IterT first_player, IterT last_player)
	{
		// Each coalition $S\subseteq\{1,2,...,N\}$ can be characterized
		// uniquely by the $\sum_{i \in S}{2^{i-1}}$, which is the sum of
		// integers associated to players belonging to a particular coalition.

		const cid_type one(1);

		cid_type cid(0);

		while (first_player != last_player)
		{
			cid += one << *first_player;

			++first_player;
		}

		return cid;
	}

	public: players_coalition(::std::size_t n, cid_type cid)
	: id_(cid),
	  players_(),
	  //players_bs_(n, id_),
	  players_bs_(::std::max(num_bits(id_), n), id_),
	  n_(players_bs_.count()),
	  v_(0)
	{
		for (size_type player = players_bs_.find_first();
			 player != bitset_container::npos;
			 player = players_bs_.find_next(player))
		{
			players_.push_back(player);
		}
	}

//	public: template <typename IterT>
//			players_coalition(::std::size_t n, IterT first_player, IterT last_player)
//	: id_(make_id(first_player, last_player)),
//	  players_(n, id_),
//	  n_(players_.count()),
//	  v_(0)
//	{
//	}

	public: template <typename IterT>
			players_coalition(IterT first_player, IterT last_player)
	: id_(make_id(first_player, last_player)),
	  players_(first_player, last_player),
	  players_bs_(players_.size(), id_),
	  n_(players_.size()),
	  v_(0)
	{
	}

	public: cid_type id() const
	{
		return id_;
	}

	public: ::std::size_t size() const
	{
		return n_;
	}

	public: bool has_player(pid_type player) const
	{
		return players_bs_.test(player);
	}

	public: ::std::vector<pid_type> players() const
	{
		return players_;
	}

	public: ::std::size_t num_players() const
	{
		return players_.size();
	}

	public: void value(real_type v)
	{
		v_ = v;
	}

	public: real_type value() const
	{
		return v_;
	}

//	private: static unsigned int count_bit_set(unsigned long v)
//	{
//		unsigned int c(0); // c accumulates the total bits set in v
//		while (v)
//		{
//			v &= v-1; // Clear the least significant bit set
//			++c;
//		}
//
//		return c;
//	}

	private: static ::std::size_t num_bits(unsigned long v)
	{
		return ::std::floor(::std::log(v)/::std::log(2)+1);
	}

	//private: ::std::bitset<8*sizeof(cid_type)> players_;
	private: cid_type id_;
	private: ::std::vector<pid_type> players_;
	private: bitset_container players_bs_;
	private: ::std::size_t n_;
	private: RealT v_;
}; // players_coalition

template <typename CharT,
		  typename CharTraitsT,
		  typename RealT>
::std::basic_ostream<CharT,CharTraitsT>& operator<<(::std::basic_ostream<CharT,CharTraitsT>& os, players_coalition<RealT> const& coalition)
{
	typedef ::std::vector<pid_type> player_container;
	typedef typename player_container::const_iterator player_iterator;

	bool first(true);

	os << "{";
	player_container players(coalition.players());
	player_iterator end_it(players.end());
	for (player_iterator it = players.begin(); it != end_it; ++it)
	{
		if (!first)
		{
			os << ",";
		}
		else
		{
			first = false;
		}
		os << *it;
	}
	os << "}";

	return os;
}

template <typename RealT>
class cooperative_game
{
	public: typedef RealT real_type;

	public: cooperative_game()
	{
	}

	public: cooperative_game(::std::size_t n,
							 ::boost::shared_ptr< characteristic_function<RealT> > const& p_v)
	: n_(n),
	  p_v_(p_v)
	{
		for (pid_type i = 0; i < n_; ++i)
		{
			players_.insert(i);
		}

		coal_struc_.insert(this->coalition(players_.begin(), players_.end()).id());
	}

	public: template <typename IterT>
			cooperative_game(IterT first_player,
							 IterT last_player,
							 ::boost::shared_ptr< characteristic_function<RealT> > const& p_v)
	: n_(0),
	  p_v_(p_v),
	  players_(first_player,last_player)
	{
		n_ = players_.size();

		coal_struc_.insert(this->coalition(players_.begin(), players_.end()).id());
	}

	public: ::std::size_t num_players() const
	{
		return n_;
	}

	public: ::std::vector<pid_type> players() const
	{
		return ::std::vector<pid_type>(players_.begin(), players_.end());
	}

	public: real_type value(cid_type cid) const
	{
		return (*p_v_)(cid);
	}

	public: void value(cid_type cid, real_type value)
	{
		(*p_v_)(cid, value);
	}

	public: players_coalition<real_type> coalition(cid_type cid) const
	{
		players_coalition<real_type> c(n_, cid);

		c.value(this->value(cid));

		return c;
	}

	public: template <typename PlayerIterT>
			players_coalition<real_type> coalition(PlayerIterT first, PlayerIterT last) const
	{
		players_coalition<real_type> c(first, last);

		c.value(this->value(c.id()));

		return c;
	}

	public: template <typename CidIterT>
			void coalition_structure(CidIterT first, CidIterT last)
	{
		coal_struc_.clear();
		while (first != last)
		{
			const cid_type cid(*first);

			coal_struc_.insert(cid);

			++first;
		}
	}

//	public: ::std::vector<cid_type> coalition_structure() const
//	{
//		return ::std::vector<cid_type>(coal_struc_.begin(), coal_struc_.end());
//	}
	public: ::std::set<cid_type> coalition_structure() const
	{
		return coal_struc_;
	}

	public: template <typename PlayerIterT>
			cooperative_game<real_type> subgame(PlayerIterT first, PlayerIterT last) const
	{
		return cooperative_game<real_type>(first, last, p_v_);
	}

	public: cooperative_game<real_type> subgame(cid_type cid) const
	{
		const ::std::vector<pid_type> players = this->coalition(n_, cid).players();

		return this->subgame(players.begin(), players.end());
	}

	private: ::std::size_t n_; ///< Number of players
	//private: ::boost::function<real_type (cid_type)> p_v_; ///< Pointer to the characteristic function
	private: ::boost::shared_ptr< characteristic_function<real_type> > p_v_; ///< Pointer to the characteristic function
	private: ::std::set<pid_type> players_; ///< The set of players
	private: ::std::set<cid_type> coal_struc_; ///< The set of coalition ID making the coalition structure for this game
}; // cooperative_game

template <typename RealT>
class core
{
	public: typedef RealT real_type;

	public: core(/*::boost::shared_ptr< cooperative_game<real_type> > const& p_game*/)
	: empty_(true)
	{
	}

	public: template <typename IterT>
			core(IterT first_payoff, IterT last_payoff)
	: empty_(false),
	  x_(first_payoff, last_payoff)
	{
	}

	public: ::std::vector<real_type> imputation() const
	{
		return x_;
	}

	public: bool empty() const
	{
		return empty_;
	}

	private: bool empty_;
	private: ::std::vector<real_type> x_;
};

/**
 * \brief Compute the Shapley value for a given player.
 *
 * Given a game \f$(N,v)\f$, the Shapley value \f$\phi_i\f$ for player \f$i\f$
 * is computed as:
 * \f[
 *  \phi_i(v)=\sum_{S \subseteq N \setminus \{i\}} \frac{|S|!\; (|N|-|S|-1)!}{|N|!}(v(S\cup\{i\})-v(S))
 * \f]
 */
template <typename RealT>
RealT shapley_value(cooperative_game<RealT> const& game, pid_type player)
{
	const ::std::size_t n(game.num_players());
	const RealT n_fact(::boost::math::factorial<RealT>(n));

	::std::vector<pid_type> players(game.players());
	::std::set<pid_type> other_players(players.begin(), players.end());
	players.clear();
	other_players.erase(player);

	RealT sv(0);

	if (other_players.size() > 0)
	{
		::dcs::algorithm::lexicographic_subset subset(other_players.size(), true);
		while (subset.has_next())
		{
			::std::size_t s(subset.size());
			RealT s_fact(::boost::math::factorial<RealT>(s));
			RealT nmsm1_fact(::boost::math::factorial<RealT>(n-s-1));

			::std::vector<pid_type> tmp_players(subset(other_players.begin(), other_players.end()));
			cid_type s_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());
			tmp_players.push_back(player);
			cid_type sui_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());

			sv += s_fact*nmsm1_fact*(game.value(sui_cid)-game.value(s_cid));

			++subset;
		}
	}
	else
	{
		cid_type cid = players_coalition<RealT>::make_id(&player, &player+1);
		sv += game.value(cid);
	}

	return sv/n_fact;
}

/**
 * \brief Compute the Shapley value for all the players of a given game.
 *
 * Given a game \f$(N,v)\f$, the Shapley value \f$\phi_i\f$ for player \f$i\f$
 * is computed as:
 * \f[
 *  \phi_i(v)=\sum_{S \subseteq N \setminus \{i\}} \frac{|S|!\; (|N|-|S|-1)!}{|N|!}(v(S\cup\{i\})-v(S))
 * \f]
 */
template <typename RealT>
::std::map<pid_type,RealT> shapley_value(cooperative_game<RealT> const& game)
{
	const ::std::size_t n(game.num_players());
	const RealT n_fact(::boost::math::factorial<RealT>(n));

	::std::vector<pid_type> players(game.players());

	::std::map<pid_type,RealT> sv_map;

	::std::vector<pid_type>::const_iterator players_end_it(players.end());
	for (::std::vector<pid_type>::const_iterator players_it = players.begin();
		 players_it != players_end_it;
		 ++players_it)
	{
		pid_type pid(*players_it);

		RealT sv(0);

		::std::set<pid_type> other_players(players.begin(), players.end());
		other_players.erase(pid);

		if (other_players.size() > 0)
		{
			::dcs::algorithm::lexicographic_subset subset(other_players.size(), true);
			while (subset.has_next())
			{
				const ::std::size_t s(subset.size());
				const RealT s_fact(::boost::math::factorial<RealT>(s));
				const RealT nmsm1_fact(::boost::math::factorial<RealT>(n-s-1));

				::std::vector<pid_type> tmp_players(subset(other_players.begin(), other_players.end()));
				cid_type s_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());
				tmp_players.push_back(pid);
				const cid_type sui_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());

				sv += s_fact*nmsm1_fact*(game.value(sui_cid)-game.value(s_cid));

				++subset;
			}
		}
		else
		{
			cid_type cid = players_coalition<RealT>::make_id(&pid, &pid+1);
			sv += game.value(cid);
		}

		sv_map[pid] = sv/n_fact;
	}

	return sv_map;
}

/**
 * \brief Compute the Banzhaf value for all the players of a given game.
 *
 * Given a game \f$(N,v)\f$, the Banzhaf value \f$\beta_i\f$ for player \f$i\f$
 * is computed as:
 * \f[
 *  \beta_i(v)=\frac{1}{2^{|N|-1}}\sum_{S \subseteq N \setminus \{i\}} (v(S\cup\{i\})-v(S))
 * \f]
 */
template <typename RealT>
::std::map<pid_type,RealT> banzhaf_value(cooperative_game<RealT> const& game)
{
	const ::std::size_t n(game.num_players());
	const RealT prod(1.0/::std::pow(2, n-1));
	//const RealT prod(1.0/(1 << (n-1)));

	::std::vector<pid_type> players(game.players());

	::std::map<pid_type,RealT> bv_map;

	::std::vector<pid_type>::const_iterator players_end_it(players.end());
	for (::std::vector<pid_type>::const_iterator players_it = players.begin();
		 players_it != players_end_it;
		 ++players_it)
	{
		pid_type player(*players_it);

		RealT bv(0);

		::std::set<pid_type> other_players(players.begin(), players.end());
		other_players.erase(player);

		if (other_players.size() > 0)
		{
			::dcs::algorithm::lexicographic_subset subset(other_players.size(), true);
			while (subset.has_next())
			{
				::std::vector<pid_type> tmp_players(subset(other_players.begin(), other_players.end()));
				cid_type s_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());
				tmp_players.push_back(player);
				cid_type sui_cid = players_coalition<RealT>::make_id(tmp_players.begin(), tmp_players.end());

				bv += (game.value(sui_cid)-game.value(s_cid));

				++subset;
			}
		}
		else
		{
			cid_type cid = players_coalition<RealT>::make_id(&player, &player+1);
			bv += game.value(cid);
		}

		bv_map[player] = bv*prod;
	}

	return bv_map;
}

/**
 * \brief Compute the normalized Banzhaf value for all the players of a given
 *  game.
 *
 * Given a game \f$(N,v)\f$, the normalized Banzhaf value \f$\bar{\beta}_i\f$
 * for player \f$i\f$ is computed as:
 * \f[
 *  \bar{\beta}_i=\frac{\beta_i}{\sum_{j \in N}\beta_j}
 * \f]
 * where
 * \f[
 *  \beta_i(v)=\frac{1}{2^{|N|-1}}\sum_{S \subseteq N \setminus \{i\}} (v(S\cup\{i\})-v(S))
 * \f]
 * is the non-normalized Banzhaf value.
 */
template <typename RealT>
::std::map<pid_type,RealT> norm_banzhaf_value(cooperative_game<RealT> const& game)
{
	::std::map<pid_type,RealT> bv_map = banzhaf_value(game);

	RealT den(0);
	// Compute the sum of all non-normalized Banzhaf values
	{
		typename ::std::map<pid_type,RealT>::const_iterator end_it(bv_map.end());
		for (typename ::std::map<pid_type,RealT>::const_iterator it = bv_map.begin();
			 it != end_it;
			 ++it)
		{
			den += it->second;
		}
	}
	// Normalize each Banzhaf value
	{
		::std::vector<pid_type> players(game.players());
		cid_type gc_cid = players_coalition<RealT>::make_id(players.begin(), players.end());
		RealT v_gc = game.value(gc_cid);
		typename ::std::map<pid_type,RealT>::iterator end_it(bv_map.end());
		for (typename ::std::map<pid_type,RealT>::iterator it = bv_map.begin();
			 it != end_it;
			 ++it)
		{
			it->second *= v_gc/den;
		}
//		typename ::std::map<pid_type,RealT>::iterator end_it(bv_map.end());
//		for (typename ::std::map<pid_type,RealT>::iterator it = bv_map.begin();
//			 it != end_it;
//			 ++it)
//		{
//			it->second /= den;
//		}
	}

	return bv_map;
}

/**
 * \brief Compute the Aumann-Dreze value for all the players of a given game.
 *
 * Given a game \f$(N,v)\f$ with coalition structure \f$\mathcal{P}\f$, the
 * Aumann-Dreze value \f$\text{AD}_i\f$ for player \f$i\f$ is computed as:
 * \f[
 *  \text{AD}_i(v)=\sum_{S \subseteq \mathcal{P}(i) \setminus \{i\}} \frac{|S|!\; (|\mathcal{P}(i)|-|S|-1)!}{|\mathcal{P}(i)|!}(v(S\cup\{i\})-v(S))
 * \f]
 * where \f$\mathcal{P}(i)\in\mathcal{P}\f$ is the coalition containing the
 * player $i$.
 *
 * References:
 * -# R.J. Aumann and J.H. Dr{\`e}ze,
 *    Cooperative games with coalition structures,
 *    International Journal of Game Theory 3:217-237, 1974
 * .
 */
template <typename RealT>
::std::map<pid_type,RealT> aumann_dreze_value(cooperative_game<RealT> const& game)
{
	const ::std::vector<cid_type> structure(game.coalition_structure());
	const ::std::size_t nc(structure.size());

	::std::map<pid_type,RealT> ad_map;

	for (::std::size_t i = 0; i < nc; ++i)
	{
		const cid_type cid(structure[i]);

		const ::std::vector<pid_type> players = game.coalition(cid).players();

		::std::map<pid_type,RealT> sh_map = shapley_value(game.subgame(cid));
		ad_map.insert(sh_map.begin(), sh_map.end());
	}

	return ad_map;
}

/**
 * \brief Compute the Chi-value for all the players of a given
 *  game.
 *
 * Given a game \f$(N,v)\f$ with coalition structure \f$\mathcal{P}\f$, the
 * Chi-value \f$\Chi_i\f$ for player \f$i\f$ is computed as:
 * \f[
 *  Chi_i=\phi_i(N,v)+\frac{v(\mathcal{P}(i)-\sum_{j\in\mathcal{P}(i)}\phi_j(N,v)}{|\mathcal{P}(i)|}
 * \f]
 * where\f$\mathcal{P}(i)\f$ is the coalition in \f$\mathcal{P}\f$ that contains
 * the player \f$i\f$.
 *
 * References:
 * -# Andre` Casajus. "Outside options, component efficiency, and stability", Games and Economic Behavior 65:49-61, 2009.
 * .
 */
template <typename RealT>
::std::map<pid_type,RealT> chi_value(cooperative_game<RealT> const& game)
{
	const ::std::vector<cid_type> structure(game.coalition_structure());
	const ::std::size_t nc(structure.size());
	const ::std::map<pid_type,RealT> shapley_map(shapley_value(game));

	::std::map< ::std::size_t,RealT> coal_shapley_map;

	for (::std::size_t i = 0; i < nc; ++i)
	{
		const cid_type cid = structure[i];
		const ::std::vector<pid_type> players = game.coalition(cid).players();
		const ::std::size_t np = players.size();

		RealT sh(0);
		for (::std::size_t j = 0; j < np; ++j)
		{
			const pid_type pid(players[j]);

			sh += shapley_map.at(pid);
		}
		coal_shapley_map[cid] = sh;
	}

	::std::map<pid_type,RealT> chi_map;

	for (::std::size_t i = 0; i < nc; ++i)
	{
		const cid_type cid = structure[i];
		const players_coalition<RealT> coalition = game.coalition(cid);
		const ::std::vector<pid_type> players = coalition.players();
		const ::std::size_t np = players.size();
		const RealT add = (coalition.value()-coal_shapley_map.at(cid))/np;

		for (::std::size_t j = 0; j < np; ++j)
		{
			const pid_type pid(players[j]);

			chi_map[pid] = shapley_map.at(pid)+add;
		}
	}

	return chi_map;
}

/**
 * \brief Compute the core of the given cooperative game.
 *
 * Given a cooperative game G=(N,v), the core C(v) is the set
 * \f[
 *  \biggl\bigl{x | \sum_{i \in N} x_i=v(N), \text{ and } x_i \ge v(i) \forall i \in N, \text{ and } \sum_{i\in S} x_i \ge v(S) \forall S \subset N\setminus \{\emptyset\bigr\}\biggr\}
 * \f]
 *
 * TODO: manage the case of games with a coalition structure CS. For such games, the core is defined as the set of payoff vectors x = (x1 , ..., xn ) and a coalition Structure CS = (C1 , ...,Ck ) such that:
 * - \forall C \subseteq N, x(C) \ge v(C) and
 * - x(C_j) = v(C_j) for any C_j \in CS
 * .
 */
template <typename RealT>
//core<RealT> find_core(typename coalition<RealT>::cid_type const& cid, ::std::vector<pid_type> const& players, ::std::map<cid_type,RealT> const& coalitions)
core<RealT> find_core(cooperative_game<RealT> const& game)
//typename coalition<RealT>::cid_type const& cid, ::std::vector<pid_type> const& players, ::std::map<cid_type,RealT> const& coalitions)
{
	typedef RealT real_type;
	typedef players_coalition<real_type> coalition_type;
	//typedef typename coalition_type::cid_type cid_type;
	typedef IloNumVarArray var_vector_type;
//	typedef IloArray<IloNumVarArray> var_matrix_type;
	typedef typename ::dcs::algorithm::lexicographic_subset subset_type;
	typedef typename subset_type::const_iterator subset_iterator;
	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container subset_container;
//	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container_const_iterator subset_container_iterator;

	core<real_type> kore;

	// Setting up vars
	try
	{
		// Initialize the Concert Technology app
		IloEnv env;

		IloModel model(env);

		model.setName("Core");

		::std::size_t n(game.num_players());
		::std::vector<pid_type> players(game.players());

		// Decision Variable

		// Variables x_{i} >= 0: the payoff for player i
		var_vector_type x(env, n);
		for (std::size_t i = 0; i < n; ++i)
		{
			std::ostringstream oss;
			oss << "x[" << i << "]";
			x[i] = IloNumVar(env, -IloInfinity, IloInfinity, ILOFLOAT, oss.str().c_str());
			model.add(x[i]);
		}

		// Constraints

		std::size_t cc(0); // Constraint counter
		std::size_t csc(0); // Constraint subcounter

		subset_type lex_subset(n, false);

		// C1: \forall S \subset N, \sum_{i \in S} x[i] >= v(S), and \sum_{i \in N} x[i] = v(N)
		++cc;
		while (lex_subset.has_next())
		{
			++csc;

			subset_container players_subset;

			//players_subset = ::dcs::algorithm::next_subset(players.begin(), players.end(), lex_subset);
			players_subset = lex_subset(players.begin(), players.end());

			cid_type cid = coalition_type::make_id(players_subset.begin(), players_subset.end());

			real_type v_cid = game.value(cid);

			std::ostringstream oss;
			oss << "C" << cc << "_{" << csc << "}";
			IloExpr lhs(env);
			subset_iterator sub_end_it(lex_subset.end());
			for (subset_iterator sub_it = lex_subset.begin();
				 sub_it != sub_end_it;
				 ++sub_it)
			{
				typename subset_iterator::value_type player_num(*sub_it);

				// check: player_num is a valid player number
				DCS_ASSERT(player_num < n,
						   DCS_EXCEPTION_THROW(::std::runtime_error, "Invalid player number"));

				lhs += x[player_num];
			}
			IloConstraint cons;
			if (players_subset.size() == n)
			{
				cons = IloConstraint(lhs == IloNum(v_cid));
			}
			else
			{
				cons = IloConstraint(lhs >= IloNum(v_cid));
			}
			cons.setName(oss.str().c_str());
			model.add(cons);

			++lex_subset;
		}

		// Set objective
		//   max z = \sum_{i=1}^N x_i
		IloObjective z;
		z = IloMaximize(env, IloSum(x));
		model.add(z);

		IloCplex solver(model);
#ifndef DCS_DEBUG
		solver.setOut(env.getNullStream());
		solver.setWarning(env.getNullStream());
#else // DCS_DEBUG
		solver.exportModel("cplex-core_model.lp");
#endif // DCS_DEBUG

		// Set Relative Gap to 1%: CPLEX will stop as soon as it has found a feasible integer solution proved to be within 1% of optimal.
		//if (relative_gap > 0)
		//{
		//	solver.setParam(IloCplex::EpGap, relative_gap);
		//}

		bool solved = solver.solve();

		IloAlgorithm::Status status = solver.getStatus();
		switch (status)
		{
			case IloAlgorithm::Optimal: // The algorithm found an optimal solution.
			case IloAlgorithm::Feasible: // The algorithm found a feasible solution, though it may not necessarily be optimal.
//
//				v = static_cast<RealT>(solver.getObjValue());
				break;
			case IloAlgorithm::Infeasible: // The algorithm proved the model infeasible (i.e., it is not possible to find an assignment of values to variables satisfying all the constraints in the model).
			case IloAlgorithm::Unbounded: // The algorithm proved the model unbounded.
			case IloAlgorithm::InfeasibleOrUnbounded: // The model is infeasible or unbounded.
			case IloAlgorithm::Error: // An error occurred and, on platforms that support exceptions, that an exception has been thrown.
			case IloAlgorithm::Unknown: // The algorithm has no information about the solution of the model.
			{
				::std::ostringstream oss;
				oss << "Optimization was stopped with status = " << status << " (CPLEX status = " << solver.getCplexStatus() << ", sub-status = " << solver.getCplexSubStatus() << ")";
				dcs::log_warn(DCS_LOGGING_AT, oss.str());
			}
		}

		if (solved)
		{
#ifdef DCS_DEBUG
			DCS_DEBUG_TRACE( "-------------------------------------------------------------------------------[" );
			DCS_DEBUG_TRACE( "- Objective value: " << static_cast<real_type>(solver.getObjValue()) );

			DCS_DEBUG_TRACE( "- Decision variables: " );

			// Output x_{i}
			for (std::size_t i = 0; i < n; ++i)
			{
				DCS_DEBUG_STREAM << x[i].getName() << " = " << solver.getValue(x[i]) << ::std::endl;
			}

			DCS_DEBUG_TRACE( "]-------------------------------------------------------------------------------" );
#endif // DCS_DEBUG

			::std::vector<real_type> payoff(n);
			for (std::size_t i = 0; i < n; ++i)
			{
				//payoff[players[i]] = solver.getValue(x[i]);
				payoff[i] = solver.getValue(x[i]);
			}
			kore = core<real_type>(payoff.begin(), payoff.end());
		}

		z.end();
		x.end();

		// Close the Concert Technology app
		env.end();
	}
	catch (IloException const& e)
	{
		::std::ostringstream oss;
		oss << "Got exception from CPLEX: " << e.getMessage();
		DCS_EXCEPTION_THROW(::std::runtime_error,
							oss.str());
	}
	catch (...)
	{
		DCS_EXCEPTION_THROW(::std::runtime_error,
							"Unexpected error during the optimization");
	}

	return kore;
}

/**
 * \brief Compute the core of the given cooperative game.
 *
 * Given a cooperative game G=(N,v), the core C(v) is the set
 * \f[
 *  \biggl\bigl{x | \sum_{i \in N} x_i=v(N), \text{ and } x_i \ge v(i) \forall i \in N, \text{ and } \sum_{i\in S} x_i \ge v(S) \forall S \subset N\setminus \{\emptyset\bigr\}\biggr\}
 * \f]
 */
template <typename RealT, typename IterT>
bool belongs_to_core(cooperative_game<RealT> const& game, IterT first_payoff, IterT last_payoff)
{
	typedef RealT real_type;
	typedef players_coalition<real_type> coalition_type;
	//typedef typename coalition_type::cid_type cid_type;
	typedef typename ::dcs::algorithm::lexicographic_subset subset_type;
	//typedef typename subset_type::const_iterator subset_iterator;
	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container subset_container;
	typedef typename ::dcs::algorithm::subset_traits<pid_type>::subset_container_const_iterator subset_container_iterator;

	::std::map<pid_type,real_type> x(first_payoff, last_payoff);
	::std::size_t n(game.num_players());
	::std::vector<pid_type> players(game.players());

	subset_type lex_subset(n, false);

	// \forall S \subset N, \sum_{i \in S} x[i] >= v(S), and \sum_{i \in N} x[i] = v(N)
	while (lex_subset.has_next())
	{
		subset_container players_subset;

		players_subset = lex_subset(players.begin(), players.end());

		cid_type cid = coalition_type::make_id(players_subset.begin(), players_subset.end());

		real_type v_cid = game.value(cid);
		real_type xs(0);

		subset_container_iterator sub_end_it(players_subset.end());
		for (subset_container_iterator sub_it = players_subset.begin();
			 sub_it != sub_end_it;
			 ++sub_it)
		{
			pid_type player_num(*sub_it);

			xs += x[player_num];
		}
		bool ok;
		if (lex_subset.size() == n)
		{
			//ok = xs == v_cid;
			ok = ::dcs::math::float_traits<real_type>::essentially_equal(xs, v_cid);
		}
		else
		{
			//ok = xs >= v_cid;
			ok = ::dcs::math::float_traits<real_type>::essentially_greater_equal(xs, v_cid);
		}
		if (!ok)
		{
			return false;
		}

		++lex_subset;
	}

	return true;
}

} // Namespace gtpack

#endif // GTPACK_COOPERATIVE_HPP


/* Copyright (c) 2016 - 2021, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#define BOOST_TEST_MODULE ThermalOperator

#include <Geometry.hh>
#include <GoldakHeatSource.hh>
#include <ThermalOperator.hh>

#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_nothing.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/numerics/matrix_tools.h>

#include <boost/property_tree/ptree.hpp>

#include "main.cc"

BOOST_AUTO_TEST_CASE(thermal_operator)
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Create the Geometry
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 12);
  geometry_database.put("length_divisions", 4);
  geometry_database.put("height", 6);
  geometry_database.put("height_divisions", 5);
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  // Create the DoFHandler
  dealii::hp::FECollection<2> fe_collection;
  fe_collection.push_back(dealii::FE_Q<2>(2));
  fe_collection.push_back(dealii::FE_Nothing<2>());
  dealii::DoFHandler<2> dof_handler(geometry.get_triangulation());
  dof_handler.distribute_dofs(fe_collection);
  dealii::AffineConstraints<double> affine_constraints;
  affine_constraints.close();
  dealii::hp::QCollection<1> q_collection;
  q_collection.push_back(dealii::QGauss<1>(3));
  q_collection.push_back(dealii::QGauss<1>(1));

  // Create the MaterialProperty
  boost::property_tree::ptree mat_prop_database;
  mat_prop_database.put("property_format", "polynomial");
  mat_prop_database.put("n_materials", 1);
  mat_prop_database.put("material_0.solid.density", 1.);
  mat_prop_database.put("material_0.powder.density", 1.);
  mat_prop_database.put("material_0.liquid.density", 1.);
  mat_prop_database.put("material_0.solid.specific_heat", 1.);
  mat_prop_database.put("material_0.powder.specific_heat", 1.);
  mat_prop_database.put("material_0.liquid.specific_heat", 1.);
  mat_prop_database.put("material_0.solid.thermal_conductivity", 10.);
  mat_prop_database.put("material_0.powder.thermal_conductivity", 10.);
  mat_prop_database.put("material_0.liquid.thermal_conductivity", 10.);
  std::shared_ptr<adamantine::MaterialProperty<2>> mat_properties(
      new adamantine::MaterialProperty<2>(
          communicator, geometry.get_triangulation(), mat_prop_database));

  // Create the heat souces
  boost::property_tree::ptree beam_database;
  beam_database.put("depth", 0.1);
  beam_database.put("absorption_efficiency", 0.1);
  beam_database.put("diameter", 1.0);
  beam_database.put("max_power", 10.);
  beam_database.put("scan_path_file", "scan_path.txt");
  beam_database.put("scan_path_file_format", "segment");
  std::vector<std::shared_ptr<adamantine::HeatSource<2>>> heat_sources;
  heat_sources.resize(1);
  heat_sources[0] =
      std::make_shared<adamantine::GoldakHeatSource<2>>(beam_database);

  // Initialize the ThermalOperator
  adamantine::ThermalOperator<2, 2, dealii::MemorySpace::Host> thermal_operator(
      communicator, adamantine::BoundaryType::adiabatic, mat_properties,
      heat_sources);
  thermal_operator.reinit(dof_handler, affine_constraints, q_collection);
  thermal_operator.compute_inverse_mass_matrix(dof_handler, affine_constraints,
                                               fe_collection);
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dummy(
      thermal_operator.m());
  thermal_operator.get_state_from_material_properties();

  BOOST_CHECK(thermal_operator.m() == 99);
  BOOST_CHECK(thermal_operator.m() == thermal_operator.n());

  // Check matrix-vector multiplications
  double const tolerance = 1e-15;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> src;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_1;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_2;

  dealii::MatrixFree<2, double> const &matrix_free =
      thermal_operator.get_matrix_free();
  matrix_free.initialize_dof_vector(src);
  matrix_free.initialize_dof_vector(dst_1);
  matrix_free.initialize_dof_vector(dst_2);

  src = 1.;
  thermal_operator.vmult(dst_1, src);
  BOOST_CHECK_CLOSE(dst_1.l1_norm(), 0., tolerance);

  thermal_operator.Tvmult(dst_2, src);
  BOOST_CHECK_CLOSE(dst_2.l1_norm(), dst_1.l1_norm(), tolerance);

  dst_2 = 1.;
  thermal_operator.vmult_add(dst_2, src);
  thermal_operator.vmult(dst_1, src);
  dst_1 += src;
  BOOST_CHECK_CLOSE(dst_1.l1_norm(), dst_2.l1_norm(), tolerance);

  dst_1 = 1.;
  thermal_operator.Tvmult_add(dst_1, src);
  BOOST_CHECK_CLOSE(dst_1.l1_norm(), dst_2.l1_norm(), tolerance);
}

BOOST_AUTO_TEST_CASE(spmv)
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Create the Geometry
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 12);
  geometry_database.put("length_divisions", 4);
  geometry_database.put("height", 6);
  geometry_database.put("height_divisions", 5);
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  // Create the DoFHandler
  dealii::hp::FECollection<2> fe_collection;
  fe_collection.push_back(dealii::FE_Q<2>(2));
  fe_collection.push_back(dealii::FE_Nothing<2>());
  dealii::DoFHandler<2> dof_handler(geometry.get_triangulation());
  dof_handler.distribute_dofs(fe_collection);
  dealii::AffineConstraints<double> affine_constraints;
  affine_constraints.close();
  dealii::hp::QCollection<1> q_collection;
  q_collection.push_back(dealii::QGauss<1>(3));
  q_collection.push_back(dealii::QGauss<1>(1));

  // Create the MaterialProperty
  boost::property_tree::ptree mat_prop_database;
  mat_prop_database.put("property_format", "polynomial");
  mat_prop_database.put("n_materials", 1);
  mat_prop_database.put("material_0.solid.density", 1.);
  mat_prop_database.put("material_0.powder.density", 1.);
  mat_prop_database.put("material_0.liquid.density", 1.);
  mat_prop_database.put("material_0.solid.specific_heat", 1.);
  mat_prop_database.put("material_0.powder.specific_heat", 1.);
  mat_prop_database.put("material_0.liquid.specific_heat", 1.);
  mat_prop_database.put("material_0.solid.thermal_conductivity", 1.);
  mat_prop_database.put("material_0.powder.thermal_conductivity", 1.);
  mat_prop_database.put("material_0.liquid.thermal_conductivity", 1.);
  std::shared_ptr<adamantine::MaterialProperty<2>> mat_properties(
      new adamantine::MaterialProperty<2>(
          communicator, geometry.get_triangulation(), mat_prop_database));

  // Create the heat souces
  boost::property_tree::ptree beam_database;
  beam_database.put("depth", 0.1);
  beam_database.put("absorption_efficiency", 0.1);
  beam_database.put("diameter", 1.0);
  beam_database.put("max_power", 10.);
  beam_database.put("scan_path_file", "scan_path.txt");
  beam_database.put("scan_path_file_format", "segment");
  std::vector<std::shared_ptr<adamantine::HeatSource<2>>> heat_sources;
  heat_sources.resize(1);
  heat_sources[0] =
      std::make_shared<adamantine::GoldakHeatSource<2>>(beam_database);

  // Initialize the ThermalOperator
  adamantine::ThermalOperator<2, 2, dealii::MemorySpace::Host> thermal_operator(
      communicator, adamantine::BoundaryType::adiabatic, mat_properties,
      heat_sources);
  thermal_operator.reinit(dof_handler, affine_constraints, q_collection);
  thermal_operator.compute_inverse_mass_matrix(dof_handler, affine_constraints,
                                               fe_collection);
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dummy(
      thermal_operator.m());
  thermal_operator.get_state_from_material_properties();
  BOOST_CHECK(thermal_operator.m() == 99);
  BOOST_CHECK(thermal_operator.m() == thermal_operator.n());

  // Build the matrix. This only works in serial.
  dealii::DynamicSparsityPattern dsp(dof_handler.n_dofs());
  dealii::DoFTools::make_sparsity_pattern(dof_handler, dsp, affine_constraints);
  dealii::SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dsp);
  dealii::SparseMatrix<double> sparse_matrix(sparsity_pattern);
  dealii::MatrixCreator::create_laplace_matrix(
      dof_handler, dealii::QGauss<2>(3), sparse_matrix);

  // Compare vmult using matrix free and building the matrix
  double const tolerance = 1e-12;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> src;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_1;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_2;

  dealii::MatrixFree<2, double> const &matrix_free =
      thermal_operator.get_matrix_free();
  matrix_free.initialize_dof_vector(src);
  matrix_free.initialize_dof_vector(dst_1);
  matrix_free.initialize_dof_vector(dst_2);

  for (unsigned int i = 0; i < thermal_operator.m(); ++i)
  {
    src = 0.;
    src[i] = 1;
    thermal_operator.vmult(dst_1, src);
    sparse_matrix.vmult(dst_2, src);
    for (unsigned int j = 0; j < thermal_operator.m(); ++j)
      BOOST_CHECK_CLOSE(dst_1[j], -dst_2[j], tolerance);
  }
}

BOOST_AUTO_TEST_CASE(spmv_rad)
{
  MPI_Comm communicator = MPI_COMM_WORLD;

  // Create the Geometry
  boost::property_tree::ptree geometry_database;
  geometry_database.put("import_mesh", false);
  geometry_database.put("length", 12);
  geometry_database.put("length_divisions", 4);
  geometry_database.put("height", 6);
  geometry_database.put("height_divisions", 5);
  adamantine::Geometry<2> geometry(communicator, geometry_database);
  // Create the DoFHandler
  dealii::hp::FECollection<2> fe_collection;
  fe_collection.push_back(dealii::FE_Q<2>(2));
  fe_collection.push_back(dealii::FE_Nothing<2>());
  dealii::DoFHandler<2> dof_handler(geometry.get_triangulation());
  dof_handler.distribute_dofs(fe_collection);
  dealii::AffineConstraints<double> affine_constraints;
  affine_constraints.close();
  dealii::hp::QCollection<1> q_collection;
  q_collection.push_back(dealii::QGauss<1>(3));
  q_collection.push_back(dealii::QGauss<1>(1));

  // Create the MaterialProperty
  boost::property_tree::ptree mat_prop_database;
  mat_prop_database.put("property_format", "polynomial");
  mat_prop_database.put("n_materials", 1);
  mat_prop_database.put("material_0.solid.density", 1.);
  mat_prop_database.put("material_0.powder.density", 1.);
  mat_prop_database.put("material_0.liquid.density", 1.);
  mat_prop_database.put("material_0.solid.specific_heat", 1.);
  mat_prop_database.put("material_0.powder.specific_heat", 1.);
  mat_prop_database.put("material_0.liquid.specific_heat", 1.);
  mat_prop_database.put("material_0.solid.thermal_conductivity", 1.);
  mat_prop_database.put("material_0.powder.thermal_conductivity", 1.);
  mat_prop_database.put("material_0.liquid.thermal_conductivity", 1.);
  mat_prop_database.put("material_0.solid.emissivity", 1.);
  mat_prop_database.put("material_0.powder.emissivity", 1.);
  mat_prop_database.put("material_0.liquid.emissivity", 1.);
  mat_prop_database.put("material_0.solid.radiation_heat_transfer_coef", 1.);
  mat_prop_database.put("material_0.powder.radiation_heat_transfer_coef", 1.);
  mat_prop_database.put("material_0.liquid.radiation_heat_transfer_coef", 1.);
  mat_prop_database.put("material_0.solid.convection_heat_transfer_coef", 1.);
  mat_prop_database.put("material_0.powder.convection_heat_transfer_coef", 1.);
  mat_prop_database.put("material_0.liquid.convection_heat_transfer_coef", 1.);
  mat_prop_database.put("material_0.radiation_temperature_infty", 0.0);
  mat_prop_database.put("material_0.convection_temperature_infty", 0.0);
  std::shared_ptr<adamantine::MaterialProperty<2>> mat_properties(
      new adamantine::MaterialProperty<2>(
          communicator, geometry.get_triangulation(), mat_prop_database));

  // Create the heat souces
  boost::property_tree::ptree beam_database;
  beam_database.put("depth", 0.1);
  beam_database.put("absorption_efficiency", 0.1);
  beam_database.put("diameter", 1.0);
  beam_database.put("max_power", 10.);
  beam_database.put("scan_path_file", "scan_path.txt");
  beam_database.put("scan_path_file_format", "segment");
  std::vector<std::shared_ptr<adamantine::HeatSource<2>>> heat_sources;
  heat_sources.resize(1);
  heat_sources[0] =
      std::make_shared<adamantine::GoldakHeatSource<2>>(beam_database);

  // Initialize the ThermalOperator
  adamantine::ThermalOperator<2, 2, dealii::MemorySpace::Host> thermal_operator(
      communicator, adamantine::BoundaryType::radiative, mat_properties,
      heat_sources);
  thermal_operator.reinit(dof_handler, affine_constraints, q_collection);
  thermal_operator.compute_inverse_mass_matrix(dof_handler, affine_constraints,
                                               fe_collection);
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host>
      temperature(thermal_operator.m());
  for (unsigned int i = 0; i < temperature.locally_owned_size(); ++i)
  {
    temperature.local_element(i) = 1.;
  }
  thermal_operator.evaluate_material_properties(temperature);
  thermal_operator.get_state_from_material_properties();
  BOOST_CHECK(thermal_operator.m() == 99);
  BOOST_CHECK(thermal_operator.m() == thermal_operator.n());

  // Build the matrix. This only works in serial.
  dealii::DynamicSparsityPattern dsp(dof_handler.n_dofs());
  dealii::DoFTools::make_sparsity_pattern(dof_handler, dsp, affine_constraints);
  dealii::SparsityPattern sparsity_pattern;
  sparsity_pattern.copy_from(dsp);
  dealii::SparseMatrix<double> sparse_matrix(sparsity_pattern);
  dealii::MatrixCreator::create_laplace_matrix(
      dof_handler, dealii::QGauss<2>(3), sparse_matrix);
  // Take care of the boundary condition
  unsigned int const fe_degree = 2;
  dealii::FE_Q<2> fe(fe_degree);
  dealii::QGauss<1> face_quadrature_formula(fe_degree + 1);
  unsigned int const n_face_q_points = face_quadrature_formula.size();
  dealii::FEFaceValues<2> fe_face_values(fe, face_quadrature_formula,
                                         dealii::update_values |
                                             dealii::update_quadrature_points |
                                             dealii::update_JxW_values);
  double const heat_transfer_coeff =
      1. * adamantine::Constant::stefan_boltzmann * 1. * 1.;
  unsigned int const dofs_per_cell = fe.n_dofs_per_cell();
  dealii::FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  for (auto const &cell : dof_handler.active_cell_iterators())
  {
    if (cell->at_boundary())
    {
      cell_matrix = 0.;
      for (auto const &face : cell->face_iterators())
      {
        if (face->at_boundary())
        {
          fe_face_values.reinit(cell, face);
          for (unsigned int q = 0; q < n_face_q_points; ++q)
            for (unsigned i = 0; i < dofs_per_cell; ++i)
              for (unsigned j = 0; j < dofs_per_cell; ++j)
              {
                cell_matrix(i, j) +=
                    heat_transfer_coeff * fe_face_values.shape_value(i, q) *
                    fe_face_values.shape_value(j, q) * fe_face_values.JxW(q);
              }
        }
      }
      cell->get_dof_indices(local_dof_indices);
      affine_constraints.distribute_local_to_global(
          cell_matrix, local_dof_indices, sparse_matrix);
    }
  }

  // Compare vmult using matrix free and building the matrix
  double const tolerance = 1e-12;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> src;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_1;
  dealii::LA::distributed::Vector<double, dealii::MemorySpace::Host> dst_2;

  dealii::MatrixFree<2, double> const &matrix_free =
      thermal_operator.get_matrix_free();
  matrix_free.initialize_dof_vector(src);
  matrix_free.initialize_dof_vector(dst_1);
  matrix_free.initialize_dof_vector(dst_2);

  for (unsigned int i = 0; i < thermal_operator.m(); ++i)
  {
    src = 0.;
    src[i] = 1;
    thermal_operator.vmult(dst_1, src);
    sparse_matrix.vmult(dst_2, src);
    for (unsigned int j = 0; j < thermal_operator.m(); ++j)
      BOOST_CHECK_CLOSE(dst_1[j], -dst_2[j], tolerance);
  }
}


#include<boost/numeric/ublas/vector.hpp>
#include<boost/numeric/ublas/io.hpp>

// #include "vector_concepts.hpp"
#include <boost/numeric/linear_algebra/vector_concepts.hpp>

typedef double Type;

namespace ublas = boost::numeric::ublas;

typedef ublas::vector<Type> Vector;

namespace math {

concept_map AdditiveAbelianGroup<Vector> {
  typedef boost::numeric::ublas::vector_binary<boost::numeric::ublas::vector<double, boost::numeric::ublas::unbounded_array<double, std::allocator<double> > >, boost::numeric::ublas::vector<double, boost::numeric::ublas::unbounded_array<double, std::allocator<double> > >, boost::numeric::ublas::scalar_minus<double, double> > result_type;
}

concept_map math::VectorSpace<Vector,Type>
{
    typedef AdditiveAbelianGroup<Vector>::result_type          result_type;
    typedef AdditiveAbelianGroup<Vector>::assign_result_type   assign_result_type;
}
}

template< typename Vec, typename Scalar>
requires math::VectorSpace <Vec,Scalar>
void cg(Vec& u, Scalar s)
{

}

int main() {
  Vector u(2);
  u(0)=1.; u(1)=2.;;
  Type s=1.;
  cg(u,s);
}


#include <fstream>  // NOLINT
#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>
#include <descriptor-projection/descriptor-projection.h>
#include <descriptor-projection/flags.h>
#include <descriptor-projection/projected-descriptor-quantizer.h>
#include <gflags/gflags.h>
#include <glog/logging.h>
#include <loopclosure-common/types.h>
#include <maplab-common/test/testing-entrypoint.h>
#include <maplab-common/test/testing-predicates.h>
#include <vocabulary-tree/helpers.h>
#include <vocabulary-tree/simple-kmeans.h>
#include <vocabulary-tree/tree-builder.h>

DECLARE_string(data_directory);

TEST(PlacelessLoopClosure, QuantizerSerialization) {
  std::mt19937 generator(42);
  static const uint32_t kNumWords = 100;
  static const int kDescriptorBytes = 64;

  static const int kTargetDimensionality = 10;

  std::string quantizer_filename = "./quantizer.dat";
  descriptor_projection::ProjectedDescriptorQuantizer quantizer(
      kTargetDimensionality);
  {
    quantizer.projection_matrix_.setRandom(
        loop_closure::kFreakDescriptorLengthBits,
        loop_closure::kFreakDescriptorLengthBits);

    descriptor_projection::ProjectedDescriptorType descriptor_zero;
    descriptor_zero.setConstant(kTargetDimensionality, 1, 0);

    Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic> descriptors;
    descriptors.setRandom(kDescriptorBytes, 40 * 100);

    descriptor_projection::DescriptorVector projected_descriptors;
    projected_descriptors.resize(descriptors.cols(), descriptor_zero);
    for (int i = 0; i < descriptors.cols(); ++i) {
      aslam::common::FeatureDescriptorConstRef raw_descriptor(
          &descriptors.coeffRef(0, i), kDescriptorBytes);

      descriptor_projection::ProjectDescriptor(
          raw_descriptor, quantizer.projection_matrix_,
          quantizer.target_dimensionality_, projected_descriptors[i]);
    }

    // Create tree.
    typedef loop_closure::TreeBuilder<
        descriptor_projection::ProjectedDescriptorType,
        loop_closure::distance::L2<
            descriptor_projection::ProjectedDescriptorType> >
        TreeBuilder;
    TreeBuilder builder(descriptor_zero);
    builder.kmeans().SetRestarts(5);
    builder.Build(projected_descriptors, kNumWords, 1);
    VLOG(3) << "Done. Got " << builder.tree().centers().size() << " centers";

    quantizer.vocabulary_ = builder.tree();

    std::ofstream ofs(quantizer_filename.c_str());
    ASSERT_TRUE(ofs.is_open());

    quantizer.Save(&ofs);
  }

  descriptor_projection::ProjectedDescriptorQuantizer quantizer_load(
      kTargetDimensionality);

  EXPECT_FALSE(quantizer_load.vocabulary_.IsBinaryEqual(quantizer.vocabulary_));

  std::ifstream ifs(quantizer_filename.c_str());
  ASSERT_TRUE(ifs.is_open());
  ASSERT_TRUE(ifs.good());
  quantizer_load.Load(&ifs);

  EXPECT_TRUE(
      quantizer_load.projection_matrix_ == quantizer.projection_matrix_);
  EXPECT_EQ(
      quantizer_load.target_dimensionality_, quantizer.target_dimensionality_);
  EXPECT_TRUE(quantizer_load.vocabulary_.IsBinaryEqual(quantizer.vocabulary_));
}

MAPLAB_UNITTEST_ENTRYPOINT


#include <boost/test/auto_unit_test.hpp>

#include <interpreter/clusterer.h>

#include <random>

void add_cluster_near(std::vector<svg::location>& locations, svg::location where, std::size_t elems_in_cluster, double dev)
{
	std::mt19937 gen(4);

	std::normal_distribution<> xs(where.x, dev);
	std::normal_distribution<> ys(where.y, dev);

	for (std::size_t i = 0; i < elems_in_cluster; ++i)
		locations.push_back(svg::location(xs(gen), ys(gen)));
}

BOOST_AUTO_TEST_SUITE(interpreter_tests)
BOOST_AUTO_TEST_SUITE(clusterer_tests)

BOOST_AUTO_TEST_CASE(two_simple_clusters_are_identified)
{
	std::vector<svg::location> locations;
	add_cluster_near(locations, svg::location(500, 250), 10, 5);
	add_cluster_near(locations, svg::location(105, 110), 10, 5);

	interpreter::clusterer c(locations);

	auto clusters = c.clusterize();

	BOOST_REQUIRE_EQUAL(2, clusters.size());
	BOOST_CHECK_CLOSE_FRACTION(105.0, clusters[0].x, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(110.0, clusters[0].y, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(500.0, clusters[1].x, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(250.0, clusters[1].y, 0.1);
}

BOOST_AUTO_TEST_CASE(five_simple_clusters_are_identified)
{
	std::vector<svg::location> locations;
	add_cluster_near(locations, svg::location(5, 20), 40, 5);
	add_cluster_near(locations, svg::location(100, 478), 30, 5);
	add_cluster_near(locations, svg::location(800, 900), 50, 5);
	add_cluster_near(locations, svg::location(500, 200), 18, 5);
	add_cluster_near(locations, svg::location(90, 80), 35, 5);

	interpreter::clusterer c(locations);

	auto clusters = c.clusterize();

	BOOST_REQUIRE_EQUAL(5, clusters.size());
	BOOST_CHECK_CLOSE_FRACTION(5.0, clusters[0].x, 1);
	BOOST_CHECK_CLOSE_FRACTION(20.0, clusters[0].y, 0.4);
	BOOST_CHECK_CLOSE_FRACTION(90.0, clusters[1].x, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(80.0, clusters[1].y, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(100.0, clusters[2].x, 0.3);
	BOOST_CHECK_CLOSE_FRACTION(478.0, clusters[2].y, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(500.0, clusters[3].x, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(200.0, clusters[3].y, 0.5);
	BOOST_CHECK_CLOSE_FRACTION(800.0, clusters[4].x, 0.1);
	BOOST_CHECK_CLOSE_FRACTION(900.0, clusters[4].y, 0.1);
}

BOOST_AUTO_TEST_SUITE_END()
BOOST_AUTO_TEST_SUITE_END()


#pragma once

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

#include "camera_array/ModelInterfaces.h"
#include "argus_utils/random/RandomUtils.hpp"

namespace argus
{

template <typename State, typename Action>
class StochasticGreedyPolicy
: public DecisionPolicy<State, Action>
{
public:

	typedef typename DecisionPolicy<State,Action>::ActionList ActionList;

	StochasticGreedyPolicy( const typename RewardFunction<State,Action>::Ptr& r,
							double scale )
	: rewardFunc( r ), scaling( scale )
	{
		boost::random::random_device rng;
		generator.seed( rng );
	}

	virtual Action ChooseAction( const State& state, const ActionList& actions )
	{
		std::vector<double> weights( actions.size() );
		double maxReward = -std::numeric_limits<double>::infinity();
		double minReward = std::numeric_limits<double>::infinity();
		for( unsigned int i = 0; i < actions.size(); i++ )
		{
			weights[i] = rewardFunc->CalculateReward( state, actions[i] );
			if( weights[i] < minReward ) { minReward = weights[i]; }
			if( weights[i] > maxReward ) { maxReward = weights[i]; }
		}

		double rewardRange = maxReward - minReward;
		if( rewardRange < 1E-6 ) { rewardRange = 1.0; }
		double k = scaling / ( rewardRange );
		// Normalize the exponents by making sure the min raw weight is 0 and the max weight is 1
		for( unsigned int i = 0; i < actions.size(); i++ )
		  {
		    weights[i] = std::exp( k * ( weights[i] - minReward ) );
			std::cout << "Action: " << actions[i] << " weight: " << weights[i] << std::endl;
		  }
		std::vector<unsigned int> inds;
		argus::NaiveWeightedSample( weights, 1, inds, generator );

		return actions[ inds[0] ];
	}

private:

	typename RewardFunction<State,Action>::Ptr rewardFunc;

	double scaling;
	boost::random::mt19937 generator;

};

}