#include "LBFGSB.h"
#include <Eigen/Core>
#include <vector>
#include <memory>
#include <sstream>
#include <iostream>
#include <fstream>
#include <chrono>

using Eigen::VectorXd;
using Eigen::MatrixXd;
using namespace LBFGSpp;

typedef double Scalar;
typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> Vector;

class Timer
{
public:
    void start()
    {
        m_StartTime = std::chrono::system_clock::now();
        m_bRunning = true;
    }

    void stop()
    {
        m_EndTime = std::chrono::system_clock::now();
        m_bRunning = false;
    }

    double elapsedMilliseconds()
    {
        std::chrono::time_point<std::chrono::system_clock> endTime;

        if(m_bRunning)
        {
            endTime = std::chrono::system_clock::now();
        }
        else
        {
            endTime = m_EndTime;
        }

        return std::chrono::duration_cast<std::chrono::milliseconds>(endTime - m_StartTime).count();
    }

    double elapsedSeconds()
    {
        return elapsedMilliseconds() / 1000.0;
    }

private:
    std::chrono::time_point<std::chrono::system_clock> m_StartTime;
    std::chrono::time_point<std::chrono::system_clock> m_EndTime;
    bool                                               m_bRunning = false;
};

class SegmentCCDF {
private:
    std::vector<long> occCounts;
    std::vector<long> occCountFrequency;
public:
    SegmentCCDF(
            std::vector<long>&& occCounts_,
            std::vector<long>&& occCountFrequency_
            ) {
        occCounts = std::move(occCounts_);
        occCountFrequency = std::move(occCountFrequency_);
    }
    void compute(double bias, double* totalResult, double* gradientResult) const {
        size_t n = occCounts.size();
        double total = 0;
        double totalDeriv = 0;
        for (size_t i = 0; i < n; i++) {
            if (occCounts[i] > bias) {
                total += (occCounts[i] - bias) * occCountFrequency[i];
                totalDeriv -= occCountFrequency[i];
            }
        }
        *totalResult = total;
        *gradientResult = totalDeriv;
    }
    void print() const {
        for (long x : occCounts) {
            std::cout << x << " ";
        }
        std::cout << std::endl;
        for (long x : occCountFrequency) {
            std::cout << x << " ";
        }
        std::cout << std::endl;
    }
};

class RMSErrorFunction
{
private:
    std::vector<SegmentCCDF> segments;
    std::vector<int> segmentSpaces;
public:
    RMSErrorFunction(
            std::vector<SegmentCCDF>&& segments_,
            std::vector<int>&& segmentSpaces_
            ) {
        segments = std::move(segments_);
        segmentSpaces = std::move(segmentSpaces_);
    }

    Scalar operator()(const Vector& xBuffer, Vector& grad)
    {
        int nSeg = segments.size();
        double biasTerm = 0;
        double varTerm = 0;

        double ni, dni;
        for (int i = 0; i < nSeg; i++) {
            double x = xBuffer[i];
            const SegmentCCDF& segCDF = segments[i];
//            std::cout << "cdf: " << i << std::endl;
//            segCDF.print();
            segCDF.compute(x, &ni, &dni);

            biasTerm += x;
            double scaledTotal = ni / segmentSpaces[i];
            varTerm += .25*scaledTotal*scaledTotal;
            grad[i] = .5*scaledTotal*dni/segmentSpaces[i];
        }

        for (int i = 0; i < nSeg; i++) {
            grad[i] += 2*biasTerm;
        }
        return biasTerm*biasTerm + varTerm;
    }

    int dim() {
        return segmentSpaces.size();
    }
};

RMSErrorFunction testFunction(int n) {
    std::vector<long> occCounts {1, 2, 3, 5};
    std::vector<long> occCountFreqs {50, 30, 10, 10};

    SegmentCCDF ccdf (
            std::move(occCounts),
            std::move(occCountFreqs)
            );

    std::vector<SegmentCCDF> segments;
    std::vector<int> segmentSpaces;
    for (int i = 0; i < n; i++) {
        segments.push_back(ccdf);
        segmentSpaces.push_back(5);
    }
    return RMSErrorFunction(
            std::move(segments),
            std::move(segmentSpaces)
            );
}

RMSErrorFunction parseFile(const std::string& filePath) {
    std::vector<int> segmentSpaces;
    std::vector<SegmentCCDF> segments;

    std::ifstream infile;
    infile.open(filePath.c_str());

    std::string line;
    std::istringstream iss;
    long number;
    int numInt;

    std::getline(infile, line);
    iss = std::istringstream(line);
    while (iss >> numInt) {
        segmentSpaces.push_back(numInt);
    }

    int numSegments = segmentSpaces.size();

    for (int i = 0; i < numSegments; i++) {
        std::getline(infile, line);
        iss = std::istringstream(line);
        std::vector<long> occCounts;
        occCounts.reserve(line.size());
        while (iss >> number) {
            occCounts.push_back(number);
        }

        std::getline(infile, line);
        iss = std::istringstream(line);
        std::vector<long> occCountFrequency;
        occCountFrequency.reserve(occCounts.size());
        while (iss >> number) {
            occCountFrequency.push_back(number);
        }

        segments.emplace_back(
                std::move(occCounts),
                std::move(occCountFrequency)
                );
    }

    return RMSErrorFunction(
            std::move(segments),
            std::move(segmentSpaces)
            );
}

int main(int argc, char *argv[])
{
    LBFGSBParam<double> param;
    param.max_iterations = 30;
    LBFGSBSolver<double> solver(param);

//    const int n = 10;
//    RMSErrorFunction fun = testFunction(n);
    Timer parseTimer;
    parseTimer.start();
    RMSErrorFunction fun = parseFile(argv[1]);
    parseTimer.stop();
    std::cerr << "Parse Time: " << parseTimer.elapsedMilliseconds() << std::endl;
    int n = fun.dim();

    Vector lb = Vector::Constant(n, 0.0);
    Vector ub = Vector::Constant(n, std::numeric_limits<Scalar>::infinity());
    VectorXd x = VectorXd::Zero(n);
    double fx = 0;

    VectorXd g = VectorXd::Zero(n);
    int niter = solver.minimize(fun, x, fx, lb, ub);

//    std::cout << niter << " iterations" << std::endl;
//    std::cout << "x = \n" << x.transpose() << std::endl;
//    std::cout << "f(x) = " << fx << std::endl;
    std::cout << x.transpose() << std::endl;
    return 0;
}


/* Copyright (C) 2017 IBM Corp.
 *  Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *     http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific
 * language governing permissions and limitations under the License.
 */
/*******************************************************************
Gaussian1Dsampler: defines a 1D pseudo-random number sampler, either
using Martin Albresht's online library or implementing two optional
methods, Box–Muller transform Marsaglia polar method
********************************************************************/
#include <NTL/ZZ.h>
#include <NTL/mat_lzz_p.h>
#include <limits>
#include <NTL/BasicThreadPool.h>
#include <NTL/ZZ.h>
#include <NTL/FFT.h>
#include <NTL/SmartPtr.h>

NTL_CLIENT
#include "mat_l.h"
#include "vec_l.h"
#include "DGaussSampler.h"
#include "utils/tools.h"

#ifdef NTL_HAVE_AVX
#warning "HAVE_AVX"

#include <immintrin.h>

#ifdef NTL_HAVE_FMA
#define MUL_ADD(a, b, c) a = _mm256_fmadd_pd(b, c, a)
#else
#define MUL_ADD(a, b, c) a = _mm256_add_pd(a, _mm256_mul_pd(b, c))
#endif
#endif

// Two static variables for debugging purposes
std::atomic<double> Gaussian1Dsampler::maxSigma(0.0);// keep largest stdev we've seen
std::atomic<int> Gaussian1Dsampler::maxSample(0);   // keep largest sample ever drawn

long Gaussian1Dsampler::writeToFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fwrite(&sigma,sizeof(sigma),1,handle);

    return count;
}

long Gaussian1Dsampler::readFromFile(FILE* handle)
{
    FHE_TIMER_START;
    long count = fread(&sigma,sizeof(sigma),1,handle);

    return count;
}

#ifdef CONTINUOUS_GAUSSIAN_SAMPLING
/* generateGaussianNoiseMars uses Marsaglia polar method
 * to generate Gaussian samples
 */
NTL::Pair<double,double>
Gaussian1Dsampler::generateGaussianNoiseMars(double mean, double stdDev)
{
    FHE_TIMER_START;
    double u, v, s;
    do    // choose (u,v) in the square [-1,1] x [-1,1]
    {
        u = (NTL::RandomBnd(NTL_SP_BOUND)/((double)NTL_SP_BOUND)) *2.0 -1.0;
        v = (NTL::RandomBnd(NTL_SP_BOUND)/((double)NTL_SP_BOUND)) *2.0 -1.0;
        s = u * u + v * v;
    }
    while( (s >= 1.0) || (s == 0.0) );// until you find a pair in the unit sphere

    s = sqrt(-2.0 * log(s) / s);
    double sample1 = mean + stdDev * u * s;
    double sample2 = mean + stdDev * v * s;

    return NTL::Pair<double,double>(sample1,sample2);
}

/* generateGaussianNoiseBox uses Box-Muller to generate Gaussian Samples
 */
NTL::Pair<double,double>
Gaussian1Dsampler::generateGaussianNoiseBox(double mu, double stDev)
{
    FHE_TIMER_START;
    const double epsilon = std::numeric_limits<double>::min();
    const double two_pi = 2.0*3.14159265358979323846;

    double u1, u2;
    do
    {
        u1 = ((NTL::RandomBnd(NTL_SP_BOUND))/((double)NTL_SP_BOUND));
        u2 = ((NTL::RandomBnd(NTL_SP_BOUND))/((double)NTL_SP_BOUND));
    }
    while ( u1 <= epsilon );

    double z0 = sqrt(-2.0 * log(u1)) * cos(two_pi * u2);
    double z1 = sqrt(-2.0 * log(u1)) * sin(two_pi * u2);

    double sample1 = z0 * stDev + mu;
    double sample2 = z1 * stDev + mu;

    return NTL::Pair<double,double>(sample1,sample2);
}
#endif // ifdef CONTINUOUS_GAUSSIAN_SAMPLING


#include "HasseLiftExt.h"
#include "Util.h"
#include "BaseChange.h"
#include "MultipointEval.h"
#include "FrobComp.h"
#include "MultiComposeMod.h"
#include "BalancedMul.h"
#include "HasseLift.h"
#include <NTL/matrix.h>
#include <cmath>
#include <NTL/ZZX.h>

HasseLiftExt::HasseLiftExt(const ZZ_pX &g,
                           const ZZ_pX &delta,
                           const ZZ_pXModulus &F) {
    this->g = g;
    this->delta = delta;
    this->F = F;
    PowerXMod(this->frobenius, ZZ_p::modulus(), F);
}

HasseLiftExt::~HasseLiftExt() {
    this->g.kill();
    this->delta.kill();
    this->F.f.kill();
    this->frobenius.kill();
    this->evalModulus.f.kill();
}

void HasseLiftExt::computeNaive(ZZ_pX& result, long n) {
    long l = (long) pow(n, BETA);
    long m = n / l;
    n = m * l + l + 1;

    ZZ_pX r0;
    ZZ_pX r1;
    ZZ_pX gTemp;
    ZZ_pX deltaTemp;
    ZZ_pX xq;
    ZZ_pX x;
    ZZ_pX temp1;
    ZZ_pX temp2;

    set(r0);
    r1 = g;
    PowerMod(gTemp, g, ZZ_p::modulus(), F);
    deltaTemp = delta;
    SetX(x);
    xq = this->frobenius;

    for (long i = 2; i <= n; i++) {
        sub(temp1, xq, x);
        MulMod(temp1, temp1, deltaTemp, F);
        MulMod(temp1, temp1, r0, F);
        MulMod(temp2, gTemp, r1, F);
        r0 = r1;
        sub(r1, temp2, temp1);

        PowerMod(gTemp, gTemp, ZZ_p::modulus(), F);
        PowerMod(deltaTemp, deltaTemp, ZZ_p::modulus(), F);
        PowerMod(xq, xq, ZZ_p::modulus(), F);
    }

    result = r1;

    r0.kill();
    r1.kill();
    gTemp.kill();
    deltaTemp.kill();
    xq.kill();
    x.kill();
    temp1.kill();
    temp2.kill();
}

void HasseLiftExt::compute(ZZ_pX& result, long n, int verbose) {

    long l = (long) pow(n, BETA);
    long m = n / l;

    ZZ_pE::init(F.f);

    Mat<ZZ_pEX> B;
    B.SetDims(2, 2);
    buildInitialMatrix(B);

    Util util;
    long start = util.getTimeMillis();
    // compute x, tau^{-l}(x), tau^{-2l}(x), ..., tau^{-lm}(x)
    Vec<ZZ_pX> inverseFrobs;
    inverseFrobs.SetLength(m + 1);
    FrobComp frobComp;
    frobComp.computeInverseFrobes(inverseFrobs, m, l, frobenius, F);
    buildEvalModulus(inverseFrobs);
    if (verbose == 1)
        cout << "computeInverseFrobes: " << util.getTimeMillis() - start << endl;

    start = util.getTimeMillis();
    // compute the product tau^{l - 1}(A)...tau(A)A
    computeFrobProduct(B, B, l);
    if (verbose == 1)
        cout << "computeFrobProduct: " << util.getTimeMillis() - start << endl;

    start = util.getTimeMillis();
    // compute B(inverseFrobs[0]), ..., B(inverseFrobs[m])
    Mat<Vec < ZZ_pX>> BEval;
    BEval.SetDims(2, 2);
    evaluate(BEval, B, inverseFrobs);
    if (verbose == 1)
        cout << "evaluate BEval: " << util.getTimeMillis() - start << endl;

    start = util.getTimeMillis();
    // compute the product tau^{lm}(BEval(i))...tau^{l}(BEval(i)) BEval(i)
    Mat<ZZ_pX> frobProduct;
    frobProduct.SetDims(2, 2);
    computeFrobProduct(frobProduct, BEval, l);
    if (verbose == 1)
        cout << "computeFrobProduct2: " << util.getTimeMillis() - start << endl;

    MulMod(result, frobProduct[1][1], g, F);
    add(result, result, frobProduct[1][0]);

    B.kill();
    inverseFrobs.kill();
    BEval.kill();
    frobProduct.kill();
}

void HasseLiftExt::buildEvalModulus(const Vec<ZZ_pX> &inverseFrobs) {
    long m = inverseFrobs.length();
    Vec<ZZ_pEX> tempVec;
    tempVec.SetLength(m);

    for (long i = 0; i < m; i++) {
        clear(tempVec[i]);
        SetCoeff(tempVec[i], 0, -to_ZZ_pE(inverseFrobs[i]));
        SetCoeff(tempVec[i], 1, 1);
    }

    BalancedMul balancedMul;
    balancedMul.compute(tempVec);
    evalModulus = tempVec[0];
    tempVec.kill();
}

void HasseLiftExt::evaluate(Mat<Vec<ZZ_pX> >& result,
                            const Mat<ZZ_pEX>& A,
                            const Vec<ZZ_pX>& points) {

    MultipointEval multiPointEval(points);

    for (int i = 0; i < 2; i++) {
        for (int j = 0; j < 2; j++) {

            result[i][j].SetLength(points.length());
            multiPointEval.eval(result[i][j], A[i][j]);
        }
    }
}

void HasseLiftExt::buildInitialMatrix(Mat<ZZ_pEX>& result) {
    clear(result[0][0]);
    set(result[0][1]);

    ZZ_pX temp;

    MulMod(temp, frobenius, delta, F);
    NTL::negate(temp, temp);
    clear(result[1][0]);
    SetCoeff(result[1][0], 0, to_ZZ_pE(temp));
    SetCoeff(result[1][0], 1, to_ZZ_pE(delta));

    MultiComposeMod().compose(temp, g, frobenius, F);
    clear(result[1][1]);
    SetCoeff(result[1][1], 0, to_ZZ_pE(temp));

    temp.kill();
}

//void HasseLiftExt::computeFrobProduct(Mat<ZZ_pEX>& result,
//                                      const Mat<ZZ_pEX>& A,
//                                      long l) {
//    Vec<ZZ_pX> frobCompos;
//    frobCompos.SetLength(3);
//    frobCompos[0] = coeff(A[1][0], 1)._ZZ_pE__rep;
//    frobCompos[1] = coeff(A[1][1], 0)._ZZ_pE__rep;
//    frobCompos[2] = coeff(A[1][0], 0)._ZZ_pE__rep;
//
//    Mat<ZZ_p> frobMat;
//    ZZ_pXMultiplier frobMultiplier;
//    MultiComposeMod multiComposeMod;
//    multiComposeMod.precompute(frobMat, frobMultiplier, 3, frobenius, F);
//
//    Vec<Mat<ZZ_pEX>> tempVec;
//    tempVec.SetLength(l);
//
//    for (long i = 0; i < l; i++)
//        tempVec[i].SetDims(2, 2);
//
//    tempVec[l - 1] = A;
//
//    for (long i = l - 2; i >= 0; i--) {
//        multiComposeMod.compose(frobCompos, frobCompos, frobMat,
//                                frobMultiplier, F);
//
//        clear(tempVec[i][0][0]);
//        set(tempVec[i][0][1]);
//        clear(tempVec[i][1][0]);
//        clear(tempVec[i][1][1]);
//        SetCoeff(tempVec[i][1][0], 1, to_ZZ_pE(frobCompos[0]));
//        SetCoeff(tempVec[i][1][1], 0, to_ZZ_pE(frobCompos[1]));
//        SetCoeff(tempVec[i][1][0], 0, to_ZZ_pE(frobCompos[2]));
//    }
//
//    BalancedMul balancedMul;
//    balancedMul.compute(tempVec, evalModulus);
//    result = tempVec[0];
//
//    frobCompos.kill();
//    frobMat.kill();
//    frobMultiplier.b.kill();
//    for (long i = 0; i < l; i++)
//        tempVec[i].kill();
//    tempVec.kill();
//}

//void HasseLiftExt::computeFrobProduct(Mat<ZZ_pEX>& result,
//                                      const Mat<ZZ_pEX>& A,
//                                      long l) {
//    Vec<ZZ_pX> frobCompos;
//    frobCompos.SetLength(3);
//    frobCompos[0] = coeff(A[1][0], 1)._ZZ_pE__rep;
//    frobCompos[1] = coeff(A[1][1], 0)._ZZ_pE__rep;
//    frobCompos[2] = coeff(A[1][0], 0)._ZZ_pE__rep;
//
//
//    Vec<Mat < ZZ_pEX>> tempVec;
//    tempVec.SetLength(l);
//
//    for (long i = 0; i < l; i++)
//        tempVec[i].SetDims(2, 2);
//
//    tempVec[l - 1] = A;
//
//    Util util;
//    long start = util.getTimeMillis();
//    for (long i = l - 2; i >= 0; i--) {
//        for (long j = 0; j < frobCompos.length(); j++)
//            PowerMod(frobCompos[j], frobCompos[j], ZZ_p::modulus(), F);
//
//        clear(tempVec[i][0][0]);
//        set(tempVec[i][0][1]);
//        clear(tempVec[i][1][0]);
//        clear(tempVec[i][1][1]);
//        SetCoeff(tempVec[i][1][0], 1, to_ZZ_pE(frobCompos[0]));
//        SetCoeff(tempVec[i][1][1], 0, to_ZZ_pE(frobCompos[1]));
//        SetCoeff(tempVec[i][1][0], 0, to_ZZ_pE(frobCompos[2]));
//    }
//    cout << "powering time: " << util.getTimeMillis() - start << endl;
//
//    start = util.getTimeMillis();
//    BalancedMul balancedMul;
//    balancedMul.compute(tempVec, evalModulus);
//    result = tempVec[0];
//    cout << "balanced mul time: " << util.getTimeMillis() - start << endl;
//
//    frobCompos.kill();
//    for (long i = 0; i < l; i++)
//        tempVec[i].kill();
//    tempVec.kill();
//}

void HasseLiftExt::computeFrobProduct(Mat<ZZ_pEX>& result,
                                      const Mat<ZZ_pEX>& A,
                                      long l) {

    Vec<ZZ_pEX> tempVec;
    Mat<ZZ_pEX> tempMat;

    tempVec.SetLength(2);
    tempVec[0] = A[1][0];
    tempVec[1] = A[1][1];
    tempMat = A;

    BalancedMul balancedMul;
    for (long i = l - 2; i >= 0; i--) {

        SetCoeff(tempVec[0], 0, power(coeff(tempVec[0], 0), ZZ_p::modulus()));
        SetCoeff(tempVec[0], 1, power(coeff(tempVec[0], 1), ZZ_p::modulus()));
        SetCoeff(tempVec[1], 0, power(coeff(tempVec[1], 0), ZZ_p::modulus()));

        balancedMul.mul_ZZ_pEXMatSpec(tempMat, tempVec, tempMat, evalModulus);
    }

    result = tempMat;

    tempVec.kill();
    tempMat.kill();
}

void HasseLiftExt::computeFrobProduct(Mat<ZZ_pX> &result,
                                      Mat<Vec<ZZ_pX>> &B,
                                      long l) {
    long m = B[0][0].length();
    ZZ_pX xqInit;
    SetX(xqInit);
    FrobComp frobComp;
    frobComp.computeFrobPower(xqInit, xqInit, l, frobenius, F);

    // compute tau^2, tau^4, ..., tau^{2^k}
    // where k = floor(log m)
    Vec<ZZ_pX> frobPowers;
    frobPowers.SetLength(NumBits(m - 1));
    frobPowers[0] = xqInit;
    MultiComposeMod multiComposeMod;
    for (long i = 1; i < frobPowers.length(); i++)
        multiComposeMod.compose(frobPowers[i], frobPowers[i - 1],
                                frobPowers[i - 1], F);

    for (int i = 0; i < 2; i++)
        for (int j = 0; j < 2; j++)
            actFrobPowers(B[i][j], frobPowers);

    Mat<ZZ_pX> tempMat;
    tempMat.SetDims(2, 2);

    result[0][0] = B[0][0][0];
    result[0][1] = B[0][1][0];
    result[1][0] = B[1][0][0];
    result[1][1] = B[1][1][0];

    BalancedMul balancedMul;
    for (long i = 1; i < m; i++) {
        for (int j = 0; j < 2; j++)
            for (int k = 0; k < 2; k++) {
                tempMat[j][k] = B[j][k][i];
            }

        balancedMul.mul_ZZ_pXMat(result, tempMat, result, F);
    }

    xqInit.kill();
    frobPowers.kill();
    tempMat.kill();
}

void HasseLiftExt::actFrobPowers(Vec<ZZ_pX> &vec,
                                 const Vec<ZZ_pX> &frobPowers) {
    long n = vec.length();
    long m = 1;
    long frobIndex = 0;

    Vec<ZZ_pX> tempVec;
    MultiComposeMod multiComposeMod;

    while (m < n) {
        for (long i = 0; i < n; i++) {
            if ((i & m) != 0)
                tempVec.append(vec[i]);
        }

        multiComposeMod.compose(tempVec, tempVec, frobPowers[frobIndex], F);

        long k = 0;
        for (long i = 0; i < n; i++) {
            if ((i & m) != 0) {
                vec[i] = tempVec[k];
                k++;
            }
        }

        tempVec.SetLength(0);
        m <<= 1;
        frobIndex++;
    }

    tempVec.kill();
}


/* =========================================================================
   Copyright (c) 2010-2016, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example iterative-armadillo.cpp
*
*   The following tutorial shows how to use the iterative solvers in ViennaCL with objects from the Eigen Library directly.
*
*   \note Eigen provides its own iterative solvers in the meanwhile. Check these first.
*
*   We begin with including the necessary headers:
**/

// System headers
#include <iostream>

#ifndef NDEBUG
 #define NDEBUG
#endif

// Armadillo headers (disable BLAS and LAPACK to avoid linking issues)
#define ARMA_DONT_USE_BLAS
#define ARMA_DONT_USE_LAPACK
#include <armadillo>

// IMPORTANT: Must be set prior to any ViennaCL includes if you want to use ViennaCL algorithms on Armadillo objects
#define VIENNACL_WITH_ARMADILLO 1

// ViennaCL headers
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/linalg/gmres.hpp"
#include "viennacl/io/matrix_market.hpp"

// Some helper functions for this tutorial:
#include "vector-io.hpp"

/**
*  In the following we run the CG method, the BiCGStab method, and the GMRES method with Armadillo types directly.
*  First, the matrices are set up, then the respective solvers are called.
**/
int main(int, char *[])
{
  typedef float ScalarType;

  /**
  * Read system from file. This is a little tricky, since Armadillo does not provide a fast enough element-insertion.
  * Therefore, we read the matrix market file to an STL-matrix and then pass the data on when creating the Armadillo sparse matrix object.
  **/
  std::vector<std::map<unsigned int, ScalarType> > stl_matrix;
  std::cout << "Reading matrix (this might take some time)..." << std::endl;
  if (!viennacl::io::read_matrix_market_file(stl_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file. Make sure you run from the build/-folder." << std::endl;
    return EXIT_FAILURE;
  }

  // Copy over to Armadillo sparse matrix by putting the indices into a matrix and the values into a vector:
  std::size_t num_nnz = 0;
  for (std::size_t i=0; i<stl_matrix.size(); ++i)
    num_nnz += stl_matrix[i].size();

  arma::Mat<arma::uword>  arma_indices(2, num_nnz);
  arma::Col<ScalarType>   arma_values(num_nnz);

  std::size_t index = 0;
  for (std::size_t i=0; i<stl_matrix.size(); ++i)
  {
    for (std::map<unsigned int, ScalarType>::const_iterator it = stl_matrix[i].begin(); it != stl_matrix[i].end(); ++it)
    {
      arma_indices(0, index) = i;
      arma_indices(1, index) = it->first;
      arma_values(index) = it->second;
      ++index;
    }
  }
  std::cout << "Done: reading matrix" << std::endl;

  /**
  * Initialize Armadillo types for iterative solvers
  **/
  arma::SpMat<ScalarType> arma_matrix(arma_indices, arma_values, 65025, 65025);
  arma::Col<ScalarType>   arma_rhs;
  arma::Col<ScalarType>   arma_result;
  arma::Col<ScalarType>   residual;

  /**
   * Read the right hand side as well as the result vector from files:
   **/
  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", arma_rhs))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return EXIT_FAILURE;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", arma_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return EXIT_FAILURE;
  }

  /**
  *  Conjugate Gradient (CG) solver:
  **/
  std::cout << "----- Running CG -----" << std::endl;
  arma_result = viennacl::linalg::solve(arma_matrix, arma_rhs, viennacl::linalg::cg_tag());

  residual = arma_matrix * arma_result - arma_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(arma_rhs) << std::endl;

  /**
  *  Stabilized Bi-Conjugate Gradient (BiCGStab) solver:
  **/
  std::cout << "----- Running BiCGStab -----" << std::endl;
  arma_result = viennacl::linalg::solve(arma_matrix, arma_rhs, viennacl::linalg::bicgstab_tag());

  residual = arma_matrix * arma_result - arma_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(arma_rhs) << std::endl;

  /**
  *  Generalized Minimum Residual (GMRES) solver:
  **/
  std::cout << "----- Running GMRES -----" << std::endl;
  arma_result = viennacl::linalg::solve(arma_matrix, arma_rhs, viennacl::linalg::gmres_tag());

  residual = arma_matrix * arma_result - arma_rhs;
  std::cout << "Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(arma_rhs) << std::endl;

  /**
  *   That's it. Print a success message and exit.
  **/
  std::cout << std::endl;
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;
  std::cout << std::endl;
}


#ifndef PARMCB_DETAIL_FVS_HPP_
#define PARMCB_DETAIL_FVS_HPP_

//    Copyright (C) Dimitrios Michail 2019 - 2021.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          https://www.boost.org/LICENSE_1_0.txt)

#include <iostream>

#include <boost/scoped_array.hpp>
#include <boost/throw_exception.hpp>
#include <boost/functional/hash.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/property_map/function_property_map.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/heap/pairing_heap.hpp>

namespace parmcb {

    namespace detail {

        template<class Vertex>
        struct LessVertex {
            LessVertex(std::map<Vertex, double> &priority) :
                    priority(priority) {
            }

            bool operator()(const Vertex &v, const Vertex &u) const {
                return priority[v] >= priority[u];
            }

            std::map<Vertex, double> &priority;
        };

    } // detail

    template<class Graph, class VertexOutputIterator>
    void greedy_fvs(const Graph &g, VertexOutputIterator out) {

        typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
        typedef typename boost::graph_traits<Graph>::vertex_iterator VertexIt;
        typedef typename boost::property_map<Graph, boost::vertex_index_t>::type VertexIndexMapType;
        typedef typename boost::heap::pairing_heap<Vertex, boost::heap::compare<detail::LessVertex<Vertex>>>::handle_type HeapHandleType;

        std::size_t n = boost::num_vertices(g);
        std::vector<bool> exists(n);
        std::vector<std::size_t> degree(n);
        std::vector<HeapHandleType> handle(n);
        std::map<Vertex, double> priority;
        boost::heap::pairing_heap<Vertex, boost::heap::compare<detail::LessVertex<Vertex>>> heap(
                detail::LessVertex<Vertex> { priority });

        const VertexIndexMapType &index_map = boost::get(boost::vertex_index, g);
        boost::associative_property_map<std::map<Vertex, double>> priority_map(priority);

        // initialize
        std::deque<Vertex> forRemoval;
        VertexIt vi, viend;
        for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
            auto v = *vi;
            auto vindex = index_map[v];
            auto d = boost::out_degree(v, g);
            exists[vindex] = true;
            if ((degree[vindex] = d) <= 1) {
                forRemoval.push_front(v);
            } else {
                priority[v] = 1.0 / d;
            }
        }

        // cleanup
        // repeatedly remove degree 0 or 1
        while (!forRemoval.empty()) {
            Vertex u = forRemoval.front();
            forRemoval.pop_front();
            auto uindex = index_map[u];
            exists[uindex] = false;

            auto eiRange = boost::out_edges(u, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto w = boost::target(*ei, g);
                auto windex = index_map[w];
                if (!exists[windex]) {
                    continue;
                }
                degree[windex]--;
                if (degree[windex] <= 1) {
                    // collect for removal
                    forRemoval.push_front(w);
                } else {
                    priority[w] = 1.0 / degree[windex];
                }
            }
        }

        // add remaining vertices into the priority queue
        for (boost::tie(vi, viend) = boost::vertices(g); vi != viend; ++vi) {
            auto v = *vi;
            auto vindex = index_map[v];
            if (!exists[vindex]) {
                continue;
            }
            handle[vindex] = heap.push(v);
        }

        // main loop
        while (!heap.empty()) {
            auto v = heap.top();
            auto vindex = index_map[v];
            heap.pop();

            if (!exists[vindex]) {
                continue;
            }

            // add to feedback vertex set
            *out++ = v;

            // remove from graph
            exists[vindex] = false;

            auto eiRange = boost::out_edges(v, g);
            for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                auto u = boost::target(*ei, g);
                auto uindex = index_map[u];
                if (!exists[uindex]) {
                    continue;
                }
                degree[uindex]--;
                if (degree[uindex] <= 1) {
                    // collect for removal
                    forRemoval.push_front(u);
                } else {
                    priority[u] = 1.0 / degree[uindex];
                    heap.decrease(handle[uindex]);
                }
            }

            // cleanup
            while (!forRemoval.empty()) {
                Vertex u = forRemoval.front();
                forRemoval.pop_front();
                auto uindex = index_map[u];
                exists[uindex] = false;

                eiRange = boost::out_edges(u, g);
                for (auto ei = eiRange.first; ei != eiRange.second; ++ei) {
                    auto w = boost::target(*ei, g);
                    auto windex = index_map[w];
                    if (!exists[windex]) {
                        continue;
                    }
                    degree[windex]--;
                    if (degree[windex] <= 1) {
                        // collect for removal
                        forRemoval.push_front(w);
                    } else {
                        priority[w] = 1.0 / degree[windex];
                        heap.decrease(handle[windex]);
                    }

                }

            }

        }

    }

} // parmcb

#endif


// Copyright (C) 2011-2012 The Trustees of Indiana University.

// Use, modification and distribution is subject to 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)

//  Authors: Jeremiah Willcock
//           Andrew Lumsdaine

#ifndef BOOST_SPLITTABLE_ECUYER1988_HPP
#define BOOST_SPLITTABLE_ECUYER1988_HPP

#include <boost/cstdint.hpp>
#include <boost/iterator.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include <utility>

namespace boost { namespace graph { namespace random {

    template <typename Base> // To get around incomplete class issues
    class generic_split_iterator: public boost::iterator_facade<generic_split_iterator<Base>, Base, std::input_iterator_tag, Base> {
      public:
      Base base;
      uint32_t new_multiplier1, new_multiplier2;

      generic_split_iterator(Base base, uint32_t new_multiplier1, uint32_t new_multiplier2)
	: base(base), new_multiplier1(new_multiplier1), new_multiplier2(new_multiplier2) {}

      generic_split_iterator(): base(), new_multiplier1(0), new_multiplier2(0) {}

      Base dereference() const {
	Base result = base;
	result.multiplier1 = new_multiplier1;
	result.multiplier2 = new_multiplier2;
	return result;
      }

      bool equal(const generic_split_iterator& it) const {
	return base.state1 == it.base.state1 && base.state2 == it.base.state2;
      }

      void increment() {
	base(); // Advance seeds
      }
    };

    class splittable_ecuyer1988 {
      uint32_t state1, state2, multiplier1, multiplier2;

      static const uint64_t modulus1 = 2147483563;
      static const uint64_t modulus2 = 2147483399;

      template <uint64_t Modulus>
      static uint32_t mul_mod(uint32_t a, uint32_t b) {
	return uint32_t((uint64_t(a) * b) % Modulus);
      }

      template <uint64_t Modulus>
      static uint32_t exp_mod(uint32_t a, uint32_t b) {
	if (b == 0) {
	  return 1;
	} else if (b == 1) {
	  return a;
	} else {
	  uint32_t temp = exp_mod<Modulus>(a, b>>1);
	  temp = mul_mod<Modulus>(temp, temp);
	  if (b % 2) temp = mul_mod<Modulus>(a, temp);
	  return temp;
	}
      }

      public:
      splittable_ecuyer1988(): state1(1), state2(1), multiplier1(40014), multiplier2(40692) {}
      splittable_ecuyer1988(uint32_t seed1, uint32_t seed2): state1(seed1), state2(seed2), multiplier1(40014), multiplier2(40692) {}

      void seed(uint32_t seed1, uint32_t seed2) {
	state1 = seed1;
	state2 = seed2;
      }

      void split(splittable_ecuyer1988& a, splittable_ecuyer1988& b) const {
	a = *this;
	a.multiplier1 = mul_mod<modulus1>(a.multiplier1, a.multiplier1);
	a.multiplier2 = mul_mod<modulus2>(a.multiplier2, a.multiplier2);
	b = *this;
	b();
	b.multiplier1 = a.multiplier1;
	b.multiplier2 = a.multiplier2;
      }

      typedef generic_split_iterator<splittable_ecuyer1988> split_iterator;
      typedef std::pair<split_iterator, split_iterator> split_iterator_pair;
      friend class generic_split_iterator<splittable_ecuyer1988>;

      split_iterator_pair split_n(uint32_t n) const { // This RNG is not usable after the split
	split_iterator begin(*this, exp_mod<modulus1>(multiplier1, n), exp_mod<modulus2>(multiplier2, n));
	split_iterator end(*this, begin.new_multiplier1, begin.new_multiplier2);
	end.base.state1 = mul_mod<modulus1>(end.base.state1, end.new_multiplier1);
	end.base.state2 = mul_mod<modulus2>(end.base.state2, end.new_multiplier2);
	return std::make_pair(begin, end);
      }

      split_iterator_pair split_off_n(uint32_t n) { // Allows this RNG to keep being used
	split_iterator_pair p = split_n(n + 1);
	*this = *p.first;
	++p.first;
	return p;
      }

      typedef uint32_t result_type;

      uint32_t operator()() {
	state1 = mul_mod<modulus1>(state1, multiplier1);
	state2 = mul_mod<modulus2>(state2, multiplier2);
	return (state1 + modulus1 - 1 - state2) % (modulus1 - 1);
      }

      BOOST_STATIC_CONSTANT(bool, has_fixed_range = true);
      BOOST_STATIC_CONSTANT(uint32_t, min_value = 0);
      BOOST_STATIC_CONSTANT(uint32_t, max_value = modulus1 - 1);
      uint32_t min BOOST_PREVENT_MACRO_SUBSTITUTION () const {return min_value;}
      uint32_t max BOOST_PREVENT_MACRO_SUBSTITUTION () const {return max_value;}

      static bool validation(uint32_t x) {
	return x == 831582319;
      }
    };

    template <typename Gen>
    class uniform_01_wrapper {
      Gen& gen;

      public:
      uniform_01_wrapper(Gen& gen): gen(gen) {}

      double operator()() const {return boost::uniform_01<double>()(gen);}
    };

} } } // end namespace boost::graph::random

#endif // BOOST_SPLITTABLE_ECUYER1988_HPP


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MAX_OF_SUMS_INCLUDE
#define MTL_MAX_OF_SUMS_INCLUDE

#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

#include <numeric>
#include <cmath>

namespace mtl { namespace impl {

// We need property map of the minor index
template <typename Matrix, typename MinorIndex>
typename RealMagnitude<typename Collection<Matrix>::value_type>::type
inline max_of_sums(const Matrix& matrix, bool aligned, MinorIndex minor_index, std::size_t dim2)
{
    vampir_trace<2012> tracer;
    using std::max; using std::abs; using math::zero;

    typedef typename Collection<Matrix>::value_type   value_type;
    typedef typename RealMagnitude<value_type>::type  real_type;
    real_type ref, my_zero= zero(ref);

    // If matrix is empty then the result is the identity from the default-constructed value
    if (num_rows(matrix) == 0 || num_cols(matrix) == 0)
	return my_zero;

    typedef typename traits::range_generator<tag::major, Matrix>::type     cursor_type;
    typedef typename traits::range_generator<tag::nz, cursor_type>::type   icursor_type;
    typename traits::const_value<Matrix>::type                             value(matrix);

    if (aligned) {
	real_type maxv= my_zero;
	for (cursor_type cursor = begin<tag::major>(matrix), cend = end<tag::major>(matrix); cursor != cend; ++cursor) {
	    real_type sum= my_zero;
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		sum+= abs(value(*icursor));
	    maxv= max(maxv, sum);
	}
	return maxv;
    }

    // If matrix has other orientation, we compute all sums in a vector
    dense_vector<real_type>   sums(dim2, my_zero);
    for (cursor_type cursor = begin<tag::major>(matrix), cend = end<tag::major>(matrix); cursor != cend; ++cursor)
	for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
	    sums[minor_index(*icursor)]+= abs(value(*icursor));
    // replace by mtl::accumulate<8>
    return std::accumulate(sums.begin(), sums.end(), my_zero, math::max<real_type>());
}

}} // namespace mtl::impl

#endif // MTL_MAX_OF_SUMS_INCLUDE


//---------------------------------------------------------------------------//
//!
//! \file   Utility_GaussKronrodQuadratureSetTraits.hpp
//! \author Luke Kersting
//! \brief  Gauss-Kronrod quadrature set traits
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_GAUSS_KRONROD_QUADRATURE_SET_TRAITS_HPP
#define UTILITY_GAUSS_KRONROD_QUADRATURE_SET_TRAITS_HPP

// Boost Includes
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_floating_point.hpp>

// FRENSIE Includes
#include "Utility_GaussKronrodQuadratureSetTraitsDecl.hpp"
#include "Utility_IsFloatingPoint.hpp"

namespace Utility{

// Gauss-Kronrod quadrature set traits 15 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<15,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 21 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<21,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 31 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<31,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 41 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<41,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 51 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<51,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

// Gauss-Kronrod quadrature set traits 61 point rule
template<typename FloatType>
struct GaussKronrodQuadratureSetTraits<61,FloatType,typename boost::enable_if<IsFloatingPoint<FloatType> >::type>
{
  // Valid rule
  static const bool valid_rule = true;

  // Gauss quadrature weights
  static const std::vector<FloatType> gauss_weights;

  // Kronrod quadrature weights
  static const std::vector<FloatType> kronrod_weights;

  // Kronrod quadrature abscissae
  static const std::vector<FloatType> kronrod_abscissae;

private:

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeGaussWeights();

  // Initialize the kronrod weight array
  static std::vector<FloatType> initializeKronrodWeights();

  // Initialize the gauss weight array
  static std::vector<FloatType> initializeKronrodAbscissae();
};

} // end Utility namespace

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_GaussKronrodQuadratureSetTraits_def.hpp"

//---------------------------------------------------------------------------//

#endif // end UTILITY_GAUSS_KRONROD_QUADRATURE_SET_TRAITS_HPP

//---------------------------------------------------------------------------//
// end Utility_GaussKronrodQuadratureSetTraits.hpp
//---------------------------------------------------------------------------//


/*
 *  model_gmm.cpp
 *
 *  Created by Ania M. Kedzierska on 11/11/11.
 *  Copyright 2011 Politecnic University of Catalonia, Center for Genomic Regulation.  This is program can be redistributed, modified or else as given by the terms of the GNU General Public License.
 *
 */

#include "model_gmm.h"
#include "model_ssm.h"
#include "random.h"
#include "parameters.h"
#include <cstdlib>
#include <iostream>
#include <cmath>
#include <algorithm>

#include <boost/math/special_functions/erf.hpp>
#include <boost/numeric/ublas/matrix.hpp>

#include "expm.hpp"

#include "miscelania.h"

#include <stdexcept>

//////////////////////////////////////////////////////////////////
// This file implements functions specific to the GMM model
//////////////////////////////////////////////////////////////////

// Fills in a GMM row
void GMM_row(double a, double b, double c, double d, long r, TMatrix &tm) {
  tm[r][0] = a;
  tm[r][1] = b;
  tm[r][2] = c;
  tm[r][3] = d;
}

long GMM_matrix_structure(long i, long j) {
  return 4*i + j;
}

long GMM_root_structure(long i) {
  return i;
}

Permutation GMM_perm(long a, long b, long c, long d) {
  Permutation p;
  p.resize(4);
  p[0] = a; p[1] = b; p[2] = c; p[3] = d;
  return p;
}

void GMM_list_permutations(std::list<Permutation> &L) {
  L.clear();
  L.push_back(GMM_perm(0,1,2,3));
  L.push_back(GMM_perm(0,1,3,2));
  L.push_back(GMM_perm(0,2,1,3));
  L.push_back(GMM_perm(0,2,3,1));
  L.push_back(GMM_perm(0,3,1,2));
  L.push_back(GMM_perm(0,3,2,1));

  L.push_back(GMM_perm(1,0,2,3));
  L.push_back(GMM_perm(1,0,3,2));
  L.push_back(GMM_perm(1,2,0,3));
  L.push_back(GMM_perm(1,2,3,0));
  L.push_back(GMM_perm(1,3,0,2));
  L.push_back(GMM_perm(1,3,2,0));

  L.push_back(GMM_perm(2,0,1,3));
  L.push_back(GMM_perm(2,0,3,1));
  L.push_back(GMM_perm(2,1,0,3));
  L.push_back(GMM_perm(2,1,3,0));
  L.push_back(GMM_perm(2,3,0,1));
  L.push_back(GMM_perm(2,3,1,0));

  L.push_back(GMM_perm(3,0,1,2));
  L.push_back(GMM_perm(3,0,2,1));
  L.push_back(GMM_perm(3,1,0,2));
  L.push_back(GMM_perm(3,1,2,0));
  L.push_back(GMM_perm(3,2,0,1));
  L.push_back(GMM_perm(3,2,1,0));
}

// Computes the mle for the transition matrix of a given edge from the
// the marginalization of the counts on that edge.
// N is a matrix representing the counts on that edge
// tm is the matrix where the mle is stored. tm must be a stochastic matrix
void GMM_mle_edge(TMatrix &N, TMatrix &tm) {
    double s[4];
    long i,j;

    for (i=0; i < 4; i++) {
      s[i] = 0;
      for(j=0; j < 4; j++) {
        s[i] = s[i] + N[i][j];
      }
    }

    tm[0][0] = N[0][0] / s[0];
    tm[0][1] = N[0][1] / s[0];
    tm[0][2] = N[0][2] / s[0];
    tm[0][3] = N[0][3] / s[0];

    tm[1][0] = N[1][0] / s[1];
    tm[1][1] = N[1][1] / s[1];
    tm[1][2] = N[1][2] / s[1];
    tm[1][3] = N[1][3] / s[1];

    tm[2][0] = N[2][0] / s[2];
    tm[2][1] = N[2][1] / s[2];
    tm[2][2] = N[2][2] / s[2];
    tm[2][3] = N[2][3] / s[2];

    tm[3][0] = N[3][0] / s[3];
    tm[3][1] = N[3][1] / s[3];
    tm[3][2] = N[3][2] / s[3];
    tm[3][3] = N[3][3] / s[3];
}

// Computes the mle for the root distribution of a given edge from the
// marginalization of the counts to the root node.
// s is a vector representing the counts on the root
// r is a vector where the root distribution is stored.
void GMM_mle_root(Root &s, Root &r) {
  long i;
  double sum;
  sum = s[0] + s[1] + s[2] + s[3];

  for (i=0; i < 4; i++) {
    r[i] = s[i] / sum;
  }
}

// Uniform random stochastic vector of length 3.
void GMM_random_stochastic_vector3(std::vector<double> &v) {
  v[1] = uniform_real(0,1);
  v[2] = uniform_real(0,1);

  if (v[1] + v[2] > 1) {
    v[1] = 1 - v[1];
    v[2] = 1 - v[2];
  }

  v[0] = 1 - v[1] - v[2];
}

// Uniform random stochastic vector of length 4.
void GMM_random_stochastic_vector4(std::vector<double> &v) {
  double s;

  // A uniform point with x1 + ... + x3 <= 1 is generated
  // by producing a point with x1 + ... + x3 = 1 and scaling
  // by s, s is distributed according to f(x) = 3x^2 (cdf is F(x)=x^3).
  GMM_random_stochastic_vector3(v);

  s = uniform_real(0,1);
  s = s*s*s;

  for (long i=0; i < 3; i++) {
    v[i] = s*v[i];
  }
  v[3] = 1 - s;
}

// Produces a random GMM root distribution.
void GMM_random_root(Root &r) {
  GMM_random_stochastic_vector4(r);
}

// Produces a random GMM transition matrix
void GMM_random_edge(TMatrix &tm) {
  GMM_random_stochastic_vector4(tm[0]);
  GMM_random_stochastic_vector4(tm[1]);
  GMM_random_stochastic_vector4(tm[2]);
  GMM_random_stochastic_vector4(tm[3]);
}

void GMM_random_rate_matrix(double tr, TMatrix &Q) {
  std::vector<double> v;
  v.resize(Q.size());

  // Fill in the diagonal
  GMM_random_stochastic_vector4(v);
  for(long i=0; i < 4; i++) {
    Q[i][i] = tr*v[i];
  }

  // Complete the rows.
  long k;
  for(long i=0; i < 4; i++) {
    GMM_random_stochastic_vector3(v);
    k=0;
    for(long j=0; j < 4; j++) {
      if (j == i) continue;
      Q[i][j] = -Q[i][i] * v[k];
      k++;
    }
  }
}

// translating transition matrix to 'our' format
void GMM_matrix_exponential(TMatrix &Q, TMatrix &A) {
  using namespace boost::numeric;

  ublas::matrix<double> QQ(4,4);
  ublas::matrix<double> AA(4,4);

  for(unsigned long i = 0; i < Q.size(); i++) {
    for(unsigned long j = 0; j < Q.size(); j++) {
      QQ(i,j) = Q[i][j];
    }
  }

  AA = expm_pad(QQ);

  for(unsigned long i = 0; i < Q.size(); i++) {
    for(unsigned long j = 0; j < Q.size(); j++) {
      A[i][j] = AA(i,j);
    }
  }
}

// Random GMM transition matrix of a given branch length
void GMM_random_edge_length(double len, TMatrix &tm) {
  double t;
  TMatrix Q, A, B;
  Q.resize(tm.size());
  A.resize(tm.size());
  B.resize(tm.size());
  for(unsigned long l=0; l < tm.size(); l++) {
    Q[l].resize(tm.size());
    A[l].resize(tm.size());
    B[l].resize(tm.size());
  }

  t = uniform_real(-4*len, 0);
  GMM_random_rate_matrix(t, Q);
  GMM_matrix_exponential(Q, A);

  SSM_random_edge_length(len + t/4, B);

  for(long i = 0; i < 4; i++) {
    for(long j=0; j < 4; j++) {
      tm[i][j] = 0;
      for(long k=0; k < 4; k++) {
        tm[i][j] = tm[i][j] + B[i][k]*A[k][j];
      }
    }
  }
}

// Generates a biologically meaningful GMM matrix (Chang's DLC condition)
void GMM_random_edge_bio_length(double len, TMatrix &tm) {
  TMatrix tmaux;
  long j, k;
  Permutation p;
  p.resize(4);

  tmaux.resize(4);
  for(j=0; j < 4; j++) {
    tmaux[j].resize(4);
  }

  // Loop until there is a permutation that puts it into the DLC form.
  long timeout=0;
  do {
    GMM_random_edge_length(len, tmaux);

    p[0] = max_in_col(tmaux, 0);
    p[1] = max_in_col(tmaux, 1);
    p[2] = max_in_col(tmaux, 2);
    p[3] = max_in_col(tmaux, 3);

    timeout++;
  } while(!(is_permutation(p) && permutation_sign(p) == 1) && (timeout < 1000));

  if (timeout >= 1000) {
    throw std::length_error("ERROR: In sampling for GMM model. Can't generate DLC matrix of length " );
  }

  for(j=0; j < 4; j++) {
    for(k=0; k < 4; k++) {
      tm[j][k] = tmaux[p[j]][k];
    }
  }
}


/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RWLIBS_ALGORITHMS_QPCONTROLLER_QPSOLVER_HPP
#define RWLIBS_ALGORITHMS_QPCONTROLLER_QPSOLVER_HPP

/**
 * @file QPSolver.hpp
 */

#include <Eigen/Core>

namespace rwlibs { namespace algorithms { namespace qpcontroller {

    /**
     * @brief Class providing an algorithms for solving the quadratic optimization
     * problem associated with the QPController
     */
    class QPSolver
    {
      public:
        /**
         * @brief Enumeration used to indicate status
         */
        enum Status {
            SUCCESS = 0, /* Solved */
            SUBOPTIMAL, /* Constraint satisfied but the result may be suboptimal. This may occur due
                           to round off errors */
            FAILURE     /* Could not find a solution statisfying all the constraints */
        };

        /**
         * Solves the quadratic problem of minimizing 1/2 x^T.G.x+d^T.x subject
         * to A.x>=b The method used is an iterative method from "Numerical
         * Optimization" by Jorge Nocedal and Stephen J. Wright, Springer 1999.
         * In this implementation we'll require that b<= s.t. x=0 is a feasible
         * initial value
         *
         * \param G [in] The G matrix. It is required that G is n times n and is
         * positive semidefinite
         *
         * \param d [in] Vector of length n
         *
         * \param A [in] Matrix used to represent the linear inequality constraints.
         * The dimensions should be m times n
         *
         * \param b [in] Vector with the lower limit for the constraints. We'll
         * assume that b<=0. The length of b should be m
         *
         * \param xstart [in] Default start configuration of the iterative algorithm
         *
         * \param status [out] Gives the status of the solving
         */
        static Eigen::VectorXd inequalitySolve (const Eigen::MatrixXd& G, const Eigen::VectorXd& d,
                                                Eigen::MatrixXd& A, const Eigen::VectorXd& b,
                                                const Eigen::VectorXd& xstart, Status& status);

        // TODO Investigate the possibility of making a hot-start of the
        // algorithm for better performance

      private:
        static Eigen::VectorXd getInitialConfig (Eigen::MatrixXd& A, const Eigen::VectorXd& b);

        static Eigen::VectorXd safeApprox (Eigen::MatrixXd& A, const Eigen::VectorXd& b);
    };

}}}    // namespace rwlibs::algorithms::qpcontroller

#endif    // end include guard


// An RLC bandpass filter using Boost.odeint for simulation and Eigen for circuit manipulation
// Author: Jeff Trull <edaskel@att.net>

/*
Copyright (c) 2014 Jeffrey E. Trull

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

#include <iostream>
#include <array>
#include <cmath>

#include <boost/numeric/odeint.hpp>
#include <boost/math/constants/constants.hpp>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>

#include "mna.hpp"

typedef std::array<double, 2> state_t;   // 0 = V_out, 1 = I_L

struct circuit {
    circuit(double r, double l, double c) : circuit(r, l, c, 0, true) {}

    circuit(double r, double l, double c,
            double freq,        // cycles per second for sine wave
            bool   step=false   // step response instead
        ) : freq_(boost::math::constants::two_pi<double>() * freq),  // radian/s conversion
        step_(step) {

        using namespace Eigen;

        typedef Matrix<double, 5, 5> matrix5_t;
        matrix5_t G = matrix5_t::Zero();
        matrix5_t C = matrix5_t::Zero();

        // state assignment: 0, 1, 2 = node voltages; 3 = I_L, 4 = I_in
        stamp_r(G, C, 2, r);
        stamp_c(G, C, 1, 2, c);
        stamp_l(G, C, 0, 1, 3, l);     // n1, n2, I_L
        stamp_i(G, C, 0, 4);           // V_in, I_in

        // input application vector - connects single input to appropriate equation
        typedef Matrix<double, 5, 1> vector5_t;
        vector5_t B;
        B << 0, 0, 0, 0, -1;

        // output extraction vector - connects state element to output
        Matrix<double, 1, 5> D;
        D << 0, 0, 1, 0, 0;         // state element idx 2 = V_out

        Matrix<double, 1, 1> E = Matrix<double, 1, 1>::Zero();
        Matrix<double, 5, 1> DT = D.transpose();
        auto momentvec = moments(G, C, B, DT, E, 4);

        // Now we have C*dX/dt = -G*X + B*u, and the output is = D * X

        // we could not use Su regularization here because after LU factorization Cprime was
        // not square (UL elements were 2x3 because one row was -1 times another row)
        Matrix<double, Dynamic, Dynamic> Gnew, Cnew;
        Matrix<double, Dynamic, 1> Bnew, Dnew;
        Matrix<double, 1, 1> Enew;
        std::tie(Gnew, Cnew, Bnew, Dnew, Enew) = regularize(G, C, B, DT);

        Matrix<double, Dynamic, 1> DNT = Dnew.transpose();
        momentvec = moments(Gnew, Cnew, Bnew, DNT, E, 4);

        // verify the new C is non-singular
        assert(Cnew.rows() == Cnew.fullPivLu().rank());
        // and there is no feedthrough term
        assert(Enew.isZero());

        // factor Cnew out of our equation by multiplying both sides by Cnew^-1
        // new equation will be dX/dt = - Cnew^-1 * Gnew * X + Cnew^-1 * Bnew * u
        drift_term_ = - Cnew.fullPivLu().solve(Gnew);
        input_term_ =   Cnew.fullPivLu().solve(Bnew);

        // Vout may have been moved in the reduction process, so we must supply a map
        s2o_        = Dnew;

        // calculate poles of resulting system
        // they are the eigenvalues of the drift term (a.k.a. the "system matrix")
        auto evs = EigenSolver<MatrixXd>(drift_term_).eigenvalues();
        std::cerr << "poles are:\n" << std::endl;
        for (int row = 0; row < drift_term_.rows(); ++row) {
            // convert to standard frequency (cycles/s) and display
            std::cerr << evs(row, 0).real() / boost::math::constants::two_pi<double>() << std::endl;
        }

    }

    double state2output(state_t const& x) const {
        Eigen::Map<const Eigen::Matrix<double, 2, 1> > xvec(x.data());

        return (s2o_ * xvec)(0, 0);
    }

    void operator()(state_t const& x, state_t& dxdt, double t) {
        using namespace Eigen;

        Map<const Matrix<double, 2, 1> > xvec(x.data());
        Map<Matrix<double, 2, 1> > result(dxdt.data());

        Matrix<double, 1, 1> input;
        if (step_) {
            input << 1.0;
        } else {
            // calculate current value of sine wave input
            input << std::sin(freq_ * t);
        }

        result = drift_term_ * xvec + input_term_ * input;

    }

private:
    Eigen::MatrixXd drift_term_;            // connects current state to development over time
    Eigen::MatrixXd input_term_;            // connects input to development
    Eigen::Matrix<double, 1, Eigen::Dynamic> s2o_; // transforms state to output
    double freq_;                           // remembers frequency for calculating input
    bool   step_;                           // indicates using step function instead of sine wave
};

int main() {
    using namespace boost::numeric::odeint;
    const double freq = 50e3;
    circuit ckt(100.0, 20e-6, 20e-9, freq);
    state_t x{0.0, 0.0};                    // initial conditions

    integrate( ckt, x, 0.0, 10/freq, 0.1e-6,  // time range and increment
               [ckt,freq](state_t const& x, double t) {
                   double inp = std::sin(freq*boost::math::constants::two_pi<double>()*t);
                   std::cout << t << " " << inp << " " << ckt.state2output(x) << std::endl;
               });
}


#include <boost/python.hpp>
#include <numpy/arrayobject.h>

#include <cassert>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <numpy_eigen.h>
#include <vector>
#include <iostream>

PyObject* ewald_sum(const int &NLa, const double &CLa, const int &NSr, const double &CSr, PyObject *nV, const int &Nxy, const int &Nz)
{
    assert(((PyArrayObject *)nV)->dimensions[0] == NLa + NSr);
    import_array1(NULL);
    double sigma = M_PI;
    int N = NLa + NSr;
    double *C = new double[2*N],
           *pt_nV = (double *) PyArray_DATA((PyArrayObject *) nV);
    double *x = new double[2*N],
           *y = new double[2*N],
           *z = new double[2*N],
           *Vsi = new double[N];
    std::complex<double>  *Vli = new std::complex<double>[N];
    double sum_nV = 0.;
    for (int i = 0; i < N; ++i)
        sum_nV += pt_nV[i];

    for (int i = 0; i < N; ++i) {
        C[i] = pt_nV[i]/sum_nV * (-NLa*CLa - NSr*CSr);
        C[N+i] = (i < NLa) ? CLa : CSr;
        x[i] = y[i] = 0.;
        x[N+i] = y[N+i] = 0.5;
        z[i] = double(i);
        z[N+i] = -0.5+double(i);
        Vsi[i] = 0.;
        Vli[i] = std::complex<double>(0., 0.);
    }

    double e_charge = -1.;
    int Nx = Nxy, Ny = Nxy;
    double *R = new double[2*N],
           kv[3], ksquare = 0.;

    for (int i = 0; i < N; ++i)
        for (int m = -Nx; m < Nx; ++m)
            for (int n = -Ny; n < Ny; ++n)
                for (int l = - Nz; l < Nz; ++l) {
                    for (int k = 0; k < 2*N; ++k)
                        R[k] = sqrt(pow(m+x[k], 2) + pow(n+y[k],2) + pow(N*l+z[i]-z[k], 2));
                    // for short-range terms
                    if (m == 0 and n == 0 and l == 0) {
                        for (int k = 0; k < 2*N; ++k)
                            if (k != i)
                                Vsi[i] += e_charge*C[k] / R[k] * erfc(R[k]/sqrt(2)/sigma);
                        continue;
                    }
                    for (int k = 0; k < 2*N; ++k)
                        Vsi[i] += e_charge * C[k] / R[k] * erfc(R[k]/sqrt(2)/sigma);

                    // for long-range terms
                    kv[0] = 2*M_PI*n;
                    kv[1] = 2*M_PI*m;
                    kv[2] = 2*M_PI/N*l;
                    ksquare = pow(kv[0], 2) + pow(kv[1], 2) + pow(kv[2], 2);
                    for (int k = 0; k < 2*N; ++k)
                        Vli[i] += 4*M_PI/N * e_charge*C[k] / ksquare *
                            exp(std::complex<double>(0., -kv[0]*x[k] - kv[1]*y[k] + kv[2]*(z[i] - z[k]))) * exp(-pow(sigma,2)*ksquare/2.);
                }
    npy_intp Nconvert = npy_intp(N);
    PyArrayObject *result = (PyArrayObject *)PyArray_SimpleNew(1, &Nconvert, NPY_DOUBLE);
    Py_INCREF(result);
    double *result_buf = (double *)PyArray_DATA(result);
    for (int i = 0; i < N; ++i)
        result_buf[i] = Vsi[i] + Vli[i].real() - e_charge*C[i]*sqrt(2/M_PI)*1./sigma;

    delete[] R;
    delete[] C;
    delete[] x;
    delete[] y;
    delete[] z;
    delete[] Vsi;
    delete[] Vli;

    return PyArray_Return(result);
}

using namespace Eigen;

typedef struct {
    Vector3d r; // position in the unitcell
    double C;   // ion charge
    double ne;  // electron occupation
    double V;   // dimensionless electric potential (whose unit is smt like V :)
} AtomInfo;
typedef std::vector<AtomInfo> UnitCell;

void general_ewald_sum(UnitCell &uc, const Vector3d &a1, const Vector3d &a2, const Vector3d &a3, const Vector3i &Rcutoff);

PyObject* new_ewald_sum(const int &NLa, const double &CLa, const int &NSr, const double &CSr, PyObject *nVpy, const int &Nxy, const int &Nz)
{
    try {
        int N = NLa + NSr;
        VectorXd nV;
        numpy::from_numpy(nVpy, nV);

        UnitCell uc(2*N);
        for (int i = 0; i < N; ++i) {
            uc[2*i].C = (i < NLa) ? CLa : CSr;
            uc[2*i].ne = 0;
            uc[2*i].r << 0.5, 0.5, -0.5 + double(i);

            uc[2*i+1].C = 0;
            uc[2*i+1].ne = nV(i);
            uc[2*i+1].r << 0, 0, i;
        }

        Vector3d a1(1,0,0), a2(0,1,0), a3(0,0,N);
        Vector3i Rcutoff(Nxy, Nxy, Nz);
        general_ewald_sum(uc, a1, a2, a3, Rcutoff);

        VectorXd out(N);
        for (int i = 0; i < N; ++i)
            out(i) = -uc[2*i+1].V;
        return numpy::to_numpy(out);
    } catch (const char *str) {
        std::cerr << str << std::endl;
        return Py_None;
    }
}

void general_ewald_sum(UnitCell &uc, const Vector3d &a1, const Vector3d &a2, const Vector3d &a3, const Vector3i &Rcutoff)
{
    int N = uc.size();
    const double sigma = M_PI;

    // renormalize electron occupation
    double unrenorm_Ne = 0, Ne = 0;
    for (UnitCell::iterator it = uc.begin(); it < uc.end(); ++it) {
        unrenorm_Ne += it->ne;
        Ne += it->C;
    }
    for (UnitCell::iterator it = uc.begin(); it < uc.end(); ++it)
        it->ne *= Ne/unrenorm_Ne;

    // get reciprocal vectors
    Vector3d b1, b2, b3;
    double uc_vol = a1.dot(a2.cross(a3));
    b1 = 2*M_PI*a2.cross(a3)/uc_vol;
    b2 = 2*M_PI*a3.cross(a1)/uc_vol;
    b3 = 2*M_PI*a1.cross(a2)/uc_vol;

    // Ewald sum
    double Vs, Vl;
    Vector3d k;
    for (int id = 0; id < N; ++id) {
        Vs = 0; Vl = 0;
        for (int m = -Rcutoff[0]; m < Rcutoff[0]; ++m)
            for (int n = -Rcutoff[1]; n < Rcutoff[1]; ++n)
                for (int l = -Rcutoff[2]; l < Rcutoff[2]; ++l) {
                    k = n*b1 + m*b2 + l*b3;
                    double ksquare = k.dot(k);
                    for (int i = 0; i < N; ++i) {
                        double dR = (uc[id].r - (uc[i].r + m*a1 + n*a2 + l*a3)).norm();
                        // for long-range terms
                        if ( !(m == 0 && n == 0 && l == 0) )
                            Vl += 4*M_PI/uc_vol*(uc[i].C-uc[i].ne)/ksquare * cos(k.dot(uc[id].r-uc[i].r)) * exp(-pow(sigma,2)*ksquare/2.);

                        // for short-range terms
                        if ( !(m == 0 && n == 0 && l == 0 && i == id) )
                            Vs += (uc[i].C - uc[i].ne) / dR * erfc(dR/sqrt(2)/sigma);
                    }
                }
        uc[id].V = Vs + Vl - (uc[id].C - uc[id].ne)*sqrt(2/M_PI)/sigma;
    }
}


/****************************
 * 题目：给定一组世界坐标系下的3D点(p3d.txt)以及它在相机中对应的坐标(p2d.txt)，以及相机的内参矩阵。
 * 使用bundle adjustment 方法（g2o库实现）来估计相机的位姿T。初始位姿T为单位矩阵。
 *
* 本程序学习目标：
 * 熟悉g2o库编写流程，熟悉顶点定义方法。
 *
 * 公众号：计算机视觉life。发布于公众号旗下知识星球：从零开始学习SLAM
 * 时间：2019.02
****************************/

#include <vector>
#include <fstream>
#include <iostream>
#include <opencv2/core/core.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace Eigen;

using namespace cv;
using namespace std;

string p3d_file = "/home/lab-307/LIO-SAM_ws/src/Scout-LIO/test/data/p3d.txt";
string p2d_file = "/home/lab-307/LIO-SAM_ws/src/Scout-LIO/test/data/p2d.txt";

//// 自定义顶点，6DOF的姿态
class myVertex : public g2o::BaseVertex<6, g2o::SE3Quat>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    myVertex() = default;
    bool read(std::istream& is) override{};
    bool write(std::ostream& os) const override{};
    void setToOriginImpl() override {
        //// 初始值设置为 SE3单位矩阵
        _estimate = g2o::SE3Quat();
    }

    void oplusImpl(const number_t* update_) override {
        Eigen::Map<const g2o::Vector6> update(update_);
        setEstimate(g2o::SE3Quat::exp(update)*estimate());        //更新方式
    }
};

//// 自定义边
class myEdge: public g2o::BaseBinaryEdge<2, g2o::Vector2 , g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    myEdge() = default;
    bool read(istream& in) override{};
    bool write(ostream& out) const override{};
    void computeError() override
    {
        const g2o::VertexSE3Expmap* v1 = static_cast<const g2o::VertexSE3Expmap*>(_vertices[1]);
        const g2o::VertexSBAPointXYZ* v2 = static_cast<const g2o::VertexSBAPointXYZ*>(_vertices[0]);
        const g2o::CameraParameters * cam
                = static_cast<const g2o::CameraParameters *>(parameter(0));
        g2o::Vector2 obs(_measurement);
        _error = obs-cam->cam_map(v1->estimate().map(v2->estimate()));
    }

    void linearizeOplus() override
    {
        g2o::VertexSE3Expmap * vj = static_cast<g2o::VertexSE3Expmap *>(_vertices[1]);
        g2o::SE3Quat T(vj->estimate());
        g2o::VertexSBAPointXYZ* vi = static_cast<g2o::VertexSBAPointXYZ*>(_vertices[0]);
        g2o::Vector3 xyz = vi->estimate();
        g2o::Vector3 xyz_trans = T.map(xyz);

        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];
        double z_2 = z*z;

        const g2o::CameraParameters * cam = static_cast<const g2o::CameraParameters *>(parameter(0));

        Matrix<double,2,3,Eigen::ColMajor> tmp;
        tmp(0,0) = cam->focal_length;
        tmp(0,1) = 0;
        tmp(0,2) = -x/z*cam->focal_length;

        tmp(1,0) = 0;
        tmp(1,1) = cam->focal_length;
        tmp(1,2) = -y/z*cam->focal_length;

        _jacobianOplusXi =  -1./z * tmp * T.rotation().toRotationMatrix();

        _jacobianOplusXj(0,0) =  x*y/z_2 *cam->focal_length;
        _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *cam->focal_length;
        _jacobianOplusXj(0,2) = y/z *cam->focal_length;
        _jacobianOplusXj(0,3) = -1./z *cam->focal_length;
        _jacobianOplusXj(0,4) = 0;
        _jacobianOplusXj(0,5) = x/z_2 *cam->focal_length;

        _jacobianOplusXj(1,0) = (1+y*y/z_2) *cam->focal_length;
        _jacobianOplusXj(1,1) = -x*y/z_2 *cam->focal_length;
        _jacobianOplusXj(1,2) = -x/z *cam->focal_length;
        _jacobianOplusXj(1,3) = 0;
        _jacobianOplusXj(1,4) = -1./z *cam->focal_length;
        _jacobianOplusXj(1,5) = y/z_2 *cam->focal_length;
    }
private:
    // data
};

void bundleAdjustment (
        const vector<Point3f> points_3d,
        const vector<Point2f> points_2d,
        Mat& K,
        bool useDefaultEdge
        );

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

    bool useDefaultEdge = true;
    vector< Point3f > p3d;
    vector< Point2f > p2d;

    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );

    // 导入3D点和对应的2D点

    ifstream fp3d(p3d_file);
    if (!fp3d){
        cout<< "No p3d.text file" << endl;
        return -1;
    }
    else {
        while (!fp3d.eof()){
            double pt3[3] = {0};
            for (auto &p:pt3) {
                fp3d >> p;
            }
            p3d.push_back(Point3f(pt3[0],pt3[1],pt3[2]));
        }
    }
    ifstream fp2d(p2d_file);
    if (!fp2d){
        cout<< "No p2d.text file" << endl;
        return -1;
    }
    else {
        while (!fp2d.eof()){
            double pt2[2] = {0};
            for (auto &p:pt2) {
                fp2d >> p;
            }
            Point2f p2(pt2[0],pt2[1]);
            p2d.push_back(p2);
        }
    }

    assert(p3d.size() == p2d.size());

    int iterations = 100;
    double cost = 0, lastCost = 0;
    int nPoints = p3d.size();
    cout << "points: " << nPoints << endl;

    bundleAdjustment ( p3d, p2d, K , useDefaultEdge);
    return 0;
}

void bundleAdjustment (
        const vector< Point3f > points_3d,
        const vector< Point2f > points_2d,
        Mat& K,
        bool useDefaultEdge )
{
    // creat g2o
    // new g2o version. Ref:https://www.cnblogs.com/xueyuanaichiyu/p/7921382.html

    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
    // 第1步：创建一个线性求解器LinearSolver
    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>();

    // 第2步：创建 BlockSolver。并用上面定义的线性求解器初始化
    Block* solver_ptr = new Block (  std::unique_ptr<Block::LinearSolverType>(linearSolver) );

    // 第3步：创建总求解器solver。并从GN, LM, DogLeg 中选一个，再用上述块求解器BlockSolver初始化
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( std::unique_ptr<Block>(solver_ptr) );

    // 第4步：创建稀疏优化器
    g2o::SparseOptimizer optimizer;
    optimizer.setAlgorithm ( solver );
    optimizer.setVerbose(true);
//    // old g2o version
//    typedef g2o::BlockSolver< g2o::BlockSolverTraits<6,3> > Block;  // pose 维度为 6, landmark 维度为 3
//    Block::LinearSolverType* linearSolver = new g2o::LinearSolverCSparse<Block::PoseMatrixType>(); // 线性方程求解器
//    Block* solver_ptr = new Block ( linearSolver );     // 矩阵块求解器
//    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr );
//    g2o::SparseOptimizer optimizer;
//    optimizer.setAlgorithm ( solver );

    // 第5步：定义图的顶点和边。并添加到SparseOptimizer中

    // ----------------------开始你的代码：设置并添加顶点，初始位姿为单位矩阵

//    g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
//    pose->setId(0);
//    pose->setEstimate(g2o::SE3Quat());
//    optimizer.addVertex(pose);
    myVertex *pose = new myVertex();
    pose->setId(0);
    pose->setEstimate(g2o::SE3Quat());
    optimizer.addVertex(pose);

    int index = 1;
    for ( const Point3f p:points_3d )   // landmarks
    {
        g2o::VertexSBAPointXYZ *point = new g2o::VertexSBAPointXYZ();
        point->setId ( index++ );
        point->setEstimate ( Eigen::Vector3d ( p.x, p.y, p.z ) );
        point->setMarginalized ( true );
        optimizer.addVertex ( point );
    }
    // ----------------------结束你的代码

    // 设置相机内参
    g2o::CameraParameters* camera = new g2o::CameraParameters (
            K.at<double> ( 0,0 ), Eigen::Vector2d ( K.at<double> ( 0,2 ), K.at<double> ( 1,2 ) ), 0);
    camera->setId ( 0 );
    optimizer.addParameter ( camera );

    // 设置边
    if (useDefaultEdge){
        index = 1;
        for ( const Point2f p:points_2d )
        {
            g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
            edge->setId ( index );
            edge->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
            edge->setVertex ( 1, pose );
            edge->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  //设置观测值
            edge->setParameterId ( 0,0 );
            edge->setInformation ( Eigen::Matrix2d::Identity() );
            optimizer.addEdge ( edge );
            index++;
        }
    } else {
        index = 1;
        for( const Point2f p:points_2d )
        {
            myEdge *XYZ2UV = new myEdge();
            XYZ2UV->setId(index);
            XYZ2UV->setVertex ( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> ( optimizer.vertex ( index ) ) );
            XYZ2UV->setVertex ( 1, pose );
            XYZ2UV->setMeasurement ( Eigen::Vector2d ( p.x, p.y ) );  //设置观测值
            XYZ2UV->setParameterId ( 0,0 );
            XYZ2UV->setInformation ( Eigen::Matrix2d::Identity() );
            optimizer.addEdge ( XYZ2UV );
            index++;
        }
    }

    // 第6步：设置优化参数，开始执行优化
    optimizer.setVerbose ( false );
    optimizer.initializeOptimization();
    optimizer.optimize ( 100 );

    // 输出优化结果
    cout<< endl <<"after optimization:"<<endl;
    cout<< "T=" << endl << Eigen::Isometry3d ( pose->estimate() ).matrix() << endl;
}