/**
 * @file   unitary.cc
 * @date   12/2018
 * @author Imran Ashraf
 * @author Anneriet Krol
 * @brief  unitary matrix (decomposition) implementation
 */

#include <unitary.h>

#ifndef WITHOUT_UNITARY_DECOMPOSITION
#include <Eigen/MatrixFunctions>
#include <src/misc/lapacke.h>
#endif

typedef unsigned int uint;

#include <chrono>

namespace ql
{

unitary::unitary() : name(""), is_decomposed(false) {}

unitary::unitary(std::string name, std::vector<std::complex<double>> array) :
name(name), array(array), is_decomposed(false) {}

double unitary::size() {
    // JvS: Note that the original unitary::size() used
    // Eigen::Matrix::size() if the array is empty. However, if the array
    // is empty, _matrix is never initialized beyond its default ctor,
    // which "allocates" a 0x0 matrix, and is thus size 0, exactly what
    // array.size() would return.
    // Don't get me started about why this returns a double.
    return (double) array.size();
}

#ifdef WITHOUT_UNITARY_DECOMPOSITION

void unitary::decompose() {
    throw std::runtime_error("unitary decomposition was explicitly disabled in this build!");
}

#else

// JvS: this was originally the class "unitary" itself, but compile times of
// Eigen are so excessive that I moved it into its own compile unit and
// provided a wrapper instead. It doesn't actually NEED to be wrapped like
// this, because the Eigen::Matrix<...> member is actually only used within
// the scope of a single method (calling other methods), but I'm not touching
// this code.
class UnitaryDecomposer
{
private:
    Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> _matrix;

public:
    std::string name;
    std::vector<std::complex<double>> array;
    std::vector<std::complex<double>> SU;
    double delta;
    double alpha;
    double beta;
    double gamma;
    bool is_decomposed;
    std::vector<double> instructionlist;

    typedef Eigen::Matrix<std::complex<double>, Eigen::Dynamic, Eigen::Dynamic> complex_matrix ;

    UnitaryDecomposer() : name(""), is_decomposed(false) {}

    UnitaryDecomposer(std::string name, std::vector<std::complex<double>> array) :
            name(name), array(array), is_decomposed(false)
    {
        DOUT("constructing unitary: " << name
                  << ", containing: " << array.size() << " elements");
    }

    double size()
    {
        if(!array.empty())
            return (double) array.size();
        else
            return (double) _matrix.size();
    }

    complex_matrix getMatrix()
    {
        if (!array.empty())
        {
            int matrix_size = (int)std::pow(array.size(), 0.5);

            Eigen::Map<complex_matrix> matrix(array.data(), matrix_size, matrix_size);
            _matrix = matrix.transpose();
        }
        return _matrix;
    }

    void decompose()
    {
        DOUT("decomposing Unitary: " << name);

        getMatrix();
        int matrix_size = _matrix.rows();

        // compute the number of qubits: length of array is collumns*rows, so log2(sqrt(array.size))
        int numberofbits = uint64_log2(matrix_size);

        Eigen::MatrixXcd identity = Eigen::MatrixXcd::Identity(matrix_size, matrix_size);
        Eigen::MatrixXcd matmatadjoint = (_matrix.adjoint()*_matrix);
        // very little accuracy because of tests using printed-from-matlab code that does not have many digits after the comma
        if( !matmatadjoint.isApprox(identity, 0.001))
        {
            //Throw an error
            EOUT("Unitary " << name <<" is not a unitary matrix!");

            throw ql::exception("Error: Unitary '"+ name+"' is not a unitary matrix. Cannot be decomposed!" + to_string(matmatadjoint), false);
        }
        // initialize the general M^k lookuptable
        genMk();

        decomp_function(_matrix, numberofbits); //needed because the matrix is read in columnmajor

        DOUT("Done decomposing");
        is_decomposed = true;
    }

    std::string to_string(complex_matrix m, std::string vector_prefix = "",
                            std::string elem_sep = ", ")
    {
        std::ostringstream ss;
        ss << m << "\n";
        return ss.str();
    }

    // std::chrono::duration<double> CSD_time;
    // std::chrono::duration<double> CSD_time2;
    // std::chrono::duration<double> CSD_time3;
    // std::chrono::duration<double> zyz_time;
    // std::chrono::duration<double> multiplexing_time;
    // std::chrono::duration<double> demultiplexing_time;

    void decomp_function(const Eigen::Ref<const complex_matrix>& matrix, int numberofbits)
    {
        DOUT("decomp_function: \n" << to_string(matrix));
        if(numberofbits == 1)
        {
            zyz_decomp(matrix);
        }
        else
        {
            int n = matrix.rows()/2;

            complex_matrix V(n,n);
            complex_matrix W(n,n);
            Eigen::VectorXcd D(n);
            // if q2 is zero, the whole thing is a demultiplexing problem instead of full CSD
            if(matrix.bottomLeftCorner(n,n).isZero(10e-14) && matrix.topRightCorner(n,n).isZero(10e-14))
            {
                DOUT("Optimization: q2 is zero, only demultiplexing will be performed.");
                instructionlist.push_back(200.0);
                if(matrix.topLeftCorner(n, n).isApprox(matrix.bottomRightCorner(n,n),10e-4))
                {
                    DOUT("Optimization: Unitaries are equal, skip one step in the recursion for unitaries of size: " << n << " They are both: " << matrix.topLeftCorner(n, n));
                    instructionlist.push_back(300.0);
                    decomp_function(matrix.topLeftCorner(n, n), numberofbits-1);
                }
                else
                {
                    demultiplexing(matrix.topLeftCorner(n, n), matrix.bottomRightCorner(n,n), V, D, W, numberofbits-1);

                    decomp_function(W, numberofbits-1);
                    multicontrolledZ(D, D.rows());
                    decomp_function(V, numberofbits-1);
                }
            }
            // Check to see if it the kronecker product of a bigger matrix and the identity matrix.
            // By checking if the first row is equal to the second row one over, and if thelast two rows are equal
            // Which means the last qubit is not affected by this gate
            else if (matrix(Eigen::seqN(0, n, 2), Eigen::seqN(1, n, 2)).isZero()  && matrix(Eigen::seqN(1, n, 2), Eigen::seqN(0, n, 2)).isZero()  && matrix.block(0,0,1,2*n-1) == matrix.block(1,1,1,2*n-1) &&  matrix.block(2*n-2,0,1,2*n-1) ==  matrix.block(2*n-1,1,1,2*n-1))
            {
                DOUT("Optimization: last qubit is not affected, skip one step in the recursion.");
                // Code for last qubit not affected
                instructionlist.push_back(100.0);
                decomp_function(matrix(Eigen::seqN(0, n, 2), Eigen::seqN(0, n, 2)), numberofbits-1);

            }
            else
            {
            complex_matrix ss(n,n);
            complex_matrix L0(n,n);
            complex_matrix L1(n,n);
            complex_matrix R0(n,n);
            complex_matrix R1(n,n);
            // auto start = std::chrono::steady_clock::now();
            CSD(matrix, L0, L1, R0, R1, ss);
            // CSD_time += (std::chrono::steady_clock::now() - start);
            demultiplexing(R0, R1, V, D, W, numberofbits-1);
            decomp_function(W, numberofbits-1);
            multicontrolledZ(D, D.rows());
            decomp_function(V, numberofbits-1);

            multicontrolledY(ss.diagonal(), n);

            demultiplexing(L0, L1, V, D, W, numberofbits-1);
            decomp_function(W, numberofbits-1);
            multicontrolledZ(D, D.rows());
            decomp_function(V, numberofbits-1);
            }
        }
    }

    void CSD(const Eigen::Ref<const complex_matrix>& U, Eigen::Ref<complex_matrix> u1, Eigen::Ref<complex_matrix> u2, Eigen::Ref<complex_matrix> v1, Eigen::Ref<complex_matrix> v2, Eigen::Ref<complex_matrix> s)
    {
        // auto start = std::chrono::steady_clock::now();
        //Cosine sine decomposition
        // U = [q1, U01] = [u1    ][c  s][v1  ]
        //     [q2, U11] = [    u2][-s c][   v2]
        int n = U.rows();
        // complex_matrix c(n,n); // c matrix is not needed for the higher level
        // complex_matrix q1 = U.topLeftCorner(n/2,m/2);

        Eigen::BDCSVD<complex_matrix> svd(n/2,n/2);
        svd.compute(U.topLeftCorner(n/2,n/2), Eigen::ComputeThinU | Eigen::ComputeThinV); // possible because it's square anyway

        // thinCSD: q1 = u1*c*v1.adjoint()
        //          q2 = u2*s*v1.adjoint()
        int p = n/2;
        // complex_matrix z = Eigen::MatrixXd::Identity(p, p).colwise().reverse();
        complex_matrix c(svd.singularValues().reverse().asDiagonal());
        u1.noalias() = svd.matrixU().rowwise().reverse();
        v1.noalias() = svd.matrixV().rowwise().reverse(); // Same v as in matlab: u*s*v.adjoint() = q1

        complex_matrix q2 = U.bottomLeftCorner(p,p)*v1;

        int k = 0;
        for(int j = 1; j < p; j++)
        {
            if(c(j,j).real() <= 0.70710678119)
            {
                k = j;
            }
        }
        //complex_matrix b = q2.block( 0,0, p, k+1);

        Eigen::HouseholderQR<complex_matrix> qr(p,k+1);
        qr.compute(q2.block( 0,0, p, k+1));
        u2 = qr.householderQ();
        s.noalias() = u2.adjoint()*q2;
        if(k < p-1)
        {
            DOUT("k is smaller than size of q1 = "<< p << ", adjustments will be made, k = " << k);
            k = k+1;
            Eigen::BDCSVD<complex_matrix> svd2(p-k, p-k);
            svd2.compute(s.block(k, k, p-k, p-k), Eigen::ComputeThinU | Eigen::ComputeThinV);
            s.block(k, k, p-k, p-k) = svd2.singularValues().asDiagonal();
            c.block(0,k, p,p-k) = c.block(0,k, p,p-k)*svd2.matrixV();
            u2.block(0,k, p,p-k) = u2.block(0,k, p,p-k)*svd2.matrixU();
            v1.block(0,k, p,p-k) = v1.block(0,k, p,p-k)*svd2.matrixV();

            Eigen::HouseholderQR<complex_matrix> qr2(p-k, p-k);

            qr2.compute(c.block(k,k, p-k,p-k));
            c.block(k,k,p-k,p-k) = qr2.matrixQR().triangularView<Eigen::Upper>();
            u1.block(0,k, p,p-k) = u1.block(0,k, p,p-k)*qr2.householderQ();
        }
        // CSD_time2 += (std::chrono::steady_clock::now() - start);

        // auto start2 = std::chrono::steady_clock::now();

        std::vector<int> c_ind;
        std::vector<int> s_ind;
        for(int j = 0; j < p; j++)
        {
            if(c(j,j).real() < 0)
            {
                c_ind.push_back(j);
            }
            if(s(j,j).real() < 0)
            {
                s_ind.push_back(j);
            }
        }

        c(c_ind,c_ind) = -c(c_ind,c_ind);
        u1(Eigen::all, c_ind) = -u1(Eigen::all, c_ind);

        //s.diagonal()(s_ind) = -s.diagonal()(s_ind);
        s(s_ind,s_ind) = -s(s_ind,s_ind);
        u2(Eigen::all, s_ind) = -u2(Eigen::all, s_ind);

        if(!U.topLeftCorner(p,p).isApprox(u1*c*v1.adjoint(), 10e-8) || !U.bottomLeftCorner(p,p).isApprox(u2*s*v1.adjoint(), 10e-8))
        {
            if(U.topLeftCorner(p,p).isApprox(u1*c*v1.adjoint(), 10e-8))
            {
                DOUT("q1 is correct");
            }
            else
            {
                DOUT("q1 is not correct! (is not usually an issue");
                DOUT("q1: \n" << U.topLeftCorner(p,p));
                DOUT("reconstructed q1: \n" << u1*c*v1.adjoint());

            }
            if(U.bottomLeftCorner(p,p).isApprox(u2*s*v1.adjoint(), 10e-8))
            {
                DOUT("q2 is correct");
            }
            else
            {
                DOUT("q2 is not correct! (is not usually an issue)");
                DOUT("q2: " << U.bottomLeftCorner(p,p));
                DOUT("reconstructed q2: " << u2*s*v1.adjoint());
            }
        }
        v1.adjointInPlace(); // Use this instead of = v1.adjoint (to avoid aliasing issues)
        s = -s;

        complex_matrix tmp_s = u1.adjoint()*U.topRightCorner(p,p);
        complex_matrix tmp_c = u2.adjoint()*U.bottomRightCorner(p,p);

        // std::vector<int> c_ind_row;
        // std::vector<int> s_ind_row;
        for(int i = 0; i < p; i++)
        {
            if(std::abs(s(i,i)) > std::abs(c(i,i)))
            {
                // std::vector<int> s_ind_row;
                v2.row(i).noalias() = tmp_s.row(i)/s(i,i);
            }
            else
            {
                // c_ind_row.push_back(i);
                v2.row(i).noalias() = tmp_c.row(i)/c(i,i);
            }
        }

        // v2(s_ind_row, Eigen::all) = tmp_s(s_ind_row, Eigen::all).rowwise() / s(s_ind_row, s_ind_row).array();
        // v2(c_ind_row, Eigen::all) = tmp_c(c_ind_row, Eigen::all).rowwise() / c(c_ind_row, c_ind_row).array();
        // U = [q1, U01] = [u1    ][c  s][v1  ]
        //     [q2, U11] = [    u2][-s c][   v2]

        complex_matrix tmp(n,n);
        tmp.topLeftCorner(p,p) = u1*c*v1;
        tmp.bottomLeftCorner(p,p) = -u2*s*v1;
        tmp.topRightCorner(p,p) = u1*s*v2;
        tmp.bottomRightCorner(p,p) = u2*c*v2;
        // Just to see if it kinda matches
        if(!tmp.isApprox(U, 10e-2))
        {
            throw ql::exception("CSD of unitary '"+ name+"' is wrong! Failed at matrix: \n"+to_string(tmp) + "\nwhich should be: \n" + to_string(U), false);
        }
            // CSD_time3 += (std::chrono::steady_clock::now() - start2);

    }

    void zyz_decomp(const Eigen::Ref<const complex_matrix>& matrix)
    {
        // auto start = std::chrono::steady_clock::now();

        ql::complex_t det = matrix.determinant();// matrix(0,0)*matrix(1,1)-matrix(1,0)*matrix(0,1);

        double delta = atan2(det.imag(), det.real())/matrix.rows();
        std::complex<double> A = exp(std::complex<double>(0,-1)*delta)*matrix(0,0);
        std::complex<double> B = exp(std::complex<double>(0,-1)*delta)*matrix(0,1); //to comply with the other y-gate definition

        double sw = sqrt(pow((double) B.imag(),2) + pow((double) B.real(),2) + pow((double) A.imag(),2));
        double wx = 0;
        double wy = 0;
        double wz = 0;

        if(sw > 0)
        {
        wx = B.imag()/sw;
        wy = B.real()/sw;
        wz = A.imag()/sw;
        }

        double t1 = atan2(A.imag(),A.real());
        double t2 = atan2(B.imag(), B.real());
        alpha = t1+t2;
        gamma = t1-t2;
        beta = 2*atan2(sw*sqrt(pow((double) wx,2)+pow((double) wy,2)),sqrt(pow((double) A.real(),2)+pow((wz*sw),2)));
        instructionlist.push_back(-gamma);
        instructionlist.push_back(-beta);
        instructionlist.push_back(-alpha);
        // zyz_time += (std::chrono::steady_clock::now() - start);
    }

    void demultiplexing(const Eigen::Ref<const complex_matrix> &U1, const Eigen::Ref<const complex_matrix> &U2,  Eigen::Ref<complex_matrix> V,  Eigen::Ref<Eigen::VectorXcd> D,  Eigen::Ref<complex_matrix> W, int numberofcontrolbits)
    {
        // [U1 0 ]  = [V 0][D 0 ][W 0]
        // [0  U2]    [0 V][0 D*][0 W]
        // auto start = std::chrono::steady_clock::now();
        complex_matrix check = U1*U2.adjoint();
        // complex_matrix D;
        // complex_matrix V;
        // complex_matrix W;
        if(check == check.adjoint())
        {
            IOUT("Demultiplexing matrix is self-adjoint()");
            Eigen::SelfAdjointEigenSolver<Eigen::MatrixXcd> eigslv(check);
            D.noalias() = ((complex_matrix) eigslv.eigenvalues()).cwiseSqrt();
            V.noalias() = eigslv.eigenvectors();
            W.noalias() = D.asDiagonal()*V.adjoint()*U2;
        }
        else
        {
            if (numberofcontrolbits < 5) //schur is faster for small matrices
            {
                Eigen::ComplexSchur<complex_matrix> decomposition(check);
                D.noalias() = decomposition.matrixT().diagonal().cwiseSqrt();
                V.noalias() = decomposition.matrixU();
                W.noalias() = D.asDiagonal() * V.adjoint() * U2;
            }
            else
            {
                Eigen::ComplexEigenSolver<complex_matrix> decomposition(check);
                D.noalias() = decomposition.eigenvalues().cwiseSqrt();
                V.noalias() = decomposition.eigenvectors();
                W.noalias() = D.asDiagonal() * V.adjoint() * U2;
            }
        }

        // demultiplexing_time += (std::chrono::steady_clock::now() - start);
        if(!(V*V.adjoint()).isApprox(Eigen::MatrixXd::Identity(V.rows(), V.rows()), 10e-3))
        {
            DOUT("Eigenvalue decomposition incorrect: V is not unitary, adjustments will be made");
            Eigen::BDCSVD<complex_matrix> svd3(V.block(0,0,V.rows(),2), Eigen::ComputeFullU);
            V.block(0,0,V.rows(),2) = svd3.matrixU();
            svd3.compute(V(Eigen::all,Eigen::seq(Eigen::last-1,Eigen::last)), Eigen::ComputeFullU);
            V(Eigen::all,Eigen::seq(Eigen::last-1,Eigen::last)) = svd3.matrixU();

        }

        complex_matrix Dtemp = D.asDiagonal();
        if(!U1.isApprox(V*Dtemp*W, 10e-2) || !U2.isApprox(V*Dtemp.adjoint()*W, 10e-2))
        {
            EOUT("Demultiplexing not correct!");
            throw ql::exception("Demultiplexing of unitary '"+ name+"' not correct! Failed at matrix U1: \n"+to_string(U1)+ "and matrix U2: \n" +to_string(U2) + "\nwhile they are: \n" + to_string(V*D.asDiagonal()*W) + "\nand \n" + to_string(V*D.conjugate().asDiagonal()*W), false);
        }

    }

    std::vector<Eigen::MatrixXd> genMk_lookuptable;

    // returns M^k = (-1)^(b_(i-1)*g_(i-1)), where * is bitwise inner product, g = binary gray code, b = binary code.
    void genMk()
    {
        int numberqubits = uint64_log2(_matrix.rows());
        for(int n = 1; n <= numberqubits; n++)
        {
            int size=1<<n;
            Eigen::MatrixXd Mk(size,size);
            for(int i = 0; i < size; i++)
            {
                for(int j = 0; j < size ;j++)
                {
                    Mk(i,j) =std::pow(-1, bitParity(i&(j^(j>>1))));
                }
            }
        genMk_lookuptable.push_back(Mk);
        }

        // return genMk_lookuptable[numberqubits-1];
    }

    // source: https://stackoverflow.com/questions/994593/how-to-do-an-integer-log2-in-c user Todd Lehman
    int uint64_log2(uint64_t n)
    {
    #define S(k) if (n >= (UINT64_C(1) << k)) { i += k; n >>= k; }

    int i = -(n == 0); S(32); S(16); S(8); S(4); S(2); S(1); return i;

    #undef S
    }

    int bitParity(int i)
    {
        if (i < 2 << 16)
        {
            i = (i >> 16) ^ i;
            i = (i >> 8) ^ i;
            i = (i >> 4) ^ i;
            i = (i >> 2) ^ i;
            i = (i >> 1) ^ i;
            return i % 2;
        }
        else
        {
            throw ql::exception("Bit parity number too big!", false);
        }
    }

    void multicontrolledY(const Eigen::Ref<const Eigen::VectorXcd> &ss, int halfthesizeofthematrix)
    {
        // auto start = std::chrono::steady_clock::now();
        Eigen::VectorXd temp =  2*Eigen::asin(ss.array()).real();
        Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> dec(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]);
        Eigen::VectorXd tr = dec.solve(temp);
        // Check is very approximate to account for low-precision input matrices
        if(!temp.isApprox(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]*tr, 10e-2))
        {
                EOUT("Multicontrolled Y not correct!");
                throw ql::exception("Demultiplexing of unitary '"+ name+"' not correct! Failed at demultiplexing of matrix ss: \n"  + to_string(ss), false);
        }

        instructionlist.insert(instructionlist.end(), &tr[0], &tr[halfthesizeofthematrix]);
        // multiplexing_time += std::chrono::steady_clock::now() - start;
    }

    void multicontrolledZ(const Eigen::Ref<const Eigen::VectorXcd> &D, int halfthesizeofthematrix)
    {
        // auto start = std::chrono::steady_clock::now();

        Eigen::VectorXd temp =  (std::complex<double>(0,-2)*Eigen::log(D.array())).real();
        Eigen::CompleteOrthogonalDecomposition<Eigen::MatrixXd> dec(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]);
        Eigen::VectorXd tr = dec.solve(temp);
        // Check is very approximate to account for low-precision input matrices
        if(!temp.isApprox(genMk_lookuptable[uint64_log2(halfthesizeofthematrix)-1]*tr, 10e-2))
        {
                EOUT("Multicontrolled Z not correct!");
                throw ql::exception("Demultiplexing of unitary '"+ name+"' not correct! Failed at demultiplexing of matrix D: \n"+ to_string(D), false);
        }

        instructionlist.insert(instructionlist.end(), &tr[0], &tr[halfthesizeofthematrix]);
        // multiplexing_time += std::chrono::steady_clock::now() - start;

    }
    ~UnitaryDecomposer()
    {
        // destroy unitary
        DOUT("destructing unitary: " << name);
    }
};

void unitary::decompose() {
    UnitaryDecomposer decomposer(name, array);
    decomposer.decompose();
    SU = decomposer.SU;
    alpha = decomposer.alpha;
    beta = decomposer.beta;
    gamma = decomposer.gamma;
    is_decomposed = decomposer.is_decomposed;
    instructionlist = decomposer.instructionlist;
}

#endif

}


#include <iostream>
#include <fstream>
#include <algorithm>
#include <complex>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>

#include "../include/TriMesh.h"
#include "../include/utils.h"
#include "../include/lagrange.h"
#include "../include/geometry.h"
#include "../include/ConstructCurveMesh.h"
#include "../include/GetQuadraturePointsWeight2D.h"
#include "../include/solver.h"
#include "../include/euler.h"
#include "../include/Param.h"
#include "../include/Collective.h"
#include "../include/InvertMatrix.h"

using namespace std;

int main(int argc, char *argv[])
{
    Param param;
    // Set up the param struct
    param = ReadParamIn(string(argv[1]));
    TriMesh mesh(param.mesh_file);
    // Testing Calculate Residaul
    int p = param.order;
    int q = param.order_geo;
    int Np = int((p + 1) * (p + 2) / 2);
    TriMesh curved_mesh = mesh;
    string boundary_name="bottom";
    ConstructCurveMesh(mesh, curved_mesh, geometry::BumpFunction, boundary_name, q);

    ResData resdata_postproc;
    if (p == 0)
    {
        solver::CalcResData(curved_mesh, 1, resdata_postproc);
    }else
    {
        solver::CalcResData(curved_mesh, p, resdata_postproc);
    }

    int Np_solution;
    if (p == 0)
        Np_solution = 3;
    else
        Np_solution = Np;
    ublas::vector<ublas::matrix<double> > Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 2, 0.0));
    ublas::vector<ublas::matrix<double> > State_on_Nodes(curved_mesh.E.size(), ublas::matrix<double>(Np_solution, 4, 0.0));

    ifstream file_nodes, file_state;
    file_nodes.open("nodes.dat");
    file_state.open("states.dat");
    for (int ielem = 0; ielem < curved_mesh.E.size(); ielem++)
    {
        for (int ip = 0; ip < Np_solution; ip++)
        {
            double x, y, s0, s1, s2, s3;
            char temp;
            file_nodes  >> x  >> y;
            file_state  >> s0 >> s1 >> s2 >> s3;
            Nodes(ielem)(ip, 0) = x; Nodes(ielem)(ip, 1) = y;
            State_on_Nodes(ielem)(ip, 0) = s0; State_on_Nodes(ielem)(ip, 1) = s1;
            State_on_Nodes(ielem)(ip, 2) = s2; State_on_Nodes(ielem)(ip, 3) = s3;
        }
    }
    file_nodes.close();
    file_state.close();
    param.h = 0.0625;
    double err_entropy, coeff_lift, coeff_drag;
    std::vector<std::vector<double> > p_coeff_dist;
    solver::CalcScalarOutputs(curved_mesh, resdata_postproc, State_on_Nodes, Nodes, param,
                                    err_entropy, coeff_lift, coeff_drag, p_coeff_dist);
    cout.setf(ios::scientific, ios::floatfield);
    std::cout << setprecision(8) << err_entropy << ' ' << coeff_lift << ' ' << coeff_drag << endl;
    ofstream file_pdist;
    file_pdist.open("pressure_distribution.dat");
    for (int i = 0; i < p_coeff_dist.size(); i++)
    {
        cout.setf(ios::scientific, ios::floatfield);
        file_pdist  << setprecision(10) << p_coeff_dist[i][0] << ' ' << p_coeff_dist[i][1] << std::endl;
    }
    file_pdist.close();
    return 0;
}


#include <mex.h>
#include <Eigen/Core>
#include "splineGeneration.h"
#include "drakeMexUtil.h"
#include <iostream>
#include <limits>

using namespace std;
using namespace Eigen;

const int GRID_STEPS = 10;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  string usage = "[coefs, ts, objective_value] = nWaypointCubicSplineFreeKnotTimesmex.cpp(t0, tf, xs, xd0, xdf)";
  if (nrhs != 5)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfInputs", usage.c_str());
  if (nlhs < 2 || nlhs > 3)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:WrongNumberOfOutputs", usage.c_str());

  double t0 = mxGetPrSafe(prhs[0])[0];
  double tf = mxGetPrSafe(prhs[1])[0];
  MatrixXd xs = matlabToEigen<Dynamic, Dynamic>(prhs[2]);
  auto xd0 = matlabToEigen<Dynamic, 1>(prhs[3]);
  auto xdf = matlabToEigen<Dynamic, 1>(prhs[4]);

  mwSize ndof = static_cast<mwSize>(xs.rows());
  mwSize num_segments = static_cast<mwSize>(xs.cols()) - 1;
  mwSize num_knots = num_segments - 1;
  if (num_knots >= 3)
    mexWarnMsgTxt("More knots than two is likely to be super slow in a grid search!\n");
  if (num_knots <= 0)
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:NotEnoughKnotsToJustifyThisFunction", usage.c_str());
  mwSize num_coeffs_per_segment = 4;
  mwSize dims[] = { ndof, num_segments, num_coeffs_per_segment };
  plhs[0] = mxCreateNumericArray(3, dims, mxDOUBLE_CLASS, mxREAL);

  std::vector<double> segment_times;
  segment_times.resize(static_cast<size_t>(num_segments) + 1);
  segment_times[0] = t0;
  segment_times[static_cast<size_t>(num_segments)] = tf;
  std::vector<double> best_segment_times = segment_times;
  double t_step = (tf - t0) / GRID_STEPS;
  double min_objective_value = numeric_limits<double>::infinity();

  // assemble the knot point locations for input to nWaypointCubicSpline
  MatrixXd xi = xs.block(0, 1, ndof, num_knots);

  if (GRID_STEPS <= num_knots) {
    // If we have have too few grid steps, then by pigeonhole it's
    // impossible to give each a unique time in our grid search.
    mexErrMsgIdAndTxt("Drake:nWaypointCubicSplineFreeKnotTimesmex.cpp:TooManyKnotsForNumGridSteps", usage.c_str());
  }
  std::vector<int> t_indices;
  t_indices.reserve(num_knots);
  for (int i = 0; i < num_knots; i++) {
    t_indices.push_back(i + 1); // assume knot point won't be the same time as the initial state, or previous knot point
  }

  while (t_indices[0] < GRID_STEPS - num_knots + 1) {
    for (int i = 0; i < num_knots; i++)
      segment_times[i + 1] = t0 + t_indices[i] * t_step;

    bool valid_solution = true;
    double objective_value = 0.0;
    for (int dof = 0; dof < ndof && valid_solution; dof++) {
      try {
        PiecewisePolynomial<double> spline = nWaypointCubicSpline(segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments), xdf[dof], xi.row(dof).transpose());
        PiecewisePolynomial<double> acceleration_squared = spline.derivative(2);
        acceleration_squared *= acceleration_squared;
        PiecewisePolynomial<double> acceleration_squared_integral = acceleration_squared.integral();
        objective_value += acceleration_squared_integral.scalarValue(spline.getEndTime()) - acceleration_squared_integral.scalarValue(spline.getStartTime());
      } catch (ConstraintMatrixSingularError&) {
        valid_solution = false;
      }
    }

    if (valid_solution && objective_value < min_objective_value) {
      best_segment_times = segment_times;
      min_objective_value = objective_value;
    }

    // Advance grid search counter or terminate, counting from
    // the latest t_index, and on overflow carrying to the
    // next lowest t_index and resetting to the new value of that
    // next lowest t_index. (since times must always be in order!)
    t_indices[num_knots - 1]++;
    // carry, except for the lowest place, which we
    // use to detect doneness.
    for (size_t i = num_knots - 1; i > 0; i--) {
      if ((i == num_knots - 1 && t_indices[i] >= GRID_STEPS) || (i < num_knots - 1 && t_indices[i] >= t_indices[i + 1])) {
        t_indices[i - 1]++;
        t_indices[i] = t_indices[i - 1] + 1;
      }
    }
  }

  for (mwSize dof = 0; dof < ndof; dof++) {
    PiecewisePolynomial<double> spline = nWaypointCubicSpline(best_segment_times, xs(dof, 0), xd0[dof], xs(dof, num_segments), xdf[dof], xi.row(dof).transpose());
    for (mwSize segment_index = 0; segment_index < spline.getNumberOfSegments(); segment_index++) {
      for (mwSize coefficient_index = 0; coefficient_index < num_coeffs_per_segment; coefficient_index++) {
        mwSize sub[] = { dof, segment_index, num_coeffs_per_segment - coefficient_index - 1 }; // Matlab's reverse coefficient indexing...
        *(mxGetPr(plhs[0]) + sub2ind(3, dims, sub)) = spline.getPolynomial(static_cast<int>(segment_index)).getCoefficients()[coefficient_index];
      }
    }
  }
  plhs[1] = stdVectorToMatlab(best_segment_times);

  if (nlhs > 2)
    plhs[2] = mxCreateDoubleScalar(min_objective_value);
}


#include <cstdlib>
#include <iostream>
#include <complex>
#define BOOST_UBLAS_NO_ELEMENT_PROXIES
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/bindings/ublas/vector.hpp>
#include <boost/numeric/bindings/ublas/matrix.hpp>
#include <boost/numeric/bindings/ublas/triangular.hpp>
#include <boost/numeric/bindings/blas/level2.hpp>
#include "print.hpp"
#include "random.hpp"

namespace ublas=boost::numeric::ublas;
namespace blas=boost::numeric::bindings::blas;

int main(int argc, char *argv[]) {
  {
    typedef std::complex<double> complex;
    typedef ublas::vector<complex> vector;
    typedef ublas::matrix<complex, ublas::column_major> matrix;
    typedef ublas::triangular_matrix<complex, ublas::lower, ublas::column_major> matrix_l;
    typedef ublas::triangular_matrix<complex, ublas::upper, ublas::column_major> matrix_u;
    typedef typename vector::size_type size_type;
    rand_normal<complex>::reset();
    size_type n=8;
    matrix_l A_l(n, n);
    matrix_u A_u(n, n);
    for (size_type j=0; j<n; ++j) {
      A_u(j, j)=rand_normal<complex>::get().real();
      A_l(j, j)=A_u(j, j);
      for (size_type i=0; i<j; ++i) {
        A_u(i, j)=rand_normal<complex>::get();
        A_l(j, i)=std::conj(A_u(i, j));
      }
    }
    vector x(n);
    for (size_type i=0; i<n; ++i)
      x(i)=rand_normal<complex>::get();
    vector y(n);
    for (size_type i=0; i<n; ++i)
      y(i)=rand_normal<complex>::get();
    complex alpha(rand_normal<complex>::get());
    matrix P;
    {
      P=ublas::outer_prod(alpha*x, ublas::conj(y));
      P+=ublas::outer_prod(y, ublas::conj(alpha*x));
      for (size_type j=0; j<n; ++j)
        for (size_type i=0; i<j; ++i)
          P(i, j)=0;
      matrix A1(P+A_l);
      matrix_l A2(A_l);
      blas::hpr2(alpha, x, y, A2);
      std::cout << print_mat(A1) << '\n'
                << print_mat(A2) << '\n';
    }
    {
      P=ublas::outer_prod(alpha*x, ublas::conj(y));
      P+=ublas::outer_prod(y, ublas::conj(alpha*x));
      for (size_type j=0; j<n; ++j)
        for (size_type i=j+1; i<n; ++i)
          P(i, j)=0;
      matrix A1(P+A_u);
      matrix_u A2(A_u);
      blas::hpr2(alpha, x, y, A2);
      std::cout << print_mat(A1) << '\n'
                << print_mat(A2) << '\n';
    }
  }
  return EXIT_SUCCESS;
}


/***********************************************************************
This file is part of the librjmcmc project source files.

Copyright : Institut Geographique National (2008-2012)
Contributors : Mathieu Brédif, Olivier Tournaire, Didier Boldo
email : librjmcmc@ign.fr

This software is a generic C++ library for stochastic optimization.

This software is governed by the CeCILL license under French law and
abiding by the rules of distribution of free software. You can use,
modify and/or redistribute the software under the terms of the CeCILL
license as circulated by CEA, CNRS and INRIA at the following URL
"http://www.cecill.info".

As a counterpart to the access to the source code and rights to copy,
modify and redistribute granted by the license, users are provided only
with a limited warranty and the software's author, the holder of the
economic rights, and the successive licensors have only limited liability.

In this respect, the user's attention is drawn to the risks associated
with loading, using, modifying and/or developing or reproducing the
software by the user in light of its specific status of free software,
that may mean that it is complicated to manipulate, and that also
therefore means that it is reserved for developers and experienced
professionals having in-depth computer knowledge. Users are therefore
encouraged to load and test the software's suitability as regards their
requirements in conditions enabling the security of their systems and/or
data to be ensured and, more generally, to use and operate it in the
same conditions as regards security.

The fact that you are presently reading this means that you have had
knowledge of the CeCILL license and that you accept its terms.

***********************************************************************/

#ifndef BUILDING_FOOTPRINT_RECTANGLE_HPP
#define BUILDING_FOOTPRINT_RECTANGLE_HPP

#include "rjmcmc/util/random.hpp"
//<-
/************** application-specific types ****************/
//->

//[building_footprint_rectangle_definition_geometry
#include "geometry.hpp"
typedef Rectangle_2 object;
//]

//[building_footprint_rectangle_definition_kernels
#include "rjmcmc/rjmcmc/kernel/raster_variate.hpp"
#include "rjmcmc/mpp/kernel/uniform_birth.hpp"
typedef marked_point_process::uniform_birth<object,rjmcmc::raster_variate<5> > uniform_birth;
#include "rjmcmc/mpp/kernel/uniform_birth_death_kernel.hpp"
typedef marked_point_process::uniform_birth_death_kernel<uniform_birth>::type  birth_death_kernel;

#include "rjmcmc/rjmcmc/kernel/transform.hpp"
#include "rjmcmc/geometry/transform/rectangle_corner_translation_transform.hpp"
#include "rjmcmc/geometry/transform/rectangle_edge_translation_transform.hpp"
#include "rjmcmc/geometry/transform/rectangle_split_merge_transform.hpp"
typedef geometry::rectangle_edge_translation_transform<0>    edge_transform0;
typedef geometry::rectangle_edge_translation_transform<1>    edge_transform1;
typedef geometry::rectangle_edge_translation_transform<2>    edge_transform2;
typedef geometry::rectangle_edge_translation_transform<3>    edge_transform3;
typedef geometry::rectangle_corner_translation_transform<0>  corner_transform0;
typedef geometry::rectangle_corner_translation_transform<1>  corner_transform1;
typedef geometry::rectangle_corner_translation_transform<2>  corner_transform2;
typedef geometry::rectangle_corner_translation_transform<3>  corner_transform3;
typedef geometry::rectangle_split_merge_transform_v2 split_merge_transform;

#include "rjmcmc/mpp/kernel/uniform_kernel.hpp"
typedef marked_point_process::uniform_kernel<object,1,1,edge_transform0>::type  edge_kernel0;
typedef marked_point_process::uniform_kernel<object,1,1,edge_transform1>::type  edge_kernel1;
typedef marked_point_process::uniform_kernel<object,1,1,edge_transform2>::type  edge_kernel2;
typedef marked_point_process::uniform_kernel<object,1,1,edge_transform3>::type  edge_kernel3;
typedef marked_point_process::uniform_kernel<object,1,1,corner_transform0>::type  corner_kernel0;
typedef marked_point_process::uniform_kernel<object,1,1,corner_transform1>::type  corner_kernel1;
typedef marked_point_process::uniform_kernel<object,1,1,corner_transform2>::type  corner_kernel2;
typedef marked_point_process::uniform_kernel<object,1,1,corner_transform3>::type  corner_kernel3;
typedef marked_point_process::uniform_kernel<object,1,2,split_merge_transform>::type  split_merge_kernel;
//]

//[building_footprint_rectangle_definition_energies
#include "rjmcmc/rjmcmc/energy/energy_operators.hpp"
#include "rjmcmc/mpp/energy/image_gradient_unary_energy.hpp"
#include "rjmcmc/image/gradient_functor.hpp"
#include "rjmcmc/image/oriented.hpp"
typedef oriented<gradient_image_t>                          oriented_gradient_image;
typedef image_gradient_unary_energy<oriented_gradient_image> unary_energy;

#include "rjmcmc/mpp/energy/intersection_area_binary_energy.hpp"
typedef intersection_area_binary_energy<>                   binary_energy;
//]

//<-
/************** rjmcmc library types ****************/
//->

//[building_footprint_rectangle_definition_simulated_annealing
/*< Choice of the schedule (/include/ and /typedef/) >*/
#include "rjmcmc/simulated_annealing/schedule/geometric_schedule.hpp"
typedef simulated_annealing::geometric_schedule<double> schedule;
/*< Choice of the end_test (/include/ and /typedef/) >*/
#include "rjmcmc/simulated_annealing/end_test/max_iteration_end_test.hpp"
typedef simulated_annealing::max_iteration_end_test     end_test;
//]

//[building_footprint_rectangle_definition_configuration

#include "rjmcmc/mpp/energy/image_center_unary_energy.hpp"
typedef oriented<boost::gil::gray16_image_t> mask_type;
#include "rjmcmc/mpp/configuration/graph_configuration.hpp"
typedef marked_point_process::graph_configuration<
        object,
        minus_energy<constant_energy<>,multiplies_energy<constant_energy<>,unary_energy> >,
        multiplies_energy<constant_energy<>,binary_energy>
        > configuration;
//]

//[building_footprint_rectangle_definition_distribution
#include "rjmcmc/rjmcmc/distribution/poisson_distribution.hpp"
typedef rjmcmc::poisson_distribution                           distribution;
//]

//[building_footprint_rectangle_definition_sampler
#include "rjmcmc/rjmcmc/sampler/sampler.hpp"

#include "rjmcmc/mpp/direct_sampler.hpp"
typedef marked_point_process::direct_sampler<distribution,uniform_birth> d_sampler;

#include "rjmcmc/rjmcmc/acceptance/metropolis_acceptance.hpp"
typedef rjmcmc::metropolis_acceptance acceptance;

//typedef rjmcmc::sampler<d_sampler,acceptance,birth_death_kernel> sampler;
typedef rjmcmc::sampler<d_sampler,acceptance
        ,birth_death_kernel
        ,split_merge_kernel
        ,edge_kernel0
        ,edge_kernel1
        ,edge_kernel2
        ,edge_kernel3
        ,corner_kernel0
        ,corner_kernel1
        ,corner_kernel2
        ,corner_kernel3
        > sampler;
//]

//[building_footprint_rectangle_bbox_accessors
Iso_rectangle_2 get_bbox(const param *p) {
    int x0 = p->get<int>("xmin");
    int x1 = p->get<int>("xmax");
    int y0 = p->get<int>("ymin");
    int y1 = p->get<int>("ymax");
    if(x0>x1) std::swap(x0,x1);
    if(y0>y1) std::swap(y0,y1);
    return Iso_rectangle_2(x0,y0,x1,y1);
}

void set_bbox(param *p, const Iso_rectangle_2& r) {
    p->set("xmin",(int) r.min().x());
    p->set("ymin",(int) r.min().y());
    p->set("xmax",(int) r.max().x());
    p->set("ymax",(int) r.max().y());
}
//]

//[building_footprint_rectangle_create_configuration
#include "rjmcmc/image/conversion_functor.hpp"
#include <boost/gil/extension/io_new/tiff_write.hpp>

void create_configuration(const param *p, const oriented_gradient_image& grad, configuration *&c) {
    std::string mask_file = p->get<boost::filesystem::path>("mask" ).string();
    if(mask_file!="")
    {
        Iso_rectangle_2 bbox = get_bbox(p);
        clip_bbox(bbox,mask_file);
        mask_type mask(mask_file , bbox, conversion_functor() );
        boost::gil::write_view( mask_file+"_x0.tif" , boost::gil::nth_channel_view(grad.view(),0), boost::gil::tiff_tag() );
        boost::gil::write_view( mask_file+"_y0.tif" , boost::gil::nth_channel_view(grad.view(),1), boost::gil::tiff_tag() );
        for(int j=0; j<grad.view().height();++j)
        {
            for(int i=0; i<grad.view().width();++i)
            {
                std::cout << mask.view()(i,j)<< " ";
                if (i < mask.view().width() && j < mask.view().height() && mask.view()(i,j)<=0)
                {
                    boost::gil::at_c<0>(grad.view()(i,j)) = boost::gil::at_c<1>(grad.view()(i,j)) = 0;
                }
            }
        }
        boost::gil::write_view( mask_file+"_x.tif" , boost::gil::nth_channel_view(grad.view(),0), boost::gil::tiff_tag() );
        boost::gil::write_view( mask_file+"_y.tif" , boost::gil::nth_channel_view(grad.view(),1), boost::gil::tiff_tag() );
    }

    // empty initial configuration
    c = new configuration( p->get<double>("energy")-(p->get<double>("ponderation_grad")*unary_energy(grad)),
                           p->get<double>("ponderation_surface")*binary_energy());
}
//]

//[building_footprint_rectangle_create_sampler
void create_sampler(const param *p, sampler *&s) {
    Iso_rectangle_2 r = get_bbox(p);
    double maxsize  = p->get<double>("maxsize");
    double maxratio = p->get<double>("maxratio");
    double minratio = 1./maxratio;

    int size[] = {1,1,1,1,1};
    short unsigned prob0[] = {1};
    short unsigned *prob = prob0;
    std::string birth_prob_file = p->get<boost::filesystem::path>("birth_prob" ).string();
    if(birth_prob_file!="")
    {
        unsigned short birth_prob_offset = (unsigned short) (p->get<double>("birth_prob_offset" ));
        clip_bbox(r,birth_prob_file);
        mask_type birth_prob(birth_prob_file , r, conversion_functor() );
        mask_type::view_t view = birth_prob.view();
        size[0] = view.width ();
        size[1] = view.height();
        prob = boost::gil::interleaved_view_get_raw_data(view); // should work in general on single channel images
        for(int i=0; i< size[0]*size[1]; ++i) prob[i] = std::max(prob[i],birth_prob_offset)-birth_prob_offset;
    }

    K::Vector_2 v(maxsize,maxsize);
    uniform_birth birth(
            Rectangle_2(r.min(),-v,minratio),
            Rectangle_2(r.max(), v,maxratio),
            rjmcmc::raster_variate<5>(prob,size)
            );

    distribution cs(p->get<double>("poisson"));

    d_sampler ds( cs, birth );

    typedef rjmcmc::mt19937_generator Engine;
    Engine& e = rjmcmc::random();
    marked_point_process::graph_configuration<object, constant_energy<>, constant_energy<> > c(1,1);
    ds(e,c);
    double p_birthdeath  = p->get<double>("p_birthdeath");
    double p_birth  = p->get<double>("p_birth");
    double p_edge   = 0.25*p->get<double>("p_edge");
    double p_corner = 0.25*p->get<double>("p_corner");
    double p_split_merge = p->get<double>("p_split_merge");
    double p_split       = p->get<double>("p_split");
    birth_death_kernel k_birth_death = marked_point_process::make_uniform_birth_death_kernel(birth, p_birthdeath, p_birth );
    edge_kernel0 k_edge0 = marked_point_process::make_uniform_kernel<object,1,1>(edge_transform0(minratio,maxratio),p_edge);
    edge_kernel1 k_edge1 = marked_point_process::make_uniform_kernel<object,1,1>(edge_transform1(minratio,maxratio),p_edge);
    edge_kernel2 k_edge2 = marked_point_process::make_uniform_kernel<object,1,1>(edge_transform2(minratio,maxratio),p_edge);
    edge_kernel3 k_edge3 = marked_point_process::make_uniform_kernel<object,1,1>(edge_transform3(minratio,maxratio),p_edge);
    corner_kernel0 k_corner0 = marked_point_process::make_uniform_kernel<object,1,1>(corner_transform0(),p_corner);
    corner_kernel1 k_corner1 = marked_point_process::make_uniform_kernel<object,1,1>(corner_transform1(),p_corner);
    corner_kernel2 k_corner2 = marked_point_process::make_uniform_kernel<object,1,1>(corner_transform2(),p_corner);
    corner_kernel3 k_corner3 = marked_point_process::make_uniform_kernel<object,1,1>(corner_transform3(),p_corner);
    k_birth_death.name(0,"birth");
    k_birth_death.name(1,"death");
    k_edge0.name(0,"edge00");
    k_edge0.name(1,"edge01");
    k_edge1.name(0,"edge10");
    k_edge1.name(1,"edge11");
    k_edge2.name(0,"edge20");
    k_edge2.name(1,"edge21");
    k_edge3.name(0,"edge30");
    k_edge3.name(1,"edge31");
    k_corner0.name(0,"corner00");
    k_corner0.name(1,"corner01");
    k_corner1.name(0,"corner10");
    k_corner1.name(1,"corner11");
    k_corner2.name(0,"corner20");
    k_corner2.name(1,"corner21");
    k_corner3.name(0,"corner30");
    k_corner3.name(1,"corner31");

    split_merge_kernel k_split_merge = marked_point_process::make_uniform_kernel<object,1,2>(split_merge_transform(),p_split_merge, p_split);
    k_split_merge.name(0,"split");
    k_split_merge.name(1,"merge");

    s = new sampler( ds, acceptance()
                     , k_birth_death
                     , k_split_merge
                     , k_edge0
                     , k_edge1
                     , k_edge2
                     , k_edge3
                     , k_corner0
                     , k_corner1
                     , k_corner2
                     , k_corner3
                     );
}
//]

//[building_footprint_rectangle_create_schedule
void create_schedule(const param *p, schedule *&t)
{
    t = new schedule(
            p->get<double>("temp"),
            p->get<double>("deccoef")
            );
}
//]

//[building_footprint_rectangle_create_end_test
void create_end_test(const param *p, end_test *&e)
{
    e = new end_test(
            p->get<int>("nbiter")
            );
}
//]

//[building_footprint_rectangle_init_visitor
template<typename Visitor>
void init_visitor(const param *p, Visitor& v)
{
    v.init(
            p->get<int>("nbdump"),
            p->get<int>("nbsave")
            );
}
//]

//[building_footprint_rectangle_image_include_tpl_instanciations
#include "rjmcmc/image/image_types.hpp"
#include "rjmcmc/image/oriented_inc.hpp"
#include "rjmcmc/image/gradient_functor_inc.hpp"
//]

//[building_footprint_rectangle_optimization
#include "rjmcmc/simulated_annealing/simulated_annealing.hpp"
//]

#endif // BUILDING_FOOTPRINT_RECTANGLE_HPP


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * testDiscreteLookupDAG.cpp
 *
 *  @date January, 2022
 *  @author Frank Dellaert
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/discrete/DiscreteLookupDAG.h>

#include <boost/assign/list_inserter.hpp>
#include <boost/assign/std/map.hpp>

using namespace gtsam;
using namespace boost::assign;

/* ************************************************************************* */
TEST(DiscreteLookupDAG, argmax) {
  using ADT = AlgebraicDecisionTree<Key>;

  // Declare 2 keys
  DiscreteKey A(0, 2), B(1, 2);

  // Create lookup table corresponding to "marginalIsNotMPE" in testDFG.
  DiscreteLookupDAG dag;

  ADT adtB(DiscreteKeys{B, A}, std::vector<double>{0.5, 1. / 3, 0.5, 2. / 3});
  dag.add(1, DiscreteKeys{B, A}, adtB);

  ADT adtA(A, 0.5 * 10 / 19, (2. / 3) * (9. / 19));
  dag.add(1, DiscreteKeys{A}, adtA);

  // The expected MPE is A=1, B=1
  DiscreteValues mpe;
  insert(mpe)(0, 1)(1, 1);

  // check:
  auto actualMPE = dag.argmax();
  EXPECT(assert_equal(mpe, actualMPE));
}
/* ************************************************************************* */
int main() {
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


//
//  HeightmapGenerator.cpp
// Kuplung
//
//  Created by Sergey Petrov on 12/21/15.
//  Copyright © 2015 supudo.net. All rights reserved.
//

// http://www.chadvernon.com/blog/resources/directx9/terrain-generation-with-a-heightmap/
// http://www.rastertek.com/tertut02.html

#include "HeightmapGenerator.hpp"
#include <boost/algorithm/string/replace.hpp>
#include <fstream>
#include <glm/glm.hpp>
#include <glm/gtc/matrix_transform.hpp>

void HeightmapGenerator::initPosition() {
  this->position_x1 = 0.0f;
  this->position_x2 = 1.0f;
  this->position_y1 = 0.0f;
  this->position_y2 = 1.0f;

  this->Setting_Octaves = 3;
  this->Setting_Frequency = 2.0f;
  this->Setting_Persistence = 0.5f;
  this->Setting_ColorTerrain = true;
  this->Setting_TerrainType = 0;

  this->Setting_OffsetHorizontal = 0.0f;
  this->Setting_OffsetVertical = 0.0f;
  this->Setting_ScaleCoeficient = 10.0f;
  this->Setting_HeightCoeficient = 10.0f;

  this->Setting_Seed = 1;
}

void HeightmapGenerator::generateTerrain(const std::string& assetFolder, const int& width, const int& height) {
  this->assetsFolder = assetFolder;
  this->width = width;
  this->height = height;

  this->position_x1 += this->Setting_OffsetHorizontal;
  this->position_x2 += this->Setting_OffsetHorizontal;
  this->position_y1 += this->Setting_OffsetVertical;
  this->position_y2 += this->Setting_OffsetVertical;

  module::Perlin perlinNoiser;

  perlinNoiser.SetOctaveCount(this->Setting_Octaves);
  perlinNoiser.SetFrequency(static_cast<double>(this->Setting_Frequency));
  perlinNoiser.SetPersistence(static_cast<double>(this->Setting_Persistence));
  if (this->Setting_SeedRandom)
    this->Setting_Seed = rand();
  perlinNoiser.SetSeed(this->Setting_Seed);

  // heightmap
  if (this->Setting_TerrainType != GeometryTerrainType_Sphere) {
    utils::NoiseMapBuilderPlane heightMapBuilder;
    heightMapBuilder.SetSourceModule(perlinNoiser);
    heightMapBuilder.SetDestNoiseMap(this->heightMap);
    heightMapBuilder.SetDestSize(this->width, this->height);
    heightMapBuilder.SetBounds(static_cast<double>(this->position_x1), static_cast<double>(this->position_x2), static_cast<double>(this->position_y1), static_cast<double>(this->position_y2));
    heightMapBuilder.Build();
  }
  else {
    utils::NoiseMapBuilderSphere heightMapBuilder;
    heightMapBuilder.SetSourceModule(perlinNoiser);
    heightMapBuilder.SetDestNoiseMap(this->heightMap);
    heightMapBuilder.SetDestSize(this->width, this->height);
    heightMapBuilder.SetBounds(-90.0, 90.0, -180.0, 180.0);
    heightMapBuilder.Build();
  }

  // render
  utils::RendererImage renderer;
  renderer.SetSourceNoiseMap(this->heightMap);
  renderer.SetDestImage(this->image);
  if (this->Setting_ColorTerrain) {
    renderer.ClearGradient();
    renderer.AddGradientPoint(-1.0000, utils::Color(0, 0, 128, 255)); // deeps
    renderer.AddGradientPoint(-0.2500, utils::Color(0, 0, 255, 255)); // shallow
    renderer.AddGradientPoint(0.0000, utils::Color(0, 128, 255, 255)); // shore
    renderer.AddGradientPoint(0.0625, utils::Color(240, 240, 64, 255)); // sand
    renderer.AddGradientPoint(0.1250, utils::Color(32, 160, 0, 255)); // grass
    renderer.AddGradientPoint(0.3750, utils::Color(224, 224, 0, 255)); // dirt
    renderer.AddGradientPoint(0.7500, utils::Color(128, 128, 128, 255)); // rock
    renderer.AddGradientPoint(1.0000, utils::Color(255, 255, 255, 255)); // snow
  }
  renderer.Render();

  std::string filename;
  if (Settings::Instance()->Terrain_HeightmapImageHistory) {
    time_t t = time(0);
    const struct tm* now = localtime(&t);

    const int year = now->tm_year + 1900;
    const int month = now->tm_mon + 1;
    const int day = now->tm_mday;
    const int hour = now->tm_hour;
    const int minute = now->tm_min;
    const int seconds = now->tm_sec;

    std::string fileSuffix = std::to_string(year) + std::to_string(month) + std::to_string(day) + std::to_string(hour) + std::to_string(minute) + std::to_string(seconds);
    filename = "terrain_heightmap_" + fileSuffix + ".bmp";
  }
  else
    filename = "terrain_heightmap.bmp";
  this->heightmapImage = assetsFolder + "/" + filename;

  Settings::Instance()->funcDoLog(Settings::Instance()->string_format("Generating terrain [O = %i, F = %f, P = %f] = %s", this->Setting_Octaves, double(this->Setting_Frequency), double(this->Setting_Persistence), this->heightmapImage.c_str()));

#ifdef _WIN32
#else
  boost::replace_all(this->assetsFolder, "Kuplung.app/Contents/Resources", "");
#endif
  utils::WriterBMP writer;
  writer.SetSourceImage(this->image);
  writer.SetDestFilename(this->heightmapImage);
  writer.WriteDestFile();

  switch (this->Setting_TerrainType) {
    case GeometryTerrainType_Cubic:
      this->generatePlaneGeometryCubic();
      break;
    case GeometryTerrainType_Sphere:
      this->generateSphereGeometry();
      break;
    default:
      this->generatePlaneGeometrySmooth();
      break;
  }
}

void HeightmapGenerator::generatePlaneGeometrySmooth() {
  // BIG thanks to http://stackoverflow.com/a/10114636/69897 !!!!
  unsigned int heightmapHeight = static_cast<unsigned int>(this->heightMap.GetHeight());
  unsigned int heightmapWidth = static_cast<unsigned int>(this->heightMap.GetWidth());

  this->vertices.clear();
  this->uvs.clear();
  this->indices.clear();
  this->colors.clear();

  const float worldCenter = -1.0f * heightmapWidth / 2.0f;

  const float rr = 1.0f / static_cast<float>(heightmapHeight - 1);
  const float ss = 1.0f / static_cast<float>(heightmapWidth - 1);

  float p_x = 0, p_y = 0, p_z = 0;
  glm::vec3 position, color;
  glm::vec2 uv;
  utils::Color c;
  for (unsigned int y = 0; y < heightmapHeight; ++y) {
    for (unsigned int x = 0; x < heightmapWidth; ++x) {
      p_x = x + worldCenter;
      p_y = this->heightMap.GetValue(static_cast<int>(x), static_cast<int>(y)) * this->Setting_HeightCoeficient;
      p_z = y + worldCenter;
      position = glm::vec3(p_x, p_y, p_z) / this->Setting_ScaleCoeficient;
      uv = glm::vec2(x * ss, 1.0f - y * rr);

      c = this->image.GetValue(static_cast<int>(x), static_cast<int>(y));
      color = glm::vec3(c.red / 255.0f, c.green / 255.0f, c.blue / 255.0f);

      this->vertices.push_back(position);
      this->uvs.push_back(uv);
      this->normals.push_back(glm::vec3(0, 1, 0));
      this->colors.push_back(color);
    }
  }

  glm::vec3 v1, v2, v3, normal;
  for (unsigned int y = 0; y < (heightmapHeight - 1); ++y) {
    for (unsigned int x = 0; x < (heightmapWidth - 1); ++x) {
      unsigned int start = y * heightmapWidth + x;
      this->indices.push_back(start);
      this->indices.push_back(start + 1);
      this->indices.push_back(start + heightmapWidth);
      v1 = this->vertices[start];
      v2 = this->vertices[start + 1];
      v3 = this->vertices[start + heightmapWidth];
      normal = glm::cross(v2 - v1, v3 - v1);
      this->normals[start] = normal;
      this->normals[start + 1] = normal;
      this->normals[start + heightmapWidth] = normal;

      this->indices.push_back(start + 1);
      this->indices.push_back(start + 1 + heightmapWidth);
      this->indices.push_back(start + heightmapWidth);
      v1 = this->vertices[start];
      v2 = this->vertices[start + heightmapWidth];
      v3 = this->vertices[start + 1 + heightmapWidth];
      normal = glm::cross(v2 - v1, v3 - v1);
      this->normals[start] = normal;
      this->normals[start + heightmapWidth] = normal;
      this->normals[start + 1 + heightmapWidth] = normal;
    }
  }

  this->generateMeshModel();
}

void HeightmapGenerator::generateSphereGeometry() {
  unsigned int heightmapHeight = static_cast<unsigned int>(this->heightMap.GetHeight());
  unsigned int heightmapWidth = static_cast<unsigned int>(this->heightMap.GetWidth());

  this->vertices.clear();
  this->uvs.clear();
  this->normals.clear();
  this->colors.clear();
  this->indices.clear();

  std::string grapher("");

  const float rr = 1.0f / static_cast<float>(heightmapHeight - 1);
  const float ss = 1.0f / static_cast<float>(heightmapWidth - 1);

  static const double pi = glm::pi<double>();
  static const double pi_2 = glm::half_pi<double>();

  float hmValue = 0;
  float p_x = 0, p_y = 0, p_z = 0;
  glm::vec3 position, color;
  glm::vec2 uv;
  utils::Color c;
  for (unsigned int y = 0; y < heightmapHeight; ++y) {
    for (unsigned int x = 0; x < heightmapWidth; ++x) {
      hmValue = this->heightMap.GetValue(static_cast<int>(x), static_cast<int>(y));
      p_x = static_cast<float>(cos(2 * pi * x * ss) * sin(pi * y * rr));
      p_y = static_cast<float>(sin(-pi_2 + pi * y * rr));
      p_z = static_cast<float>(sin(2 * pi * x * ss) * sin(pi * y * rr));
      position = glm::vec3(p_x, p_y, p_z) * this->Setting_ScaleCoeficient;
      position += glm::normalize(position) * hmValue;

      if (Settings::Instance()->logDebugInfo)
        grapher += Settings::Instance()->string_format("%g,%g,%g\n", position.x, position.y, position.z);

      uv = glm::vec2(x * 1.0f / heightmapWidth, y * 1.0f / heightmapHeight);
      c = this->image.GetValue(static_cast<int>(x), static_cast<int>(y));
      color = glm::vec3(c.red / 255.0f, c.green / 255.0f, c.blue / 255.0f);

      this->vertices.push_back(position);
      this->uvs.push_back(uv);
      this->normals.push_back(glm::vec3(0, 1, 0));
      this->colors.push_back(color);
    }
  }

  glm::vec3 v1, v2, v3, normal;
  for (unsigned int y = 0; y < (heightmapHeight - 1); ++y) {
    for (unsigned int x = 0; x < (heightmapWidth - 1); ++x) {
      unsigned int start = y * heightmapWidth + x;
      this->indices.push_back(start);
      this->indices.push_back(start + 1);
      this->indices.push_back(start + heightmapWidth);
      v1 = this->vertices[start];
      v2 = this->vertices[start + 1];
      v3 = this->vertices[start + heightmapWidth];
      normal = glm::cross(v2 - v1, v3 - v1);
      this->normals[start] = normal;
      this->normals[start + 1] = normal;
      this->normals[start + heightmapWidth] = normal;

      this->indices.push_back(start + 1);
      this->indices.push_back(start + 1 + heightmapWidth);
      this->indices.push_back(start + heightmapWidth);
      v1 = this->vertices[start];
      v2 = this->vertices[start + heightmapWidth];
      v3 = this->vertices[start + 1 + heightmapWidth];
      normal = glm::cross(v2 - v1, v3 - v1);
      this->normals[start] = normal;
      this->normals[start + heightmapWidth] = normal;
      this->normals[start + 1 + heightmapWidth] = normal;
    }
  }

  this->generateMeshModel();

  if (Settings::Instance()->logDebugInfo) {
    std::ofstream out(this->assetsFolder + "/terrain.txt");
    if (out.is_open()) {
      out << grapher;
      out.close();
    }
  }
}

void HeightmapGenerator::generatePlaneGeometryCubic() {
  const int heightmapHeight = this->heightMap.GetHeight();
  const int heightmapWidth = this->heightMap.GetWidth();

  this->vertices.clear();
  this->uvs.clear();
  this->normals.clear();
  this->colors.clear();
  this->indices.clear();

  std::string grapher("");

  const float rr = 1.0f / static_cast<float>(heightmapHeight - 1);
  const float ss = 1.0f / static_cast<float>(heightmapWidth - 1);

  unsigned int vertIndex = 0;
  const float worldCenter = -1.0f * heightmapWidth / 2.0f;
  glm::vec3 v0, v1, v2, v3, v4, v5, v10, v11, n, n2, n3, color;
  glm::vec2 uv, uv2, uv3;
  float hmValue = 0, hmValue2 = 0, hmValue3 = 0;
  utils::Color c;

  for (int y = 0; y < (heightmapHeight - 1) * 3; ++y) {
    for (int x = 0; x < heightmapWidth - 1; ++x) {
      hmValue = this->heightMap.GetValue(x, y) * this->Setting_HeightCoeficient;
      hmValue2 = this->heightMap.GetValue(x + 1, y) * this->Setting_HeightCoeficient;
      hmValue3 = this->heightMap.GetValue(x, y + 1) * this->Setting_HeightCoeficient;

      c = this->image.GetValue(x, y);
      color = glm::vec3(c.red / 255.0f, c.green / 255.0f, c.blue / 255.0f);
      //uv = glm::vec2(glm::clamp(float(x), 0.0f, 1.0f), glm::clamp(float(y), 0.0f, 1.0f));
      uv = glm::vec2(x * ss, y * rr);
      uv2 = glm::vec2((x + 1) * ss, y * rr);
      uv3 = glm::vec2(x * ss, (y + 1) * rr);

      // counter clockwise direction
      //
      //   ^
      //   |  11 --- 10 --- 12 ---- 13 --- 14
      //   |   |    / |    / |    / |    / |
      //   |   |   /  |   /  |   /  |   /  |
      //   |   |  /   |  /   |  /   |  /   |
      //   |   | /    | /    | /    | /    |
      //   |   3 ---- 2 ---- 5 ---- 7 ---- 9
      //   |   |    / |    / |    / |    / |
      //   |   |   /  |   /  |   /  |   /  |
      //   |   |  /   |  /   |  /   |  /   |
      //   |   | /    | /    | /    | /    |
      //   Y   0 ---- 1 ---- 4 ---- 6 ---- 8
      //   |
      //   0---X---------------------------------->
      //

      v0 = glm::vec3(x + worldCenter, y + worldCenter, hmValue);
      v1 = glm::vec3(x + worldCenter + 1, y + worldCenter, hmValue);
      v2 = glm::vec3(x + worldCenter + 1, y + worldCenter + 1, hmValue);
      v3 = glm::vec3(x + worldCenter, y + worldCenter + 1, hmValue);
      v4 = glm::vec3(x + worldCenter + 1, y + worldCenter, hmValue2);
      v5 = glm::vec3(x + worldCenter + 1, y + worldCenter + 1, hmValue2);
      v10 = glm::vec3(x + worldCenter + 1, y + worldCenter + 1, hmValue3);
      v11 = glm::vec3(x + worldCenter, y + worldCenter + 1, hmValue3);
      n = glm::cross(v1 - v0, v2 - v0);
      n2 = glm::cross(v4 - v1, v5 - v1);
      n3 = glm::cross(v10 - v3, v11 - v3);

      // triangle 1
      this->vertices.push_back(v0 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v1 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v2 / this->Setting_ScaleCoeficient);

      this->uvs.push_back(uv);
      this->uvs.push_back(uv);
      this->uvs.push_back(uv);

      this->normals.push_back(n);
      this->normals.push_back(n);
      this->normals.push_back(n);

      this->indices.push_back(vertIndex);

      vertIndex += 1;

      this->colors.push_back(color);
      this->colors.push_back(color);
      this->colors.push_back(color);

      if (Settings::Instance()->logDebugInfo)
        grapher += Settings::Instance()->string_format(" %g,%g,%g;%g,%g,%g;%g,%g,%g \n", v0.x, v0.y, v0.z, v1.x, v1.y, v1.z, v2.x, v2.y, v2.z);

      // triangle 2
      this->vertices.push_back(v0 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v2 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v3 / this->Setting_ScaleCoeficient);

      this->uvs.push_back(uv);
      this->uvs.push_back(uv);
      this->uvs.push_back(uv);

      this->normals.push_back(n);
      this->normals.push_back(n);
      this->normals.push_back(n);

      this->indices.push_back(vertIndex);

      vertIndex += 1;

      this->colors.push_back(color);
      this->colors.push_back(color);
      this->colors.push_back(color);

      // connecting triangle 1 - right
      this->vertices.push_back(v1 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v4 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v5 / this->Setting_ScaleCoeficient);

      this->uvs.push_back(uv2);
      this->uvs.push_back(uv2);
      this->uvs.push_back(uv2);

      this->normals.push_back(n2);
      this->normals.push_back(n2);
      this->normals.push_back(n2);

      this->indices.push_back(vertIndex);

      vertIndex += 1;

      this->colors.push_back(color);
      this->colors.push_back(color);
      this->colors.push_back(color);

      // connecting triangle 2 - right
      this->vertices.push_back(v1 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v5 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v2 / this->Setting_ScaleCoeficient);

      this->uvs.push_back(uv2);
      this->uvs.push_back(uv2);
      this->uvs.push_back(uv2);

      this->normals.push_back(n2);
      this->normals.push_back(n2);
      this->normals.push_back(n2);

      this->indices.push_back(vertIndex);

      vertIndex += 1;

      this->colors.push_back(color);
      this->colors.push_back(color);
      this->colors.push_back(color);

      // connecting triangle 1 - top
      this->vertices.push_back(v3 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v2 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v10 / this->Setting_ScaleCoeficient);

      this->uvs.push_back(uv3);
      this->uvs.push_back(uv3);
      this->uvs.push_back(uv3);

      this->normals.push_back(n3);
      this->normals.push_back(n3);
      this->normals.push_back(n3);

      this->indices.push_back(vertIndex);

      vertIndex += 1;

      this->colors.push_back(color);
      this->colors.push_back(color);
      this->colors.push_back(color);

      // connecting triangle 2 - top
      this->vertices.push_back(v3 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v10 / this->Setting_ScaleCoeficient);
      this->vertices.push_back(v11 / this->Setting_ScaleCoeficient);

      this->uvs.push_back(uv3);
      this->uvs.push_back(uv3);
      this->uvs.push_back(uv3);

      this->normals.push_back(n3);
      this->normals.push_back(n3);
      this->normals.push_back(n3);

      this->indices.push_back(vertIndex);

      vertIndex += 1;

      this->colors.push_back(color);
      this->colors.push_back(color);
      this->colors.push_back(color);
    }
  }

  this->generateMeshModel();

  if (Settings::Instance()->logDebugInfo) {
    std::ofstream out(this->assetsFolder + "/terrain.txt");
    if (out.is_open()) {
      out << grapher;
      out.close();
    }
  }
}

void HeightmapGenerator::generateMeshModel() {
  this->modelTerrain = {};
  this->modelTerrain.vertices.clear();
  this->modelTerrain.texture_coordinates.clear();
  this->modelTerrain.normals.clear();
  this->modelTerrain.indices.clear();

  this->modelTerrain.ID = 1;
  this->modelTerrain.MaterialTitle = "MaterialTerrain";
  this->modelTerrain.ModelTitle = "Terrain";

  this->modelTerrain.vertices = this->vertices;
  this->modelTerrain.texture_coordinates = this->uvs;
  this->modelTerrain.normals = this->normals;
  this->modelTerrain.indices = this->indices;

  this->modelTerrain.countVertices = static_cast<int>(this->vertices.size());
  this->modelTerrain.countTextureCoordinates = static_cast<int>(this->uvs.size());
  this->modelTerrain.countNormals = static_cast<int>(this->normals.size());
  this->modelTerrain.countIndices = static_cast<int>(this->indices.size());

  MeshModelMaterial material = {};
  material.MaterialID = 1;
  material.MaterialTitle = "MaterialTerrain";
  material.AmbientColor = glm::vec3(0.7f);
  material.DiffuseColor = glm::vec3(0.7f);
  material.SpecularExp = 99.0f;
  material.IlluminationMode = 2;
  material.OpticalDensity = 1.0f;
  material.Transparency = 1.0f;
  this->modelTerrain.ModelMaterial = material;
}


#include <tiny.h>
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

// 2011-11-12 Kenny: Does these test depend on the order in which vertices are added to the simplex? We always use a,b,c

BOOST_AUTO_TEST_SUITE(convex_reduce_triangle);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef tiny::MathTypes<double>         math_types;
  typedef math_types::vector3_type        vector3_type;
  typedef math_types::real_type           real_type;

  typedef convex::Simplex<vector3_type>           simplex_type;

  // Inside face-region new simplex should be ABC
  {
    vector3_type const a = vector3_type::make(-1.0, -1.0, 0.0);
    vector3_type const b = vector3_type::make( 1.0, -1.0, 0.0);
    vector3_type const c = vector3_type::make( 0.5,  0.1, 0.0);

    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    for( size_t i=1u; i<10u; ++i)
      for( size_t j=1u; j<10u; ++j)
        for( size_t k=1u; k<10u; ++k)
        {
          real_type u = i*0.1;
          real_type v = j*0.1;
          real_type w = k*0.1;

          real_type lgh = u + v + w;
          u /= lgh;
          v /= lgh;
          w /= lgh;

          vector3_type const p = u*a + v*b + w*c;

          convex::reduce_triangle( p, S );

          BOOST_CHECK( convex::dimension( S ) == 3u );

          int bit_A    = 0;
          size_t idx_A = 0;
          int bit_B    = 0;
          size_t idx_B = 0;
          int bit_C    = 0;
          size_t idx_C = 0;
          convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B , idx_C, bit_C );

          BOOST_CHECK(S.m_bitmask == (bit_A | bit_B | bit_C));

          BOOST_CHECK(S.m_v[idx_A] == a);
          BOOST_CHECK(S.m_a[idx_A] == a);
          BOOST_CHECK(S.m_b[idx_A] == a);

          BOOST_CHECK(S.m_v[idx_B] == b);
          BOOST_CHECK(S.m_a[idx_B] == b);
          BOOST_CHECK(S.m_b[idx_B] == b);

          BOOST_CHECK(S.m_v[idx_C] == c);
          BOOST_CHECK(S.m_a[idx_C] == c);
          BOOST_CHECK(S.m_b[idx_C] == c);
          BOOST_CHECK_CLOSE(S.m_w[idx_A], u, 0.01);
          BOOST_CHECK_CLOSE(S.m_w[idx_B], v, 0.01);
          BOOST_CHECK_CLOSE(S.m_w[idx_C], w, 0.01);

        }
  }
  // First we create a simplex that represents an triangle
  vector3_type const a = vector3_type::make(-1.0, -1.0, 0.0);
  vector3_type const b = vector3_type::make( 1.0, -1.0, 0.0);
  vector3_type const c = vector3_type::make( 0.0,  1.0, 0.0);

  // Inside face-region new simplex should be ABC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( 0.0, 0.0,  0.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 3u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    int bit_C    = 0;
    size_t idx_C = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B , idx_C, bit_C );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B | bit_C));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK(S.m_v[idx_C] == c);
    BOOST_CHECK(S.m_a[idx_C] == c);
    BOOST_CHECK(S.m_b[idx_C] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.25, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.25, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_C], 0.5, 0.01);
  }
  // Inside A voronoi region new simplex should be A
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( -2.0, -1.0,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // Inside B voronoi region new simplex should be B
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make(  2.0, -1.0,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.00, 0.01);
  }
  // Inside C voronoi region new simplex should be C
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make(  0.0, 2.0,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A  );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == c);
    BOOST_CHECK(S.m_a[idx_A] == c);
    BOOST_CHECK(S.m_b[idx_A] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // Inside AB voronoi region new simplex should be AB
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( 0.0, -2.0,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // Inside BC voronoi region new simplex should be BC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( 1.5, 0.5,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // Inside AC voronoi region new simplex should be AC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( -1.5, 0.5,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // On vertex A new simplex should be A
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    convex::reduce_triangle( a, S );

    BOOST_CHECK( convex::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // On vertex B new simplex should be B
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    convex::reduce_triangle( b, S );

    BOOST_CHECK( convex::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.00, 0.01);
  }
  // On vertex C new simplex should be C
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    convex::reduce_triangle( c, S );

    BOOST_CHECK( convex::dimension( S ) == 1u );

    int bit_A    = 0;
    size_t idx_A = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A  );

    BOOST_CHECK(S.m_bitmask == bit_A);
    BOOST_CHECK(S.m_v[idx_A] == c);
    BOOST_CHECK(S.m_a[idx_A] == c);
    BOOST_CHECK(S.m_b[idx_A] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 1.0, 0.01);
  }
  // On edge AB new simplex should be AB
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( 0.0, -1.0,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // On edge BC new simplex should be BC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( 0.5, 0.0,  1.0);// this point is inside the triangle?!

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // On edge AC new simplex should be AC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = vector3_type::make( -0.5, 0.0,  1.0);

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.5, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.5, 0.01);
  }
  // Assymmetric test cases

  // New simplex should be AB
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.4*a + 0.6*b;

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.4, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.6, 0.01);
  }
  // New simplex should be BC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.4*b + 0.6*c;// point is inside triangle?!

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == b);
    BOOST_CHECK(S.m_a[idx_A] == b);
    BOOST_CHECK(S.m_b[idx_A] == b);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.4, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.6, 0.01);
  }
  // New simplex should be AC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.6*a + 0.4*c;

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 2u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B ));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == c);
    BOOST_CHECK(S.m_a[idx_B] == c);
    BOOST_CHECK(S.m_b[idx_B] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.6, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.4, 0.01);
  }
  // New simplex should be ABC
  {
    simplex_type S;

    convex::add_point_to_simplex( a, a, a, S);
    convex::add_point_to_simplex( b, b, b, S);
    convex::add_point_to_simplex( c, c, c, S);

    vector3_type const p = 0.1*a + 0.2*b + 0.7*c;

    convex::reduce_triangle( p, S );

    BOOST_CHECK( convex::dimension( S ) == 3u );

    int bit_A    = 0;
    size_t idx_A = 0;
    int bit_B    = 0;
    size_t idx_B = 0;
    int bit_C    = 0;
    size_t idx_C = 0;
    convex::get_used_indices( S.m_bitmask, idx_A, bit_A, idx_B, bit_B , idx_C, bit_C );

    BOOST_CHECK(S.m_bitmask == (bit_A | bit_B | bit_C));
    BOOST_CHECK(S.m_v[idx_A] == a);
    BOOST_CHECK(S.m_a[idx_A] == a);
    BOOST_CHECK(S.m_b[idx_A] == a);

    BOOST_CHECK(S.m_v[idx_B] == b);
    BOOST_CHECK(S.m_a[idx_B] == b);
    BOOST_CHECK(S.m_b[idx_B] == b);

    BOOST_CHECK(S.m_v[idx_C] == c);
    BOOST_CHECK(S.m_a[idx_C] == c);
    BOOST_CHECK(S.m_b[idx_C] == c);

    BOOST_CHECK_CLOSE(S.m_w[idx_A], 0.1, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_B], 0.2, 0.01);
    BOOST_CHECK_CLOSE(S.m_w[idx_C], 0.7, 0.01);
  }
}

BOOST_AUTO_TEST_SUITE_END();


/**
 * @file k_means_clustering_long_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-09-14
 */

#include "test_utils/logger.hpp"
#include "test_utils/read_two_dimensional_data.hpp"
#include "test_utils/test_result_check.hpp"
#include "test_utils/get_test_dir.hpp"
#include "test_utils/system.hpp"

#include "paal/clustering/k_means_clustering.hpp"
#include "paal/utils/parse_file.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/range/algorithm/max_element.hpp>

#include <iostream>
#include <algorithm>

const bool PRINT_SVG = false;

inline auto scale(double e, double max_value) { return e / max_value * 1e3; };

template <class Stream> struct print_svg_visitor : public paal::k_means_visitor {
    print_svg_visitor(Stream &stream, double max_value) : m_stream(stream), m_max_value(max_value) {};
    template <class Center, class New_center>
    void move_center(Center &&last_center, New_center &&new_center) {
        m_stream << "<line x1=\"" << scale(last_center[0], m_max_value)
                 <<    "\" y1=\"" << scale(last_center[1], m_max_value)
                 <<    "\" x2=\"" << scale(new_center[0], m_max_value)
                 <<    "\" y2=\"" << scale(new_center[1], m_max_value)
                 << "\" style=\"stroke:rgb(255,0,0);stroke-width:1\" />\n";
    }

  private:

    Stream &m_stream;
    double m_max_value;
};
BOOST_AUTO_TEST_CASE(k_means_clustering_long_test) {
    using Point = std::vector<double>;

    std::string test_dir = paal::system::get_test_data_dir("CLUSTERING");
    using paal::system::build_path;
    paal::parse(build_path(test_dir, "index"),
                [&](const std::string &fname, std::istream &is_test_cases) {

        LOGLN("TEST " << fname);
        int number_of_clusters;
        is_test_cases >> number_of_clusters;
        std::ifstream ifs(build_path(test_dir, "/cases/" + fname + ".txt"));
        assert(ifs.good());

        auto points = paal::read_two_dimensional_data<>(ifs);

        std::vector<Point> start_centers;
        paal::get_random_centers(points,number_of_clusters,back_inserter(start_centers));

        std::vector<std::pair<Point, int>> point_cluster_pair;

        double max_value=0;
        for(auto point : points){
            max_value = std::max(max_value, *boost::max_element(point));
        }

        if(!PRINT_SVG) {
            auto centers=paal::k_means(points, start_centers,
                back_inserter(point_cluster_pair));
            return;
        }

        std::ofstream ofs (fname+".svg", std::ofstream::out);
        ofs << "<svg width=\"1000\" height=\"1000\">\n" <<
                "<circle cx=\"0\" cy=\"0\" r=\"2000\" stroke=\"green\" stroke-width=\"0\" fill=\"black\" />\n";
        auto centers=paal::k_means(points, start_centers,
                back_inserter(point_cluster_pair), print_svg_visitor<std::ofstream>(ofs, max_value));

        std::vector<std::string> colors = {
                                    "brown",
                                    "burlywood",
                                    "cadetblue",
                                    "chartreuse",
                                    "chocolate",
                                    "coral",
                                    "cornflowerblue",
                                    "cornsilk",
                                    "crimson",
                                    "darkblue",
                                    "darkcyan",
                                    "darkgoldenrod"
                                };

        auto scale_1 = [=](double d){ return scale(d, max_value); };
        for (auto i : point_cluster_pair) {
            ofs
                << "<circle cx=\"" << scale_1(i.first[0])
                << "\" cy=\""      << scale_1(i.first[1])
                << "\" r=\"1\" stroke=\"green\" stroke-width=\"0\" fill=\""
                << colors[i.second%colors.size()] << "\" />"
                << "\n";
        }
        for (auto i : centers) {
            if(i.size()>1){
            ofs
                << "<circle cx=\"" << scale_1(i[0])
                << "\" cy=\""      << scale_1(i[1])
                << "\" r=\"5\" stroke=\"green\" stroke-width=\"0\" fill=\""
                << "red" << "\" />"
                << "\n";
                }
        }
        ofs << "</svg>\n";
        ofs.close();
    });

}


/**
 *  .file test/oglplus/quaternion.cpp
 *  .brief Test case for Quaternion class and related functionality.
 *
 *  .author Matus Chochlik
 *
 *  Copyright 2011-2019 Matus Chochlik. 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)
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE OGLPLUS_Quaternion
#include <boost/test/unit_test.hpp>

#include <oglplus/gl.hpp>
#include <oglplus/math/quaternion.hpp>

BOOST_AUTO_TEST_SUITE(Quaternion)

BOOST_AUTO_TEST_CASE(Quaternion_construction) {
    using Quatf = oglplus::Quaternion<float>;
    Quatf q1(1, 0, 0, 0);
    Quatf q2(oglplus::Vec3f::Unit(0), oglplus::Degrees(45));
    Quatf q3(0, oglplus::Vec3f::Unit(1));
    Quatf q4 = q3;
}

BOOST_AUTO_TEST_CASE(Quaternion_real_imag) {
    using Quatf = oglplus::Quaternion<float>;
    Quatf q(1, 2, 3, 4);

    BOOST_CHECK(q.Real() == 1);
    BOOST_CHECK(q.Imag().x() == 2);
    BOOST_CHECK(q.Imag().y() == 3);
    BOOST_CHECK(q.Imag().z() == 4);
}

BOOST_AUTO_TEST_CASE(Quaternion_real_imag_at) {
    using Quatf = oglplus::Quaternion<float>;
    Quatf q(1, 2, 3, 4);

    BOOST_CHECK(q.Real() == q.At(0));
    BOOST_CHECK(q.Imag().x() == q.At(1));
    BOOST_CHECK(q.Imag().y() == q.At(2));
    BOOST_CHECK(q.Imag().z() == q.At(3));
}

BOOST_AUTO_TEST_CASE(Quaternion_equality) {
    using Quatf = oglplus::Quaternion<float>;
    Quatf q1(1, 0, 0, 0);
    Quatf q2 = q1;
    Quatf q3(1, 2, 3, 4);

    BOOST_CHECK(Equal(q1, q2));
    BOOST_CHECK(Equal(q2, q1));
    BOOST_CHECK(q1 == q2);
    BOOST_CHECK(q2 == q1);

    BOOST_CHECK(!Equal(q1, q3));
    BOOST_CHECK(!Equal(q3, q2));
    BOOST_CHECK(q1 != q3);
    BOOST_CHECK(q3 != q2);
}

BOOST_AUTO_TEST_CASE(Quaternion_dot) {
    using Quatf = oglplus::Quaternion<float>;
    Quatf q0(1, 2, 3, 4);
    Quatf q1(1, 0, 0, 0);
    Quatf q2(0, 1, 0, 0);
    Quatf q3(0, 0, 1, 0);
    Quatf q4(0, 0, 0, 1);
    BOOST_CHECK(Dot(q0, q0) == 1 * 1 + 2 * 2 + 3 * 3 + 4 * 4);
    BOOST_CHECK(Dot(q0, q1) == 1);
    BOOST_CHECK(Dot(q0, q2) == 2);
    BOOST_CHECK(Dot(q0, q3) == 3);
    BOOST_CHECK(Dot(q0, q4) == 4);
    BOOST_CHECK(Dot(q1, q2) == 0);
    BOOST_CHECK(Dot(q2, q3) == 0);
    BOOST_CHECK(Dot(q3, q4) == 0);
    BOOST_CHECK(Dot(q4, q1) == 0);
}

BOOST_AUTO_TEST_CASE(Quaternion_is_degenerate) {
    using Quatf = oglplus::Quaternion<float>;
    BOOST_CHECK(Quatf(0, 0, 0, 0).IsDegenerate());
}

BOOST_AUTO_TEST_CASE(Quaternion_is_normal) {
    using Quatf = oglplus::Quaternion<float>;
    using oglplus::Degrees;
    using oglplus::Vec3f;
    int deg[5] = {0, 90, 180, 270, 360};
    float eps = 1e-7f;
    for(int i = 0; i != 5; ++i) {
        BOOST_CHECK(Quatf(Vec3f::Unit(0), Degrees(deg[i])).IsNormal(eps));
        BOOST_CHECK(Quatf(Vec3f::Unit(1), Degrees(deg[i])).IsNormal(eps));
        BOOST_CHECK(Quatf(Vec3f::Unit(2), Degrees(deg[i])).IsNormal(eps));
    }
    for(int i = 0; i != 1000; ++i) {
        float rdeg = (float(std::rand()) / RAND_MAX - 0.5f);
        BOOST_CHECK(Quatf(Vec3f::Unit(0), Degrees(rdeg)).IsNormal(eps));
        BOOST_CHECK(Quatf(Vec3f::Unit(1), Degrees(rdeg)).IsNormal(eps));
        BOOST_CHECK(Quatf(Vec3f::Unit(2), Degrees(rdeg)).IsNormal(eps));
    }
}

BOOST_AUTO_TEST_CASE(Quaternion_normalize) {
    using Quatd = oglplus::Quaternion<double>;
    double eps = 1e-15;
    for(int i = 0; i < 1000; ++i) {
        Quatd q(
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5));
        if(!q.IsDegenerate()) {
            q.Normalize();
            BOOST_CHECK(q.IsNormal(eps));
        }
    }
}

BOOST_AUTO_TEST_CASE(Quaternion_conjugate) {
    using Quatf = oglplus::Quaternion<float>;
    for(int i = 0; i < 1000; ++i) {
        Quatf q(
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f));
        BOOST_CHECK(q.Real() == Conjugate(q).Real());
        BOOST_CHECK(q.Imag() == -Conjugate(q).Imag());
        BOOST_CHECK(Equal(q, ~~q));
    }
}

BOOST_AUTO_TEST_CASE(Quaternion_addition) {
    using Quatf = oglplus::Quaternion<float>;
    float eps = 1e-11;
    for(int i = 0; i < 1000; ++i) {
        Quatf q1(
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f));
        Quatf q2(
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f),
          (float(std::rand()) / RAND_MAX - 0.5f));
        BOOST_CHECK_CLOSE((q1 + q2).Real(), (q1.Real() + q2.Real()), eps);
        BOOST_CHECK((q1 + q2).Imag() == (q1.Imag() + q2.Imag()));
        BOOST_CHECK((q1 + q2) == (q2 + q1));
    }
}

BOOST_AUTO_TEST_CASE(Quaternion_multiplication) {
    using Quatd = oglplus::Quaternion<double>;
    double eps = 1e-11;
    for(int i = 0; i < 1000; ++i) {
        Quatd q1(
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5));
        Quatd q2(
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5));
        Quatd q3(
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5));
        Quatd q4(
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5),
          (double(std::rand()) / RAND_MAX - 0.5));
        BOOST_CHECK(Close(q1 * q2 * q3 * q4, (q1 * q2) * (q3 * q4), eps));
        BOOST_CHECK(Close(q1 * q2 * q3 * q4, q1 * (q2 * q3) * q4, eps));
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef MP_PAGERANK_HPP
#define MP_PAGERANK_HPP

#include "defs.hpp"
#include "tools/iter.hpp"

#include <boost/property_map/property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_concepts.hpp>

namespace mp {

namespace detail {

/*
 * Params:
 *  g            - The bidirectional Graph (read-only).
 *  from_rank    - The page-rank of the last step (read-only).
 *  to_rank      - The page-rank of the next iteration (write-only).
 *  damping      - The damping factor.
 *  sinks        - A Collection of sinks (vertices without outgoing edges; read-only).
 *  weight       - A mapping from edge-descriptor to edge-weight (read-only).
 *  total_weight - A mapping from vertex-descriptor to the sum of all edge-weights beginning at that vertex (read-only).
 */
template<typename Graph, typename RankMap1, typename RankMap2, typename SinkCollection, typename EdgeWeightMap, typename TotalWeightMap>
void page_rank_step(const Graph &g,
                    RankMap1 from_rank, RankMap2 to_rank, typename boost::property_traits<RankMap1>::value_type damping,
                    const SinkCollection &sinks, EdgeWeightMap weight, TotalWeightMap total_weight)
{
    using rank_type = typename boost::property_traits<RankMap1>::value_type;

    // Iterate over all vertices and their incoming edges.
    for (auto vertex : make_iter_range(vertices(g))) {
        rank_type rank(0);

        // Consider all incoming edges.
        // Sources of these edges give the current vertex a part of their
        // rank, according to the edge's weight.
        for(auto edge : make_iter_range(in_edges(vertex, g)))  {
            auto vertex_source = source(edge, g);

            // Using this coefficient instead of 1/out_degree(vertex_source)
            // to support weighted edges.
            rank_type coeff = rank_type(get(weight, edge)) / rank_type(get(total_weight, vertex_source));
            rank += get(from_rank, vertex_source) * coeff;
        }

        // Treat all "sinks" as incoming edges, too.
        for (auto sink : sinks) {
            // Sinks distribute their rank evenly.
            rank += get(from_rank, sink) / num_vertices(g);
        }

        rank = ((rank_type(1) - damping) / num_vertices(g)) + damping * rank;
        put(to_rank, vertex, rank);
    }
}

} // namespace datail

/**
  * Computes PageRank for the given Graph.
  *
  * The algorithm will run until the StopPredicate returns true.
  *
  * \param g
  *      The Graph
  * \param[out] rank
  *      A map from vertex to rank. Will store the output once the function returns.
  * \param damping
  *     The damping factor. Between 0 and 1.
  * \param weight
  *     A map from edge descriptor to edge weight. Values will be normalized by this function.
  * \param stop
  *     The predicate.
  */
// Vertex index: http://www.boost.org/doc/libs/1_56_0/libs/graph/doc/faq.html (item 5)
template<typename Graph, typename RankMap, typename EdgeWeightMap, typename StopPredicate>
void page_rank(const Graph &g,
               RankMap rank, typename boost::property_traits<RankMap>::value_type damping,
               EdgeWeightMap weight,
               StopPredicate &&stop)
{
    using graph_traits       = boost::graph_traits<Graph>;
    using edge_descriptor    = typename graph_traits::edge_descriptor;
    using vertex_descriptor  = typename graph_traits::vertex_descriptor;
    using rank_type          = typename boost::property_traits<RankMap>::value_type;
    using weight_type        = typename boost::property_traits<EdgeWeightMap>::value_type;

    BOOST_CONCEPT_ASSERT(( boost::BidirectionalGraphConcept<Graph> ));
    BOOST_CONCEPT_ASSERT(( boost::ReadWritePropertyMapConcept<RankMap, vertex_descriptor> ));
    BOOST_CONCEPT_ASSERT(( boost::ReadablePropertyMapConcept<EdgeWeightMap, edge_descriptor> ));

    // The start rank of every vertex is 1/n
    // where n is the number of vertices.
    {
        rank_type start(rank_type(1) / num_vertices(g));

        for (auto vertex : make_iter_range(vertices(g))) {
            put(rank, vertex, start);
        }
    }

    auto indices = get(boost::vertex_index, g);

    // A second rank_map to store the ranks of the previous or next iteration.
    std::vector<rank_type> aux_rank_vector(num_vertices(g));
    auto aux_rank = boost::make_iterator_property_map(
                aux_rank_vector.begin(), indices);

    // For every vertex, compute the total of its outgoing edges' weights.
    std::vector<weight_type> total_weight_vector(num_vertices(g));
    auto total_weight = boost::make_iterator_property_map(
                total_weight_vector.begin(), indices);
    {
        for (auto vertex : make_iter_range(vertices(g))) {
            weight_type total(0);

            for(auto edge : make_iter_range(out_edges(vertex, g))) {
                total += get(weight, edge);
            }

            put(total_weight, vertex, total);
        }
    }

    // Collection of "sinks" - vertices that have no outgoing edges.
    std::vector<vertex_descriptor> sinks;
    {
        for (auto vertex : make_iter_range(vertices(g))) {
            if (out_degree(vertex, g) == 0) {
                sinks.push_back(vertex);
            }
        }
    }

    bool toggle = true;
    while (1) {
        if (toggle) {
            if (stop(rank, g)) {
                break;
            }

            detail::page_rank_step(g, rank, aux_rank, damping, sinks, weight, total_weight);
        } else {
            if (stop(aux_rank, g)) {
                break;
            }

            detail::page_rank_step(g, aux_rank, rank, damping, sinks, weight, total_weight);
        }

        toggle = !toggle;
    }

    if (!toggle) {
        // Copy results from "aux_rank" to "rank".
        for (auto vertex : make_iter_range(vertices(g))) {
            put(rank, vertex, get(aux_rank, vertex));
        }
    }
}

} // namespace mp

#endif // MP_PAGERANK_HPP