#include "computing_data.h"

#include <deal.II/fe/fe_update_flags.h>
#include <deal.II/fe/fe_dgq.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_raviart_thomas.h>

XSections::XSections (MaterialBase& material)
    :
    diffusion_coef(material.GetDiffusionCoef()),
    sigt(material.GetSigT()),
    inv_sigt(material.GetInvSigT()),
    q(material.GetQ()),
    q_per_ster(material.GetQPerSter()),
    is_material_fissile(material.GetFissileIDMap()),
    nu_sigf(material.GetNuSigF()),
    sigs(material.GetSigS()),
    sigs_per_ster(material.GetSigSPerSter()),
    fiss_transfer(material.GetChiNuSigF()),
    fiss_transfer_per_ster(material.GetChiNuSigFPerSter())
{}

template <int dim>
FundamentalData<dim>::FundamentalData (dealii::ParameterHandler &prm,
    dealii::Triangulation<dim> &tria)
    :
    pcout(std::cout,
        (dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD)==0)),
    aq(AQBase<dim>::CreateAQ(prm)),
    //material(prm),
    mesh(prm),
    mat_vec(std::shared_ptr<MatrixVector> (new MatrixVector())),
    //xsec(std::shared_ptr<XSections> (new XSections(material))),
    fe_data(prm),
    dof_handler(tria) {
  MaterialProtobuf material{prm};
  xsec = std::make_shared<XSections>(material);
  aq->MakeAQ();
}

template <int dim>
FundamentalData<dim>::~FundamentalData () {}

template <int dim>
FEData<dim>::FEData (const dealii::ParameterHandler &prm) {
  InitializeFESpaces(prm);
  const dealii::UpdateFlags update_flags =
      dealii::update_values | dealii::update_gradients |
      dealii::update_quadrature_points |
      dealii::update_JxW_values;
  const dealii::UpdateFlags update_face_flags = dealii::update_normal_vectors |
      dealii::update_values | dealii::update_gradients |
      dealii::update_quadrature_points |
      dealii::update_JxW_values;

  for (auto &f : fe) {
    // f.first is the equation name, f.second is the correspoding FiniteElement
    // object
    // TODO: all fe use the same finite element for now.
    if (f.first!="nda" && f.first!="tg_nda")
      discretization[f.first] = prm.get("ho spatial discretization");
    else
      discretization[f.first] = prm.get("nda spatial discretization");
    p_order[f.first] = prm.get_integer("finite element polynomial degree");
    q_rule[f.first] = std::shared_ptr<dealii::QGauss<dim>>(
        new dealii::QGauss<dim>(p_order[f.first]+1));
    qf_rule[f.first] = std::shared_ptr<dealii::QGauss<dim-1>>(
        new dealii::QGauss<dim-1>(p_order[f.first]+1));

    fv[f.first] = std::shared_ptr<dealii::FEValues<dim>> (
        new dealii::FEValues<dim> (*f.second, *q_rule[f.first], update_flags));

    fvf[f.first] = std::shared_ptr<dealii::FEFaceValues<dim>> (
        new dealii::FEFaceValues<dim>(
            *f.second, *qf_rule[f.first], update_face_flags));

    fvf_nei[f.first] = std::shared_ptr<dealii::FEFaceValues<dim>> (
        new dealii::FEFaceValues<dim>(
            *f.second, *qf_rule[f.first], update_face_flags));

    dofs_per_cell[f.first] = fe[f.first]->dofs_per_cell;
    n_q[f.first] = q_rule[f.first]->size();
    n_qf[f.first] = qf_rule[f.first]->size();

    local_dof_indices[f.first].resize (dofs_per_cell[f.first]);
    neigh_dof_indices[f.first].resize (dofs_per_cell[f.first]);

    if (f.first=="nda") {
      // corrections related objects
      q_rule["corr"] = std::shared_ptr<dealii::QGauss<dim>>(
          new dealii::QGauss<dim>(p_order[f.first]+3));
      qf_rule["corr"] = std::shared_ptr<dealii::QGauss<dim-1>>(
          new dealii::QGauss<dim-1>(p_order[f.first]+3));

      fv["corr"] = std::shared_ptr<dealii::FEValues<dim>> (
          new dealii::FEValues<dim> (*f.second, *q_rule["corr"], update_face_flags));

      fvf["corr"] = std::shared_ptr<dealii::FEFaceValues<dim>> (
          new dealii::FEFaceValues<dim>(
              *f.second, *qf_rule["corr"], update_face_flags));

      fvf_nei["corr"] = std::shared_ptr<dealii::FEFaceValues<dim>> (
          new dealii::FEFaceValues<dim>(
              *f.second, *qf_rule["corr"], update_face_flags));

      n_q["corr"] = q_rule["corr"]->size();
      n_qf["corr"] = qf_rule["corr"]->size();
    }
  }
}

template <int dim>
void FEData<dim>::InitializeFESpaces(const dealii::ParameterHandler &prm) {
    // getting parameter values
  const bool do_nda = prm.get_bool ("do nda");
  const int p_order = prm.get_integer("finite element polynomial degree");
  const std::string ho_discretization = prm.get ("ho spatial discretization");
  const std::string nda_discretization = prm.get ("nda spatial discretization");
  const std::string ho_equ_name = prm.get ("transport model");

  std::unordered_map<std::string, int> discretization_ind = {
      {"cfem", 0}, {"dfem", 1}, {"cmfd", 2}, {"rtk", 3}};

  switch (discretization_ind[ho_discretization]) {
    case 0:
      fe[ho_equ_name] = new dealii::FE_Q<dim> (p_order);
      break;

    case 1:
      fe[ho_equ_name] = new dealii::FE_DGQ<dim> (p_order);
      break;

    default:
      AssertThrow (false,
          dealii::ExcMessage("Invalid HO discretization name"));
      break;
  }

  if (do_nda) {
    switch (discretization_ind[nda_discretization]) {
      case 0:
        fe["nda"] = new dealii::FE_Q<dim> (p_order);
        break;

      case 1:
        fe["nda"] = new dealii::FE_DGQ<dim> (p_order);
        break;

      case 2:
        fe["nda"] = new dealii::FE_DGQ<dim> (0);
        break;

      case 3:
        fe["nda"] = new dealii::FE_RaviartThomas<dim> (p_order);
        break;

      default:
        AssertThrow (false,
            dealii::ExcMessage("Invalid NDA discretization name"));
        break;
    }
  }
}

template struct FundamentalData<1>;
template struct FundamentalData<2>;
template struct FundamentalData<3>;

template struct FEData<1>;
template struct FEData<2>;
template struct FEData<3>;


/*
 Copyright (C) 2020 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 <orea/aggregation/dimregressioncalculator.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/vectorutils.hpp>
#include <ql/errors.hpp>
#include <ql/time/calendars/weekendsonly.hpp>
#include <ql/version.hpp>

#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/generallinearleastsquares.hpp>
#include <ql/math/kernelfunctions.hpp>
#include <ql/methods/montecarlo/lsmbasissystem.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#include <qle/math/nadarayawatson.hpp>
#include <qle/math/stabilisedglls.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>

using namespace std;
using namespace QuantLib;

using namespace boost::accumulators;

namespace ore {
namespace analytics {

RegressionDynamicInitialMarginCalculator::RegressionDynamicInitialMarginCalculator(
    const boost::shared_ptr<Portfolio>& portfolio, const boost::shared_ptr<NPVCube>& cube,
    const boost::shared_ptr<CubeInterpretation>& cubeInterpretation,
    const boost::shared_ptr<AggregationScenarioData>& scenarioData, Real quantile, Size horizonCalendarDays,
    Size regressionOrder, std::vector<std::string> regressors, Size localRegressionEvaluations,
    Real localRegressionBandWidth, const std::map<std::string, Real>& currentIM)
    : DynamicInitialMarginCalculator(portfolio, cube, cubeInterpretation, scenarioData, quantile, horizonCalendarDays,
                                     currentIM),
      regressionOrder_(regressionOrder), regressors_(regressors),
      localRegressionEvaluations_(localRegressionEvaluations), localRegressionBandWidth_(localRegressionBandWidth) {
    Size dates = cube_->dates().size();
    Size samples = cube_->samples();
    for (Size i = 0; i < nettingSetIds_.size(); ++i) {
        regressorArray_[nettingSetIds_[i]] = vector<vector<Array>>(dates, vector<Array>(samples));
        nettingSetLocalDIM_[nettingSetIds_[i]] = vector<vector<Real>>(dates, vector<Real>(samples, 0.0));
        nettingSetZeroOrderDIM_[nettingSetIds_[i]] = vector<Real>(dates, 0.0);
        nettingSetSimpleDIMh_[nettingSetIds_[i]] = vector<Real>(dates, 0.0);
        nettingSetSimpleDIMp_[nettingSetIds_[i]] = vector<Real>(dates, 0.0);
    }
}

const vector<vector<Real>>&
RegressionDynamicInitialMarginCalculator::localRegressionResults(const std::string& nettingSet) {
    if (nettingSetLocalDIM_.find(nettingSet) != nettingSetLocalDIM_.end())
        return nettingSetLocalDIM_[nettingSet];
    else
        QL_FAIL("netting set " << nettingSet << " not found in Local DIM results");
}

const vector<Real>& RegressionDynamicInitialMarginCalculator::zeroOrderResults(const std::string& nettingSet) {
    if (nettingSetZeroOrderDIM_.find(nettingSet) != nettingSetZeroOrderDIM_.end())
        return nettingSetZeroOrderDIM_[nettingSet];
    else
        QL_FAIL("netting set " << nettingSet << " not found in Zero Order DIM results");
}

const vector<Real>& RegressionDynamicInitialMarginCalculator::simpleResultsUpper(const std::string& nettingSet) {
    if (nettingSetSimpleDIMp_.find(nettingSet) != nettingSetSimpleDIMp_.end())
        return nettingSetSimpleDIMp_[nettingSet];
    else
        QL_FAIL("netting set " << nettingSet << " not found in Simple DIM (p) results");
}

const vector<Real>& RegressionDynamicInitialMarginCalculator::simpleResultsLower(const std::string& nettingSet) {
    if (nettingSetSimpleDIMh_.find(nettingSet) != nettingSetSimpleDIMh_.end())
        return nettingSetSimpleDIMh_[nettingSet];
    else
        QL_FAIL("netting set " << nettingSet << " not found in Simple DIM (c) results");
}

void RegressionDynamicInitialMarginCalculator::build() {
    LOG("DIM Analysis by polynomial regression");

    map<string, Real> currentDim = unscaledCurrentDIM();

    Size stopDatesLoop = datesLoopSize_;
    Size samples = cube_->samples();

    Size polynomOrder = regressionOrder_;
    LOG("DIM regression polynom order = " << regressionOrder_);
    LsmBasisSystem::PolynomType polynomType = LsmBasisSystem::Monomial;
    Size regressionDimension = regressors_.empty() ? 1 : regressors_.size();
    LOG("DIM regression dimension = " << regressionDimension);
#if QL_HEX_VERSION > 0x01150000
    std::vector<ext::function<Real(Array)>> v(
        LsmBasisSystem::multiPathBasisSystem(regressionDimension, polynomOrder, polynomType));
#else // QL 1.14 and below
    std::vector<boost::function1<Real, Array>> v(
        LsmBasisSystem::multiPathBasisSystem(regressionDimension, polynomOrder, polynomType));
#endif
    Real confidenceLevel = QuantLib::InverseCumulativeNormal()(quantile_);
    LOG("DIM confidence level " << confidenceLevel);

    Size simple_dim_index_h = Size(floor(quantile_ * (samples - 1) + 0.5));
    Size simple_dim_index_p = Size(floor((1.0 - quantile_) * (samples - 1) + 0.5));

    Size nettingSetCount = 0;
    for (auto n : nettingSetIds_) {
        LOG("Process netting set " << n);

        if (currentIM_.find(n) != currentIM_.end()) {
            Real t0im = currentIM_[n];
            QL_REQUIRE(currentDim.find(n) != currentDim.end(), "current DIM not found for netting set " << n);
            Real t0dim = currentDim[n];
            Real t0scaling = t0im / t0dim;
            LOG("t0 scaling for netting set " << n << ": t0im" << t0im << " t0dim=" << t0dim
                                              << " t0scaling=" << t0scaling);
            nettingSetScaling_[n] = t0scaling;
        }

        Real nettingSetDimScaling =
            nettingSetScaling_.find(n) == nettingSetScaling_.end() ? 1.0 : nettingSetScaling_[n];
        LOG("Netting set DIM scaling factor: " << nettingSetDimScaling);

        for (Size j = 0; j < stopDatesLoop; ++j) {
            accumulator_set<double, stats<tag::mean, tag::variance>> accDiff;
            accumulator_set<double, stats<tag::mean>> accOneOverNumeraire;
            for (Size k = 0; k < samples; ++k) {
                Real numDefault = cubeInterpretation_->getDefaultAggrionScenarioData(
                    scenarioData_, AggregationScenarioDataType::Numeraire, j, k);
                Real numCloseOut = cubeInterpretation_->getCloseOutAggrionScenarioData(
                    scenarioData_, AggregationScenarioDataType::Numeraire, j, k);
                Real npvDefault = nettingSetNPV_[n][j][k];
                Real flow = nettingSetFLOW_[n][j][k];
                Real npvCloseOut = nettingSetCloseOutNPV_[n][j][k];
                accDiff((npvCloseOut * numCloseOut) + (flow * numDefault) - (npvDefault * numDefault));
                accOneOverNumeraire(1.0 / numDefault);
            }

            Size mporCalendarDays = cubeInterpretation_->getMporCalendarDays(cube_, j);
            Real horizonScaling = sqrt(1.0 * horizonCalendarDays_ / mporCalendarDays);

            Real stdevDiff = sqrt(variance(accDiff));
            Real E_OneOverNumeraire =
                mean(accOneOverNumeraire); // "re-discount" (the stdev is calculated on non-discounted deltaNPVs)

            nettingSetZeroOrderDIM_[n][j] = stdevDiff * horizonScaling * confidenceLevel;
            nettingSetZeroOrderDIM_[n][j] *= E_OneOverNumeraire;

            vector<Real> rx0(samples, 0.0);
            vector<Array> rx(samples, Array());
            vector<Real> ry1(samples, 0.0);
            vector<Real> ry2(samples, 0.0);
            for (Size k = 0; k < samples; ++k) {
                Real numDefault = cubeInterpretation_->getDefaultAggrionScenarioData(
                    scenarioData_, AggregationScenarioDataType::Numeraire, j, k);
                Real numCloseOut = cubeInterpretation_->getCloseOutAggrionScenarioData(
                    scenarioData_, AggregationScenarioDataType::Numeraire, j, k);
                Real x = nettingSetNPV_[n][j][k] * numDefault;
                Real f = nettingSetFLOW_[n][j][k] * numDefault;
                Real y = nettingSetCloseOutNPV_[n][j][k] * numCloseOut;
                Real z = (y + f - x);
                rx[k] = regressors_.empty() ? Array(1, nettingSetNPV_[n][j][k]) : regressorArray(n, j, k);
                rx0[k] = rx[k][0];
                ry1[k] = z;     // for local regression
                ry2[k] = z * z; // for least squares regression
                nettingSetDeltaNPV_[n][j][k] = z;
                regressorArray_[n][j][k] = rx[k];
            }
            vector<Real> delNpvVec_copy = nettingSetDeltaNPV_[n][j];
            sort(delNpvVec_copy.begin(), delNpvVec_copy.end());
            Real simpleDim_h = delNpvVec_copy[simple_dim_index_h];
            Real simpleDim_p = delNpvVec_copy[simple_dim_index_p];
            simpleDim_h *= horizonScaling;                                  // the usual scaling factors
            simpleDim_p *= horizonScaling;                                  // the usual scaling factors
            nettingSetSimpleDIMh_[n][j] = simpleDim_h * E_OneOverNumeraire; // discounted DIM
            nettingSetSimpleDIMp_[n][j] = simpleDim_p * E_OneOverNumeraire; // discounted DIM

            QL_REQUIRE(rx.size() > v.size(), "not enough points for regression with polynom order " << polynomOrder);
            if (close_enough(stdevDiff, 0.0)) {
                LOG("DIM: Zero std dev estimation at step " << j);
                // Skip IM calculation if all samples have zero NPV (e.g. after latest maturity)
                for (Size k = 0; k < samples; ++k) {
                    nettingSetDIM_[n][j][k] = 0.0;
                    nettingSetLocalDIM_[n][j][k] = 0.0;
                }
            } else {
                // Least squares polynomial regression with specified polynom order
                QuantExt::StabilisedGLLS ls(rx, ry2, v, QuantExt::StabilisedGLLS::MeanStdDev);
                LOG("DIM data normalisation at time step "
                    << j << ": " << scientific << setprecision(6) << " x-shift = " << ls.xShift() << " x-multiplier = "
                    << ls.xMultiplier() << " y-shift = " << ls.yShift() << " y-multiplier = " << ls.yMultiplier());
                LOG("DIM regression coefficients at time step " << j << ": " << fixed << setprecision(6)
                                                                << ls.transformedCoefficients());

                // Local regression versus first regression variable (i.e. we do not perform a
                // multidimensional local regression):
                // We evaluate this at a limited number of samples only for validation purposes.
                // Note that computational effort scales quadratically with number of samples.
                // NadarayaWatson needs a large number of samples for good results.
                QuantExt::NadarayaWatson lr(rx0.begin(), rx0.end(), ry1.begin(),
                                            GaussianKernel(0.0, localRegressionBandWidth_));
                Size localRegressionSamples = samples;
                if (localRegressionEvaluations_ > 0)
                    localRegressionSamples = Size(floor(1.0 * samples / localRegressionEvaluations_ + .5));

                // Evaluate regression function to compute DIM for each scenario
                for (Size k = 0; k < samples; ++k) {
                    // Real num1 = scenarioData_->get(j, k, AggregationScenarioDataType::Numeraire);
                    Real numDefault = cubeInterpretation_->getDefaultAggrionScenarioData(
                        scenarioData_, AggregationScenarioDataType::Numeraire, j, k);
                    Array regressor = regressors_.empty() ? Array(1, nettingSetNPV_[n][j][k]) : regressorArray(n, j, k);
                    Real e = ls.eval(regressor, v);
                    if (e < 0.0)
                        LOG("Negative variance regression for date " << j << ", sample " << k
                                                                     << ", regressor = " << regressor);

                    // Note:
                    // 1) We assume vanishing mean of "z", because the drift over a MPOR is usually small,
                    //    and to avoid a second regression for the conditional mean
                    // 2) In particular the linear regression function can yield negative variance values in
                    //    extreme scenarios where an exact analytical or delta VaR calculation would yield a
                    //    variance aproaching zero. We correct this here by taking the positive part.
                    Real std = sqrt(std::max(e, 0.0));
                    Real scalingFactor = horizonScaling * confidenceLevel * nettingSetDimScaling;
                    // Real dim = std * scalingFactor / num1;
                    Real dim = std * scalingFactor / numDefault;
                    dimCube_->set(dim, nettingSetCount, j, k);
                    nettingSetDIM_[n][j][k] = dim;
                    nettingSetExpectedDIM_[n][j] += dim / samples;

                    // Evaluate the Kernel regression for a subset of the samples only (performance)
                    if (localRegressionEvaluations_ > 0 && (k % localRegressionSamples == 0))
                        // nettingSetLocalDIM_[n][j][k] = lr.standardDeviation(regressor[0]) * scalingFactor / num1;
                        nettingSetLocalDIM_[n][j][k] = lr.standardDeviation(regressor[0]) * scalingFactor / numDefault;
                    else
                        nettingSetLocalDIM_[n][j][k] = 0.0;
                }
            }
        }

        nettingSetCount++;
    }
    LOG("DIM by polynomial regression done");
}

Disposable<Array> RegressionDynamicInitialMarginCalculator::regressorArray(string nettingSet, Size dateIndex,
                                                                           Size sampleIndex) {
    Array a(regressors_.size());
    for (Size i = 0; i < regressors_.size(); ++i) {
        string variable = regressors_[i];
        if (boost::to_upper_copy(variable) ==
            "NPV") // this allows possibility to include NPV as a regressor alongside more fundamental risk factors
            a[i] = nettingSetNPV_[nettingSet][dateIndex][sampleIndex];
        else if (scenarioData_->has(AggregationScenarioDataType::IndexFixing, variable))
            a[i] = cubeInterpretation_->getDefaultAggrionScenarioData(
                scenarioData_, AggregationScenarioDataType::IndexFixing, dateIndex, sampleIndex, variable);
        else if (scenarioData_->has(AggregationScenarioDataType::FXSpot, variable))
            a[i] = cubeInterpretation_->getDefaultAggrionScenarioData(
                scenarioData_, AggregationScenarioDataType::FXSpot, dateIndex, sampleIndex, variable);
        else if (scenarioData_->has(AggregationScenarioDataType::Generic, variable))
            a[i] = cubeInterpretation_->getDefaultAggrionScenarioData(
                scenarioData_, AggregationScenarioDataType::Generic, dateIndex, sampleIndex, variable);
        else
            QL_FAIL("scenario data does not provide data for " << variable);
    }
    return a;
}

map<string, Real> RegressionDynamicInitialMarginCalculator::unscaledCurrentDIM() {
    // In this function we proxy the model-implied T0 IM by looking at the
    // cube grid horizon lying closest to t0+mpor. We measure diffs relative
    // to the mean of the distribution at this same time horizon, thus avoiding
    // any cashflow-specific jumps

    Date today = cube_->asof();
    Size relevantDateIdx = 0;
    Real sqrtTimeScaling = 1.0;
    for (Size i = 0; i < cube_->dates().size(); ++i) {
        Size daysFromT0 = (cube_->dates()[i] - today);
        if (daysFromT0 < horizonCalendarDays_) {
            // iterate until we straddle t0+mpor
            continue;
        } else if (daysFromT0 == horizonCalendarDays_) {
            // this date corresponds to t0+mpor, so use it
            relevantDateIdx = i;
            sqrtTimeScaling = 1.0;
            break;
        } else if (daysFromT0 > horizonCalendarDays_) {
            // the first date greater than t0+MPOR, check if it is closest
            Size lastIdx = (i == 0) ? 0 : (i - 1);
            Size lastDaysFromT0 = (cube_->dates()[lastIdx] - today);
            int daysFromT0CloseOut = daysFromT0 - horizonCalendarDays_;
            int prevDaysFromT0CloseOut = lastDaysFromT0 - horizonCalendarDays_;
            if (std::abs(daysFromT0CloseOut) <= std::abs(prevDaysFromT0CloseOut)) {
                relevantDateIdx = i;
                sqrtTimeScaling = std::sqrt(Real(horizonCalendarDays_) / Real(daysFromT0));
            } else {
                relevantDateIdx = lastIdx;
                sqrtTimeScaling = std::sqrt(Real(horizonCalendarDays_) / Real(lastDaysFromT0));
            }
            break;
        }
    }
    // set some reasonable bounds on the sqrt time scaling, so that we are not looking at a ridiculous time horizon
    if (sqrtTimeScaling < std::sqrt(0.5) || sqrtTimeScaling > std::sqrt(2.0)) {
        WLOG("T0 IM Estimation - The estimation time horizon from grid is not sufficiently close to t0+MPOR - "
             << QuantLib::io::iso_date(cube_->dates()[relevantDateIdx])
             << ", the T0 IM estimate might be inaccurate. Consider inserting a first grid tenor closer to the dim "
                "horizon");
    }

    // TODO: Ensure that the simulation containers read-from below are indeed populated

    Real confidenceLevel = QuantLib::InverseCumulativeNormal()(quantile_);
    Size simple_dim_index_h = Size(floor(quantile_ * (cube_->samples() - 1) + 0.5));
    map<string, Real> t0dimReg, t0dimSimple;
    for (auto it_map = nettingSetNPV_.begin(); it_map != nettingSetNPV_.end(); ++it_map) {
        string key = it_map->first;
        vector<Real> t0_dist = it_map->second[relevantDateIdx];
        Size dist_size = t0_dist.size();
        QL_REQUIRE(dist_size == cube_->samples(),
                   "T0 IM - cube samples size mismatch - " << dist_size << ", " << cube_->samples());
        Real mean_t0_dist = std::accumulate(t0_dist.begin(), t0_dist.end(), 0.0);
        mean_t0_dist /= dist_size;
        vector<Real> t0_delMtM_dist(dist_size, 0.0);
        accumulator_set<double, stats<tag::mean, tag::variance>> acc_delMtm;
        accumulator_set<double, stats<tag::mean>> acc_OneOverNum;
        for (Size i = 0; i < dist_size; ++i) {
            Real numeraire = scenarioData_->get(relevantDateIdx, i, AggregationScenarioDataType::Numeraire);
            Real deltaMtmFromMean = numeraire * (t0_dist[i] - mean_t0_dist) * sqrtTimeScaling;
            t0_delMtM_dist[i] = deltaMtmFromMean;
            acc_delMtm(deltaMtmFromMean);
            acc_OneOverNum(1.0 / numeraire);
        }
        Real E_OneOverNumeraire = mean(acc_OneOverNum);
        Real variance_t0 = variance(acc_delMtm);
        Real sqrt_t0 = sqrt(variance_t0);
        t0dimReg[key] = (sqrt_t0 * confidenceLevel * E_OneOverNumeraire);
        std::sort(t0_delMtM_dist.begin(), t0_delMtM_dist.end());
        t0dimSimple[key] = (t0_delMtM_dist[simple_dim_index_h] * E_OneOverNumeraire);

        LOG("T0 IM (Reg) - {" << key << "} = " << t0dimReg[key]);
        LOG("T0 IM (Simple) - {" << key << "} = " << t0dimSimple[key]);
    }
    LOG("T0 IM Calculations Completed");

    return t0dimReg;
}

void RegressionDynamicInitialMarginCalculator::exportDimEvolution(ore::data::Report& dimEvolutionReport) {

    Size samples = dimCube_->samples();
    Size stopDatesLoop = datesLoopSize_;
    Date asof = cube_->asof();

    dimEvolutionReport.addColumn("TimeStep", Size())
        .addColumn("Date", Date())
        .addColumn("DaysInPeriod", Size())
        .addColumn("ZeroOrderDIM", Real(), 6)
        .addColumn("AverageDIM", Real(), 6)
        .addColumn("AverageFLOW", Real(), 6)
        .addColumn("SimpleDIM", Real(), 6)
        .addColumn("NettingSet", string())
        .addColumn("Time", Real(), 6);

    for (auto nettingSet : dimCube_->ids()) {

        LOG("Export DIM evolution for netting set " << nettingSet);
        for (Size i = 0; i < stopDatesLoop; ++i) {
            Real expectedFlow = 0.0;
            for (Size j = 0; j < samples; ++j) {
                expectedFlow += nettingSetFLOW_[nettingSet][i][j] / samples;
            }

            Date defaultDate = dimCube_->dates()[i];
            Time t = ActualActual().yearFraction(asof, defaultDate);
            Size days = cubeInterpretation_->getMporCalendarDays(dimCube_, i);
            dimEvolutionReport.next()
                .add(i)
                .add(defaultDate)
                .add(days)
                .add(nettingSetZeroOrderDIM_[nettingSet][i])
                .add(nettingSetExpectedDIM_[nettingSet][i])
                .add(expectedFlow)
                .add(nettingSetSimpleDIMh_[nettingSet][i])
                .add(nettingSet)
                .add(t);
        }
    }
    dimEvolutionReport.end();
    LOG("Exporting expected DIM through time done");
}

void RegressionDynamicInitialMarginCalculator::exportDimRegression(
    const std::string& nettingSet, const std::vector<Size>& timeSteps,
    const std::vector<boost::shared_ptr<ore::data::Report>>& dimRegReports) {

    QL_REQUIRE(dimRegReports.size() == timeSteps.size(),
               "number of file names (" << dimRegReports.size() << ") does not match number of time steps ("
                                        << timeSteps.size() << ")");
    for (Size ii = 0; ii < timeSteps.size(); ++ii) {
        Size timeStep = timeSteps[ii];
        LOG("Export DIM by sample for netting set " << nettingSet << " and time step " << timeStep);

        Size dates = dimCube_->dates().size();
        const std::vector<std::string>& ids = dimCube_->ids();

        int index = -1;
        for (Size i = 0; i < ids.size(); ++i) {
            if (ids[i] == nettingSet) {
                index = i;
                break;
            }
        }
        QL_REQUIRE(index >= 0, "netting set " << nettingSet << " not found in DIM cube");

        QL_REQUIRE(timeStep < dates - 1, "selected time step " << timeStep << " out of range [0, " << dates - 1 << "]");

        Size samples = cube_->samples();
        vector<Real> numeraires(samples, 0.0);
        for (Size k = 0; k < samples; ++k)
            // numeraires[k] = scenarioData_->get(timeStep, k, AggregationScenarioDataType::Numeraire);
            numeraires[k] = cubeInterpretation_->getDefaultAggrionScenarioData(
                scenarioData_, AggregationScenarioDataType::Numeraire, timeStep, k);

        auto p = sort_permutation(regressorArray_[nettingSet][timeStep], lessThan);
        vector<Array> reg = apply_permutation(regressorArray_[nettingSet][timeStep], p);
        vector<Real> dim = apply_permutation(nettingSetDIM_[nettingSet][timeStep], p);
        vector<Real> ldim = apply_permutation(nettingSetLocalDIM_[nettingSet][timeStep], p);
        vector<Real> delta = apply_permutation(nettingSetDeltaNPV_[nettingSet][timeStep], p);
        vector<Real> num = apply_permutation(numeraires, p);

        boost::shared_ptr<ore::data::Report> regReport = dimRegReports[ii];
        regReport->addColumn("Sample", Size());
        for (Size k = 0; k < reg[0].size(); ++k) {
            ostringstream o;
            o << "Regressor_" << k << "_";
            o << (regressors_.empty() ? "NPV" : regressors_[k]);
            regReport->addColumn(o.str(), Real(), 6);
        }
        regReport->addColumn("RegressionDIM", Real(), 6)
            .addColumn("LocalDIM", Real(), 6)
            .addColumn("ExpectedDIM", Real(), 6)
            .addColumn("ZeroOrderDIM", Real(), 6)
            .addColumn("DeltaNPV", Real(), 6)
            .addColumn("SimpleDIM", Real(), 6);

        // Note that RegressionDIM, LocalDIM, DeltaNPV are _not_ reduced by the numeraire in this output,
        // but ExpectedDIM, ZeroOrderDIM and SimpleDIM _are_ reduced by the numeraire.
        // This is so that the regression formula can be manually validated

        for (Size j = 0; j < reg.size(); ++j) {
            regReport->next().add(j);
            for (Size k = 0; k < reg[j].size(); ++k)
                regReport->add(reg[j][k]);
            regReport->add(dim[j] * num[j])
                .add(ldim[j] * num[j])
                .add(nettingSetExpectedDIM_[nettingSet][timeStep])
                .add(nettingSetZeroOrderDIM_[nettingSet][timeStep])
                .add(delta[j])
                .add(nettingSetSimpleDIMh_[nettingSet][timeStep]);
        }
        regReport->end();
        LOG("Exporting DIM by Sample done for");
    }
}

} // namespace analytics
} // namespace ore


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/trigonometric/include/functions/cosine.hpp>

#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/bench/benchmark.hpp>
#include <nt2/sdk/bench/timing.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/include/constants/pi.hpp>

using nt2::tag::cosine_;

#define RS(T,V1,V2) (T, (V1) ,(V2))

namespace n1 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace n011 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::direct_small_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace n11 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::direct_small_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nn011 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nn11 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nn0 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::direct_medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nn1 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::direct_medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace nn0111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::clipped_medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace nn111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::clipped_medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}

namespace n11 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,T(-1.0e36),T(1.0e36))))
}
namespace n02 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace n2 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}

namespace n21 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}

namespace n211 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}

namespace n211 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::small_>,(RS(vT,T(-1.0e15),T(1.0e15))))
}
namespace m01 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace m02 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}namespace m1 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace m2 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace m11 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace m21 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace m111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace m211 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace m111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,T(-1.0e36),T(1.0e36))))
}
namespace m211 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::medium_>,(RS(vT,T(-1.0e36),T(1.0e36))))
}
namespace n01 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}
namespace n02 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-nt2::Pi<T>()/4,nt2::Pi<T>()/4)))
}namespace n1 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace n2 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-20*nt2::Pi<T>(),20*nt2::Pi<T>())))
}
namespace n11 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace n21 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-60*nt2::Pi<T>(),60*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace n211 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,-10000*nt2::Pi<T>(),10000*nt2::Pi<T>())))
}
namespace n111 {
  typedef float T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,T(-1.0e36),T(1.0e36))))
}
namespace n211 {
  typedef double T;
  typedef boost::simd::meta::vector_of<T, BOOST_SIMD_BYTES/sizeof(T)>::type vT;
  NT2_TIMING(cosine_<nt2::big_>,(RS(vT,T(-1.0e36),T(1.0e36))))
}

#undef RS


/*

 Copyright (c) 2014 University of Edinburgh, Imperial College, University of
 Manchester. Developed in the PAMELA project, EPSRC Programme Grant EP/K008730/1

 This code is licensed under the MIT License.

 Copyright 2016 Emanuele Vespa, Imperial College London

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

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

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

 3. Neither the name of the copyright holder nor the names of its contributors
 may be used to endorse or promote products derived from this software without
 specific prior written permission.

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

#pragma once

#include <supereight/backend/fields.hpp>
#include <supereight/memory/memory_pool_cuda.hpp>
#include <supereight/octree.hpp>

#include <Eigen/Dense>
#include <sophus/se3.hpp>

namespace se {

void projectiveUpdate(Octree<FieldType, MemoryPoolCUDA>& octree,
    voxel_traits<FieldType>::update_func_type& func, Sophus::SE3f Tcw,
    Eigen::Matrix4f K, Eigen::Vector2i frame_size);

} // namespace se


/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#include "bayesopt/bayesopt.hpp"

#include <limits>
#include <boost/numeric/ublas/matrix_proxy.hpp>

#include "lhs.hpp"
#include "randgen.hpp"
#include "log.hpp"
#include "boundingbox.hpp"
#include "inneroptimization.hpp"

namespace bayesopt  {

  class CritCallback: public RBOptimizable
  {
  public:
    explicit CritCallback(ContinuousModel* model):mBO(model){};
    double evaluate(const vectord &query)
    {
      return mBO->evaluateCriteria(query);
    }
  private:
    ContinuousModel* mBO;
  };

  ContinuousModel::ContinuousModel(size_t dim, Parameters parameters):
    BayesOptBase(dim,parameters)
  {
    mCallback.reset(new CritCallback(this));
    cOptimizer.reset(new NLOPT_Optimization(mCallback.get(),dim));
    cOptimizer->setAlgorithm(COMBINED);
    cOptimizer->setMaxEvals(parameters.n_inner_iterations);

    vectord lowerBound = zvectord(mDims);
    vectord upperBound = svectord(mDims,1.0);
    mBB.reset(new utils::BoundingBox<vectord>(lowerBound,upperBound));
  } // Constructor

  ContinuousModel::~ContinuousModel()
  {
    //    delete cOptimizer;
  } // Default destructor

  void ContinuousModel::setBoundingBox(const vectord &lowerBound,
				       const vectord &upperBound)
  {
    // We don't change the bounds of the inner optimization because,
    // thanks to this bounding box model, everything is mapped to the
    // unit hypercube, thus the default inner optimization are just
    // right.
    mBB.reset(new utils::BoundingBox<vectord>(lowerBound,upperBound));

    FILE_LOG(logINFO) << "Bounds: ";
    FILE_LOG(logINFO) << lowerBound;
    FILE_LOG(logINFO) << upperBound;
  } //setBoundingBox

  //////////////////////////////////////////////////////////////////////

  vectord ContinuousModel::samplePoint()
  {
    randFloat drawSample(mEngine,realUniformDist(0,1));
    vectord Xnext(mDims);
    for(vectord::iterator x = Xnext.begin(); x != Xnext.end(); ++x)
      {
	*x = drawSample();
      }
    return Xnext;
  };

  void ContinuousModel::findOptimal(vectord &xOpt)
  {
    double minf = cOptimizer->run(xOpt);

    //Let's try some local exploration like spearmint
    randNFloat drawSample(mEngine,normalDist(0,0.001));
    for(size_t ii = 0;ii<5; ++ii)
      {
	vectord pert = getPointAtMinimum();
	for(size_t j=0; j<xOpt.size(); ++j)
	  {
	    pert(j) += drawSample();
	  }
	try
	  {
	    double minf2 = cOptimizer->localTrialAround(pert);
	    if (minf2<minf)
	      {
		minf = minf2;
		FILE_LOG(logDEBUG) << "Local beats Global";
		xOpt = pert;
	      }
	  }
	catch(std::invalid_argument& e)
	  {
	    //We ignore this one
	  }
      }
  };

  vectord ContinuousModel::remapPoint(const vectord& x)
  {
    return mBB->unnormalizeVector(x);
  }

  void ContinuousModel::generateInitialPoints(matrixd& xPoints)
  {
    utils::samplePoints(xPoints,mParameters.init_method,mEngine);
  }
}  //namespace bayesopt


// Copyright (c) 2014-2015 The Pebblecoin developers
// Copyright (c) 2012-2013 The Cryptonote developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <algorithm>
#include <cassert>
#include <cstddef>
#include <cstdint>
#include <vector>

#include <boost/algorithm/clamp.hpp>

#include "common/int-util.h"
#include "crypto/hash.h"
#include "difficulty.h"

namespace cryptonote
{
  using std::size_t;
  using std::uint64_t;
  using std::vector;

#if defined(_MSC_VER)
#include <windows.h>
#include <winnt.h>

  static inline void mul(uint64_t a, uint64_t b, uint64_t &low, uint64_t &high) {
    low = mul128(a, b, &high);
  }

#else

  static inline void mul(uint64_t a, uint64_t b, uint64_t &low, uint64_t &high) {
    typedef unsigned __int128 uint128_t;
    uint128_t res = (uint128_t) a * (uint128_t) b;
    low = (uint64_t) res;
    high = (uint64_t) (res >> 64);
  }

#endif

  static inline bool cadd(uint64_t a, uint64_t b) {
    return a + b < a;
  }

  static inline bool cadc(uint64_t a, uint64_t b, bool c) {
    return a + b < a || (c && a + b == (uint64_t) -1);
  }

  bool check_hash(const crypto::hash &hash, difficulty_type difficulty) {
    uint64_t low, high, top, cur;
    // First check the highest word, this will most likely fail for a random hash.
    mul(swap64le(((const uint64_t *) &hash)[3]), difficulty, top, high);
    if (high != 0) {
      return false;
    }
    mul(swap64le(((const uint64_t *) &hash)[0]), difficulty, low, cur);
    mul(swap64le(((const uint64_t *) &hash)[1]), difficulty, low, high);
    bool carry = cadd(cur, low);
    cur = high;
    mul(swap64le(((const uint64_t *) &hash)[2]), difficulty, low, high);
    carry = cadc(cur, low, carry);
    carry = cadc(high, top, carry);
    return !carry;
  }

  difficulty_type next_difficulty_v1(std::vector<std::uint64_t> timestamps,
    std::vector<difficulty_type> cumulative_difficulties, size_t target_seconds)
  {
    if (timestamps.size() > DIFFICULTY_WINDOW)
    {
      timestamps.resize(DIFFICULTY_WINDOW);
      cumulative_difficulties.resize(DIFFICULTY_WINDOW);
    }

    size_t length = timestamps.size();
    assert(length == cumulative_difficulties.size());
    if (length <= 1)
    {
      return 1;
    }

    uint64_t weighted_timespans = 0;
    for (size_t i = 1; i < length; i++)
    {
      uint64_t timespan;
      if (timestamps[i - 1] >= timestamps[i])
      {
        timespan = 1;
      }else
      {
        timespan = timestamps[i] - timestamps[i - 1];
      }
      if (timespan > 10 * target_seconds)
      {
        timespan = 10 * target_seconds;
      }
      weighted_timespans += i * timespan;
    }

    // N = length - 1
    uint64_t minimum_timespan = target_seconds * (length - 1) / 2;
    if (weighted_timespans < minimum_timespan)
    {
      weighted_timespans = minimum_timespan;
    }

    difficulty_type total_work = cumulative_difficulties.back() - cumulative_difficulties.front();
    assert(total_work > 0);

    uint64_t low, high;
    // adjust = 0.99 for N=60 ; length = N + 1
    uint64_t target = 99 * (length / 2) * target_seconds / 100;
    mul(total_work, target, low, high);
    if (high != 0)
    {
      return 0;
    }
    return low / weighted_timespans;
  }

  difficulty_type next_difficulty(uint64_t height, std::vector<uint64_t> timestamps,
    std::vector<difficulty_type> cumulative_difficulties, size_t target_seconds/*=cryptonote::config::difficulty_target()*/)
  {
    return next_difficulty_v1(std::move(timestamps), std::move(cumulative_difficulties), target_seconds);
  }
}


#include <ros/ros.h>
#include <stdio.h>
#include <boost/function.hpp>

#include "common.h"
#include "gps_process.h"
#include "track_calibration.h"
#include "weight_calculation.h"
#include "gpsCalibration/IMTrack.h"
#include "gpsCalibration/IMLocalXYZT.h"

#define MAXITERATOR 5
#define IMRATE 1.0

ros::Publisher data_pub2;    // publish message handle
gpsCalibration::IMTrack msgWithWeight;    // total GPS track message
vector<COORDXYZTW> totalTrack;       // total GPS track
GPSPro gpsProcess;        // gps process class

// merge all GPS track
void merge(vector<COORDXYZT> localCoor,vector<double> weightCoe)
{
    if(localCoor.size() != weightCoe.size())
    {
        cout << "ERROR: ENU coordinate size is not equal to weight size" << endl;
        exit(-1);
    }
    for(int index = 0; index < localCoor.size(); index ++)    // merge slam track
    {
        COORDXYZTW tmp;
        tmp.x = localCoor[index].x;
        tmp.y = localCoor[index].y;
        tmp.z = localCoor[index].z;
        tmp.t = localCoor[index].t;
        tmp.w = weightCoe[index];
        totalTrack.push_back(tmp);
    }

}

void longDisTrackPro(const gpsCalibration::IMTrack::ConstPtr& msg)
{
    if(0 == msg->track_flag)
    {
        if(msg->track.empty())
        {
            if(totalTrack.empty())
            {
                cout << "WARN: no GPS track,please check it" << endl;
                exit(-1);
            }
            msgWithWeight = fromCOORDXYZTWtoIMTrack(totalTrack);      // convert GPS track to GPS track message
            data_pub2.publish(msgWithWeight);               // publish total GPS track to short_distance_track_process_node
        }
        else
        {
            vector<COORDXYZT> SLAMTrackTmp = fromIMTracktoCOORDXYZT(msg);   // slam track message convert to slam track
            WeightCoeCal wcca;
            vector<double> weightCoe;
            wcca.ICPWeightCoeCal(SLAMTrackTmp,weightCoe);      // calculate weight according to car speed

            vector<COORDXYZT> localCoor = gpsProcess.GPSToENU(SLAMTrackTmp);     // return ENUgps coordinate,this will be used ICP

            //construct icp class to complete match
            trackCalibration trackICPHandle(SLAMTrackTmp,localCoor,weightCoe);
            trackICPHandle.doICP();
            //get ICP result
            vector<COORDXYZT> proENUTrack;
            trackICPHandle.doCalibration(proENUTrack);

            // iteration
            for(int i = 1; i <= MAXITERATOR; i ++)
            {
                weightCoe.clear();

                wcca.ICPWeightCoeCal(SLAMTrackTmp,weightCoe,localCoor,proENUTrack);   // update weight according to ICP

                trackCalibration trackICPHandle2(proENUTrack,localCoor,weightCoe);
                proENUTrack.clear();
                trackICPHandle2.doICP();
                trackICPHandle2.doCalibration(proENUTrack);           // get ICP result
            }
            merge(localCoor,weightCoe);       // merge slam track
            SLAMTrackTmp.clear();

        }
    }
}

int main(int argc,char **argv)
{
    if(argc < 4)
    {
        perror("ERROR: arguments error usage argv[1]=gps_original_path argv[2]=projectmethod(UTM/Gaussion) argv[3]=bandwidth(3/6)");
        return -1;
    }

    if(strcmp(argv[2],"UTM") && strcmp(argv[2],"Gaussion"))    // UTM method or Gaussion method
    {
        perror("ERROR: argv[2]=projectmethod(UTM/Gaussion)");
        return -1;
    }

    if(atof(argv[3]) != 3 && atof(argv[3]) != 6)
    {
        perror("ERROR: argv[3]=bandwidth(3/6)");
        return -1;
    }
    string originalGPS = argv[1];
    string method = argv[2];
    int type = atof(argv[3]);

    //init gps process
    gpsProcess.setGPSPath(originalGPS);
    gpsProcess.setMethod(method);
    gpsProcess.setType(type);

    ros::init(argc,argv,"long_distance_track_process");
    ros::NodeHandle nh;

    ros::Rate rate(IMRATE);    // set update rate

    ros::Subscriber data_sub = nh.subscribe("slam_track",1000,longDisTrackPro);
    data_pub2 = nh.advertise<gpsCalibration::IMTrack>("gps_weight",2);

    //ros::spin();

    while(1)
    {
        rate.sleep();
        ros::spinOnce();
    }

    cout << "long distance track finished." << endl;
    return 0;
}


/* Copyright Institue of Sound and Vibration Research - All rights reserved. */

#include <librbbl/float_sequence.hpp>

#include <boost/test/unit_test.hpp>

#include <ciso646>
#include <algorithm>
#include <iostream>
#include <iterator>
#include <stdexcept>
#include <string>
#include <vector>

namespace visr
{
namespace rbbl
{
namespace test
{

BOOST_AUTO_TEST_CASE( initFloatSequenceDefault )
{
  FloatSequence<float> const fs;
  BOOST_CHECK( fs.size() == 0 );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceRepetitionCount )
{
  FloatSequence<float> const fs( 2.37f, 5 );
  BOOST_CHECK( fs.size( ) == 5 );
  BOOST_CHECK( fs.at( 4 ) == 2.37f );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceFromInitializerList )
{
  FloatSequence<float> fs( { 0.0f, 1.5f, 3.7f, 7.01f, 2.57e-5f } );
  BOOST_CHECK( fs.size( ) == 5 );

  BOOST_CHECK_NO_THROW( fs.at( 4 ) );

  BOOST_CHECK_CLOSE( fs[1], 1.5f, 1e-5f );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceFromStringSimple )
{
  std::string const initString( "0.0, 1.5, 3.7, 7.01, 2.57e-5" );
  FloatSequence<float> fs( initString );
  BOOST_CHECK( fs.size( ) == 5 );

  BOOST_CHECK_NO_THROW( fs.at( 4 ) );

  BOOST_CHECK_CLOSE( fs[1], 1.5f, 1e-5f );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceFromStringRange1 )
{
  std::string const initString( "0:3,7" );
  FloatSequence<float> fs( initString );
  BOOST_CHECK( fs.size( ) == 5 );

  BOOST_CHECK_NO_THROW( fs.at( 4 ) );

  BOOST_CHECK_CLOSE( fs[1], 1.0f, std::numeric_limits<float>::epsilon() );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceFromStringRangeStride )
{
  std::string const initString( "0.0:2:7.0,2.1375" );
  FloatSequence<float> fs( initString );
  BOOST_CHECK( fs.size( ) == 5 );

  BOOST_CHECK_NO_THROW( fs.at( 4 ) );

  BOOST_CHECK_CLOSE( fs[3], 6.0f, std::numeric_limits<float>::epsilon() );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceFromStringRangeStrideEmpty )
{
  std::string const initString( "5.27:-0.12:8.3" );
  FloatSequence<float> il1( initString );
  BOOST_CHECK( il1.size( ) == 0 );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceFromStringRangeStrideReversed )
{
  std::string const initString( "2,7:-2:0" );
  FloatSequence<float> fs( initString );
  BOOST_CHECK( fs.size( ) == 5 );

  BOOST_CHECK_NO_THROW( fs.at( 4 ) );

  BOOST_CHECK( fs[2] == 5 );
}

BOOST_AUTO_TEST_CASE( initFloatSequenceFromStringWeirdWhiteSpace )
{
  std::string const initString( "    7 :    -2    : 0   ,   2.13527         " );
  FloatSequence<float> fs( initString );
  BOOST_CHECK( fs.size( ) == 5 );

  BOOST_CHECK_NO_THROW( fs.at( 4 ) );

  BOOST_CHECK( fs[1] == 5 );
}

} // namespace test
} // namespace rbbl
} // namespace visr


/*
 * UtilTest.cpp
 *
 *  Created on: May 7, 2012
 *      Author: joe
 */

#define BOOST_TEST_MODULE UtilTest
#define BOOST_TEST_MAIN
#include "../src/config.h"
#ifdef USE_DYNAMIC_UNIT_TEST
#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#else
#include <boost/test/included/unit_test.hpp>
#endif
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int.hpp>
#include <iostream>
#include "../src/util/RNG.h"

BOOST_AUTO_TEST_CASE( rng ) {
    boost::mt19937 gen;
    RNG<boost::mt19937::result_type> rng = rngFromRef<boost::mt19937::result_type>(gen);

    BOOST_CHECK_EQUAL(gen(), 3499211612);
    BOOST_CHECK_EQUAL(rng(), 581869302);
    BOOST_CHECK_EQUAL(gen(), 3890346734);
    BOOST_CHECK_EQUAL(rng(), 3586334585);
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/ext/std/integral_constant.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>

#include <type_traits>
using namespace boost::hana;

int main() {
    BOOST_HANA_CONSTANT_ASSERT(std::integral_constant<int, 1>{} == int_<1>);
    BOOST_HANA_CONSTANT_ASSERT(std::integral_constant<int, 1>{} == long_<1>);

    BOOST_HANA_CONSTANT_ASSERT(std::integral_constant<int, 2>{} != int_<3>);
}


#include "segmentation.hpp"
#include "util.hpp"
#include <boost/functional/hash.hpp>
#include <unordered_set>
#include <tuple>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

namespace r = ranges;
namespace rv = ranges::views;

/*--------------------------------------------------------------------------------------------------------------*/
// edge_set

class ch::segmentation::edge_set {
private:

    struct hasher {
        size_t operator()(const std::tuple<int, int>& edge) const {
            const auto& [i, j] = edge;
            std::size_t seed = 0;
            boost::hash_combine(seed, i);
            boost::hash_combine(seed, j);
            return seed;
        }
    };

    std::unordered_set<std::tuple<int, int>, hasher> set_;

public:
    void insert(int i, int j) {
        set_.insert(std::tuple<int, int>(i, j));
    }

    bool contains(int i, int j) {
        return set_.find(std::tuple<int, int>(i, j)) != set_.end();
    }
};

struct ch::segmentation::connected_component {
    int index;
    uchar value;
    cv::Rect bounds;
    cv::Point2i point;
    int area;
    std::vector<connected_component*> neighbors;

    connected_component(int i = -1, cv::Rect b = {}, int a = 0, uchar v = 255, ch::point p = { -1,-1 }) :
        index(i), point(p), area(a), value(v)
    {}
};

/*--------------------------------------------------------------------------------------------------------------*/
// segmentation

void ch::segmentation::get_neighbors(int x, int y, std::array<int, 4>& ary) {
    int label = label_mat_.at<int>(y, x);
    std::fill(ary.begin(), ary.end(), -1);
    int i = 0;
    int neigh;
    if ((y > 0) && ((neigh = label_mat_.at<int>(y - 1, x)) != label)) {
        ary[i++] = neigh;
    }
    if ((y < label_mat_.rows - 1) && ((neigh = label_mat_.at<int>(y + 1, x)) != label)) {
        ary[i++] = neigh;
    }
    if ((x > 0) && ((neigh = label_mat_.at<int>(y, x - 1)) != label)) {
        ary[i++] = neigh;
    }
    if ((x < label_mat_.cols - 1) && ((neigh = label_mat_.at<int>(y, x + 1)) != label)) {
        ary[i++] = neigh;
    }
}

void ch::segmentation::insert_edges(int label, int x, int y, edge_set& edges) {
    std::array<int, 4> neighbors;
    get_neighbors(x, y, neighbors);
    for (auto neigh : neighbors) {
        if (neigh == -1) {
            break;
        }
        if (edges.contains(label, neigh)) {
            continue;
        }
        edges.insert(label, neigh);
        connected_components_[label].neighbors.push_back(&connected_components_[neigh]);
    }
}

void ch::segmentation::init_new_connected_component(int label, uchar val, int x, int y, edge_set& edges) {
    auto& c = connected_components_[label];
    c.index = label;
    c.value = val;
    c.bounds = cv::Rect(x, y, 1, 1);
    c.area = 1;
    c.point = { x,y };
    insert_edges(label, x, y, edges);
}

void ch::segmentation::update_connected_component(int label, int x, int y, edge_set& edges) {
    auto& c = connected_components_[label];
    c.index = label;
    c.area++;
    c.bounds = union_rect_and_pt(c.bounds, { x,y });
    ch::segmentation::insert_edges(label, x, y, edges);
}

bool ch::segmentation::is_uninitialized(int index) {
    return connected_components_[index].index == -1;
}

ch::segmentation::segmentation(cv::Mat img, cv::Mat label_image) :
    label_mat_(label_image) {
    edge_set edges;
    int n = max_val_in_mat(label_image) + 1;
    connected_components_.resize(n);
    for (int y = 0; y < img.rows; y++) {
        for (int x = 0; x < img.cols; x++) {
            int label = label_image.at<int>(y, x);
            if (is_uninitialized(label)) {
                uchar value = img.at<uchar>(y, x);
                init_new_connected_component(label, value, x, y, edges);
            } else {
                update_connected_component(label, x, y, edges);
            }
        }
    }
}

ch::segmentation::segmentation(cv::Mat mask) {
    cv::Mat stats;
    cv::Mat centroids;
    cv::connectedComponentsWithStats(mask, label_mat_, stats, centroids);

    int n = stats.rows - 1;
    connected_components_.resize(n);
    for (int row = 1; row < n + 1; ++row) {
        int index = row - 1;
        auto& cc = connected_components_[index];
        cc.index = index;
        cc.bounds = cv::Rect(
            stats.at<int>(cv::Point(0, row)),
            stats.at<int>(cv::Point(1, row)),
            stats.at<int>(cv::Point(2, row)),
            stats.at<int>(cv::Point(3, row))
        );
        cc.area = stats.at<int>(cv::Point(4, row));
    }

    label_mat_.forEach<int>(
        [this, &mask](int& lbl, const int* position) -> void {
            lbl--;
            if (lbl >= 0) {
                int x = position[1];
                int y = position[0];
                auto& cc = this->connected_components_[lbl];
                if (cc.point.x < 0) {
                    cc.point = { x,y };
                    cc.value = mask.at<uchar>(y, x);
                }
            }
        }
    );
}

uchar ch::segmentation::value(int index) const {
    return connected_components_[index].value;
}

std::vector<int> ch::segmentation::neighbor_indices(int index) const {
    const auto& neighbors = connected_components_[index].neighbors;
    return neighbors |
        rv::transform(
            [](const auto* cc)->int {
                return cc->index;
            }
        ) |
        r::to_vector;
}

cv::Rect ch::segmentation::bounds(int index) const {
    return connected_components_[index].bounds;
}

cv::Size ch::segmentation::dimensions() const {
    return { label_mat_.cols, label_mat_.rows };
}

int ch::segmentation::count() const {
    return static_cast<int>(connected_components_.size());
}

void ch::segmentation::paint_component(int index, cv::Mat dst) const {
    const auto& c = connected_components_.at(index);
    paint_component(index, dst, c.value);
}

cv::Point2i ch::segmentation::component_to_point(int index) const {
    return connected_components_.at(index).point;
}

int ch::segmentation::point_to_component(const cv::Point2i& pt) const {
    return label_mat_.at<int>(pt.y, pt.x);
}

ch::segmentation::cc_properties ch::segmentation::properties(int index) const {
    const auto& cc = connected_components_.at(index);
    return {
        cc.index,
        cc.value,
        cc.area
    };
}

cv::Mat ch::segmentation::paint_components() const {
    cv::Mat img(dimensions(), CV_8UC1, cv::Scalar(255));
    for (int i = 0; i < count(); ++i) {
        paint_component(i, img);
    }
    return img;
}

ch::segmentation::~segmentation() = default;

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

cv::Mat ch::grayscale_to_label_image(cv::Mat input) {
    auto values = ch::unique_gray_values(input);
    cv::Mat output(input.size(), CV_32SC1, cv::Scalar(-1));
    int label = 0;
    for (auto val : values) {
        cv::Mat mask;
        cv::inRange(input, val, val, mask);
        ch::segmentation seg(mask);
        for (int i = 0; i < seg.count(); ++i) {
            seg.paint_component(i, output, label++);
        }
    }
    return output;
}


#include <boost/python/class.hpp>

#include <scitbx/math/numeric_limits.h>

namespace scitbx { namespace math { namespace boost_python {

template<typename FloatType>
struct numeric_limits_wrapper
{
  typedef numeric_limits<FloatType> wt;

  // Workaround for a mysterious runtime failure on Windows with VC 8
  // when wrap() is executed on import of this Boost.Python extension
  static int radix()                 { return wt::radix;             }
  static FloatType min()             { return wt::min();             }
  static FloatType max()             { return wt::max();             }
  static int min_exponent()          { return wt::min_exponent;      }
  static int max_exponent()          { return wt::max_exponent;      }
  static int min_exponent10()        { return wt::min_exponent10;    }
  static int max_exponent10()        { return wt::max_exponent10;    }
  static int digits()                { return wt::digits;            }
  static int digits10()              { return wt::digits10;          }
  static FloatType epsilon()         { return wt::epsilon();         }
  static FloatType epsilon_x_radix() { return wt::epsilon_x_radix(); }
  static FloatType safe_min()        { return wt::safe_min();        }

  static void wrap(char const *name) {
    using namespace boost::python;
    typedef numeric_limits_wrapper<FloatType> wt;
    class_<wt>(name, no_init)
      .add_static_property("radix", &wt::radix)
      .add_static_property("min", &wt::min)
      .add_static_property("max", &wt::max)
      .add_static_property("min_exponent", &wt::min_exponent)
      .add_static_property("max_exponent", &wt::max_exponent)
      .add_static_property("min_exponent10", &wt::min_exponent10)
      .add_static_property("max_exponent10", &wt::max_exponent10)
      .add_static_property("digits", &wt::digits)
      .add_static_property("digits10", &wt::digits10)
      .add_static_property("epsilon", &wt::epsilon)
      .add_static_property("epsilon_x_radix", &wt::epsilon_x_radix)
      .add_static_property("safe_min", &wt::safe_min)
      ;
  }
};

void wrap_numeric_limits() {
  numeric_limits_wrapper<double>::wrap("double_numeric_limits");
}

}}} // scitbx::af::boost_python


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "detach-transition.h"
#include "hmm/state-vector/peptide-state-vector.h"

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
const double TOL = 0.000000001;
}  // namespace

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(step_suite)
BOOST_AUTO_TEST_SUITE(detach_transition_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    BOOST_CHECK_EQUAL(dt.p_detach, p_detach);
}

BOOST_AUTO_TEST_CASE(forward_in_place_trivial_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 1.0;  // loc is {0, 0}
    int edmans = 0;
    dt.forward(&edmans, &psv);
    BOOST_TEST(psv.tensor[loc] == 1.0);  // loc is {0, 0}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_trivial_test, *tolerance(TOL)) {
//     double p_detach = 0.05;
//     DetachTransition dt(p_detach);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 1;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 1.0;  // loc is {0, 0}
//     int edmans = 0;
//     dt.forward(tsr1, &edmans, &psv2);
//     BOOST_TEST(psv2.tensor[loc] == 1.0);  // loc is {0, 0}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_basic_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.7;  // loc is {0, 1}
    int edmans = 0;
    dt.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3 + 0.7 * p_detach);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.7 * (1 - p_detach));  // loc is {0, 1}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_basic_test, *tolerance(TOL)) {
//     double p_detach = 0.05;
//     DetachTransition dt(p_detach);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 2;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.3;  // loc is {0, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.7;  // loc is {0, 1}
//     int edmans = 0;
//     dt.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     BOOST_TEST(psv2.tensor[loc] == 0.3 + 0.7 * p_detach);  // loc is {0, 0}
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.7 * (1 - p_detach));  // loc is {0, 1}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_bigger_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.6;  // loc is {0, 1}
    loc[1] = 2;
    psv.tensor[loc] = 0.1;  // loc is {0, 2}
    int edmans = 0;
    dt.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc]
               == 0.3 + (0.6 + 0.1) * p_detach);  // loc is {0, 0}
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.6 * (1 - p_detach));  // loc is {0, 1}
    loc[1] = 2;
    BOOST_TEST(psv.tensor[loc] == 0.1 * (1 - p_detach));  // loc is {0, 2}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_bigger_test, *tolerance(TOL)) {
//     double p_detach = 0.05;
//     DetachTransition dt(p_detach);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 3;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.3;  // loc is {0, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.6;  // loc is {0, 1}
//     loc[1] = 2;
//     psv1.tensor[loc] = 0.1;  // loc is {0, 2}
//     int edmans = 0;
//     dt.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     BOOST_TEST(psv2.tensor[loc]
//                == 0.3 + (0.6 + 0.1) * p_detach);  // loc is {0, 0}
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.6 * (1 - p_detach));  // loc is {0, 1}
//     loc[1] = 2;
//     BOOST_TEST(psv2.tensor[loc] == 0.1 * (1 - p_detach));  // loc is {0, 2}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_multiple_edmans_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 3;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.1;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.2;  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {1, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.4;  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    psv.tensor[loc] = 0.5;  // loc is {2, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.6;  // loc is {2, 1}
    int edmans = 2;
    dt.forward(&edmans, &psv);
    // Just testing the ones with at least one lit amino acid here. See below
    // for other tests.
    loc[0] = 0;
    loc[1] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.2 * (1 - p_detach));  // loc is {0, 1}
    loc[0] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.4 * (1 - p_detach));  // loc is {1, 1}
    loc[0] = 2;
    BOOST_TEST(psv.tensor[loc] == 0.6 * (1 - p_detach));  // loc is {2, 1}
    // Distribution between empty states is of no importance. We only care about
    // the sum.
    double sum_empties = 0.0;
    loc[0] = 0;
    loc[1] = 0;
    sum_empties += psv.tensor[loc];  // loc is {0, 0}
    loc[0] = 1;
    sum_empties += psv.tensor[loc];  // loc is {1, 0};
    loc[0] = 2;
    sum_empties += psv.tensor[loc];  // loc is {2, 0};
    BOOST_TEST(sum_empties == 0.1 + 0.3 + 0.5 + (0.2 + 0.4 + 0.6) * p_detach);
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_multiple_edmans_test, *tolerance(TOL)) {
//     double p_detach = 0.05;
//     DetachTransition dt(p_detach);
//     int order = 2;
//     int* shape = new int[order];
//     shape[0] = 3;
//     shape[1] = 2;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.1;  // loc is {0, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.2;  // loc is {0, 1}
//     loc[0] = 1;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.3;  // loc is {1, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.4;  // loc is {1, 1}
//     loc[0] = 2;
//     loc[1] = 0;
//     psv1.tensor[loc] = 0.5;  // loc is {2, 0}
//     loc[1] = 1;
//     psv1.tensor[loc] = 0.6;  // loc is {2, 1}
//     int edmans = 2;
//     dt.forward(tsr1, &edmans, &psv2);
//     // Just testing the ones with at least one lit amino acid here. See below
//     // for other tests.
//     loc[0] = 0;
//     loc[1] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.2 * (1 - p_detach));  // loc is {0, 1}
//     loc[0] = 1;
//     BOOST_TEST(psv2.tensor[loc] == 0.4 * (1 - p_detach));  // loc is {1, 1}
//     loc[0] = 2;
//     BOOST_TEST(psv2.tensor[loc] == 0.6 * (1 - p_detach));  // loc is {2, 1}
//     // Distribution between empty states is of no importance. We only care
//     about
//     // the sum.
//     double sum_empties = 0.0;
//     loc[0] = 0;
//     loc[1] = 0;
//     sum_empties += psv2.tensor[loc];  // loc is {0, 0}
//     loc[0] = 1;
//     sum_empties += psv2.tensor[loc];  // loc is {1, 0};
//     loc[0] = 2;
//     sum_empties += psv2.tensor[loc];  // loc is {2, 0};
//     BOOST_TEST(sum_empties == 0.1 + 0.3 + 0.5 + (0.2 + 0.4 + 0.6) *
//     p_detach); delete[] loc;
// }

BOOST_AUTO_TEST_CASE(forward_in_place_multiple_dye_colors_test,
                     *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 3;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    shape[2] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    psv.tensor[loc] = 0.1;  // loc is {0, 0, 0}
    loc[2] = 1;
    psv.tensor[loc] = 0.2;  // loc is {0, 0, 1}
    loc[1] = 1;
    loc[2] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 1, 0}
    loc[2] = 1;
    psv.tensor[loc] = 0.4;  // loc is {0, 1, 1}
    int edmans = 0;
    dt.forward(&edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    // loc is {0, 0, 0}
    BOOST_TEST(psv.tensor[loc] == 0.1 + (0.2 + 0.3 + 0.4) * p_detach);
    loc[2] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.2 * (1 - p_detach));  // loc is {0, 0, 1}
    loc[1] = 1;
    loc[2] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3 * (1 - p_detach));  // loc is {0, 1, 0}
    loc[2] = 1;
    BOOST_TEST(psv.tensor[loc] == 0.4 * (1 - p_detach));  // loc is {0, 1, 1}
    delete[] loc;
}

// BOOST_AUTO_TEST_CASE(forward_new_tsr_multiple_dye_colors_test,
//                      *tolerance(TOL)) {
//     double p_detach = 0.05;
//     DetachTransition dt(p_detach);
//     int order = 3;
//     int* shape = new int[order];
//     shape[0] = 1;
//     shape[1] = 2;
//     shape[2] = 2;
//     PeptideStateVector psv1(order, shape);
//     PeptideStateVector psv2(order, shape);
//     delete[] shape;
//     int* loc = new int[order];
//     loc[0] = 0;
//     loc[1] = 0;
//     loc[2] = 0;
//     psv1.tensor[loc] = 0.1;  // loc is {0, 0, 0}
//     loc[2] = 1;
//     psv1.tensor[loc] = 0.2;  // loc is {0, 0, 1}
//     loc[1] = 1;
//     loc[2] = 0;
//     psv1.tensor[loc] = 0.3;  // loc is {0, 1, 0}
//     loc[2] = 1;
//     psv1.tensor[loc] = 0.4;  // loc is {0, 1, 1}
//     int edmans = 0;
//     dt.forward(tsr1, &edmans, &psv2);
//     loc[0] = 0;
//     loc[1] = 0;
//     loc[2] = 0;
//     // loc is {0, 0, 0}
//     BOOST_TEST(psv2.tensor[loc] == 0.1 + (0.2 + 0.3 + 0.4) * p_detach);
//     loc[2] = 1;
//     BOOST_TEST(psv2.tensor[loc]
//                == 0.2 * (1 - p_detach));  // loc is {0, 0, 1}
//     loc[1] = 1;
//     loc[2] = 0;
//     BOOST_TEST(psv2.tensor[loc]
//                == 0.3 * (1 - p_detach));  // loc is {0, 1, 0}
//     loc[2] = 1;
//     BOOST_TEST(psv2.tensor[loc]
//                == 0.4 * (1 - p_detach));  // loc is {0, 1, 1}
//     delete[] loc;
// }

BOOST_AUTO_TEST_CASE(backward_in_place_trivial_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 1;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 1.0;  // loc is {0, 0}
    int edmans = 0;
    dt.backward(psv, &edmans, &psv);
    BOOST_TEST(psv.tensor[loc] == 1.0);  // loc is {0, 0}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_trivial_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 1;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 1.0;  // loc is {0, 0}
    int edmans = 0;
    dt.backward(psv1, &edmans, &psv2);
    BOOST_TEST(psv2.tensor[loc] == 1.0);  // loc is {0, 0}
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_basic_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.7;  // loc is {0, 1}
    int edmans = 0;
    dt.backward(psv, &edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3);  // loc is {0, 0}
    loc[1] = 1;
    // loc is {0, 1}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.3 + (1 - p_detach) * 0.7);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_basic_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.7;  // loc is {0, 1}
    int edmans = 0;
    dt.backward(psv1, &edmans, &psv2);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.3);  // loc is {0, 0}
    loc[1] = 1;
    // loc is {0, 1}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.3 + (1 - p_detach) * 0.7);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_bigger_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.6;  // loc is {0, 1}
    loc[1] = 2;
    psv.tensor[loc] = 0.1;  // loc is {0, 2}
    int edmans = 0;
    dt.backward(psv, &edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.3);  // loc is {0, 0}
    loc[1] = 1;
    // loc is {0, 1}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.3 + (1 - p_detach) * 0.6);
    loc[1] = 2;
    // loc is {0, 2}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.3 + (1 - p_detach) * 0.1);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_bigger_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 0.3;  // loc is {0, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.6;  // loc is {0, 1}
    loc[1] = 2;
    psv1.tensor[loc] = 0.1;  // loc is {0, 2}
    int edmans = 0;
    dt.backward(psv1, &edmans, &psv2);
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.3);  // loc is {0, 0}
    loc[1] = 1;
    // loc is {0, 1}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.3 + (1 - p_detach) * 0.6);
    loc[1] = 2;
    // loc is {0, 2}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.3 + (1 - p_detach) * 0.1);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_multiple_edmans_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 3;
    shape[1] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv.tensor[loc] = 0.88;  // loc is {0, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.2;  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    psv.tensor[loc] = 0.88;  // loc is {1, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.4;  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    psv.tensor[loc] = 0.88;  // loc is {2, 0}
    loc[1] = 1;
    psv.tensor[loc] = 0.6;  // loc is {2, 1}
    int edmans = 2;
    dt.backward(psv, &edmans, &psv);
    // Just testing the ones with at least one lit amino acid here. See below
    // for other tests.
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.88);  // loc is {0, 0}
    loc[1] = 1;
    // loc is {0, 1}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.88 + (1 - p_detach) * 0.2);
    loc[0] = 1;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.88);  // loc is {1, 0}
    loc[1] = 1;
    // loc is {1, 1}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.88 + (1 - p_detach) * 0.4);
    loc[0] = 2;
    loc[1] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.88);  // loc is {2, 0}
    loc[1] = 1;
    // loc is {2, 1}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.88 + (1 - p_detach) * 0.6);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_multiple_edmans_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 3;
    shape[1] = 2;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    psv1.tensor[loc] = 0.88;  // loc is {0, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.2;  // loc is {0, 1}
    loc[0] = 1;
    loc[1] = 0;
    psv1.tensor[loc] = 0.88;  // loc is {1, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.4;  // loc is {1, 1}
    loc[0] = 2;
    loc[1] = 0;
    psv1.tensor[loc] = 0.88;  // loc is {2, 0}
    loc[1] = 1;
    psv1.tensor[loc] = 0.6;  // loc is {2, 1}
    int edmans = 2;
    dt.backward(psv1, &edmans, &psv2);
    // Just testing the ones with at least one lit amino acid here. See below
    // for other tests.
    loc[0] = 0;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.88);  // loc is {0, 0}
    loc[1] = 1;
    // loc is {0, 1}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.88 + (1 - p_detach) * 0.2);
    loc[0] = 1;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.88);  // loc is {1, 0}
    loc[1] = 1;
    // loc is {1, 1}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.88 + (1 - p_detach) * 0.4);
    loc[0] = 2;
    loc[1] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.88);  // loc is {2, 0}
    loc[1] = 1;
    // loc is {2, 1}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.88 + (1 - p_detach) * 0.6);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_in_place_multiple_dye_colors_test,
                     *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 3;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    shape[2] = 2;
    PeptideStateVector psv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    psv.tensor[loc] = 0.1;  // loc is {0, 0, 0}
    loc[2] = 1;
    psv.tensor[loc] = 0.2;  // loc is {0, 0, 1}
    loc[1] = 1;
    loc[2] = 0;
    psv.tensor[loc] = 0.3;  // loc is {0, 1, 0}
    loc[2] = 1;
    psv.tensor[loc] = 0.4;  // loc is {0, 1, 1}
    int edmans = 0;
    dt.backward(psv, &edmans, &psv);
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    BOOST_TEST(psv.tensor[loc] == 0.1);  // loc is {0, 0, 0}
    loc[2] = 1;
    // loc is {0, 0, 1}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.1 + (1 - p_detach) * 0.2);
    loc[1] = 1;
    loc[2] = 0;
    // loc is {0, 1, 0}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.1 + (1 - p_detach) * 0.3);
    loc[2] = 1;
    // loc is {0, 1, 1}
    BOOST_TEST(psv.tensor[loc] == p_detach * 0.1 + (1 - p_detach) * 0.4);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(backward_new_tsr_multiple_dye_colors_test,
                     *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 3;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 2;
    shape[2] = 2;
    PeptideStateVector psv1(order, shape);
    PeptideStateVector psv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    psv1.tensor[loc] = 0.1;  // loc is {0, 0, 0}
    loc[2] = 1;
    psv1.tensor[loc] = 0.2;  // loc is {0, 0, 1}
    loc[1] = 1;
    loc[2] = 0;
    psv1.tensor[loc] = 0.3;  // loc is {0, 1, 0}
    loc[2] = 1;
    psv1.tensor[loc] = 0.4;  // loc is {0, 1, 1}
    int edmans = 0;
    dt.backward(psv1, &edmans, &psv2);
    loc[0] = 0;
    loc[1] = 0;
    loc[2] = 0;
    BOOST_TEST(psv2.tensor[loc] == 0.1);  // loc is {0, 0, 0}
    loc[2] = 1;
    // loc is {0, 0, 1}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.1 + (1 - p_detach) * 0.2);
    loc[1] = 1;
    loc[2] = 0;
    // loc is {0, 1, 0}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.1 + (1 - p_detach) * 0.3);
    loc[2] = 1;
    // loc is {0, 1, 1}
    BOOST_TEST(psv2.tensor[loc] == p_detach * 0.1 + (1 - p_detach) * 0.4);
    delete[] loc;
}

BOOST_AUTO_TEST_CASE(improve_fit_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector fpsv(order, shape);
    PeptideStateVector bpsv(order, shape);
    PeptideStateVector nbpsv(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv.tensor[loc] = 0.32;
    nbpsv.tensor[loc] = 0.33;
    loc[1] = 1;
    fpsv.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv.tensor[loc] = 0.72;
    nbpsv.tensor[loc] = 0.73;
    loc[1] = 2;
    fpsv.tensor[loc] = 0.91;  // loc is {0, 2}
    bpsv.tensor[loc] = 0.92;
    nbpsv.tensor[loc] = 0.93;
    delete[] loc;
    int edmans = 0;
    double probability = 0.31 * 0.32 + 0.71 * 0.72 + 0.91 * 0.92;
    ErrorModelFitter emf;
    dt.improve_fit(fpsv, bpsv, nbpsv, edmans, probability, &emf);
    BOOST_TEST(emf.p_detach_fit.get()
               == (0.71 * p_detach * 0.33 + 0.91 * p_detach * 0.33)
                          / (0.71 * 0.72 + 0.91 * 0.92));
}

BOOST_AUTO_TEST_CASE(improve_fit_twice_test, *tolerance(TOL)) {
    double p_detach = 0.05;
    DetachTransition dt(p_detach);
    int order = 2;
    int* shape = new int[order];
    shape[0] = 1;
    shape[1] = 3;
    PeptideStateVector fpsv1(order, shape);
    PeptideStateVector bpsv1(order, shape);
    PeptideStateVector nbpsv1(order, shape);
    PeptideStateVector fpsv2(order, shape);
    PeptideStateVector bpsv2(order, shape);
    PeptideStateVector nbpsv2(order, shape);
    delete[] shape;
    int* loc = new int[order];
    loc[0] = 0;
    loc[1] = 0;
    fpsv1.tensor[loc] = 0.31;  // loc is {0, 0}
    bpsv1.tensor[loc] = 0.32;
    nbpsv1.tensor[loc] = 0.33;
    fpsv2.tensor[loc] = 0.231;
    bpsv2.tensor[loc] = 0.232;
    nbpsv2.tensor[loc] = 0.233;
    loc[1] = 1;
    fpsv1.tensor[loc] = 0.71;  // loc is {0, 1}
    bpsv1.tensor[loc] = 0.72;
    nbpsv1.tensor[loc] = 0.73;
    fpsv2.tensor[loc] = 0.271;
    bpsv2.tensor[loc] = 0.272;
    nbpsv2.tensor[loc] = 0.273;
    loc[1] = 2;
    fpsv1.tensor[loc] = 0.91;  // loc is {0, 2}
    bpsv1.tensor[loc] = 0.92;
    nbpsv1.tensor[loc] = 0.93;
    fpsv2.tensor[loc] = 0.291;
    bpsv2.tensor[loc] = 0.292;
    nbpsv2.tensor[loc] = 0.293;
    delete[] loc;
    int edmans = 0;
    double prob1 = 0.31 * 0.32 + 0.71 * 0.72 + 0.91 * 0.92;
    double prob2 = 0.231 * 0.232 + 0.271 * 0.272 + 0.291 * 0.292;
    ErrorModelFitter emf;
    dt.improve_fit(fpsv1, bpsv1, nbpsv1, edmans, prob1, &emf);
    dt.improve_fit(fpsv2, bpsv2, nbpsv2, edmans, prob2, &emf);
    BOOST_TEST(
            emf.p_detach_fit.get()
            == ((0.71 * p_detach * 0.33 + 0.91 * p_detach * 0.33) / prob1
                + (0.271 * p_detach * 0.233 + 0.291 * p_detach * 0.233) / prob2)
                       / ((0.71 * 0.72 + 0.91 * 0.92) / prob1
                          + (0.271 * 0.272 + 0.291 * 0.292) / prob2));
}

BOOST_AUTO_TEST_SUITE_END()  // detach_transition_suite
BOOST_AUTO_TEST_SUITE_END()  // step_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


///////////////////////////////////////////////////////////////
//  Copyright 2013 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

//[logged_adaptor

#include <boost/multiprecision/mpfi.hpp>
#include <boost/multiprecision/logged_adaptor.hpp>
#include <iostream>
#include <iomanip>
//
// Begin by overloading log_postfix_event so we can capture each arithmetic event as it happens:
//
namespace boost{ namespace multiprecision{

template <unsigned D>
inline void log_postfix_event(const mpfi_float_backend<D>& val, const char* event_description)
{
   // Print out the (relative) diameter of the interval:
   using namespace boost::multiprecision;
   number<mpfr_float_backend<D> > diam;
   mpfi_diam(diam.backend().data(), val.data());
   std::cout << "Diameter was " << diam << " after operation: " << event_description << std::endl;
}
template <unsigned D, class T>
inline void log_postfix_event(const mpfi_float_backend<D>&, const T&, const char* event_description)
{
   // This version is never called in this example.
}

}}

int main()
{
   using namespace boost::multiprecision;
   typedef number<logged_adaptor<mpfi_float_backend<17> > > logged_type;
   //
   // Test case deliberately introduces cancellation error, relative size of interval
   // gradually gets larger after each operation:
   //
   logged_type a = 1;
   a /= 10;

   for(unsigned i = 0; i < 13; ++i)
   {
      logged_type b = a * 9;
      b /= 10;
      a -= b;
   }
   std::cout << "Final value was: " << a << std::endl;
   return 0;
}

//]

/*
//[logged_adaptor_output

Diameter was nan after operation: Default construct
Diameter was 0 after operation: Assignment from arithmetic type
Diameter was 4.33681e-18 after operation: /=
Diameter was nan after operation: Default construct
Diameter was 7.70988e-18 after operation: *
Diameter was 9.63735e-18 after operation: /=
Diameter was 1.30104e-16 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 1.30104e-16 after operation: *
Diameter was 1.38537e-16 after operation: /=
Diameter was 2.54788e-15 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 2.54788e-15 after operation: *
Diameter was 2.54863e-15 after operation: /=
Diameter was 4.84164e-14 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 4.84164e-14 after operation: *
Diameter was 4.84221e-14 after operation: /=
Diameter was 9.19962e-13 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 9.19962e-13 after operation: *
Diameter was 9.19966e-13 after operation: /=
Diameter was 1.74793e-11 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 1.74793e-11 after operation: *
Diameter was 1.74793e-11 after operation: /=
Diameter was 3.32107e-10 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 3.32107e-10 after operation: *
Diameter was 3.32107e-10 after operation: /=
Diameter was 6.31003e-09 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 6.31003e-09 after operation: *
Diameter was 6.31003e-09 after operation: /=
Diameter was 1.19891e-07 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 1.19891e-07 after operation: *
Diameter was 1.19891e-07 after operation: /=
Diameter was 2.27792e-06 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 2.27792e-06 after operation: *
Diameter was 2.27792e-06 after operation: /=
Diameter was 4.32805e-05 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 4.32805e-05 after operation: *
Diameter was 4.32805e-05 after operation: /=
Diameter was 0.00082233 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 0.00082233 after operation: *
Diameter was 0.00082233 after operation: /=
Diameter was 0.0156243 after operation: -=
Diameter was nan after operation: Default construct
Diameter was 0.0156243 after operation: *
Diameter was 0.0156243 after operation: /=
Diameter was 0.296861 after operation: -=
Final value was: {8.51569e-15,1.14843e-14}

//]
*/