/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <qle/models/crossassetanalytics.hpp>
#include <qle/models/crossassetmodel.hpp>

#include <ql/math/matrixutilities/pseudosqrt.hpp>
#include <ql/processes/eulerdiscretization.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

using namespace CrossAssetAnalytics;
using namespace QuantLib;

namespace {
inline void setValue(Matrix& m, const Real& value, const QuantExt::CrossAssetModel* model,
                     const QuantExt::CrossAssetModelTypes::AssetType& t1, const Size& i1,
                     const QuantExt::CrossAssetModelTypes::AssetType& t2, const Size& i2, const Size& offset1 = 0,
                     const Size& offset2 = 0) {
    Size i = model->pIdx(t1, i1, offset1);
    Size j = model->pIdx(t2, i2, offset2);
    m[i][j] = m[j][i] = value;
}
inline void setValue(Array& a, const Real& value, const QuantExt::CrossAssetModel* model,
                     const QuantExt::CrossAssetModelTypes::AssetType& t, const Size& i, const Size& offset = 0) {
    a[model->pIdx(t, i, offset)] = value;
}
} // anonymous namespace

CrossAssetStateProcess::CrossAssetStateProcess(const CrossAssetModel* const model, discretization disc,
                                               SalvagingAlgorithm::Type salvaging)
    : StochasticProcess(), model_(model), disc_(disc), salvaging_(salvaging), cirppCount_(0) {

    updateSqrtCorrelation();
    if (disc_ == euler) {
        discretization_ = boost::make_shared<EulerDiscretization>();
    } else {
        discretization_ = boost::make_shared<CrossAssetStateProcess::ExactDiscretization>(model, salvaging);
    }

    // set up CR CIR++ processes, defer the euler discretisation check to evolve()
    for (Size i = 0; i < model_->components(CR); ++i) {
        if (model_->modelType(CR, i) == CIRPP) {
            crCirpp_.push_back(model->crcirppModel(i)->stateProcess());
            cirppCount_++;
        } else {
            crCirpp_.push_back(boost::shared_ptr<CrCirppStateProcess>());
        }
    }
}

Size CrossAssetStateProcess::size() const { return model_->dimension(); }

void CrossAssetStateProcess::flushCache() const {
    cache_m_.clear();
    cache_md_.clear();
    cache_v_.clear();
    cache_d_.clear();
    boost::shared_ptr<CrossAssetStateProcess::ExactDiscretization> tmp =
        boost::dynamic_pointer_cast<CrossAssetStateProcess::ExactDiscretization>(discretization_);
    if (tmp != NULL) {
        tmp->flushCache();
    }
    updateSqrtCorrelation();
}

void CrossAssetStateProcess::updateSqrtCorrelation() const {
    if (disc_ != euler)
        return;
    // build sqrt corr (for correlation matrix that covers all state variables)
    // this can be simplified once we use as many brownians as model->brownians()
    // instead of the full state vector
    Matrix corr(model_->dimension(), model_->dimension(), 1.0);
    Size brownianIndex = 0;
    std::vector<Size> brownianIndices;
    for (Size t = 0; t < crossAssetModelAssetTypes; ++t) {
        AssetType assetType = AssetType(t);
        for (Size i = 0; i < model_->components(assetType); ++i) {

            // 3 possibilities for number of state variables vs. the number of Brownian motions for the i-th component
            // within the current asset type.
            // 1) They are equal. Make the assumption that there is a 1-1 correspondence. This is essentially what has
            //    been happening until now outside of DK model i.e. always 1 state var and 1 Brownian motion.
            // 2) number of state vars > number of Brownian motions. Happens with DK model. Think the code below will
            //    only work when number of Brownian motions equals 1. Not changing it as not sure what was intended.
            // 3) number of state vars < number of Brownian motions. Not covered below.
            auto brownians = model_->brownians(assetType, i);
            auto stateVars = model_->stateVariables(assetType, i);

            if (brownians == stateVars) {
                for (Size k = 0; k < brownians; ++k) {
                    brownianIndices.push_back(brownianIndex++);
                }
            } else {
                for (Size j = 0; j < brownians; ++j) {
                    for (Size k = 0; k < stateVars; ++k) {
                        brownianIndices.push_back(brownianIndex);
                    }
                    ++brownianIndex;
                }
            }
        }
    }
    for (Size i = 0; i < corr.rows(); ++i) {
        for (Size j = 0; j < i; ++j) {
            corr[i][j] = corr[j][i] = model_->correlation()(brownianIndices[i], brownianIndices[j]);
        }
    }

    sqrtCorrelation_ = pseudoSqrt(corr, salvaging_);
}

Disposable<Array> CrossAssetStateProcess::initialValues() const {
    Array res(model_->dimension(), 0.0);
    /* irlgm1f processes have initial value 0 */
    for (Size i = 0; i < model_->components(FX); ++i) {
        /* fxbs processes are in log spot */
        res[model_->pIdx(FX, i, 0)] = std::log(model_->fxbs(i)->fxSpotToday()->value());
    }
    for (Size i = 0; i < model_->components(EQ); ++i) {
        /* eqbs processes are in log spot */
        res[model_->pIdx(EQ, i, 0)] = std::log(model_->eqbs(i)->eqSpotToday()->value());
    }
    // CR CIR++ components
    for (Size i = 0; i < model_->components(CR); ++i) {
        if (model_->modelType(CR, i) != CIRPP)
            continue;
        QL_REQUIRE(crCirpp_[i], "crcirpp is null!");
        Array r = crCirpp_[i]->initialValues();
        res[model_->pIdx(CR, i, 0)] = r[0]; // y0
        res[model_->pIdx(CR, i, 1)] = r[1]; // S(0,0) = 1
    }

    for (Size i = 0; i < model_->components(INF); ++i) {
        // Second component of JY model is the inflation index process.
        if (model_->modelType(INF, i) == JY) {
            res[model_->pIdx(INF, i, 1)] = std::log(model_->infjy(i)->index()->fxSpotToday()->value());
        }
    }
    /* infdk, crlgm1f, aux processes have initial value 0 */
    return res;
}

Disposable<Array> CrossAssetStateProcess::drift(Time t, const Array& x) const {
    Array res(model_->dimension(), 0.0);
    Size n = model_->components(IR);
    Size n_eq = model_->components(EQ);
    Real H0 = model_->irlgm1f(0)->H(t);
    Real Hprime0 = model_->irlgm1f(0)->Hprime(t);
    Real alpha0 = model_->irlgm1f(0)->alpha(t);
    Real zeta0 = model_->irlgm1f(0)->zeta(t);
    boost::unordered_map<double, Array>::const_iterator i = cache_m_.find(t);
    if (i == cache_m_.end()) {
        /* z0 has drift 0 in the LGM measure but non-zero drift in the bank account measure, so start loop at i = 0 */
        for (Size i = 0; i < n; ++i) {
            Real Hi = model_->irlgm1f(i)->H(t);
            Real alphai = model_->irlgm1f(i)->alpha(t);
            if (i == 0 && model_->measure() == Measure::BA) {
                // ADD z0 drift in the BA measure
                res[model_->pIdx(IR, i, 0)] = -Hi * alphai * alphai;
                // the auxiliary state variable is drift-free
                res[model_->pIdx(AUX, i, 0)] = 0.0;
            }
            if (i > 0) {
                Real sigmai = model_->fxbs(i - 1)->sigma(t);
                // ir-ir
                Real rhozz0i = model_->correlation(IR, 0, IR, i);
                // ir-fx
                Real rhozx0i = model_->correlation(IR, 0, FX, i - 1);
                Real rhozxii = model_->correlation(IR, i, FX, i - 1);
                // ir drifts
                res[model_->pIdx(IR, i, 0)] =
                    -Hi * alphai * alphai + H0 * alpha0 * alphai * rhozz0i - sigmai * alphai * rhozxii;
                // log spot fx drifts (z0, zi independent parts)
                res[model_->pIdx(FX, i - 1, 0)] = H0 * alpha0 * sigmai * rhozx0i +
                                                  model_->irlgm1f(0)->termStructure()->forwardRate(t, t, Continuous) -
                                                  model_->irlgm1f(i)->termStructure()->forwardRate(t, t, Continuous) -
                                                  0.5 * sigmai * sigmai;
                if (model_->measure() == Measure::BA) {
                    // REMOVE the LGM measure drift contributions above
                    res[model_->pIdx(IR, i, 0)] -= H0 * alpha0 * alphai * rhozz0i;
                    res[model_->pIdx(FX, i - 1, 0)] -= H0 * alpha0 * sigmai * rhozx0i;
                }
            }
        }
        /* log equity spot drifts (the cache-able parts) */
        for (Size k = 0; k < n_eq; ++k) {
            Size i = model_->ccyIndex(model_->eqbs(k)->currency());
            // ir params (for equity currency)
            Real eps_ccy = (i == 0) ? 0.0 : 1.0;
            // Real Hi = model_->irlgm1f(i)->H(t);
            // Real alphai = model_->irlgm1f(i)->alpha(t);
            // eq vol
            Real sigmask = model_->eqbs(k)->sigma(t);
            // fx vol (eq ccy / base ccy)
            Real sigmaxi = (i == 0) ? 0.0 : model_->fxbs(i - 1)->sigma(t);
            // ir-eq corr
            // Real rhozsik = model_->correlation(EQ, k, IR, i); // eq cur
            Real rhozs0k = model_->correlation(EQ, k, IR, 0); // base cur
            // fx-eq corr
            Real rhoxsik = (i == 0) ? 0.0 : // no fx process for base-ccy
                               model_->correlation(FX, i - 1, EQ, k);
            // ir instantaneous forward rate (from curve used for eq forward projection)
            Real fr_i = model_->eqbs(k)->equityIrCurveToday()->forwardRate(t, t, Continuous);
            // div yield instantaneous forward rate
            Real fq_k = model_->eqbs(k)->equityDivYieldCurveToday()->forwardRate(t, t, Continuous);
            res[model_->pIdx(EQ, k, 0)] = fr_i - fq_k + (rhozs0k * H0 * alpha0 * sigmask) -
                                          (eps_ccy * rhoxsik * sigmaxi * sigmask) - (0.5 * sigmask * sigmask);
        }

        // State independent pieces of JY inflation model, if there is a CAM JY component.
        for (Size j = 0; j < model_->components(INF); ++j) {

            if (model_->modelType(INF, j) == JY) {

                auto p = model_->infjy(j);
                Size i_j = model_->ccyIndex(p->currency());

                // JY inflation parameter values.
                Real H_y_j = p->realRate()->H(t);
                Real Hp_y_j = p->realRate()->Hprime(t);
                Real zeta_y_j = p->realRate()->zeta(t);
                Real alpha_y_j = p->realRate()->alpha(t);
                Real sigma_c_j = p->index()->sigma(t);

                // Inflation nominal currency parameter values
                Real H_i_j = model_->irlgm1f(i_j)->H(t);
                Real Hp_i_j = model_->irlgm1f(i_j)->Hprime(t);
                Real zeta_i_j = model_->irlgm1f(i_j)->zeta(t);

                // Correlations
                Real rho_zy_0j = model_->correlation(IR, 0, INF, j, 0, 0);
                Real rho_yc_ij = model_->correlation(INF, j, INF, j, 0, 1);
                Real rho_zc_0j = model_->correlation(IR, 0, INF, j, 0, 1);

                // JY real rate drift. It is state independent
                auto rrDrift = -alpha_y_j * alpha_y_j * H_y_j + rho_zy_0j * alpha0 * alpha_y_j * H_y_j -
                               rho_yc_ij * alpha_y_j * sigma_c_j;

                if (i_j > 0) {
                    Real sigma_x_i_j = model_->fxbs(i_j - 1)->sigma(t);
                    Real rho_yx_j_i_j = model_->correlation(INF, j, FX, i_j - 1, 0, 0);
                    rrDrift -= rho_yx_j_i_j * alpha_y_j * sigma_x_i_j;
                }

                res[model_->pIdx(INF, j, 0)] = rrDrift;

                // JY log inflation index drift (state independent piece).
                auto indexDrift = rho_zc_0j * alpha0 * sigma_c_j * H0 - 0.5 * sigma_c_j * sigma_c_j +
                                  zeta_i_j * Hp_i_j * H_i_j - zeta_y_j * Hp_y_j * H_y_j;

                // Add on the f_n(0, t) - f_r(0, t) piece using the initial zero inflation term structure.
                // Use the same dt below that is used in yield forward rate calculations.
                auto ts = p->realRate()->termStructure();
                Time dt = 0.0001;
                Time t1 = std::max(t - dt / 2.0, 0.0);
                Time t2 = t1 + dt;
                auto z_t = ts->zeroRate(t);
                auto z_t1 = ts->zeroRate(t1);
                auto z_t2 = ts->zeroRate(t2);
                indexDrift += std::log(1 + z_t) + (t / (1 + z_t)) * ((z_t2 - z_t1) / dt);

                if (i_j > 0) {
                    Real sigma_x_i_j = model_->fxbs(i_j - 1)->sigma(t);
                    Real rho_cx_j_i_j = model_->correlation(INF, j, FX, i_j - 1, 1, 0);
                    indexDrift -= rho_cx_j_i_j * sigma_c_j * sigma_x_i_j;
                }

                res[model_->pIdx(INF, j, 1)] = indexDrift;
            }
        }

        cache_m_.insert(std::make_pair(t, res));
    } else {
        res = i->second;
    }
    // non-cacheable sections of drifts
    for (Size i = 1; i < n; ++i) {
        // log spot fx drifts (z0, zi dependent parts)
        Real Hi = model_->irlgm1f(i)->H(t);
        Real Hprimei = model_->irlgm1f(i)->Hprime(t);
        Real zetai = model_->irlgm1f(i)->zeta(t);
        res[model_->pIdx(FX, i - 1, 0)] += x[model_->pIdx(IR, 0, 0)] * Hprime0 + zeta0 * Hprime0 * H0 -
                                           x[model_->pIdx(IR, i, 0)] * Hprimei - zetai * Hprimei * Hi;
    }
    for (Size k = 0; k < n_eq; ++k) {
        // log equity spot drifts (path-dependent parts)
        // notice the assumption in below that dividend yield curve is static
        Size i = model_->ccyIndex(model_->eqbs(k)->currency());
        // ir params (for equity currency)
        Real Hi = model_->irlgm1f(i)->H(t);
        Real Hprimei = model_->irlgm1f(i)->Hprime(t);
        Real zetai = model_->irlgm1f(i)->zeta(t);
        res[model_->pIdx(EQ, k, 0)] += (x[model_->pIdx(IR, i, 0)] * Hprimei) + (zetai * Hprimei * Hi);
    }

    // Non-cacheable portion of inflation JY drift, if there is a CAM JY component.
    for (Size j = 0; j < model_->components(INF); ++j) {
        if (model_->modelType(INF, j) == JY) {

            auto p = model_->infjy(j);
            Size i_j = model_->ccyIndex(p->currency());

            // JY inflation parameter values.
            Real Hp_y_j = p->realRate()->Hprime(t);

            // Inflation nominal currency parameter values
            Real Hp_i_j = model_->irlgm1f(i_j)->Hprime(t);

            res[model_->pIdx(INF, j, 1)] += x[model_->pIdx(IR, i_j, 0)] * Hp_i_j - x[model_->pIdx(INF, j, 0)] * Hp_y_j;
        }
    }

    /* no drift for infdk, crlgm1f components */
    return res;
}

Disposable<Matrix> CrossAssetStateProcess::diffusion(Time t, const Array& x) const {
    boost::unordered_map<double, Matrix>::const_iterator i = cache_d_.find(t);
    if (i == cache_d_.end()) {
        Matrix tmp = diffusionImpl(t, x);
        cache_d_.insert(std::make_pair(t, tmp));
        return tmp;
    } else {
        // we have to make a copy, otherwise we destroy the map entry
        // since a disposable is returned
        Matrix tmp = i->second;
        return tmp;
    }
}

Disposable<Array> CrossAssetStateProcess::marginalDiffusion(Time t, const Array& x) const {
    boost::unordered_map<double, Array>::const_iterator i = cache_md_.find(t);
    if (i == cache_md_.end()) {
        Array tmp = marginalDiffusionImpl(t, x);
        cache_md_.insert(std::make_pair(t, tmp));
        return tmp;
    } else {
        // we have to make a copy, otherwise we destroy the map entry
        // since a disposable is returned
        Array tmp = i->second;
        return tmp;
    }
}

Disposable<Matrix> CrossAssetStateProcess::diffusionImpl(Time t, const Array& x) const {
    Matrix res(model_->dimension(), model_->dimension(), 0.0);
    Array diag = marginalDiffusion(t, x);
    for (Size i = 0; i < x.size(); ++i) {
        res[i][i] = diag[i];
    }
    return res * sqrtCorrelation_;
} // namespace QuantExt

Disposable<Array> CrossAssetStateProcess::marginalDiffusionImpl(Time t, const Array&) const {
    Array res(model_->dimension(), 0.0);
    Size n = model_->components(IR);
    Size m = model_->components(FX);
    Size d = model_->components(INF);
    Size c = model_->components(CR);
    Size e = model_->components(EQ);
    // ir-ir
    for (Size i = 0; i < n; ++i) {
        Real alphai = model_->irlgm1f(i)->alpha(t);
        setValue(res, alphai, model_, IR, i, 0);
    }
    // fx-fx
    for (Size i = 0; i < m; ++i) {
        Real sigmai = model_->fxbs(i)->sigma(t);
        setValue(res, sigmai, model_, FX, i, 0);
    }
    // inf-inf
    for (Size i = 0; i < d; ++i) {
        if (model_->modelType(INF, i) == DK) {
            Real alphai = model_->infdk(i)->alpha(t);
            Real Hi = model_->infdk(i)->H(t);
            // DK z diffusion coefficient
            setValue(res, alphai, model_, INF, i, 0);
            // DK y diffusion coefficient
            setValue(res, alphai * Hi, model_, INF, i, 1);
        } else {
            auto p = model_->infjy(i);
            // JY z diffusion coefficient
            setValue(res, p->realRate()->alpha(t), model_, INF, i, 0);
            // JY I diffusion coefficient
            setValue(res, p->index()->sigma(t), model_, INF, i, 1);
        }
    }
    for (Size i = 0; i < c; ++i) {
        // Skip CR components that are not LGM
        if (model_->modelType(CR, i) != LGM1F)
            continue;
        Real alphai = model_->crlgm1f(i)->alpha(t);
        Real Hi = model_->crlgm1f(i)->H(t);
        // crz-crz
        setValue(res, alphai, model_, CR, i, 0);
        // cry-cry
        setValue(res, alphai * Hi, model_, CR, i, 1);
    }
    // eq-eq
    for (Size i = 0; i < e; ++i) {
        Real sigmai = model_->eqbs(i)->sigma(t);
        setValue(res, sigmai, model_, EQ, i, 0);
    }

    if (model_->measure() == Measure::BA) {
        // aux-aux
        Real H0 = model_->irlgm1f(0)->H(t);
        Real alpha0 = model_->irlgm1f(0)->alpha(t);
        setValue(res, alpha0 * H0, model_, AUX, 0, 0);
    }

    return res;
}

Disposable<Array> CrossAssetStateProcess::evolve(Time t0, const Array& x0, Time dt, const Array& dw) const {

    Array res;
    if (disc_ == euler) {
        const Array dz = sqrtCorrelation_ * dw;
        const Array df = marginalDiffusion(t0, x0);
        res = apply(expectation(t0, x0, dt), df * dz * std::sqrt(dt));

        // CR CIRPP components
        if (cirppCount_ > 0) {
            for (Size i = 0; i < model_->components(CR); ++i) {
                if (!crCirpp_[i])
                    continue; // ignore non-cir cr model
                Size idx1 = model_->pIdx(CR, i, 0);
                Size idx2 = model_->pIdx(CR, i, 1);
                Array x0Tmp(2), dwTmp(2);
                x0Tmp[0] = x0[idx1];
                x0Tmp[1] = x0[idx2];
                dwTmp[0] = dz[idx1];
                dwTmp[1] = 0.0; // not used
                // evolve original process
                auto r = crCirpp_[i]->evolve(t0, x0Tmp, dt, dwTmp);

                // set result
                res[idx1] = r[0]; // y
                res[idx2] = r[1]; // S(0,T)
            }
        }
    } else {
        QL_REQUIRE(cirppCount_ == 0, "only euler discretization is supported for CIR++");
        res = StochasticProcess::evolve(t0, x0, dt, dw);
    }

    return res;
}

CrossAssetStateProcess::ExactDiscretization::ExactDiscretization(const CrossAssetModel* const model,
                                                                 SalvagingAlgorithm::Type salvaging)
    : model_(model), salvaging_(salvaging) {}

Disposable<Array> CrossAssetStateProcess::ExactDiscretization::drift(const StochasticProcess& p, Time t0,
                                                                     const Array& x0, Time dt) const {
    Array res;
    cache_key k = {t0, dt};
    boost::unordered_map<cache_key, Array>::const_iterator i = cache_m_.find(k);
    if (i == cache_m_.end()) {
        res = driftImpl1(p, t0, x0, dt);
        cache_m_.insert(std::make_pair(k, res));
    } else {
        res = i->second;
    }
    Array res2 = driftImpl2(p, t0, x0, dt);
    for (Size i = 0; i < res.size(); ++i) {
        res[i] += res2[i];
    }
    return res - x0;
}

Disposable<Matrix> CrossAssetStateProcess::ExactDiscretization::diffusion(const StochasticProcess& p, Time t0,
                                                                          const Array& x0, Time dt) const {
    cache_key k = {t0, dt};
    boost::unordered_map<cache_key, Matrix>::const_iterator i = cache_d_.find(k);
    if (i == cache_d_.end()) {
        Matrix res = pseudoSqrt(covariance(p, t0, x0, dt), salvaging_);
        // note that covariance actually does not depend on x0
        cache_d_.insert(std::make_pair(k, res));
        return res;
    } else {
        // see above about the copy
        Matrix tmp = i->second;
        return tmp;
    }
}

Disposable<Matrix> CrossAssetStateProcess::ExactDiscretization::covariance(const StochasticProcess& p, Time t0,
                                                                           const Array& x0, Time dt) const {
    cache_key k = {t0, dt};
    boost::unordered_map<cache_key, Matrix>::const_iterator i = cache_v_.find(k);
    if (i == cache_v_.end()) {
        Matrix res = covarianceImpl(p, t0, x0, dt);
        cache_v_.insert(std::make_pair(k, res));
        return res;
    } else {
        // see above about the copy
        Matrix tmp = i->second;
        return tmp;
    }
}

Disposable<Array> CrossAssetStateProcess::ExactDiscretization::driftImpl1(const StochasticProcess&, Time t0,
                                                                          const Array&, Time dt) const {
    Size n = model_->components(IR);
    Size m = model_->components(FX);
    Size e = model_->components(EQ);
    Array res(model_->dimension(), 0.0);
    for (Size i = 0; i < n; ++i) {
        res[model_->pIdx(IR, i, 0)] = ir_expectation_1(model_, i, t0, dt);
    }
    for (Size j = 0; j < m; ++j) {
        res[model_->pIdx(FX, j, 0)] = fx_expectation_1(model_, j, t0, dt);
    }
    for (Size k = 0; k < e; ++k) {
        res[model_->pIdx(EQ, k, 0)] = eq_expectation_1(model_, k, t0, dt);
    }

    // If inflation is JY, need to take account of the drift.
    for (Size i = 0; i < model_->components(INF); ++i) {
        if (model_->modelType(INF, i) == JY) {
            std::tie(res[model_->pIdx(INF, i, 0)], res[model_->pIdx(INF, i, 1)]) =
                inf_jy_expectation_1(model_, i, t0, dt);
        }
    }

    return res;
}

Disposable<Array> CrossAssetStateProcess::ExactDiscretization::driftImpl2(const StochasticProcess&, Time t0,
                                                                          const Array& x0, Time dt) const {
    Size n = model_->components(IR);
    Size m = model_->components(FX);
    Size e = model_->components(EQ);
    Array res(model_->dimension(), 0.0);

    if (model_->measure() == Measure::BA) {
        // zero AUX state drift, i.e. conditional expectation equal to the previous level as for z_i
        res[model_->pIdx(AUX, 0, 0)] += x0[model_->pIdx(AUX, 0, 0)];
    }

    for (Size i = 0; i < n; ++i) {
        res[model_->pIdx(IR, i, 0)] += ir_expectation_2(model_, i, x0[model_->pIdx(IR, i, 0)]);
    }
    for (Size j = 0; j < m; ++j) {
        res[model_->pIdx(FX, j, 0)] += fx_expectation_2(model_, j, t0, x0[model_->pIdx(FX, j, 0)],
                                                        x0[model_->pIdx(IR, j + 1, 0)], x0[model_->pIdx(IR, 0, 0)], dt);
    }
    for (Size k = 0; k < e; ++k) {
        Size eqCcyIdx = model_->ccyIndex(model_->eqbs(k)->currency());
        res[model_->pIdx(EQ, k, 0)] +=
            eq_expectation_2(model_, k, t0, x0[model_->pIdx(EQ, k, 0)], x0[model_->pIdx(IR, eqCcyIdx, 0)], dt);
    }

    // Inflation: JY is state dependent. DK is not. Even for DK, still need to return the conditional expected value.
    for (Size i = 0; i < model_->components(INF); ++i) {
        if (model_->modelType(INF, i) == JY) {
            auto i_i = model_->ccyIndex(model_->infjy(i)->currency());
            auto zi_i_0 = x0[model_->pIdx(IR, i_i, 0)];
            auto state_0 = std::make_pair(x0[model_->pIdx(INF, i, 0)], x0[model_->pIdx(INF, i, 1)]);
            std::tie(res[model_->pIdx(INF, i, 0)], res[model_->pIdx(INF, i, 1)]) =
                inf_jy_expectation_2(model_, i, t0, state_0, zi_i_0, dt);
        } else {
            res[model_->pIdx(INF, i, 0)] = x0[model_->pIdx(INF, i, 0)];
            res[model_->pIdx(INF, i, 1)] = x0[model_->pIdx(INF, i, 1)];
        }
    }

    /*! cr components have integrated drift 0, we have to return the conditional
        expected value though, since x0 is subtracted later */
    Size c = model_->components(CR);
    for (Size i = 0; i < c; ++i) {
        res[model_->pIdx(CR, i, 0)] = x0[model_->pIdx(CR, i, 0)];
        res[model_->pIdx(CR, i, 1)] = x0[model_->pIdx(CR, i, 1)];
    }
    return res;
}

Disposable<Matrix> CrossAssetStateProcess::ExactDiscretization::covarianceImpl(const StochasticProcess&, Time t0,
                                                                               const Array&, Time dt) const {
    Matrix res(model_->dimension(), model_->dimension());
    Size n = model_->components(IR);
    Size m = model_->components(FX);
    Size d = model_->components(INF);
    Size c = model_->components(CR);
    Size e = model_->components(EQ);

    if (model_->measure() == Measure::BA) {
        // aux-aux
        setValue(res, aux_aux_covariance(model_, t0, dt), model_, AUX, 0, AUX, 0, 0, 0);
        // aux-ir
        for (Size j = 0; j < n; ++j) {
            setValue(res, aux_ir_covariance(model_, j, t0, dt), model_, AUX, 0, IR, j, 0, 0);
        }
        // aux-fx
        for (Size j = 0; j < m; ++j) {
            setValue(res, aux_fx_covariance(model_, j, t0, dt), model_, AUX, 0, FX, j, 0, 0);
        }
    }
    // ir-ir
    for (Size i = 0; i < n; ++i) {
        for (Size j = 0; j <= i; ++j) {
            setValue(res, ir_ir_covariance(model_, i, j, t0, dt), model_, IR, i, IR, j, 0, 0);
        }
    }
    // ir-fx
    for (Size i = 0; i < n; ++i) {
        for (Size j = 0; j < m; ++j) {
            setValue(res, ir_fx_covariance(model_, i, j, t0, dt), model_, IR, i, FX, j, 0, 0);
        }
    }
    // fx-fx
    for (Size i = 0; i < m; ++i) {
        for (Size j = 0; j <= i; ++j) {
            setValue(res, fx_fx_covariance(model_, i, j, t0, dt), model_, FX, i, FX, j);
        }
    }
    // ir,fx,inf - inf
    for (Size j = 0; j < d; ++j) {
        for (Size i = 0; i <= j; ++i) {
            // infz-infz
            setValue(res, infz_infz_covariance(model_, i, j, t0, dt), model_, INF, i, INF, j, 0, 0);
            // infz-infy
            setValue(res, infz_infy_covariance(model_, i, j, t0, dt), model_, INF, i, INF, j, 0, 1);
            setValue(res, infz_infy_covariance(model_, j, i, t0, dt), model_, INF, i, INF, j, 1, 0);
            // infy-infy
            setValue(res, infy_infy_covariance(model_, i, j, t0, dt), model_, INF, i, INF, j, 1, 1);
        }
        for (Size i = 0; i < n; ++i) {
            // ir-inf
            setValue(res, ir_infz_covariance(model_, i, j, t0, dt), model_, IR, i, INF, j, 0, 0);
            setValue(res, ir_infy_covariance(model_, i, j, t0, dt), model_, IR, i, INF, j, 0, 1);
        }
        for (Size i = 0; i < (n - 1); ++i) {
            // fx-inf
            setValue(res, fx_infz_covariance(model_, i, j, t0, dt), model_, FX, i, INF, j, 0, 0);
            setValue(res, fx_infy_covariance(model_, i, j, t0, dt), model_, FX, i, INF, j, 0, 1);
        }
    }
    // ir,fx,inf,cr - cr
    for (Size j = 0; j < c; ++j) {
        // Skip CR components that are not LGM
        if (model_->modelType(CR, j) != LGM1F)
            continue;
        for (Size i = 0; i <= j; ++i) {
            // Skip CR components that are not LGM
            if (model_->modelType(CR, i) != LGM1F)
                continue;
            // crz-crz
            setValue(res, crz_crz_covariance(model_, i, j, t0, dt), model_, CR, i, CR, j, 0, 0);
            // crz-cry
            setValue(res, crz_cry_covariance(model_, i, j, t0, dt), model_, CR, i, CR, j, 0, 1);
            setValue(res, crz_cry_covariance(model_, j, i, t0, dt), model_, CR, i, CR, j, 1, 0);
            // cry-cry
            setValue(res, cry_cry_covariance(model_, i, j, t0, dt), model_, CR, i, CR, j, 1, 1);
        }
        for (Size i = 0; i < n; ++i) {
            // ir-cr
            setValue(res, ir_crz_covariance(model_, i, j, t0, dt), model_, IR, i, CR, j, 0, 0);
            setValue(res, ir_cry_covariance(model_, i, j, t0, dt), model_, IR, i, CR, j, 0, 1);
        }
        for (Size i = 0; i < (n - 1); ++i) {
            // fx-cr
            setValue(res, fx_crz_covariance(model_, i, j, t0, dt), model_, FX, i, CR, j, 0, 0);
            setValue(res, fx_cry_covariance(model_, i, j, t0, dt), model_, FX, i, CR, j, 0, 1);
        }
        for (Size i = 0; i < d; ++i) {
            // inf-cr
            setValue(res, infz_crz_covariance(model_, i, j, t0, dt), model_, INF, i, CR, j, 0, 0);
            setValue(res, infy_crz_covariance(model_, i, j, t0, dt), model_, INF, i, CR, j, 1, 0);
            setValue(res, infz_cry_covariance(model_, i, j, t0, dt), model_, INF, i, CR, j, 0, 1);
            setValue(res, infy_cry_covariance(model_, i, j, t0, dt), model_, INF, i, CR, j, 1, 1);
        }
    }
    // ir,fx,inf,cr,eq - eq
    for (Size j = 0; j < e; ++j) {
        for (Size i = 0; i <= j; ++i) {
            // eq-eq
            setValue(res, eq_eq_covariance(model_, i, j, t0, dt), model_, EQ, i, EQ, j, 0, 0);
        }
        for (Size i = 0; i < n; ++i) {
            // ir-eq
            setValue(res, ir_eq_covariance(model_, i, j, t0, dt), model_, IR, i, EQ, j, 0, 0);
        }
        for (Size i = 0; i < (n - 1); ++i) {
            // fx-eq
            setValue(res, fx_eq_covariance(model_, i, j, t0, dt), model_, FX, i, EQ, j, 0, 0);
        }
        for (Size i = 0; i < d; ++i) {
            // inf-eq
            setValue(res, infz_eq_covariance(model_, i, j, t0, dt), model_, INF, i, EQ, j, 0, 0);
            setValue(res, infy_eq_covariance(model_, i, j, t0, dt), model_, INF, i, EQ, j, 1, 0);
        }
        for (Size i = 0; i < c; ++i) {
            // Skip CR components that are not LGM
            if (model_->modelType(CR, i) != LGM1F)
                continue;
            // cr-eq
            setValue(res, crz_eq_covariance(model_, i, j, t0, dt), model_, CR, i, EQ, j, 0, 0);
            setValue(res, cry_eq_covariance(model_, i, j, t0, dt), model_, CR, i, EQ, j, 1, 0);
        }
    }
    return res;
}

void CrossAssetStateProcess::ExactDiscretization::flushCache() const {
    cache_m_.clear();
    cache_v_.clear();
    cache_d_.clear();
}

} // namespace QuantExt


//  Copyright (c) 2006, Stephan Diederich
//
//  This code may be used under either of the following two licences:
//
//    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. OF SUCH DAMAGE.
//
//  Or:
//
//    Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//    http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_KOLMOGOROV_MAX_FLOW_HPP
#define BOOST_KOLMOGOROV_MAX_FLOW_HPP

#if defined(_MSC_VER) || defined(__BORLANDC__) || defined(__DMC__)
#  pragma message ("The kolmogorov_max_flow.hpp header is deprecated and will be removed in Boost 1.46. Use boykov_kolmogorov_max_flow.hpp instead.")
#elif defined(__GNUC__) || defined(__HP_aCC) || defined(__SUNPRO_CC) || defined(__IBMCPP__)
#  warning "The kolmogorov_max_flow.hpp header is deprecated and will be removed in Boost 1.46. Use boykov_kolmogorov_max_flow.hpp instead."
#endif

#include <boost/config.hpp>
#include <cassert>
#include <vector>
#include <list>
#include <utility>
#include <iosfwd>
#include <algorithm> // for std::min and std::max

#include <boost/pending/queue.hpp>
#include <boost/limits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/none_t.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/lookup_edge.hpp>

namespace boost {
  namespace detail {

    template <class Graph,
              class EdgeCapacityMap,
              class ResidualCapacityEdgeMap,
              class ReverseEdgeMap,
              class PredecessorMap,
              class ColorMap,
              class DistanceMap,
              class IndexMap>
    class kolmogorov{
      typedef typename property_traits<EdgeCapacityMap>::value_type tEdgeVal;
      typedef graph_traits<Graph> tGraphTraits;
      typedef typename tGraphTraits::vertex_iterator vertex_iterator;
      typedef typename tGraphTraits::vertex_descriptor vertex_descriptor;
      typedef typename tGraphTraits::edge_descriptor edge_descriptor;
      typedef typename tGraphTraits::edge_iterator edge_iterator;
      typedef typename tGraphTraits::out_edge_iterator out_edge_iterator;
      typedef boost::queue<vertex_descriptor> tQueue;                               //queue of vertices, used in adoption-stage
      typedef typename property_traits<ColorMap>::value_type tColorValue;
      typedef color_traits<tColorValue> tColorTraits;
      typedef typename property_traits<DistanceMap>::value_type tDistanceVal;

        public:
          kolmogorov(Graph& g,
                     EdgeCapacityMap cap,
                     ResidualCapacityEdgeMap res,
                     ReverseEdgeMap rev,
                     PredecessorMap pre,
                     ColorMap color,
                     DistanceMap dist,
                     IndexMap idx,
                     vertex_descriptor src,
                     vertex_descriptor sink):
          m_g(g),
          m_index_map(idx),
          m_cap_map(cap),
          m_res_cap_map(res),
          m_rev_edge_map(rev),
          m_pre_map(pre),
          m_tree_map(color),
          m_dist_map(dist),
          m_source(src),
          m_sink(sink),
          m_active_nodes(),
          m_in_active_list_vec(num_vertices(g), false),
          m_in_active_list_map(make_iterator_property_map(m_in_active_list_vec.begin(), m_index_map)),
          m_has_parent_vec(num_vertices(g), false),
          m_has_parent_map(make_iterator_property_map(m_has_parent_vec.begin(), m_index_map)),
          m_time_vec(num_vertices(g), 0),
          m_time_map(make_iterator_property_map(m_time_vec.begin(), m_index_map)),
          m_flow(0),
          m_time(1),
          m_last_grow_vertex(graph_traits<Graph>::null_vertex()){
            // initialize the color-map with gray-values
            vertex_iterator vi, v_end;
            for(boost::tie(vi, v_end) = vertices(m_g); vi != v_end; ++vi){
              set_tree(*vi, tColorTraits::gray());
            }
            // Initialize flow to zero which means initializing
            // the residual capacity equal to the capacity
            edge_iterator ei, e_end;
            for(boost::tie(ei, e_end) = edges(m_g); ei != e_end; ++ei) {
              m_res_cap_map[*ei] = m_cap_map[*ei];
              assert(m_rev_edge_map[m_rev_edge_map[*ei]] == *ei); //check if the reverse edge map is build up properly
            }
            //init the search trees with the two terminals
            set_tree(m_source, tColorTraits::black());
            set_tree(m_sink, tColorTraits::white());
            m_time_map[m_source] = 1;
            m_time_map[m_sink] = 1;
          }

          ~kolmogorov(){}

          tEdgeVal max_flow(){
            //augment direct paths from SOURCE->SINK and SOURCE->VERTEX->SINK
            augment_direct_paths();
            //start the main-loop
            while(true){
              bool path_found;
              edge_descriptor connecting_edge;
              boost::tie(connecting_edge, path_found) = grow(); //find a path from source to sink
              if(!path_found){
                //we're finished, no more paths were found
                break;
              }
              ++m_time;
              augment(connecting_edge); //augment that path
              adopt(); //rebuild search tree structure
            }
            return m_flow;
          }

          //the complete class is protected, as we want access to members in derived test-class (see $(BOOST_ROOT)/libs/graph/test/kolmogorov_max_flow_test.cpp)
        protected:
          void augment_direct_paths(){
            //in a first step, we augment all direct paths from source->NODE->sink
            //and additionally paths from source->sink
            //this improves especially graphcuts for segmentation, as most of the nodes have source/sink connects
            //but shouldn't have an impact on other maxflow problems (this is done in grow() anyway)
            out_edge_iterator ei, e_end;
            for(boost::tie(ei, e_end) = out_edges(m_source, m_g); ei != e_end; ++ei){
              edge_descriptor from_source = *ei;
              vertex_descriptor current_node = target(from_source, m_g);
              if(current_node == m_sink){
                tEdgeVal cap = m_res_cap_map[from_source];
                m_res_cap_map[from_source] = 0;
                m_flow += cap;
                continue;
              }
              edge_descriptor to_sink;
              bool is_there;
              boost::tie(to_sink, is_there) = lookup_edge(current_node, m_sink, m_g);
              if(is_there){
                tEdgeVal cap_from_source = m_res_cap_map[from_source];
                tEdgeVal cap_to_sink = m_res_cap_map[to_sink];
                if(cap_from_source > cap_to_sink){
                  set_tree(current_node, tColorTraits::black());
                  add_active_node(current_node);
                  set_edge_to_parent(current_node, from_source);
                  m_dist_map[current_node] = 1;
                  m_time_map[current_node] = 1;
                  //add stuff to flow and update residuals
                  //we dont need to update reverse_edges, as incoming/outgoing edges to/from source/sink don't count for max-flow
                  m_res_cap_map[from_source] -= cap_to_sink;
                  m_res_cap_map[to_sink] = 0;
                  m_flow += cap_to_sink;
                } else if(cap_to_sink > 0){
                  set_tree(current_node, tColorTraits::white());
                  add_active_node(current_node);
                  set_edge_to_parent(current_node, to_sink);
                  m_dist_map[current_node] = 1;
                  m_time_map[current_node] = 1;
                  //add stuff to flow and update residuals
                  //we dont need to update reverse_edges, as incoming/outgoing edges to/from source/sink don't count for max-flow
                  m_res_cap_map[to_sink] -= cap_from_source;
                  m_res_cap_map[from_source] = 0;
                  m_flow += cap_from_source;
                }
              } else if(m_res_cap_map[from_source]){
                //there is no sink connect, so we can't augment this path
                //but to avoid adding m_source to the active nodes, we just activate this node and set the approciate things
                set_tree(current_node, tColorTraits::black());
                set_edge_to_parent(current_node, from_source);
                m_dist_map[current_node] = 1;
                m_time_map[current_node] = 1;
                add_active_node(current_node);
              }
            }
            for(boost::tie(ei, e_end) = out_edges(m_sink, m_g); ei != e_end; ++ei){
              edge_descriptor to_sink = m_rev_edge_map[*ei];
              vertex_descriptor current_node = source(to_sink, m_g);
              if(m_res_cap_map[to_sink]){
                set_tree(current_node, tColorTraits::white());
                set_edge_to_parent(current_node, to_sink);
                m_dist_map[current_node] = 1;
                m_time_map[current_node] = 1;
                add_active_node(current_node);
              }
            }
          }

          /**
          * returns a pair of an edge and a boolean. if the bool is true, the edge is a connection of a found path from s->t , read "the link" and
          *   source(returnVal, m_g) is the end of the path found in the source-tree
          *   target(returnVal, m_g) is the beginning of the path found in the sink-tree
          */
          std::pair<edge_descriptor, bool> grow(){
            assert(m_orphans.empty());
            vertex_descriptor current_node;
            while((current_node = get_next_active_node()) != graph_traits<Graph>::null_vertex()){ //if there is one
              assert(get_tree(current_node) != tColorTraits::gray()  && (has_parent(current_node) || current_node==m_source || current_node==m_sink));
              if(get_tree(current_node) == tColorTraits::black()){
                //source tree growing
                out_edge_iterator ei, e_end;
                if(current_node != m_last_grow_vertex){
                  m_last_grow_vertex = current_node;
                  boost::tie(m_last_grow_edge_it, m_last_grow_edge_end) = out_edges(current_node, m_g);
                }
                for(; m_last_grow_edge_it != m_last_grow_edge_end; ++m_last_grow_edge_it){
                  edge_descriptor out_edge = *m_last_grow_edge_it;
                  if(m_res_cap_map[out_edge] > 0){ //check if we have capacity left on this edge
                    vertex_descriptor other_node = target(out_edge, m_g);
                    if(get_tree(other_node) == tColorTraits::gray()){ //it's a free node
                      set_tree(other_node, tColorTraits::black()); //aquire other node to our search tree
                      set_edge_to_parent(other_node, out_edge);   //set us as parent
                      m_dist_map[other_node] = m_dist_map[current_node] + 1;  //and update the distance-heuristic
                      m_time_map[other_node] = m_time_map[current_node];
                      add_active_node(other_node);
                    } else if(get_tree(other_node) == tColorTraits::black()){
                      if(is_closer_to_terminal(current_node, other_node)){ //we do this to get shorter paths. check if we are nearer to the source as its parent is
                        set_edge_to_parent(other_node, out_edge);
                        m_dist_map[other_node] = m_dist_map[current_node] + 1;
                        m_time_map[other_node] = m_time_map[current_node];
                      }
                    } else{
                      assert(get_tree(other_node)==tColorTraits::white());
                      //kewl, found a path from one to the other search tree, return the connecting edge in src->sink dir
                      return std::make_pair(out_edge, true);
                    }
                  }
                } //for all out-edges
              } //source-tree-growing
              else{
                assert(get_tree(current_node) == tColorTraits::white());
                out_edge_iterator ei, e_end;
                if(current_node != m_last_grow_vertex){
                  m_last_grow_vertex = current_node;
                  boost::tie(m_last_grow_edge_it, m_last_grow_edge_end) = out_edges(current_node, m_g);
                }
                for(; m_last_grow_edge_it != m_last_grow_edge_end; ++m_last_grow_edge_it){
                  edge_descriptor in_edge = m_rev_edge_map[*m_last_grow_edge_it];
                  if(m_res_cap_map[in_edge] > 0){ //check if there is capacity left
                    vertex_descriptor other_node = source(in_edge, m_g);
                    if(get_tree(other_node) == tColorTraits::gray()){ //it's a free node
                      set_tree(other_node, tColorTraits::white());      //aquire that node to our search tree
                      set_edge_to_parent(other_node, in_edge);          //set us as parent
                      add_active_node(other_node);                      //activate that node
                      m_dist_map[other_node] = m_dist_map[current_node] + 1; //set its distance
                      m_time_map[other_node] = m_time_map[current_node];     //and time
                    } else if(get_tree(other_node) == tColorTraits::white()){
                      if(is_closer_to_terminal(current_node, other_node)){
                        //we are closer to the sink than its parent is, so we "adopt" him
                        set_edge_to_parent(other_node, in_edge);
                        m_dist_map[other_node] = m_dist_map[current_node] + 1;
                        m_time_map[other_node] = m_time_map[current_node];
                      }
                    } else{
                      assert(get_tree(other_node)==tColorTraits::black());
                      //kewl, found a path from one to the other search tree, return the connecting edge in src->sink dir
                      return std::make_pair(in_edge, true);
                    }
                  }
                } //for all out-edges
              } //sink-tree growing
              //all edges of that node are processed, and no more paths were found. so remove if from the front of the active queue
              finish_node(current_node);
            } //while active_nodes not empty
            return std::make_pair(edge_descriptor(), false); //no active nodes anymore and no path found, we're done
          }

          /**
          * augments path from s->t and updates residual graph
          * source(e, m_g) is the end of the path found in the source-tree
          * target(e, m_g) is the beginning of the path found in the sink-tree
          * this phase generates orphans on satured edges, if the attached verts are from different search-trees
          * orphans are ordered in distance to sink/source. first the farest from the source are front_inserted into the orphans list,
          * and after that the sink-tree-orphans are front_inserted. when going to adoption stage the orphans are popped_front, and so we process the nearest
          * verts to the terminals first
          */
          void augment(edge_descriptor e){
            assert(get_tree(target(e, m_g)) == tColorTraits::white());
            assert(get_tree(source(e, m_g)) == tColorTraits::black());
            assert(m_orphans.empty());

            const tEdgeVal bottleneck = find_bottleneck(e);
            //now we push the found flow through the path
            //for each edge we saturate we have to look for the verts that belong to that edge, one of them becomes an orphans
            //now process the connecting edge
            m_res_cap_map[e] -= bottleneck;
            assert(m_res_cap_map[e] >= 0);
            m_res_cap_map[m_rev_edge_map[e]] += bottleneck;

            //now we follow the path back to the source
            vertex_descriptor current_node = source(e, m_g);
            while(current_node != m_source){
              edge_descriptor pred = get_edge_to_parent(current_node);
              m_res_cap_map[pred] -= bottleneck;
              assert(m_res_cap_map[pred] >= 0);
              m_res_cap_map[m_rev_edge_map[pred]] += bottleneck;
              if(m_res_cap_map[pred] == 0){
                set_no_parent(current_node);
                m_orphans.push_front(current_node);
              }
              current_node = source(pred, m_g);
            }
            //then go forward in the sink-tree
            current_node = target(e, m_g);
            while(current_node != m_sink){
              edge_descriptor pred = get_edge_to_parent(current_node);
              m_res_cap_map[pred] -= bottleneck;
              assert(m_res_cap_map[pred] >= 0);
              m_res_cap_map[m_rev_edge_map[pred]] += bottleneck;
              if(m_res_cap_map[pred] == 0){
                set_no_parent(current_node);
                m_orphans.push_front(current_node);
              }
              current_node = target(pred, m_g);
            }
            //and add it to the max-flow
            m_flow += bottleneck;
          }

          /**
           * returns the bottleneck of a s->t path (end_of_path is last vertex in source-tree, begin_of_path is first vertex in sink-tree)
           */
          inline tEdgeVal find_bottleneck(edge_descriptor e){
            BOOST_USING_STD_MIN();
            tEdgeVal minimum_cap = m_res_cap_map[e];
            vertex_descriptor current_node = source(e, m_g);
            //first go back in the source tree
            while(current_node != m_source){
              edge_descriptor pred = get_edge_to_parent(current_node);
              minimum_cap = min BOOST_PREVENT_MACRO_SUBSTITUTION(minimum_cap, m_res_cap_map[pred]);
              current_node = source(pred, m_g);
            }
            //then go forward in the sink-tree
            current_node = target(e, m_g);
            while(current_node != m_sink){
              edge_descriptor pred = get_edge_to_parent(current_node);
              minimum_cap = min BOOST_PREVENT_MACRO_SUBSTITUTION(minimum_cap, m_res_cap_map[pred]);
              current_node = target(pred, m_g);
            }
            return minimum_cap;
          }

          /**
          * rebuild search trees
          * empty the queue of orphans, and find new parents for them or just drop them from the search trees
          */
          void adopt(){
            while(!m_orphans.empty() || !m_child_orphans.empty()){
              vertex_descriptor current_node;
              if(m_child_orphans.empty()){
                //get the next orphan from the main-queue  and remove it
                current_node = m_orphans.front();
                m_orphans.pop_front();
              } else{
                current_node = m_child_orphans.front();
                m_child_orphans.pop();
              }
              if(get_tree(current_node) == tColorTraits::black()){
                //we're in the source-tree
                tDistanceVal min_distance = (std::numeric_limits<tDistanceVal>::max)();
                edge_descriptor new_parent_edge;
                out_edge_iterator ei, e_end;
                for(boost::tie(ei, e_end) = out_edges(current_node, m_g); ei != e_end; ++ei){
                  const edge_descriptor in_edge = m_rev_edge_map[*ei];
                  assert(target(in_edge, m_g) == current_node); //we should be the target of this edge
                  if(m_res_cap_map[in_edge] > 0){
                    vertex_descriptor other_node = source(in_edge, m_g);
                    if(get_tree(other_node) == tColorTraits::black() && has_source_connect(other_node)){
                      if(m_dist_map[other_node] < min_distance){
                        min_distance = m_dist_map[other_node];
                        new_parent_edge = in_edge;
                      }
                    }
                  }
                }
                if(min_distance != (std::numeric_limits<tDistanceVal>::max)()){
                  set_edge_to_parent(current_node, new_parent_edge);
                  m_dist_map[current_node] = min_distance + 1;
                  m_time_map[current_node] = m_time;
                } else{
                  m_time_map[current_node] = 0;
                  for(boost::tie(ei, e_end) = out_edges(current_node, m_g); ei != e_end; ++ei){
                    edge_descriptor in_edge = m_rev_edge_map[*ei];
                    vertex_descriptor other_node = source(in_edge, m_g);
                    if(get_tree(other_node) == tColorTraits::black() && has_parent(other_node)){
                      if(m_res_cap_map[in_edge] > 0){
                        add_active_node(other_node);
                      }
                      if(source(get_edge_to_parent(other_node), m_g) == current_node){
                        //we are the parent of that node
                        //it has to find a new parent, too
                        set_no_parent(other_node);
                        m_child_orphans.push(other_node);
                      }
                    }
                  }
                  set_tree(current_node, tColorTraits::gray());
                } //no parent found
              } //source-tree-adoption
              else{
                //now we should be in the sink-tree, check that...
                assert(get_tree(current_node) == tColorTraits::white());
                out_edge_iterator ei, e_end;
                edge_descriptor new_parent_edge;
                tDistanceVal min_distance = (std::numeric_limits<tDistanceVal>::max)();
                for(boost::tie(ei, e_end) = out_edges(current_node, m_g); ei != e_end; ++ei){
                  const edge_descriptor out_edge = *ei;
                  if(m_res_cap_map[out_edge] > 0){
                    const vertex_descriptor other_node = target(out_edge, m_g);
                    if(get_tree(other_node) == tColorTraits::white() && has_sink_connect(other_node))
                      if(m_dist_map[other_node] < min_distance){
                        min_distance = m_dist_map[other_node];
                        new_parent_edge = out_edge;
                      }
                  }
                }
                if(min_distance != (std::numeric_limits<tDistanceVal>::max)()){
                  set_edge_to_parent(current_node, new_parent_edge);
                  m_dist_map[current_node] = min_distance + 1;
                  m_time_map[current_node] = m_time;
                } else{
                  m_time_map[current_node] = 0;
                  for(boost::tie(ei, e_end) = out_edges(current_node, m_g); ei != e_end; ++ei){
                    const edge_descriptor out_edge = *ei;
                    const vertex_descriptor other_node = target(out_edge, m_g);
                    if(get_tree(other_node) == tColorTraits::white() && has_parent(other_node)){
                      if(m_res_cap_map[out_edge] > 0){
                        add_active_node(other_node);
                      }
                      if(target(get_edge_to_parent(other_node), m_g) == current_node){
                        //we were it's parent, so it has to find a new one, too
                        set_no_parent(other_node);
                        m_child_orphans.push(other_node);
                      }
                    }
                  }
                  set_tree(current_node, tColorTraits::gray());
                } //no parent found
              } //sink-tree adoption
            } //while !orphans.empty()
          } //adopt

          /**
          * return next active vertex if there is one, otherwise a null_vertex
          */
          inline vertex_descriptor get_next_active_node(){
            while(true){
              if(m_active_nodes.empty())
                return graph_traits<Graph>::null_vertex();
              vertex_descriptor v = m_active_nodes.front();

              if(!has_parent(v) && v != m_source && v != m_sink){ //if it has no parent, this node can't be active(if its not source or sink)
                m_active_nodes.pop();
                m_in_active_list_map[v] = false;
              } else{
                assert(get_tree(v) == tColorTraits::black() || get_tree(v) == tColorTraits::white());
                return v;
              }
            }
          }

          /**
          * adds v as an active vertex, but only if its not in the list already
          */
          inline void add_active_node(vertex_descriptor v){
            assert(get_tree(v) != tColorTraits::gray());
            if(m_in_active_list_map[v]){
              return;
            } else{
              m_in_active_list_map[v] = true;
              m_active_nodes.push(v);
            }
          }

          /**
           * finish_node removes a node from the front of the active queue (its called in grow phase, if no more paths can be found using this node)
           */
          inline void finish_node(vertex_descriptor v){
            assert(m_active_nodes.front() == v);
            m_active_nodes.pop();
            m_in_active_list_map[v] = false;
            m_last_grow_vertex = graph_traits<Graph>::null_vertex();
          }

          /**
          * removes a vertex from the queue of active nodes (actually this does nothing,
          * but checks if this node has no parent edge, as this is the criteria for beeing no more active)
          */
          inline void remove_active_node(vertex_descriptor v){
            assert(!has_parent(v));
          }

          /**
          * returns the search tree of v; tColorValue::black() for source tree, white() for sink tree, gray() for no tree
          */
          inline tColorValue get_tree(vertex_descriptor v) const {
            return m_tree_map[v];
          }

          /**
          * sets search tree of v; tColorValue::black() for source tree, white() for sink tree, gray() for no tree
          */
          inline void set_tree(vertex_descriptor v, tColorValue t){
            m_tree_map[v] = t;
          }

          /**
           * returns edge to parent vertex of v;
           */
          inline edge_descriptor get_edge_to_parent(vertex_descriptor v) const{
            return m_pre_map[v];
          }

          /**
           * returns true if the edge stored in m_pre_map[v] is a valid entry
           */
          inline bool has_parent(vertex_descriptor v) const{
            return m_has_parent_map[v];
          }

          /**
           * sets edge to parent vertex of v;
          */
          inline void set_edge_to_parent(vertex_descriptor v, edge_descriptor f_edge_to_parent){
            assert(m_res_cap_map[f_edge_to_parent] > 0);
            m_pre_map[v] = f_edge_to_parent;
            m_has_parent_map[v] = true;
          }

          /**
           * removes the edge to parent of v (this is done by invalidating the entry an additional map)
           */
          inline void set_no_parent(vertex_descriptor v){
            m_has_parent_map[v] = false;
          }

          /**
           * checks if vertex v has a connect to the sink-vertex (@var m_sink)
           * @param v the vertex which is checked
           * @return true if a path to the sink was found, false if not
           */
          inline bool has_sink_connect(vertex_descriptor v){
            tDistanceVal current_distance = 0;
            vertex_descriptor current_vertex = v;
            while(true){
              if(m_time_map[current_vertex] == m_time){
                //we found a node which was already checked this round. use it for distance calculations
                current_distance += m_dist_map[current_vertex];
                break;
              }
              if(current_vertex == m_sink){
                m_time_map[m_sink] = m_time;
                break;
              }
              if(has_parent(current_vertex)){
                //it has a parent, so get it
                current_vertex = target(get_edge_to_parent(current_vertex), m_g);
                ++current_distance;
              } else{
                //no path found
                return false;
              }
            }
            current_vertex=v;
            while(m_time_map[current_vertex] != m_time){
              m_dist_map[current_vertex] = current_distance--;
              m_time_map[current_vertex] = m_time;
              current_vertex = target(get_edge_to_parent(current_vertex), m_g);
            }
            return true;
          }

          /**
           * checks if vertex v has a connect to the source-vertex (@var m_source)
           * @param v the vertex which is checked
           * @return true if a path to the source was found, false if not
           */
          inline bool has_source_connect(vertex_descriptor v){
            tDistanceVal current_distance = 0;
            vertex_descriptor current_vertex = v;
            while(true){
              if(m_time_map[current_vertex] == m_time){
                //we found a node which was already checked this round. use it for distance calculations
                current_distance += m_dist_map[current_vertex];
                break;
              }
              if(current_vertex == m_source){
                m_time_map[m_source] = m_time;
                break;
              }
              if(has_parent(current_vertex)){
                //it has a parent, so get it
                current_vertex = source(get_edge_to_parent(current_vertex), m_g);
                ++current_distance;
              } else{
                //no path found
                return false;
              }
            }
            current_vertex=v;
            while(m_time_map[current_vertex] != m_time){
                m_dist_map[current_vertex] = current_distance-- ;
                m_time_map[current_vertex] = m_time;
                current_vertex = source(get_edge_to_parent(current_vertex), m_g);
            }
            return true;
          }

          /**
          * returns true, if p is closer to a terminal than q
          */
          inline bool is_closer_to_terminal(vertex_descriptor p, vertex_descriptor q){
            //checks the timestamps first, to build no cycles, and after that the real distance
            return (m_time_map[q] <= m_time_map[p] && m_dist_map[q] > m_dist_map[p]+1);
          }

          ////////
          // member vars
          ////////
          Graph& m_g;
          IndexMap m_index_map;
          EdgeCapacityMap m_cap_map;
          ResidualCapacityEdgeMap m_res_cap_map;
          ReverseEdgeMap m_rev_edge_map;
          PredecessorMap m_pre_map; //stores paths found in the growth stage
          ColorMap m_tree_map; //maps each vertex into one of the two search tree or none (gray())
          DistanceMap m_dist_map; //stores distance to source/sink nodes
          vertex_descriptor m_source;
          vertex_descriptor m_sink;

          tQueue m_active_nodes;
          std::vector<bool> m_in_active_list_vec;
          iterator_property_map<std::vector<bool>::iterator, IndexMap> m_in_active_list_map;

          std::list<vertex_descriptor> m_orphans;
          tQueue m_child_orphans; // we use a second queuqe for child orphans, as they are FIFO processed

          std::vector<bool> m_has_parent_vec;
          iterator_property_map<std::vector<bool>::iterator, IndexMap> m_has_parent_map;

          std::vector<long> m_time_vec; //timestamp of each node, used for sink/source-path calculations
          iterator_property_map<std::vector<long>::iterator, IndexMap> m_time_map;
          tEdgeVal m_flow;
          long m_time;
          vertex_descriptor m_last_grow_vertex;
          out_edge_iterator m_last_grow_edge_it;
          out_edge_iterator m_last_grow_edge_end;
    };
  } //namespace detail

  /**
   * non-named-parameter version, given everything
   * this is the catch all version
   */
  template <class Graph, class CapacityEdgeMap, class ResidualCapacityEdgeMap, class ReverseEdgeMap,
    class PredecessorMap, class ColorMap, class DistanceMap, class IndexMap>
  typename property_traits<CapacityEdgeMap>::value_type
  kolmogorov_max_flow
      (Graph& g,
       CapacityEdgeMap cap,
       ResidualCapacityEdgeMap res_cap,
       ReverseEdgeMap rev_map,
       PredecessorMap pre_map,
       ColorMap color,
       DistanceMap dist,
       IndexMap idx,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink
       )
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef typename graph_traits<Graph>::edge_descriptor edge_descriptor;
    //as this method is the last one before we instantiate the solver, we do the concept checks here
    function_requires<VertexListGraphConcept<Graph> >(); //to have vertices(), num_vertices(),
    function_requires<EdgeListGraphConcept<Graph> >(); //to have edges()
    function_requires<IncidenceGraphConcept<Graph> >(); //to have source(), target() and out_edges()
    function_requires<LvaluePropertyMapConcept<CapacityEdgeMap, edge_descriptor> >(); //read flow-values from edges
    function_requires<Mutable_LvaluePropertyMapConcept<ResidualCapacityEdgeMap, edge_descriptor> >(); //write flow-values to residuals
    function_requires<LvaluePropertyMapConcept<ReverseEdgeMap, edge_descriptor> >(); //read out reverse edges
    function_requires<Mutable_LvaluePropertyMapConcept<PredecessorMap, vertex_descriptor> >(); //store predecessor there
    function_requires<Mutable_LvaluePropertyMapConcept<ColorMap, vertex_descriptor> >(); //write corresponding tree
    function_requires<Mutable_LvaluePropertyMapConcept<DistanceMap, vertex_descriptor> >(); //write distance to source/sink
    function_requires<ReadablePropertyMapConcept<IndexMap, vertex_descriptor> >(); //get index 0...|V|-1
    assert(num_vertices(g) >= 2 && src != sink);
    detail::kolmogorov<Graph, CapacityEdgeMap, ResidualCapacityEdgeMap, ReverseEdgeMap, PredecessorMap, ColorMap, DistanceMap, IndexMap>
        algo(g, cap, res_cap, rev_map, pre_map, color, dist, idx, src, sink);
        return algo.max_flow();
  }

  /**
   * non-named-parameter version, given: capacity, residucal_capacity, reverse_edges, and an index map.
   */
  template <class Graph, class CapacityEdgeMap, class ResidualCapacityEdgeMap, class ReverseEdgeMap, class IndexMap>
  typename property_traits<CapacityEdgeMap>::value_type
   kolmogorov_max_flow
       (Graph& g,
       CapacityEdgeMap cap,
       ResidualCapacityEdgeMap res_cap,
       ReverseEdgeMap rev,
       IndexMap idx,
       typename graph_traits<Graph>::vertex_descriptor src,
       typename graph_traits<Graph>::vertex_descriptor sink)
   {
     typename graph_traits<Graph>::vertices_size_type n_verts = num_vertices(g);
     std::vector<typename graph_traits<Graph>::edge_descriptor> predecessor_vec(n_verts);
     std::vector<default_color_type> color_vec(n_verts);
     std::vector<typename graph_traits<Graph>::vertices_size_type> distance_vec(n_verts);
     return kolmogorov_max_flow
         (g, cap, res_cap, rev,
          make_iterator_property_map(predecessor_vec.begin(), idx),
          make_iterator_property_map(color_vec.begin(), idx),
          make_iterator_property_map(distance_vec.begin(), idx),
          idx, src, sink);
   }

  /**
   * non-named-parameter version, some given: capacity, residual_capacity, reverse_edges, color_map and an index map.
   * Use this if you are interested in the minimum cut, as the color map provides that info
   */
   template <class Graph, class CapacityEdgeMap, class ResidualCapacityEdgeMap, class ReverseEdgeMap, class ColorMap, class IndexMap>
   typename property_traits<CapacityEdgeMap>::value_type
   kolmogorov_max_flow
       (Graph& g,
        CapacityEdgeMap cap,
        ResidualCapacityEdgeMap res_cap,
        ReverseEdgeMap rev,
        ColorMap color,
        IndexMap idx,
        typename graph_traits<Graph>::vertex_descriptor src,
        typename graph_traits<Graph>::vertex_descriptor sink)
   {
     typename graph_traits<Graph>::vertices_size_type n_verts = num_vertices(g);
     std::vector<typename graph_traits<Graph>::edge_descriptor> predecessor_vec(n_verts);
     std::vector<typename graph_traits<Graph>::vertices_size_type> distance_vec(n_verts);

     return kolmogorov_max_flow
         (g, cap, res_cap, rev,
          make_iterator_property_map(predecessor_vec.begin(), idx),
          color,
          make_iterator_property_map(distance_vec.begin(), idx),
          idx, src, sink);
   }

  /**
   * named-parameter version, some given
   */
   template <class Graph, class P, class T, class R>
   typename property_traits<typename property_map<Graph, edge_capacity_t>::const_type>::value_type
   kolmogorov_max_flow
       (Graph& g,
        typename graph_traits<Graph>::vertex_descriptor src,
        typename graph_traits<Graph>::vertex_descriptor sink,
        const bgl_named_params<P, T, R>& params)
   {
     return kolmogorov_max_flow(g,
                                choose_const_pmap(get_param(params, edge_capacity), g, edge_capacity),
                                choose_pmap(get_param(params, edge_residual_capacity), g, edge_residual_capacity),
                                choose_const_pmap(get_param(params, edge_reverse), g, edge_reverse),
                                choose_pmap(get_param(params, vertex_predecessor), g, vertex_predecessor),
                                choose_pmap(get_param(params, vertex_color), g, vertex_color),
                                choose_pmap(get_param(params, vertex_distance), g, vertex_distance),
                                choose_const_pmap(get_param(params, vertex_index), g, vertex_index),
                                src, sink);
   }

  /**
   * named-parameter version, none given
   */
   template <class Graph>
   typename property_traits<typename property_map<Graph, edge_capacity_t>::const_type>::value_type
   kolmogorov_max_flow
       (Graph& g,
        typename graph_traits<Graph>::vertex_descriptor src,
        typename graph_traits<Graph>::vertex_descriptor sink)
   {
     bgl_named_params<int, buffer_param_t> params(0); // bogus empty param
     return kolmogorov_max_flow(g, src, sink, params);
   }
} // namespace boost

#endif // BOOST_KOLMOGOROV_MAX_FLOW_HPP


/**
 * @file dens_matrix.cpp
 * @brief full matrix
 * @date 2009-12-08
 */
#ifdef BSPY_EXPORTING_PLUGIN
#include <boost/python.hpp>
#endif
#include <sstream>

#include "dens_matrix.h"
#include "bcsr_matrix_iface.h"

#include "pyublas/numpy.hpp"

using namespace std;
using namespace boost::python;

namespace blue_sky
{
  dens_matrix::dens_matrix (bs_type_ctor_param)
        : dens_matrix_iface (),
        values (BS_KERNEL.create_object (v_float::bs_type ()))
    {
    }

  dens_matrix ::dens_matrix (const this_t & /*src*/) : bs_refcounter (),
        values (BS_KERNEL.create_object (v_float::bs_type ()))
    {
    }

  // TODO:
  int
  dens_matrix::matrix_vector_product (spv_double v_, spv_double r_) const
    {
      t_long block_rows;
      t_long block_cols;
      t_long clb = calc_block_size < 1 ? (n_rows + n_cols) : calc_block_size;

      if (n_cols != (t_long) v_->size () || n_rows != (t_long) r_->size ())
        return -1;

      t_double *v = &(*v_)[0];
      t_double *r = &(*r_)[0];

      block_rows = n_rows / clb;
      if (n_rows % clb)
        ++block_rows;

      block_cols = n_cols / clb;
      if (n_cols % clb)
        ++block_cols;

      for (t_long i = 0; i < block_rows; ++i)
        {
          for (t_long j = 0; j < block_cols; ++j)
            {
              block_mv_product (i, j, v, r);
            }
        }
      return 0;
    }

  // TODO:
  int
  dens_matrix::matrix_vector_product_t (spv_double v_,
                                                 spv_double r_) const
    {
      t_long block_rows;
      t_long block_cols;
      t_long clb = calc_block_size < 1 ? (n_rows + n_cols) : calc_block_size;

      if (n_cols != (t_long)r_->size () || n_rows != (t_long)v_->size ())
        return -1;

      t_double *v = &(*v_)[0];
      t_double *r = &(*r_)[0];

      block_rows = n_rows / clb;
      if (n_rows % clb)
        ++block_rows;

      block_cols = n_cols / clb;
      if (n_cols % clb)
        ++block_cols;

      for (t_long i = 0; i < block_rows; ++i)
        {
          for (t_long j = 0; j < block_cols; ++j)
            {
              block_mv_product_t (i, j, v, r);
            }
        }
      return 0;
    }

  int
  dens_matrix::calc_lin_comb (t_double alpha,
                                       t_double beta,
                                       spv_double u_,
                                       spv_double v_,
                                       spv_double r_) const
    {
      static const t_double eps = t_double (1.0e-12);
      t_long i;
      int r_code = 0;

      t_double *v = &(*v_)[0];
      t_double *r = &(*r_)[0];

      memset (r, 0, sizeof (t_double) * n_rows);

      if (fabs (alpha) > eps)
        {
          r_code = matrix_vector_product (u_, r_);
          for (i = 0; i < n_rows; ++i)
            r[i] *= alpha;
        }
      if (fabs (beta) > eps)
        {
          for (i = 0; i < n_rows; ++i)
            r[i] += v[i] * beta;
        }
      return r_code;
    }

  int
  dens_matrix::init_by_matrix (sp_base_t matrix_)
    {
      BS_ASSERT (matrix_);

      typedef smart_ptr<bcsr_matrix_iface, true>  sp_bcsr_matrix_iface;

      sp_dens_matrix_iface_t dense_matrix_ (matrix_, bs_dynamic_cast ());
      sp_bcsr_matrix_iface   bcsr_matrix_  (matrix_, bs_dynamic_cast ());

      if (dense_matrix_)
        {
          init (dense_matrix_->get_n_rows (), dense_matrix_->get_n_cols (),
                dense_matrix_->get_calc_block_size ());
          //TODO: copy values
        }
      else if (bcsr_matrix_)
        {
          t_long i, j, j1, j2, cl;
          if (bcsr_matrix_->get_n_block_size () > 1)
            {
              bs_throw_exception ("dens_matrix::init_by_matrix (bcsr) n_block_size must be = 1");
            }

          if (init (bcsr_matrix_->get_n_rows (), bcsr_matrix_->get_n_cols (),
                    bcsr_matrix_->get_n_block_size ()))
             return -3;

          t_long  *rows = &(*bcsr_matrix_->get_rows_ptr ())[0];
          t_long  *cols = &(*bcsr_matrix_->get_cols_ind ())[0];
          t_float *vals = &(*bcsr_matrix_->get_values ())[0];
          t_float *values_ptr = &(*values)[0];

          // set values zero. then copy matrix values to dense
          memset (values_ptr, 0, n_rows * n_cols * sizeof (t_float));
          for (i = 0; i < n_rows; ++i)
            {
              j1 = rows[i];
              j2 = rows[i + 1];
              for (j = j1; j < j2; ++j)
                {
                  cl = cols[j];
                  values_ptr[i * n_rows + cl] = vals[j];
                }
            }
        }
      else
        {
          bs_throw_exception ("dens_matrix::init_by_matrix: wrong type");
        }
      return 0;
    }

  int
  dens_matrix::init (const t_long new_n_rows,
                     const t_long new_n_cols,
                     const t_long block_size)
    {
      npy_intp dims[2];
      if (new_n_rows < 1 || new_n_cols < 1)
        return -1;
      dims[0] = n_rows = new_n_rows;
      dims[1] = n_cols = new_n_cols;
      calc_block_size = block_size;
      values->resize (n_rows * n_cols);
      // TODO: add this method
      //values->reshape (2, dims);
      //std::fill (values->numpy.as_ublas ().data ().begin (), values->numpy.as_ublas ().data ().end (), 0);
      if (values->size ())
        std::fill (values->begin (), values->end (), 0);
      return 0;
    }

  int
  dens_matrix::copy (sp_dens_matrix_iface_t matrix)
    {
      if (init (matrix->get_n_rows (), matrix->get_n_cols (), matrix->get_calc_block_size ()))
        return -3;
      values = matrix->get_values ()->clone ();
      //values->numpy.resize (matrix->get_values ()->numpy.size1 (), matrix->get_values ()->numpy.size2 ());
      //values->numpy.as_ublas () = matrix->get_values ()->numpy.as_ublas ();
                              //                  matrix->get_values ()->numpy.as_ublas ().data ().end ());

      //memcpy (&values[0], &(matrix->get_values_const ())[0], n_rows * n_cols * sizeof (t_float));
      return 0;
    }

  void
  dens_matrix::block_mv_product (t_long row_block,
                                          t_long col_block,
                                          const t_double *v,
                                          t_double *r) const
    {
      t_long block_n_rows;
      t_long block_n_cols;
      const t_float *block;
      const t_double *block_v;
      t_double *block_r;
      t_long clb = calc_block_size < 1 ? (n_rows + n_cols) : calc_block_size;

      // calculate block start position
      //block = &values[row_block * clb * n_cols + col_block * clb];
      block = &(*values)[0] + row_block * clb * n_cols + col_block * clb;
      block_v = v + col_block * clb;
      block_r = r + row_block * clb;

      if ((row_block + 1) * clb <= n_rows)
        block_n_rows = clb;
      else
        block_n_rows = n_rows - row_block * clb;

      if ((col_block + 1) * clb <= n_cols)
        block_n_cols = clb;
      else
        block_n_cols = n_cols - col_block * clb;

      for (t_long i = 0; i < block_n_rows; ++i)
        {
          for (t_long j = 0; j < block_n_cols; ++j)
            {
              block_r[i] += block[i * n_rows + j] * block_v[j];
            }
        }
    }

  void
  dens_matrix::block_mv_product_t (t_long row_block,
                                            t_long col_block,
                                            const t_double *v,
                                            t_double *r) const
    {
      t_long block_n_rows;
      t_long block_n_cols;
      const t_float *block;
      const t_double *block_v;
      t_double *block_r;
      t_long clb = calc_block_size < 1 ? (n_rows + n_cols) : calc_block_size;

      // calculate block start position
      block = &(*values)[0] + row_block * clb * n_cols + col_block * clb;
      block_v = v + row_block * clb;
      block_r = r + col_block * clb;

      if ((row_block + 1) * clb <= n_rows)
        block_n_rows = clb;
      else
        block_n_rows = n_rows - row_block * clb;

      if ((col_block + 1) * clb <= n_cols)
        block_n_cols = clb;
      else
        block_n_cols = n_cols - col_block * clb;

      for (t_long i = 0; i < block_n_rows; ++i)
        {
          for (t_long j = 0; j < block_n_cols; ++j)
            {
              block_r[j] += block[i * n_rows + j] * block_v[i];
            }
        }
    }

#ifdef BSPY_EXPORTING_PLUGIN
  std::string
  dens_matrix::py_str () const
    {
      stringstream s;

      s << "Dens matrix\n--------------------------------\n";
      s << "\tRows:                  " << n_rows << "\n";
      s << "\tColumns:               " << n_cols << "\n";
      s << "\tCalc Block:            " << calc_block_size << "\n";
      s << "\tAllocated memory (Mb): " << get_allocated_memory_in_mbytes () << "\n";
      s << "--------------------------------\n";
      return s.str ();
    }
#endif //BSPY_EXPORTING_PLUGIN
/////////////////////////////////BS Register
/////////////////////////////////Stuff//////////////////////////

  BLUE_SKY_TYPE_STD_CREATE (dens_matrix);
  BLUE_SKY_TYPE_STD_COPY (dens_matrix);

  BLUE_SKY_TYPE_IMPL (dens_matrix, dens_matrix_iface,  "dens_matrix", "Dens Matrix class", "Realization of Dens Matricies");
}  // blue_sky namespace


// Boost.Geometry - gis-projections (based on PROJ4)

// Copyright (c) 2008-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2017, 2018.
// Modifications copyright (c) 2017-2018, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle.

// 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)

// This file is converted from PROJ4, http://trac.osgeo.org/proj
// PROJ4 is originally written by Gerald Evenden (then of the USGS)
// PROJ4 is maintained by Frank Warmerdam
// PROJ4 is converted to Boost.Geometry by Barend Gehrels

// Last updated version of proj: 5.0.0

// Original copyright notice:

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

#ifndef BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP
#define BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP

#include <boost/geometry/util/math.hpp>
#include <cstdio>

#include <boost/geometry/srs/projections/impl/base_static.hpp>
#include <boost/geometry/srs/projections/impl/base_dynamic.hpp>
#include <boost/geometry/srs/projections/impl/projects.hpp>
#include <boost/geometry/srs/projections/impl/factory_entry.hpp>
#include <boost/geometry/srs/projections/impl/aasincos.hpp>

namespace boost { namespace geometry
{

namespace projections
{
    #ifndef DOXYGEN_NO_DETAIL
    namespace detail { namespace chamb
    {

            //static const double third = 0.333333333333333333;
            static const double tolerance = 1e-9;

            // specific for 'chamb'
            template <typename T>
            struct vect_ra { T r, Az; };
            template <typename T>
            struct point_xy { T x, y; };

            template <typename T>
            struct par_chamb
            {
                struct { /* control point data */
                    T phi, lam;
                    T cosphi, sinphi;
                    vect_ra<T> v;
                    point_xy<T> p;
                    T Az;
                } c[3];
                point_xy<T> p;
                T beta_0, beta_1, beta_2;
            };

            /* distance and azimuth from point 1 to point 2 */
            template <typename T>
            inline vect_ra<T> vect(T const& dphi, T const& c1, T const& s1, T const& c2, T const& s2, T const& dlam)
            {
                vect_ra<T> v;
                T cdl, dp, dl;

                cdl = cos(dlam);
                if (fabs(dphi) > 1. || fabs(dlam) > 1.)
                    v.r = aacos(s1 * s2 + c1 * c2 * cdl);
                else { /* more accurate for smaller distances */
                    dp = sin(.5 * dphi);
                    dl = sin(.5 * dlam);
                    v.r = 2. * aasin(sqrt(dp * dp + c1 * c2 * dl * dl));
                }
                if (fabs(v.r) > tolerance)
                    v.Az = atan2(c2 * sin(dlam), c1 * s2 - s1 * c2 * cdl);
                else
                    v.r = v.Az = 0.;
                return v;
            }

            /* law of cosines */
            template <typename T>
            inline T lc(T const& b, T const& c, T const& a)
            {
                return aacos(.5 * (b * b + c * c - a * a) / (b * c));
            }

            // template class, using CRTP to implement forward/inverse
            template <typename T, typename Parameters>
            struct base_chamb_spheroid
                : public base_t_f<base_chamb_spheroid<T, Parameters>, T, Parameters>
            {
                par_chamb<T> m_proj_parm;

                inline base_chamb_spheroid(const Parameters& par)
                    : base_t_f<base_chamb_spheroid<T, Parameters>, T, Parameters>(*this, par)
                {}

                // FORWARD(s_forward)  spheroid
                // Project coordinates from geographic (lon, lat) to cartesian (x, y)
                inline void fwd(T const& lp_lon, T const& lp_lat, T& xy_x, T& xy_y) const
                {
                    static const T third = detail::third<T>();

                    T sinphi, cosphi, a;
                    vect_ra<T> v[3];
                    int i, j;

                    sinphi = sin(lp_lat);
                    cosphi = cos(lp_lat);
                    for (i = 0; i < 3; ++i) { /* dist/azimiths from control */
                        v[i] = vect(lp_lat - this->m_proj_parm.c[i].phi, this->m_proj_parm.c[i].cosphi, this->m_proj_parm.c[i].sinphi,
                            cosphi, sinphi, lp_lon - this->m_proj_parm.c[i].lam);
                        if (v[i].r == 0.0)
                            break;
                        v[i].Az = adjlon(v[i].Az - this->m_proj_parm.c[i].v.Az);
                    }
                    if (i < 3) /* current point at control point */
                        { xy_x = this->m_proj_parm.c[i].p.x; xy_y = this->m_proj_parm.c[i].p.y; }
                    else { /* point mean of intersepts */
                        { xy_x = this->m_proj_parm.p.x; xy_y = this->m_proj_parm.p.y; }
                        for (i = 0; i < 3; ++i) {
                            j = i == 2 ? 0 : i + 1;
                            a = lc(this->m_proj_parm.c[i].v.r, v[i].r, v[j].r);
                            if (v[i].Az < 0.)
                                a = -a;
                            if (! i) { /* coord comp unique to each arc */
                                xy_x += v[i].r * cos(a);
                                xy_y -= v[i].r * sin(a);
                            } else if (i == 1) {
                                a = this->m_proj_parm.beta_1 - a;
                                xy_x -= v[i].r * cos(a);
                                xy_y -= v[i].r * sin(a);
                            } else {
                                a = this->m_proj_parm.beta_2 - a;
                                xy_x += v[i].r * cos(a);
                                xy_y += v[i].r * sin(a);
                            }
                        }
                        xy_x *= third; /* mean of arc intercepts */
                        xy_y *= third;
                    }
                }

                static inline std::string get_name()
                {
                    return "chamb_spheroid";
                }

            };

            template <typename T>
            inline T chamb_init_lat(srs::detail::proj4_parameters const& params, int i)
            {
                static const std::string lat[3] = {"lat_1", "lat_2", "lat_3"};
                return _pj_get_param_r<T>(params, lat[i]);
            }
            template <typename T>
            inline T chamb_init_lat(srs::dpar::parameters<T> const& params, int i)
            {
                static const srs::dpar::name_r lat[3] = {srs::dpar::lat_1, srs::dpar::lat_2, srs::dpar::lat_3};
                return _pj_get_param_r<T>(params, lat[i]);
            }

            template <typename T>
            inline T chamb_init_lon(srs::detail::proj4_parameters const& params, int i)
            {
                static const std::string lon[3] = {"lon_1", "lon_2", "lon_3"};
                return _pj_get_param_r<T>(params, lon[i]);
            }
            template <typename T>
            inline T chamb_init_lon(srs::dpar::parameters<T> const& params, int i)
            {
                static const srs::dpar::name_r lon[3] = {srs::dpar::lon_1, srs::dpar::lon_2, srs::dpar::lon_3};
                return _pj_get_param_r<T>(params, lon[i]);
            }

            // Chamberlin Trimetric
            template <typename Params, typename Parameters, typename T>
            inline void setup_chamb(Params const& params, Parameters& par, par_chamb<T>& proj_parm)
            {
                static const T pi = detail::pi<T>();

                int i, j;

                for (i = 0; i < 3; ++i) { /* get control point locations */
                    proj_parm.c[i].phi = chamb_init_lat<T>(params, i);
                    proj_parm.c[i].lam = chamb_init_lon<T>(params, i);
                    proj_parm.c[i].lam = adjlon(proj_parm.c[i].lam - par.lam0);
                    proj_parm.c[i].cosphi = cos(proj_parm.c[i].phi);
                    proj_parm.c[i].sinphi = sin(proj_parm.c[i].phi);
                }
                for (i = 0; i < 3; ++i) { /* inter ctl pt. distances and azimuths */
                    j = i == 2 ? 0 : i + 1;
                    proj_parm.c[i].v = vect(proj_parm.c[j].phi - proj_parm.c[i].phi, proj_parm.c[i].cosphi, proj_parm.c[i].sinphi,
                        proj_parm.c[j].cosphi, proj_parm.c[j].sinphi, proj_parm.c[j].lam - proj_parm.c[i].lam);
                    if (proj_parm.c[i].v.r == 0.0)
                        BOOST_THROW_EXCEPTION( projection_exception(error_control_point_no_dist) );
                    /* co-linearity problem ignored for now */
                }
                proj_parm.beta_0 = lc(proj_parm.c[0].v.r, proj_parm.c[2].v.r, proj_parm.c[1].v.r);
                proj_parm.beta_1 = lc(proj_parm.c[0].v.r, proj_parm.c[1].v.r, proj_parm.c[2].v.r);
                proj_parm.beta_2 = pi - proj_parm.beta_0;
                proj_parm.p.y = 2. * (proj_parm.c[0].p.y = proj_parm.c[1].p.y = proj_parm.c[2].v.r * sin(proj_parm.beta_0));
                proj_parm.c[2].p.y = 0.;
                proj_parm.c[0].p.x = - (proj_parm.c[1].p.x = 0.5 * proj_parm.c[0].v.r);
                proj_parm.p.x = proj_parm.c[2].p.x = proj_parm.c[0].p.x + proj_parm.c[2].v.r * cos(proj_parm.beta_0);

                par.es = 0.;
            }

    }} // namespace detail::chamb
    #endif // doxygen

    /*!
        \brief Chamberlin Trimetric projection
        \ingroup projections
        \tparam Geographic latlong point type
        \tparam Cartesian xy point type
        \tparam Parameters parameter type
        \par Projection characteristics
         - Miscellaneous
         - Spheroid
         - no inverse
        \par Projection parameters
         - lat_1: Latitude of control point 1 (degrees)
         - lon_1: Longitude of control point 1 (degrees)
         - lat_2: Latitude of control point 2 (degrees)
         - lon_2: Longitude of control point 2 (degrees)
         - lat_3: Latitude of control point 3 (degrees)
         - lon_3: Longitude of control point 3 (degrees)
        \par Example
        \image html ex_chamb.gif
    */
    template <typename T, typename Parameters>
    struct chamb_spheroid : public detail::chamb::base_chamb_spheroid<T, Parameters>
    {
        template <typename Params>
        inline chamb_spheroid(Params const& params, Parameters const& par)
            : detail::chamb::base_chamb_spheroid<T, Parameters>(par)
        {
            detail::chamb::setup_chamb(params, this->m_par, this->m_proj_parm);
        }
    };

    #ifndef DOXYGEN_NO_DETAIL
    namespace detail
    {

        // Static projection
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_STATIC_PROJECTION(srs::spar::proj_chamb, chamb_spheroid, chamb_spheroid)

        // Factory entry(s)
        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_ENTRY_F(chamb_entry, chamb_spheroid)

        BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_BEGIN(chamb_init)
        {
            BOOST_GEOMETRY_PROJECTIONS_DETAIL_FACTORY_INIT_ENTRY(chamb, chamb_entry);
        }

    } // namespace detail
    #endif // doxygen

} // namespace projections

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_PROJECTIONS_CHAMB_HPP