#include "DataFormats/PatCandidates/interface/CovarianceParameterization.h"
#include "DataFormats/Math/interface/liblogintpack.h"
#include "DataFormats/Math/interface/libminifloat.h"
#include "FWCore/ParameterSet/interface/FileInPath.h"
#include <boost/format.hpp>
#include <iostream>
#include <TParameter.h>
#include <TVector.h>
#include <TFolder.h>

uint16_t CompressionElement::pack(float value, float ref) const {
  float toCompress = 0;
  switch (target) {
    case (realValue):
      toCompress = value;
      break;
    case (ratioToRef):
      toCompress = value / ref;
      break;
    case (differenceToRef):
      toCompress = value - ref;
      break;
  }
  switch (method) {
    case (float16):
      return MiniFloatConverter::float32to16(toCompress * params[0]);
      break;
    case (reduceMantissa):
      return MiniFloatConverter::reduceMantissaToNbits(toCompress, params[0]);
      break;
    case (zero):
      return 0;
      break;
    case (one):
      return 1.0;
      break;
    case (tanLogPack):
      return 0;  //FIXME: should be implemented
      break;
    case (logPack):
      int16_t r = logintpack::pack16log(toCompress, params[0], params[1], bits);
      return *reinterpret_cast<uint16_t *>(&r);
      break;
  }
  return 0;
}
float CompressionElement::unpack(uint16_t packed, float ref) const {
  float unpacked = 0;
  switch (method) {
    case (float16):
      unpacked = MiniFloatConverter::float16to32(packed) / params[0];
      break;
    case (reduceMantissa):
      unpacked = packed;
      break;
    case (logPack):
      unpacked = logintpack::unpack16log(*reinterpret_cast<int16_t *>(&packed), params[0], params[1], bits);
      break;
    case (zero):
      unpacked = 0;
      break;
    case (one):
    case (tanLogPack):
      unpacked = 1;  //FIXME: should be implemented
  }
  switch (target) {
    case (realValue):
      return unpacked;
    case (ratioToRef):
      return unpacked * ref;
    case (differenceToRef):
      return unpacked + ref;
  }

  return ref;
}

void CovarianceParameterization::load(int version) {
  edm::FileInPath fip(
      (boost::format("DataFormats/PatCandidates/data/CovarianceParameterization_version%d.root") % version).str());
  fileToRead_ = TFile::Open(fip.fullPath().c_str());
  TFile &fileToRead = *fileToRead_;
  //Read files from here fip.fullPath().c_str();
  if (fileToRead.IsOpen()) {
    readFile(fileToRead);

    TIter next(((TDirectoryFile *)fileToRead.Get("schemas"))->GetListOfKeys());
    TKey *key;
    while ((key = (TKey *)next())) {
      TClass *cl = gROOT->GetClass(key->GetClassName());
      if (!cl->InheritsFrom("TDirectoryFile"))
        continue;
      std::string schemaNumber = key->ReadObj()->GetName();
      uint16_t schemaN = std::stoi(schemaNumber);
      //for (int folderNumber = 0; folderNumber < 6 ; folderNumber++) {
      CompressionSchema schema;
      for (int i = 0; i < 5; i++) {
        for (int j = i; j < 5; j++) {  //FILLING ONLY THE SCHEMA OF SOME ELEMENTS
          std::string folder = "schemas/" + schemaNumber + "/" + char(48 + i) + char(48 + j);
          std::string methodString = folder + "/method";
          std::string targetString = folder + "/target";
          std::string bitString = folder + "/bit";
          std::vector<float> vParams;
          TVector *p = (TVector *)fileToRead.Get((folder + "/param").c_str());
          vParams.reserve(p->GetNoElements());
          for (int k = 0; k < p->GetNoElements(); k++) {
            vParams.push_back((*p)[k]);
          }

          schema(i, j) = CompressionElement(
              (CompressionElement::Method)((TParameter<int> *)fileToRead.Get(methodString.c_str()))->GetVal(),
              (CompressionElement::Target)((TParameter<int> *)fileToRead.Get(targetString.c_str()))->GetVal(),
              (int)((TParameter<int> *)fileToRead.Get(bitString.c_str()))->GetVal(),
              vParams);
        }
      }
      schemas[schemaN] = schema;
    }

    loadedVersion_ = version;
  } else {
    loadedVersion_ = -1;
  }
}

void CovarianceParameterization::readFile(TFile &f) {
  for (int i = 0; i < 5; i++) {
    for (int j = i; j < 5; j++) {
      std::string String_first_positive = "_pixel_";
      std::string String_second_positive = "_noPixel_";

      addTheHistogram(&cov_elements_pixelHit, String_first_positive, i, j, f);
      addTheHistogram(&cov_elements_noPixelHit, String_second_positive, i, j, f);
    }
  }
}

void CovarianceParameterization::addTheHistogram(
    std::vector<TH3D *> *HistoVector, std::string StringToAddInTheName, int i, int j, TFile &fileToRead) {
  std::string List_covName[5] = {"qoverp", "lambda", "phi", "dxy", "dsz"};

  std::string histoNameString = "covariance_" + List_covName[i] + "_" + List_covName[j] + StringToAddInTheName +
                                "parametrization";  // + "_entries";
  TH3D *matrixElememtHistogramm = (TH3D *)fileToRead.Get(histoNameString.c_str());
  HistoVector->push_back(matrixElememtHistogramm);
}

float CovarianceParameterization::meanValue(
    int i, int j, int sign, float pt, float eta, int nHits, int pixelHits, float cii, float cjj) const {
  int hitNumberToUse = nHits;
  if (hitNumberToUse < 2)
    hitNumberToUse = 2;
  if (hitNumberToUse > 32)
    hitNumberToUse = 32;
  int ptBin = cov_elements_pixelHit[0]->GetXaxis()->FindBin(pt);
  int etaBin = cov_elements_pixelHit[0]->GetYaxis()->FindBin(std::abs(eta));
  int hitBin = cov_elements_pixelHit[0]->GetZaxis()->FindBin(hitNumberToUse);
  int min_idx = i;
  int max_idx = j;

  if (i > j) {
    min_idx = j;
    max_idx = i;
  }

  int indexOfTheHitogramInTheList = ((9 - min_idx) * min_idx) / 2 + max_idx;

  double meanValue = 0.;
  if (pixelHits > 0) {
    meanValue = sign * cov_elements_pixelHit[indexOfTheHitogramInTheList]->GetBinContent(ptBin, etaBin, hitBin);
  } else {
    meanValue = sign * cov_elements_noPixelHit[indexOfTheHitogramInTheList]->GetBinContent(ptBin, etaBin, hitBin);
  }
  return meanValue;
}

float CovarianceParameterization::pack(
    float value, int schema, int i, int j, float pt, float eta, int nHits, int pixelHits, float cii, float cjj) const {
  if (i > j)
    std::swap(i, j);
  float ref = meanValue(i, j, 1., pt, eta, nHits, pixelHits, cii, cjj);
  if (ref == 0) {
    schema = 0;
  }
  if (schema == 0 && i == j && (i == 2 || i == 0))
    ref = 1. / (pt * pt);
  /*  //Used for debugging, to be later removed
    uint16_t p=(*schemas.find(schema)).second(i,j).pack(value,ref);
    float up=(*schemas.find(schema)).second(i,j).unpack(p,ref);
    std::cout << "check " << i << " " << j << " " << value << " " << up << " " << p << " " << ref << " " << schema<< std::endl;*/
  return (*schemas.find(schema)).second(i, j).pack(value, ref);
}
float CovarianceParameterization::unpack(
    uint16_t packed, int schema, int i, int j, float pt, float eta, int nHits, int pixelHits, float cii, float cjj)
    const {
  if (i > j)
    std::swap(i, j);
  float ref = meanValue(i, j, 1., pt, eta, nHits, pixelHits, cii, cjj);
  if (ref == 0) {
    schema = 0;
  }
  if (schema == 0 && i == j && (i == 2 || i == 0))
    ref = 1. / (pt * pt);
  if (i == j && (*schemas.find(schema)).second(i, j).unpack(packed, ref) == 0)
    return 1e-9;
  else
    return (*schemas.find(schema)).second(i, j).unpack(packed, ref);
}


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

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

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

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

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

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

#include <boost/make_shared.hpp>

namespace QuantExt {

using namespace CrossAssetAnalytics;
using namespace QuantLib;

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

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

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

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

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

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

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

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

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

    sqrtCorrelation_ = pseudoSqrt(corr, salvaging_);
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

    return res;
}

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

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

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

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

    return res;
}

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

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

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

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

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

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

    return res;
}

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

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

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

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

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

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

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

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

} // namespace QuantExt


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<float, 3, Eigen::Dynamic> test_float_3_D(const Eigen::Matrix<float, 3, Eigen::Dynamic>& M) { return M; }
void export_float_3_D() { boost::python::def("test_float_3_D", test_float_3_D); }


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013 MetaScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_FREXP_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_FREXP_HPP_INCLUDED

/*!
  @file
  @brief Definition of frexp function
**/

#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

// Fixup for some library defining this a bit liberally
#ifdef frexp
#undef frexp
#endif

namespace boost { namespace simd
{
  namespace tag
  {
    /// @brief Hierarchy tag for frexp function
    struct frexp_ : ext::elementwise_<frexp_>
    {
      typedef ext::elementwise_<frexp_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_frexp_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site, class... Ts>
    BOOST_FORCEINLINE generic_dispatcher<tag::frexp_, Site> dispatching_frexp_(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
    {
      return generic_dispatcher<tag::frexp_, Site>();
    }
    template<class... Args>
    struct impl_frexp_;
  }

  /*!
    Returns the mantissa and exponent of the input

    @par Semantic:

    @code
    tie(m, e) = frexp(x);
    @endcode

    is similar to:

    @code
    as_integer<T > e = exponent(x)+1;
    T m = mantissa(x)/2;
    @endcode

    @par Note:

    @c frexp splits a floating point value @c v f in its signed mantissa @c m and
    exponent @c e so that

    @f$v = m\times 2^e@f$

    with absolute value of @c m between 0.5 (included) and 1 (excluded)

    @see @funcref{exponent},  @funcref{mantissa}
    @param a0 Value to decompose
    @return A pair containing the signed mantissa and exponent of @c a0

    @return The mantissa of @c a0
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::frexp_, frexp, 1)

  /*!
    Returns the mantissa computes the exponent of the input

    @code
    m = frexp(x, e);
    @endcode

    is similar to:

    @code
    as_integer<T > e = exponent(x)+1;
    T m = mantissa(x)/2;
    @endcode

    @par Note:

    @c frexp splits a floating point value @c v f in its signed mantissa @c m and
    exponent @c e so that

    @f$v = m\times 2^e@f$

    with absolute value of @c m between 0.5 (included) and 1 (excluded)

    @see @funcref{exponent},  @funcref{mantissa}
    @param a0 Value to decompose
    @param a1 L-Value that will receive the exponent of @c a0
    @return The mantissa of @c a0
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::frexp_, frexp
                                            , (A0 const&)(A1&), 2
                                            )

  /*!
    Computes the mantissa and the exponent of the input

    @code
    m = frexp(x, e);
    @endcode

    is similar to:

    @code
    as_integer<T > e = exponent(x)+1;
    T m = mantissa(x)/2;
    @endcode

    @par Note:

    @c frexp splits a floating point value @c v f in its signed mantissa @c m and
    exponent @c e so that

    @f$v = m\times 2^e@f$

    with absolute value of @c m between 0.5 (included) and 1 (excluded)

    @see @funcref{exponent},  @funcref{mantissa}
    @param a0 Value to decompose
    @param a1 L-Value that will receive the mantissa of @c a0
    @param a2 L-Value that will receive the exponent of @c a0
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::frexp_, frexp
                                            , (A0 const&)(A1&)(A2&), 3
                                            )
} }

#endif


// Copyright Aleksey Gurtovoy 2001-2004
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Source: /CVSROOT/boost/libs/mpl/test/lambda.cpp,v $
// $Date: 2007/10/29 07:32:56 $
// $Revision: 1.1.1.1 $

#include <boost/mpl/logical.hpp>
#include <boost/mpl/comparison.hpp>
#include <boost/mpl/lambda.hpp>
#include <boost/mpl/size_t.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/aux_/test.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_float.hpp>

struct my
{
    char a[100];
};

MPL_TEST_CASE()
{
    // !(x == char) && !(x == double) || sizeof(x) > 8
    typedef lambda<
        or_<
              and_<
                    not_< boost::is_same<_1, char> >
                  , not_< boost::is_float<_1> >
                  >
            , greater< sizeof_<_1>, mpl::size_t<8> >
            >
        >::type f;

    MPL_ASSERT_NOT(( apply_wrap1<f,char> ));
    MPL_ASSERT_NOT(( apply_wrap1<f,double> ));
    MPL_ASSERT(( apply_wrap1<f,long> ));
    MPL_ASSERT(( apply_wrap1<f,my> ));
}

MPL_TEST_CASE()
{
    // x == y || x == my || sizeof(x) == sizeof(y)
    typedef lambda<
        or_<
              boost::is_same<_1, _2>
            , boost::is_same<_2, my>
            , equal_to< sizeof_<_1>, sizeof_<_2> >
            >
        >::type f;

    MPL_ASSERT_NOT(( apply_wrap2<f,double,char> ));
    MPL_ASSERT_NOT(( apply_wrap2<f,my,int> ));
    MPL_ASSERT_NOT(( apply_wrap2<f,my,char[99]> ));
    MPL_ASSERT(( apply_wrap2<f,int,int> ));
    MPL_ASSERT(( apply_wrap2<f,my,my> ));
    MPL_ASSERT(( apply_wrap2<f,signed long, unsigned long> ));
}

MPL_TEST_CASE()
{
    // bind <-> lambda interaction
    typedef lambda< less<_1,_2> >::type pred;
    typedef bind2< pred, _1, int_<4> > f;

    MPL_ASSERT(( apply_wrap1< f,int_<3> > ));
}


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

#include <boost/test/unit_test.hpp>

#include "../unittest_config.h"

#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include "clotho/utility/timer.hpp"

#include "clotho/data_spaces/allele_space/qtl_allele.hpp"
#include "clotho/data_spaces/population_space/genetic_space.hpp"
#include "clotho/data_spaces/phenotype_evaluator/trait_accumulator.hpp"

#include <set>

BOOST_AUTO_TEST_SUITE( test_data_space )

BOOST_AUTO_TEST_CASE( row_grouped_trait_accum_size_test ) {

    typedef clotho::genetics::qtl_allele_vectorized< double, double >           allele_type;
    typedef unsigned long long                                                  block_type;
    typedef clotho::genetics::genetic_space< allele_type, block_type, clotho::genetics::row_grouped< 2 > >          genetic_space_type;
    typedef clotho::genetics::TraitWeightAccumulator< genetic_space_type >      trait_accumulator_type;

    size_t exp_alleles = 12, exp_genomes = 110, exp_traits = 15;
    genetic_space_type  gs;

    gs.grow( exp_genomes, exp_alleles );
    gs.getAlleleSpace().grow( exp_alleles, exp_traits );

    gs.getSequenceSpace().clear();

    trait_accumulator_type weights( gs );

    size_t obs_size = weights.size();

    BOOST_REQUIRE_MESSAGE( obs_size == exp_genomes * 2, "Unexpected number of weights; Observed: " << obs_size << "; Expected: " << (2 * exp_genomes));

    size_t i = 0;

    while( i < obs_size ) {
        size_t obs_traits = weights.getTraitAt(i)->size();

        BOOST_REQUIRE_MESSAGE( obs_traits == exp_traits, "Unexpected number of traits at index " << i << "; Observed: " << obs_traits << "; Expected: " << exp_traits );

        ++i;
    }
}

BOOST_AUTO_TEST_CASE( row_grouped_trait_accum_update_test ) {

    typedef double                                                              weight_type;
    typedef clotho::genetics::qtl_allele_vectorized< double, weight_type >      allele_type;
    typedef unsigned long long                                                  block_type;
    typedef clotho::genetics::genetic_space< allele_type, block_type, clotho::genetics::row_grouped< 2 > >          genetic_space_type;
    typedef clotho::genetics::TraitWeightAccumulator< genetic_space_type >      trait_accumulator_type;

    typedef typename allele_type::trait_iterator                                trait_iterator;

    typedef typename trait_accumulator_type::trait_vector_type                  trait_vector_type;
    typedef typename trait_accumulator_type::trait_helper_type                  trait_helper_type;

    clotho::utility::timer t;

    boost::random::mt19937  rand_engine( t.getStart() );
    boost::random::uniform_01< weight_type >  uni;
    boost::random::uniform_int_distribution< unsigned int > int_dist( 1, 200 );

    size_t exp_alleles = 4 * int_dist(rand_engine), exp_genomes = int_dist( rand_engine ), exp_traits = int_dist(rand_engine);

    genetic_space_type  gs;

    gs.grow( exp_genomes, exp_alleles );
    gs.getAlleleSpace().grow( exp_alleles, exp_traits );

    gs.getSequenceSpace().clear();
    BOOST_REQUIRE_MESSAGE( gs.getSequenceSpace().row_count() == 2 * exp_genomes, "Unexpect number of sequences for given genomes" );

    size_t i = 0, j = 0;

    boost::random::uniform_int_distribution< unsigned int > seq_int( 0, 2 * exp_genomes - 1 );
    boost::random::uniform_int_distribution< unsigned int > all_gen( 0, exp_alleles - 1 );

    boost::random::uniform_int_distribution< unsigned int > cnt_gen(1, 2 * exp_genomes * exp_alleles - 1 );

    unsigned int C = cnt_gen( rand_engine );

    std::set< size_t > p_i, p_j;

    // flip some bits
    while( C-- ) {
        i = seq_int(rand_engine);
        j = all_gen(rand_engine);
        gs.getSequenceSpace().flip( i, j);
    }

    i = 0;
    while( i < exp_alleles ) {

        j = 0;
        while( j < exp_traits ) {
            weight_type w = uni( rand_engine );
            gs.getAlleleSpace().updateTraitWeight( i, j, w );
            ++j;
        }
        ++i;
    }

    trait_accumulator_type weights( gs );

    size_t obs_size = weights.size();

    BOOST_REQUIRE_MESSAGE( obs_size == exp_genomes * 2, "Unexpected number of weights; Observed: " << obs_size << "; Expected: " << (2 * exp_genomes));

    i = 0;
    while( i < obs_size ) {
        size_t obs_traits = weights.getTraitAt(i)->size();

        BOOST_REQUIRE_MESSAGE( obs_traits == exp_traits, "Unexpected number of traits at index " << i << "; Observed: " << obs_traits << "; Expected: " << exp_traits );

        ++i;
    }

    i = 0;
    while ( i < exp_genomes * 2 ) {

        j = 0;
        trait_vector_type res = trait_helper_type::makeEmptyTrait( exp_traits );
        while( j < exp_alleles ) {

            if( gs.getSequenceSpace()( i, j ) ) {
                trait_iterator it = gs.getAlleleSpace().getTraitIterator( j );
                size_t k = 0;
                while( it.hasNext() ) {
                    weight_type r = it.next();
                    res[ k++ ] += r;
                }
            }
            ++j;
        }

        bool eq = true;
        j = 0;
        std::shared_ptr< typename trait_accumulator_type::trait_vector_type > t = weights.getTraitAt(i);
        while( eq && (j < exp_traits )) {
            eq = (res[ j ] == (*t)[j]);
            ++j;
        }

        BOOST_REQUIRE_MESSAGE( eq, "Unexpected weight vector at " << i);
        ++i;
    }
}

BOOST_AUTO_TEST_CASE( row_grouped_trait_accum_update_test2 ) {

    typedef double                                                              weight_type;
    typedef clotho::genetics::qtl_allele_vectorized< double, weight_type >      allele_type;
    typedef unsigned long long                                                  block_type;
    typedef clotho::genetics::genetic_space< allele_type, block_type, clotho::genetics::row_grouped< 2 > >          genetic_space_type;
    typedef clotho::genetics::TraitWeightAccumulator< genetic_space_type >      trait_accumulator_type;

    typedef typename allele_type::trait_iterator                                trait_iterator;

    typedef typename trait_accumulator_type::trait_vector_type                  trait_vector_type;
    typedef typename trait_accumulator_type::trait_helper_type                  trait_helper_type;

    typedef typename genetic_space_type::sequence_iterator                      sequence_iterator;

    typedef typename genetic_space_type::association_type::bit_helper_type      bit_helper_type;
    typedef typename trait_accumulator_type::evaluator_type::bit_walker_type                    bit_walker_type;

    clotho::utility::timer t;

    boost::random::mt19937  rand_engine( t.getStart() );
    boost::random::uniform_01< weight_type >  uni;
    boost::random::uniform_int_distribution< unsigned int > int_dist( 1, 200 );

    size_t exp_alleles = 4 * int_dist(rand_engine), exp_genomes = int_dist( rand_engine ), exp_traits = int_dist(rand_engine);

    genetic_space_type  gs;

    gs.grow( exp_genomes, exp_alleles );
    gs.getAlleleSpace().grow( exp_alleles, exp_traits );

    gs.getSequenceSpace().clear();

    size_t i = 0, j = 0;

    boost::random::uniform_int_distribution< unsigned int > seq_int( 0, 2 * exp_genomes - 1 );
    boost::random::uniform_int_distribution< unsigned int > all_gen( 0, exp_alleles - 1 );

    boost::random::uniform_int_distribution< unsigned int > cnt_gen(1, 2 * exp_genomes * exp_alleles - 1 );

    unsigned int C = cnt_gen( rand_engine );

    // flip some bits
    while( C-- ) {
        i = seq_int( rand_engine);
        j = all_gen( rand_engine);
        gs.getSequenceSpace().flip( i, j);
    }

    i = 0;
    while( i < exp_alleles ) {

        j = 0;
        while( j < exp_traits ) {
            weight_type w = uni( rand_engine );
            gs.getAlleleSpace().updateTraitWeight( i, j, w );
            ++j;
        }
        ++i;
    }

    trait_accumulator_type weights( gs );

    size_t obs_size = weights.size();

    BOOST_REQUIRE_MESSAGE( obs_size == exp_genomes * 2, "Unexpected number of weights; Observed: " << obs_size << "; Expected: " << (2 * exp_genomes));

    i = 0;
    while ( i < exp_genomes * 2 ) {

        j = 0;
        trait_vector_type res = trait_helper_type::makeEmptyTrait( exp_traits );

        sequence_iterator first = gs.begin_sequence(i);
        sequence_iterator last = gs.end_sequence(i);

        while( first != last ) {
            block_type b = *first;

            while( b ) {
                size_t idx = j + bit_walker_type::unset_next_index(b);

                trait_iterator it = gs.getAlleleSpace().getTraitIterator( idx );
                size_t k = 0;
                while( it.hasNext() ) {
                    weight_type r = it.next();
                    res[ k++ ] += r;
                }
            }

            j += bit_helper_type::BITS_PER_BLOCK;
            first += genetic_space_type::association_type::alignment_type::GROUP_SIZE;
        }
        bool eq = true;
        j = 0;
        std::shared_ptr< typename trait_accumulator_type::trait_vector_type > t = weights.getTraitAt(i);
        while( eq && (j < exp_traits )) {
            eq = (res[ j ] == (*t)[j]);
            ++j;
        }

        BOOST_REQUIRE_MESSAGE( eq, "Unexpected weight vector at " << i);

        i += 1;
    }
}
BOOST_AUTO_TEST_SUITE_END()


#include <boost/python.hpp>
#include <stddef.h>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include <opengm/inference/inference.hxx>
#include <opengm/operations/adder.hxx>
#include <opengm/operations/multiplier.hxx>
#include <opengm/operations/minimizer.hxx>
#include <opengm/operations/maximizer.hxx>
#include <opengm/operations/integrator.hxx>
#include <opengm/operations/logsumexp.hxx>

#include "pyInference.hxx"
#include "pyIcm.hxx"
#include "pyGraphcut.hxx"
#include "pyTrbp.hxx"
#include "pyBp.hxx"
#include "pyAstar.hxx"

//#include "pyGibbs.hxx"
#include "pyBruteforce.hxx"
#include "pyLazyflipper.hxx"
#include "pyDynp.hxx"

#include  "pyAe.hxx"
#include  "pyAbSwap.hxx"
#include  "pyPartitionMove.hxx"

#include "pyDualDecompositionSubGradient.hxx"
//#include "pyDualDecompositionMerit.hxx"
#ifdef WITH_CONICBUNDLE
   //#include "pyDualDecompositionBundle.hxx"
#endif
#ifdef WITH_LIBDAI
#include  "pyLibdai.hxx"
#endif

#ifdef WITH_CPLEX
#include "pyCplex.hxx"
#include "pyMultiCut.hxx"
#endif

#ifdef WITH_TRWS
#include "pyTrws.hxx"
#endif

#ifdef WITH_MRF
#include "pyMrf.hxx"
#endif

#ifdef WITH_FASTPD
#include "pyFastPD.hxx"
#endif

#ifdef WITH_AD3
#include "pyAd3.hxx"
#include "pyLoc.hxx"
#endif

#include "pyQpbo.hxx"

#ifdef WITH_QPBO
#include "pyQpbo.hxx"
#include "pyMQpbo.hxx"
#include "pyReducedInference.hxx"
#include  "pyAeFusion.hxx"
#endif

#if (defined(WITH_VIGRA) && defined(WITH_QPBO) ) || (defined(WITH_BLOSSOM5) && defined(WITH_PLANARITY) )
#include "pyCgc.hxx"
#endif
#if defined(WITH_QPBO) || defined(WITH_CPLEX) || defined(WITH_BLOSSOM5) && defined(WITH_PLANARITY)
#include "pyIntersectionBased.hxx"
#endif

//#include "pyPbp.hxx"

#include "pyFusionMoves.hxx"
#include "pySelfFusion.hxx"
#include "pyFusionBased.hxx"

//#include "pySwendsenWang.hxx"

//#include "pyLpInference.hxx"

#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

// include the LPDef symbols only once!
#undef OPENGM_LPDEF_NO_SYMBOLS
#include <opengm/inference/auxiliary/lpdef.hxx>

using namespace boost::python;

BOOST_PYTHON_MODULE_INIT(_inference) {

   Py_Initialize();
   PyEval_InitThreads();
   boost::python::numeric::array::set_module_and_type("numpy", "ndarray");

   std::string adderString="adder";
   std::string multiplierString="multiplier";
   std::string minimizerString="minimizer";
   std::string maximizerString="maximizer";
   std::string integratorString="integrator";
   std::string logsumexpString="logsumexp";
   std::string substring,submoduleName,subsubmoduleName,subsubstring;
   docstring_options doc_options(true,true,false);
   scope current;
   std::string currentScopeName(extract<const char*>(current.attr("__name__")));
   currentScopeName="inference";
   //import_array();
   export_inference();
   //adder
   {
      substring=adderString;
      submoduleName = currentScopeName + std::string(".") + substring;
      // Create the submodule, and attach it to the current scope.
      object submodule(borrowed(PyImport_AddModule(submoduleName.c_str())));
      current.attr(substring.c_str()) = submodule;
      submodule.attr("__package__")=submoduleName.c_str();
      // Switch the scope to the submodule, add methods and classes.
      scope submoduleScope = submodule;
      // minimizer
      {
         subsubstring=minimizerString;
         subsubmoduleName = currentScopeName + std::string(".") + substring  + std::string(".") + subsubstring ;
         // Create the submodule, and attach it to the current scope.
         object subsubmodule(borrowed(PyImport_AddModule(subsubmoduleName.c_str())));
         submoduleScope.attr(subsubstring.c_str()) = subsubmodule;
         //subsubmodule.attr("__package__")=subsubmoduleName.c_str();
         // Switch the scope to the submodule, add methods and classes.
         scope subsubmoduleScope = subsubmodule;

         export_icm<opengm::python::GmAdder,opengm::Minimizer>();
         //export_pbp<opengm::python::GmAdder,opengm::Minimizer>();
         export_bp<opengm::python::GmAdder,opengm::Minimizer>();
         export_trbp<opengm::python::GmAdder,opengm::Minimizer>();
         export_astar<opengm::python::GmAdder,opengm::Minimizer>();
         //export_gibbs<opengm::python::GmAdder,opengm::Minimizer>();

         export_dual_decomposition_subgradient<opengm::python::GmAdder,opengm::Minimizer>();

         //export_self_fusion<opengm::python::GmAdder,opengm::Minimizer>();
         export_fusion_moves<opengm::python::GmAdder,opengm::Minimizer>();

         #ifdef WITH_CONICBUNDLE
            //export_dual_decomposition_bundle<opengm::python::GmAdder,opengm::Minimizer>();
         #endif
         export_lazyflipper<opengm::python::GmAdder,opengm::Minimizer>();
         #ifdef WITH_AD3
         export_loc<opengm::python::GmAdder,opengm::Minimizer>();
         #endif
         export_bruteforce<opengm::python::GmAdder,opengm::Minimizer>();
         export_graphcut<opengm::python::GmAdder,opengm::Minimizer>();
         export_abswap<opengm::python::GmAdder,opengm::Minimizer>();
         export_ae<opengm::python::GmAdder,opengm::Minimizer>();
         export_dynp<opengm::python::GmAdder,opengm::Minimizer>();
         export_partition_move<opengm::python::GmAdder,opengm::Minimizer>();

         //export_qpbo<opengm::python::GmAdder,opengm::Minimizer>();
         #ifdef WITH_QPBO
         export_reduced_inference<opengm::python::GmAdder,opengm::Minimizer>();
         export_ae_fusion<opengm::python::GmAdder,opengm::Minimizer>();
         export_mqpbo<opengm::python::GmAdder,opengm::Minimizer>();
         export_qpbo_external<opengm::python::GmAdder,opengm::Minimizer>();
         #endif
         #ifdef WITH_TRWS
         export_trws<opengm::python::GmAdder,opengm::Minimizer>();
         #endif

         #ifdef WITH_CPLEX
         export_cplex<opengm::python::GmAdder,opengm::Minimizer>();
         export_multicut<opengm::python::GmAdder,opengm::Minimizer>();
         #endif

        #if defined(WITH_QPBO) || defined(WITH_CPLEX) || defined(WITH_BLOSSOM5) && defined(WITH_PLANARITY)
        //export_cgc<opengm::python::GmAdder,opengm::Minimizer>();
       	//export_intersection_based<opengm::python::GmAdder,opengm::Minimizer>();
        #endif

         //export_lp_inference<opengm::python::GmAdder,opengm::Minimizer>();

         #ifdef WITH_LIBDAI
         export_libdai_inference<opengm::python::GmAdder,opengm::Minimizer>();
         #endif

         #ifdef WITH_MRF
         export_mrf<opengm::python::GmAdder,opengm::Minimizer>();
         #endif

         #ifdef WITH_FASTPD
         export_fast_pd<opengm::python::GmAdder,opengm::Minimizer>();
         #endif

         #ifdef WITH_AD3
         export_ad3<opengm::python::GmAdder,opengm::Minimizer>();
         #endif

         export_self_fusion<opengm::python::GmAdder,opengm::Minimizer>();
         export_fusion_based<opengm::python::GmAdder,opengm::Minimizer>();
      }
      // maximizer
      {
         subsubstring=maximizerString;
         subsubmoduleName = currentScopeName + std::string(".") + substring  + std::string(".") + subsubstring ;
         // Create the submodule, and attach it to the current scope.
         object subsubmodule(borrowed(PyImport_AddModule(subsubmoduleName.c_str())));
         submoduleScope.attr(subsubstring.c_str()) = subsubmodule;
         //subsubmodule.attr("__package__")=subsubmoduleName.c_str();
         // Switch the scope to the submodule, add methods and classes.
         scope subsubmoduleScope = subsubmodule;
         export_icm<opengm::python::GmAdder,opengm::Maximizer>();
         export_bp<opengm::python::GmAdder,opengm::Maximizer>();
         export_trbp<opengm::python::GmAdder,opengm::Maximizer>();
         export_astar<opengm::python::GmAdder,opengm::Maximizer>();
         export_lazyflipper<opengm::python::GmAdder,opengm::Maximizer>();
         #ifdef WITH_AD3
         export_loc<opengm::python::GmAdder,opengm::Maximizer>();
         #endif
         export_bruteforce<opengm::python::GmAdder,opengm::Maximizer>();
         export_dynp<opengm::python::GmAdder,opengm::Maximizer>();

         #ifdef WITH_AD3
         export_ad3<opengm::python::GmAdder,opengm::Maximizer>();
         #endif
      }
      // integrator
      {
         subsubstring=integratorString;
         subsubmoduleName = currentScopeName + std::string(".") + substring  + std::string(".") + subsubstring ;
         // Create the submodule, and attach it to the current scope.
         object subsubmodule(borrowed(PyImport_AddModule(subsubmoduleName.c_str())));
         submoduleScope.attr(subsubstring.c_str()) = subsubmodule;
         //subsubmodule.attr("__package__")=subsubmoduleName.c_str();
         scope subsubmoduleScope = subsubmodule;

         export_bp<opengm::python::GmAdder,opengm::Integrator>();
         export_trbp<opengm::python::GmAdder,opengm::Integrator>();
         //export_dynp<opengm::python::GmMultiplier,opengm::Maximizer>();
      }
      // logsumexp
      {
         subsubstring=logsumexpString;
         subsubmoduleName = currentScopeName + std::string(".") + substring  + std::string(".") + subsubstring ;
         // Create the submodule, and attach it to the current scope.
         object subsubmodule(borrowed(PyImport_AddModule(subsubmoduleName.c_str())));
         submoduleScope.attr(subsubstring.c_str()) = subsubmodule;
         //subsubmodule.attr("__package__")=subsubmoduleName.c_str();
         scope subsubmoduleScope = subsubmodule;

         export_bp<opengm::python::GmAdder,opengm::Logsumexp>();
         export_bruteforce<opengm::python::GmAdder,opengm::Logsumexp>();
      }
   }
   //multiplier
   {
      substring=multiplierString;
      submoduleName = currentScopeName + std::string(".") + substring;
      // Create the submodule, and attach it to the current scope.
      object submodule(borrowed(PyImport_AddModule(submoduleName.c_str())));
      current.attr(substring.c_str()) = submodule;
      submodule.attr("__package__")=submoduleName.c_str();
      // Switch the scope to the submodule, add methods and classes.
      scope submoduleScope = submodule;
      // minimizer
      {
         subsubstring=minimizerString;
         subsubmoduleName = currentScopeName + std::string(".") + substring  + std::string(".") + subsubstring ;
         // Create the submodule, and attach it to the current scope.
         object subsubmodule(borrowed(PyImport_AddModule(subsubmoduleName.c_str())));
         submoduleScope.attr(subsubstring.c_str()) = subsubmodule;
         //subsubmodule.attr("__package__")=subsubmoduleName.c_str();
         // Switch the scope to the submodule, add methods and classes.
         scope subsubmoduleScope = subsubmodule;
         export_icm<opengm::python::GmMultiplier,opengm::Minimizer>();
         export_bp<opengm::python::GmMultiplier,opengm::Minimizer>();
         export_trbp<opengm::python::GmMultiplier,opengm::Minimizer>();
         export_astar<opengm::python::GmMultiplier,opengm::Minimizer>();
         export_lazyflipper<opengm::python::GmMultiplier,opengm::Minimizer>();
         //export_loc<opengm::python::GmMultiplier,opengm::Minimizer>();
         export_bruteforce<opengm::python::GmMultiplier,opengm::Minimizer>();
         export_dynp<opengm::python::GmMultiplier,opengm::Minimizer>();
      }
      // maximizer
      {
         subsubstring=maximizerString;
         subsubmoduleName = currentScopeName + std::string(".") + substring  + std::string(".") + subsubstring ;
         // Create the submodule, and attach it to the current scope.
         object subsubmodule(borrowed(PyImport_AddModule(subsubmoduleName.c_str())));
         submoduleScope.attr(subsubstring.c_str()) = subsubmodule;
         //subsubmodule.attr("__package__")=subsubmoduleName.c_str();
         // Switch the scope to the submodule, add methods and classes.
         scope subsubmoduleScope = subsubmodule;
         export_icm<opengm::python::GmMultiplier,opengm::Maximizer>();
         export_bp<opengm::python::GmMultiplier,opengm::Maximizer>();
         export_trbp<opengm::python::GmMultiplier,opengm::Maximizer>();
         export_astar<opengm::python::GmMultiplier,opengm::Maximizer>();
         //export_gibbs<opengm::python::GmMultiplier,opengm::Maximizer>();
         export_lazyflipper<opengm::python::GmMultiplier,opengm::Maximizer>();
         //export_loc<opengm::python::GmMultiplier,opengm::Maximizer>();
         export_bruteforce<opengm::python::GmMultiplier,opengm::Maximizer>();
         export_dynp<opengm::python::GmMultiplier,opengm::Maximizer>();
      }
      // integrator
      {
         subsubstring=integratorString;
         subsubmoduleName = currentScopeName + std::string(".") + substring  + std::string(".") + subsubstring ;
         // Create the submodule, and attach it to the current scope.
         object subsubmodule(borrowed(PyImport_AddModule(subsubmoduleName.c_str())));
         submoduleScope.attr(subsubstring.c_str()) = subsubmodule;
         //subsubmodule.attr("__package__")=subsubmoduleName.c_str();
         scope subsubmoduleScope = subsubmodule;

         export_bp<opengm::python::GmMultiplier,opengm::Integrator>();
         export_trbp<opengm::python::GmMultiplier,opengm::Integrator>();
         //export_dynp<opengm::python::GmMultiplier,opengm::Maximizer>();
      }
   }
}


// #include "ros/ros.h"
// #include "origarm_ros/States.h"
// #include "origarm_ros/Command_Pre_Open.h"
// #include "origarm_ros/Command_ABL.h"
// #include "origarm_ros/SegOpening.h"
// #include "origarm_ros/modenumber.h"
// #include "origarm_ros/segnumber.h"
// #include "origarm_ros/keynumber.h"
// #include "origarm_ros/PIDdata.h"
// #include "origarm_ros/PIDparam.h"

// #include "myPID.h"
// #include "myPID.cpp"
// #include "myData.h"
// #include <Eigen/Eigen>
// #include <Eigen/Dense>
// #include <Eigen/Geometry>
// #include <linux/input-event-codes.h>

// float b1[SEGNUM];
// float btem[SEGNUM];
// float b2[SEGNUM];
// float b3[SEGNUM];
// float pressureD[SEGNUM][ACTNUM];
// float alphad[SEGNUM];                //ABL desired value
// float betad[SEGNUM];
// float lengthd[SEGNUM];

// float ajoy[SEGNUM];
// float bjoy[SEGNUM];
// float ljoy[SEGNUM];

// float l0;

// float Texta[SEGNUM];                 //external torque
// float Textb[SEGNUM];
// float Tx[SEGNUM];
// float Ty[SEGNUM];
// float Fl = 0;
// float phycD[SEGNUM];
// float physD[SEGNUM];
// float phypD[SEGNUM];

// float pLimitOptimal = 0;
// float dalphaRatio[SEGNUM];
// float dbeta[SEGNUM];
// float dlength[SEGNUM];

// float alphar[SEGNUM];                //ABL real value, calculated by sensor data or by passive sensing
// float betar[SEGNUM];
// float lengthr[SEGNUM];

// float phycFeedbackAlpha[SEGNUM];
// float physFeedbackAlpha[SEGNUM];
// float phycFeedbackBeta[SEGNUM];
// float physFeedbackBeta[SEGNUM];
// float phycFeedback[SEGNUM];
// float physFeedback[SEGNUM];
// float phypFeedback[SEGNUM];
// float pressureDFeed[SEGNUM][ACTNUM];
// float pressureDBack[SEGNUM][ACTNUM];

// int mode_ = 0;
// int segnumber_;
// int feedbackFlag = 0;

// using namespace std;

// //initialization
// PID_Type *alphaPID[SEGNUM], *betaPID[SEGNUM], *lengthPID[SEGNUM];

// float pidparam_kp[SEGNUM][3]; //retrieving data from topic updatePID to update above PID_Type
// float pidparam_ki[SEGNUM][3];
// float pidparam_kd[SEGNUM][3];
// float pidparam_dt[SEGNUM][3];
// float pidparam_ilim[SEGNUM][3];
// float pidparam_ulim[SEGNUM][3];

// float pidDeadzoneAlpha = 0.015;  //rad
// float pidDeadzoneBeta = 2.5 / 180.0 * M_PI;
// float pidDeadzoneLength = 0.001;

// void initParameter()
// {
//   for (int i = 0; i < SEGNUM; i++)
//   {
//     ajoy[i] = 0;
//     bjoy[i] = 0;
//     ljoy[i] = g_length0;
//   }

//   for (int i = 0; i < SEGNUM; i++)
//   {
//     alphaPID[i]  = newPID(0.8, 0.005, 0, 0.005, 0.6, 1);
//     betaPID[i]   = newPID(0.8, 0.005, 0, 0.005, 0.5, 1.5);
//     lengthPID[i] = newPID(0.7, 0.005, 0, 0.005, 0.02, 0.02);
//   }

//   for (int i = 0; i < SEGNUM; i++)
//   {
//     //*alphaPID = newPID(0.8, 0.005, 0, 0.005, 0.6, 1);
//     pidparam_kp[i][0] = 0.8;
//     pidparam_ki[i][0] = 0.005;
//     pidparam_kd[i][0] = 0;
//     pidparam_dt[i][0] = 0.005;
//     pidparam_ilim[i][0] = 0.6;
//     pidparam_ulim[i][0] = 1;

//     //*betaPID = newPID(0.8, 0.005, 0, 0.005, 0.5, 1.5);
//     pidparam_kp[i][1] = 0.8;
//     pidparam_ki[i][1] = 0.005;
//     pidparam_kd[i][1] = 0;
//     pidparam_dt[i][1] = 0.005;
//     pidparam_ilim[i][1] = 0.5;
//     pidparam_ulim[i][1] = 1.5;

//     //*lengthPID = newPID(0.7, 0.005, 0, 0.005, 0.02, 0.02)
//     pidparam_kp[i][2] = 0.7;
//     pidparam_ki[i][2] = 0.005;
//     pidparam_kd[i][2] = 0;
//     pidparam_dt[i][2] = 0.005;
//     pidparam_ilim[i][2] = 0.02;
//     pidparam_ulim[i][2] = 0.02;
//   }
// }

// void ABLD2PD_LMS()
// {
//   if (mode_ == 2 && feedbackFlag == 1)
//   {
//     for (int i = 0; i < SEGNUM; i++)
//     {
//       if (i == segnumber_) // change to 3 actuation
//       {
//         pressureDFeed[i][0] = 0;
//         pressureDFeed[i][1] = (phycD[i]+phypD[i]+1.7320508*physD[i])/6;
//         pressureDFeed[i][2] = 0;
//         pressureDFeed[i][3] = phypD[i]/6-phycD[i]/3;
//         pressureDFeed[i][4] = 0;
//         pressureDFeed[i][5] = (phypD[i]+phycD[i]-1.7320508*physD[i])/6;

//         pressureDFeed[i][1]+=pLimitOptimal;
//         pressureDFeed[i][3]+=pLimitOptimal;
//         pressureDFeed[i][5]+=pLimitOptimal;
//       }
//       else
//       {
//         Tx[i] = Texta[i]/g_actuator_effectiveArea/g_install_radius-g_constC1*alphad[i];
//         Ty[i] = Textb[i]/g_actuator_effectiveArea/g_install_radius;
//         phycD[i] = cos(betad[i])*Tx[i]-sin(betad[i])*Ty[i];
//         physD[i] = sin(betad[i])*Tx[i]+cos(betad[i])*Ty[i];

//         l0 = g_length0;

//         phypD[i] = (Fl+6*g_k0*(lengthd[i]-l0))/g_actuator_effectiveArea;

//         pressureDFeed[i][0] = phypD[i]/6+phycD[i]/3;
//         pressureDFeed[i][1] = (phycD[i]+phypD[i]+1.7320508*physD[i])/6;
//         pressureDFeed[i][2] = (phypD[i]-phycD[i]+1.7320508*physD[i])/6;
//         pressureDFeed[i][3] = phypD[i]/6-phycD[i]/3;
//         pressureDFeed[i][4] = (phypD[i]-phycD[i]-1.7320508*physD[i])/6;
//         pressureDFeed[i][5] = (phypD[i]+phycD[i]-1.7320508*physD[i])/6;

//         pressureDFeed[i][0]-=pLimitOptimal;
//         pressureDFeed[i][1]+=pLimitOptimal;
//         pressureDFeed[i][2]-=pLimitOptimal;
//         pressureDFeed[i][3]+=pLimitOptimal;
//         pressureDFeed[i][4]-=pLimitOptimal;
//         pressureDFeed[i][5]+=pLimitOptimal;
//       }
//     }
//   }
//   else
//   {
//     for (int i = 0; i < SEGNUM; i++)
//     {
//       Tx[i] = Texta[i]/g_actuator_effectiveArea/g_install_radius-g_constC1*alphad[i];
//       Ty[i] = Textb[i]/g_actuator_effectiveArea/g_install_radius;
//       phycD[i] = cos(betad[i])*Tx[i]-sin(betad[i])*Ty[i];
//       physD[i] = sin(betad[i])*Tx[i]+cos(betad[i])*Ty[i];

//       l0 = g_length0;

//       phypD[i] = (Fl+6*g_k0*(lengthd[i]-l0))/g_actuator_effectiveArea;

//       pressureDFeed[i][0] = phypD[i]/6+phycD[i]/3;
//       pressureDFeed[i][1] = (phycD[i]+phypD[i]+1.7320508*physD[i])/6;
//       pressureDFeed[i][2] = (phypD[i]-phycD[i]+1.7320508*physD[i])/6;
//       pressureDFeed[i][3] = phypD[i]/6-phycD[i]/3;
//       pressureDFeed[i][4] = (phypD[i]-phycD[i]-1.7320508*physD[i])/6;
//       pressureDFeed[i][5] = (phypD[i]+phycD[i]-1.7320508*physD[i])/6;

//       pressureDFeed[i][0]-=pLimitOptimal;
//       pressureDFeed[i][1]+=pLimitOptimal;
//       pressureDFeed[i][2]-=pLimitOptimal;
//       pressureDFeed[i][3]+=pLimitOptimal;
//       pressureDFeed[i][4]-=pLimitOptimal;
//       pressureDFeed[i][5]+=pLimitOptimal;
//     }
//   }
// }

// //calculate PressureFeedback
// void FeedbackController(int fdFlag)
// {
//   ABLD2PD_LMS();

//   if (fdFlag)
//   {
//     if(mode_ == 0 || mode_ == 2) // with feedback from sensor or passive sensing
//     {
//       for (int i = 0; i < SEGNUM; i++)
//       {
//         if (i > 5)
//         {
//           for (int j = 0; j < ACTNUM; j++)
//           {
//             pressureDBack[i][j] = 0;
//           }
//         }
//         else
//         {
//           if (abs(alphad[i] - alphar[i]) < pidDeadzoneAlpha)
//           {
//             dalphaRatio[i] = 0;
//           }
//           else
//           {
//             float alphadTemp=alphad[i];
//             if (abs(alphad[i]) < 1e-5)
//             {
//               alphadTemp = (alphad[i] >= 0 ? 1e-5 : -1e-5);
//             }
//           dalphaRatio[i] = updatePID(alphaPID[i], 1, alphar[i]/alphadTemp);
//           }

//           if (abs(betad[i] - betar[i]) < pidDeadzoneBeta)
//           {
//             dbeta[i] = updatePID(betaPID[i], betad[i], betad[i]);
//           }
//           else
//           {
//             dbeta[i] = updatePID(betaPID[i], betad[i], betar[i]);
//           }

//           if (abs(lengthd[i] - lengthr[i]) < pidDeadzoneLength)
//           {
//             dlength[i] = updatePID(lengthPID[i], lengthd[i], lengthd[i]);
//           }
//           else
//           {
//             dlength[i] = updatePID(lengthPID[i], lengthd[i], lengthr[i]);
//           }

//           phycFeedbackBeta[i] = (cos(dbeta[i])-1)*phycD[i]-sin(dbeta[i])*physD[i];
//           physFeedbackBeta[i] = sin(dbeta[i])*phycD[i]+(cos(dbeta[i])-1)*physD[i];

//           phycFeedbackAlpha[i] = dalphaRatio[i]*(cos(dbeta[i])*phycD[i]-sin(dbeta[i])*physD[i]);
//           physFeedbackAlpha[i] = dalphaRatio[i]*(sin(dbeta[i])*phycD[i]+cos(dbeta[i])*physD[i]);

//           phycFeedback[i] = phycFeedbackBeta[i]+phycFeedbackAlpha[i];
//           physFeedback[i] = physFeedbackBeta[i]+physFeedbackAlpha[i];
//           phypFeedback[i] = 6*g_k0*dlength[i]/g_actuator_effectiveArea;

//           pressureDBack[i][0] = phypFeedback[i]/6+phycFeedback[i]/3;
//           pressureDBack[i][1] = (phycFeedback[i]+phypFeedback[i]+1.7320508*physFeedback[i])/6;
//           pressureDBack[i][2] = (phypFeedback[i]-phycFeedback[i]+1.7320508*physFeedback[i])/6;
//           pressureDBack[i][3] = phypFeedback[i]/6-phycFeedback[i]/3;
//           pressureDBack[i][4] = (phypFeedback[i]-phycFeedback[i]-1.7320508*physFeedback[i])/6;
//           pressureDBack[i][5] = (phypFeedback[i]+phycFeedback[i]-1.7320508*physFeedback[i])/6;
//         }
//       }
//     }
//     else if (mode_ == 1) // only shutdown feedback of selected segment
//     {
//       for (int i = 0; i < SEGNUM; i++)
//       {
//         if (i != segnumber_)
//         {
//           if (abs(alphad[i] - alphar[i]) < pidDeadzoneAlpha)
//           {
//             dalphaRatio[i] = 0;
//           }
//           else
//           {
//             float alphadTemp=alphad[i];
//             if (abs(alphad[i]) < 1e-5)
//             {
//               alphadTemp = (alphad[i] >= 0 ? 1e-5 : -1e-5);
//             }
//             dalphaRatio[i] = updatePID(alphaPID[i], 1, alphar[i]/alphadTemp);
//           }

//           if (abs(betad[i] - betar[i]) < pidDeadzoneBeta)
//           {
//             dbeta[i] = updatePID(betaPID[i], betad[i], betad[i]);
//           }
//           else
//           {
//             dbeta[i] = updatePID(betaPID[i], betad[i], betar[i]);
//           }

//           if (abs(lengthd[i] - lengthr[i]) < pidDeadzoneLength)
//           {
//             dlength[i] = updatePID(lengthPID[i], lengthd[i], lengthd[i]);
//           }
//           else
//           {
//             dlength[i] = updatePID(lengthPID[i], lengthd[i], lengthr[i]);
//           }

//           phycFeedbackBeta[i] = (cos(dbeta[i])-1)*phycD[i]-sin(dbeta[i])*physD[i];
//           physFeedbackBeta[i] = sin(dbeta[i])*phycD[i]+(cos(dbeta[i])-1)*physD[i];

//           phycFeedbackAlpha[i] = dalphaRatio[i]*(cos(dbeta[i])*phycD[i]-sin(dbeta[i])*physD[i]);
//           physFeedbackAlpha[i] = dalphaRatio[i]*(sin(dbeta[i])*phycD[i]+cos(dbeta[i])*physD[i]);

//           phycFeedback[i] = phycFeedbackBeta[i]+phycFeedbackAlpha[i];
//           physFeedback[i] = physFeedbackBeta[i]+physFeedbackAlpha[i];
//           phypFeedback[i] = 6*g_k0*dlength[i]/g_actuator_effectiveArea;

//           pressureDBack[i][0] = phypFeedback[i]/6+phycFeedback[i]/3;
//           pressureDBack[i][1] = (phycFeedback[i]+phypFeedback[i]+1.7320508*physFeedback[i])/6;
//           pressureDBack[i][2] = (phypFeedback[i]-phycFeedback[i]+1.7320508*physFeedback[i])/6;
//           pressureDBack[i][3] = phypFeedback[i]/6-phycFeedback[i]/3;
//           pressureDBack[i][4] = (phypFeedback[i]-phycFeedback[i]-1.7320508*physFeedback[i])/6;
//           pressureDBack[i][5] = (phypFeedback[i]+phycFeedback[i]-1.7320508*physFeedback[i])/6;
//         }
//         else
//         {
//           for (int j = 0; j < ACTNUM; j++)
//           {
//             pressureDBack[i][j] = 0;
//           }

//           printf("Segmemt[%d] went wrong!\n", i);
//         }
//       }
//     }
//   }
//   else // no feedback
//   {
//     for (int i = 0; i < SEGNUM; i++)
//     {
//       for (int j = 0; j < ACTNUM; j++)
//       {
//           pressureDBack[i][j] = 0;
//       }
//     }
//   }

//   for (int i = 0; i < SEGNUM; i++)
//   {
//     for (int j = 0; j < ACTNUM; j++)
//     {
//         pressureD[i][j] = pressureDFeed[i][j] + pressureDBack[i][j];
//     }
//   }
// }

// class ABL_controller
// {
//   public:
//     ABL_controller()
//     {
//       sub1_ = n_.subscribe("States", 1, &ABL_controller::States, this);
//       sub2_ = n_.subscribe("Cmd_ABL_joy", 1, &ABL_controller::ABL_joy, this);
//       sub3_ = n_.subscribe("modenumber", 1, &ABL_controller::mode, this);
//       sub4_ = n_.subscribe("segnumber", 1, &ABL_controller::segnumber, this);
//       sub5_ = n_.subscribe("key_number", 1, &ABL_controller::keyCallback, this);
//       sub6_ = n_.subscribe("setPIDparam", 1, &ABL_controller::pidparamCallback, this);
//       pub_ = n_.advertise<origarm_ros::Command_Pre_Open>("Command_Pre_Open", 100);
//       pub2_ = n_.advertise<origarm_ros::Command_Pre_Open>("Command_Pre_feedback", 100);
//       pub3_ = n_.advertise<origarm_ros::Command_Pre_Open>("Command_Pre_feedforward", 100);
//       pub4_ = n_.advertise<origarm_ros::PIDdata>("PID_data", 100);
//     }

//     void States(const origarm_ros::States& msg)
//     {
//       for (int i = 0; i < SEGNUM; i++)
//       {
//         alphar[i] = msg.ABL.segment[i].A;
//         betar[i] = msg.ABL.segment[i].B;
//         lengthr[i] = msg.ABL.segment[i].L;
//       }
//     }

//     void mode(const origarm_ros::modenumber& msg)
//     {
//       mode_ = msg.modeNumber;
//     }

//     void segnumber(const origarm_ros::segnumber& msg)
//     {
//       segnumber_ = msg.segmentNumber;
//     }

//     void ABL_joy(const origarm_ros::Command_ABL& msg)
//     {
//       for (int i = 0; i < SEGNUM; i++)
//       {
//         ajoy[i] = msg.segment[i].A;
//         bjoy[i] = msg.segment[i].B;
//         ljoy[i] = msg.segment[i].L;
//       }
//   }

//     void keyCallback(const origarm_ros::keynumber &key)
//     {
//       if (key.keycodePressed == KEY_W) // 'C' pressed
//       {
//         printf(" KEY_W pressed!\r\n");
//         feedbackFlag = 1 - feedbackFlag;
//       }
//     }

//     void pidparamCallback(const origarm_ros::PIDparam &msg)
//   {
//     int segn = msg.segIndex;
//     int ablInd = msg.ablIndex;
//     const char *paraName = msg.paraName.c_str();
//     float paraValue = msg.paraValue;

//     if (!strcmp(paraName, "kp"))
//     {
//       setKp(segmentPID[segn][ablInd], paraValue);
//     }
//     else if (!strcmp(paraName, "ki"))
//     {
//       setKi(segmentPID[segn][ablInd], paraValue);
//     }
//     else if (!strcmp(paraName, "kd"))
//     {
//       setKd(segmentPID[segn][ablInd], paraValue);
//     }
//     else if (!strcmp(paraName, "dt"))
//     {
//       setDt(segmentPID[segn][ablInd], paraValue);
//     }
//     else if (!strcmp(paraName, "ilim"))
//     {
//       setIlim(segmentPID[segn][ablInd], paraValue);
//     }
//     else if (!strcmp(paraName, "ulim"))
//     {
//       setUlim(segmentPID[segn][ablInd], paraValue);
//     }
//     else if (!strcmp(paraName, "deadzone"))
//     {
//       setDeadZone(segmentPID[segn][ablInd], paraValue);
//     }

//     printf("get new PID parameters. for seg %d, ABL %d, %s, %f\r\n", segn, ablInd, paraName, paraValue);
//   }

//     void pub()
//     {
//       //control mode
//       //mode[0]: normal; mode[1]: fail; mode[2]: passive sensing;
//       for (int i = 0; i < SEGNUM; i++)
//       {
//         for (int j = 0; j < ACTNUM; j++)
//         {
//           Cmd_P_O.segment[i].command[j].pressure = pressureD[i][j]/100;     //send hPa to spi_node
//           if (mode_ == 1 || mode_ == 2)
//           {
//             if (j == segnumber_)
//             {
//               Cmd_P_O.segment[i].command[j].valve    = 0;                    //bool == 0,
//             }
//             else
//             {
//               Cmd_P_O.segment[i].command[j].valve    = 1;                    //bool == 1, commandType == pressureCommandType
//             }
//           }
//           else
//           {
//             Cmd_P_O.segment[i].command[j].valve    = 1;                      //bool == 1, commandType == pressureCommandType
//           }

//           Cmd_P_feedback.segment[i].command[j].pressure = pressureDBack[i][j] / 100;    // publish pressurefeedback
//           Cmd_P_feedforward.segment[i].command[j].pressure = pressureDFeed[i][j] / 100; // publish pressurefeedforward
//         }

//         PID_data.dalphaRatio[i] = dalphaRatio[i];
//         PID_data.alphaP[i] = alphaPID[i]->P;
//         PID_data.alphaI[i] = alphaPID[i]->I;
//         PID_data.alphaD[i] = alphaPID[i]->D;

//         PID_data.dbeta[i] = dbeta[i];
//         PID_data.betaP[i] = betaPID[i]->P;
//         PID_data.betaI[i] = betaPID[i]->D;
//         PID_data.betaD[i] = betaPID[i]->I;

//         PID_data.dlength[i] = dlength[i];
//         PID_data.lengthP[i] = lengthPID[i]->P;
//         PID_data.lengthI[i] = lengthPID[i]->I;
//         PID_data.lengthD[i] = lengthPID[i]->D;
//       }

//       pub_.publish(Cmd_P_O);
//       pub2_.publish(Cmd_P_feedback);
//       pub3_.publish(Cmd_P_feedforward);
//       pub4_.publish(PID_data);
//     }

//     private:
//       ros::NodeHandle n_;
//       ros::Subscriber sub1_;
//       ros::Subscriber sub2_;
//       ros::Subscriber sub3_;
//       ros::Subscriber sub4_;
//       ros::Subscriber sub5_;
//       ros::Subscriber sub6_;
//       ros::Publisher pub_ ;
//       ros::Publisher pub2_;
//       ros::Publisher pub3_;
//       ros::Publisher pub4_;

//       origarm_ros::Command_Pre_Open Cmd_P_O;
//       origarm_ros::Command_ABL Command_ABL;
//       origarm_ros::Command_Pre_Open Cmd_P_feedback;
//       origarm_ros::Command_Pre_Open Cmd_P_feedforward;
//       origarm_ros::PIDdata PID_data;
//   };

int main(int argc, char **argv)
{
  // ros::init(argc, argv, "ABL_controller_updated");

  // initParameter();

  // ABL_controller ABL_controller_updated;

  // ros::AsyncSpinner s(6);
  // s.start();

  // ros::Rate loop_rate(100);

  // ROS_INFO("Ready for ABL_controller_node");

  // while(ros::ok())
  // {
  //   FeedbackController(feedbackFlag);

  //   ABL_controller_updated.pub();

  //   // for (int i = 0; i < SEGNUM; i++)
  //   // {
  //   //   printf("Command[%d]: %f %f %f %f %f %f\r\n", i,
  //   //     pressureD[i][0]/100,
  //   //     pressureD[i][1]/100,
  //   //     pressureD[i][2]/100,
  //   //     pressureD[i][3]/100,
  //   //     pressureD[i][4]/100,
  //   //     pressureD[i][5]/100);
  //   // }

  //   loop_rate.sleep();
  // }

  // ros::waitForShutdown();
   return 0;
}


/*
 * #%L
 * OME-XML C++ library for working with OME-XML metadata structures.
 * %%
 * Copyright © 2006 - 2016 Open Microscopy Environment:
 *   - Massachusetts Institute of Technology
 *   - National Institutes of Health
 *   - University of Dundee
 *   - Board of Regents of the University of Wisconsin-Madison
 *   - Glencoe Software, Inc.
 * %%
 * 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.
 *
 * 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 HOLDERS 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.
 *
 * The views and conclusions contained in the software and documentation are
 * those of the authors and should not be interpreted as representing official
 * policies, either expressed or implied, of any organization.
 * #L%
 */

#include <boost/range/size.hpp>

#include <ome/xml/model/primitives/PositiveFloat.h>

#include "constrained-numeric.h"

using ome::xml::model::primitives::PositiveFloat;

// Direct equality comparisons are not safe for floating point types.
// The tests here use an allowed error of 0.05.

template<>
struct CompareEqual<PositiveFloat>
{
  bool compare(PositiveFloat lhs, PositiveFloat rhs)
  { return lhs > static_cast<PositiveFloat::value_type>(rhs) - 0.05 && lhs < static_cast<PositiveFloat::value_type>(rhs) + 0.05; }

  bool compare(PositiveFloat lhs, PositiveFloat::value_type rhs)
  { return lhs > static_cast<PositiveFloat::value_type>(rhs) - 0.05 && lhs < static_cast<PositiveFloat::value_type>(rhs) + 0.05; }

  bool compare(PositiveFloat::value_type lhs, PositiveFloat rhs)
  { return lhs > static_cast<PositiveFloat::value_type>(rhs) - 0.05 && lhs < static_cast<PositiveFloat::value_type>(rhs) + 0.05; }
};

template<>
struct CompareNotEqual<PositiveFloat>
{
  bool compare(PositiveFloat lhs, PositiveFloat rhs)
  { return lhs < static_cast<PositiveFloat::value_type>(rhs) - 0.05 || lhs > static_cast<PositiveFloat::value_type>(rhs) + 0.05; }

  bool compare(PositiveFloat lhs, PositiveFloat::value_type rhs)
  { return lhs < static_cast<PositiveFloat::value_type>(rhs) - 0.05 || lhs > static_cast<PositiveFloat::value_type>(rhs) + 0.05; }

  bool compare(PositiveFloat::value_type lhs, PositiveFloat rhs)
  { return lhs < static_cast<PositiveFloat::value_type>(rhs) - 0.05 || lhs > static_cast<PositiveFloat::value_type>(rhs) + 0.05; }
};

template<>
struct CompareLessOrEqual<PositiveFloat>
{
  bool compare(PositiveFloat lhs, PositiveFloat rhs)
  { return lhs < static_cast<PositiveFloat::value_type>(rhs) + 0.05; }

  bool compare(PositiveFloat lhs, PositiveFloat::value_type rhs)
  { return lhs < static_cast<PositiveFloat::value_type>(rhs) + 0.05; }

  bool compare(PositiveFloat::value_type lhs, PositiveFloat rhs)
  { return lhs < static_cast<PositiveFloat::value_type>(rhs) + 0.05; }
};

template<>
struct CompareGreaterOrEqual<PositiveFloat>
{
  bool compare(PositiveFloat lhs, PositiveFloat rhs)
  { return lhs > static_cast<PositiveFloat::value_type>(rhs) - 0.05; }

  bool compare(PositiveFloat lhs, PositiveFloat::value_type rhs)
  { return lhs > static_cast<PositiveFloat::value_type>(rhs) - 0.05; }

  bool compare(PositiveFloat::value_type lhs, PositiveFloat rhs)
  { return lhs > static_cast<PositiveFloat::value_type>(rhs) - 0.05; }
};

// Floating point types don't implement modulo, increment or
// decrement, so make them a no-op.
template<>
struct OperationModulo<PositiveFloat>
{
  PositiveFloat eval(PositiveFloat lhs,             PositiveFloat /* rhs */) { return lhs; }
  PositiveFloat eval(PositiveFloat lhs, PositiveFloat::value_type /* rhs */) { return lhs; }
};

template<>
struct OperationModuloAssign<PositiveFloat>
{
  PositiveFloat eval(PositiveFloat lhs,             PositiveFloat /* rhs */) { return lhs; }
  PositiveFloat eval(PositiveFloat lhs, PositiveFloat::value_type /* rhs */) { return lhs; }
};

template<>
struct OperationIncrement<PositiveFloat>
{
  PositiveFloat eval(PositiveFloat lhs,             PositiveFloat /* rhs */) { return lhs; }
  PositiveFloat eval(PositiveFloat lhs, PositiveFloat::value_type /* rhs */) { return lhs; }
};

template<>
struct OperationDecrement<PositiveFloat>
{
  PositiveFloat eval(PositiveFloat lhs,             PositiveFloat /* rhs */) { return lhs; }
  PositiveFloat eval(PositiveFloat lhs, PositiveFloat::value_type /* rhs */) { return lhs; }
};

INSTANTIATE_TYPED_TEST_CASE_P(PositiveFloat, NumericTest, PositiveFloat);

template<>
const std::vector<NumericTest<PositiveFloat>::test_str>
NumericTest<PositiveFloat>::strings =
  { // str     pos      strpass pospass
    {"23",      23.0,   true,   true},
    {"-42.12", -42.12,  false,  false},
    {"1.0",    -53.0,   true,   false},
    {"82.232",  82.232, true,   true},
    {"0",        0,     false,  false},
    {"1.0",      0.0,   true,   false},
    {"invalid",  1.0,   false,  true},
  };

template<>
const std::vector<NumericTest<PositiveFloat>::test_op>
NumericTest<PositiveFloat>::ops =
  { // v1   v2        expected        operation         pass   except rhsexcept
    {   23.0,   23.0,            1.0, EQUAL,            true,  false, false},
    {   23.0,  432.0,            0.0, EQUAL,            false, false, false},
    {   23.0,   35.0,            1.0, NOT_EQUAL,        true,  false, false},
    {   54.0,   54.0,            0.0, NOT_EQUAL,        false, false, false},

    { 5432.0, 8272.0,            1.0, LESS,             true,  false, false},
    {  534.0,  534.0,            0.0, LESS,             false, false, false},
    {  947.0,   34.0,            0.0, LESS,             false, false, false},
    { 5432.0, 8272.0,            1.0, LESS_OR_EQUAL,    true,  false, false},
    {  534.0,  534.0,            1.0, LESS_OR_EQUAL,    true,  false, false},
    {  947.0,   34.0,            0.0, LESS_OR_EQUAL,    false, false, false},

    {10873.0, 8420.0,            1.0, GREATER,          true,  false, false},
    { 3622.0, 3622.0,            0.0, GREATER,          false, false, false},
    {  872.0, 2701.0,            0.0, GREATER,          false, false, false},
    {10873.0, 8420.0,            1.0, GREATER_OR_EQUAL, true,  false, false},
    { 3622.0, 3622.0,            1.0, GREATER_OR_EQUAL, true,  false, false},
    {  872.0, 2701.0,            0.0, GREATER_OR_EQUAL, false, false, false},

    {  432.0,  743.0, 432.0 +  743.0, ADD,              true,  false, false},
    {  432.0,  -74.0, 432.0 -   74.0, ADD,              true,  false, true},
    {  432.0, -743.0, 432.0 -  743.0, ADD,              false, true,  true},
    {  432.0,  743.0,            1.0, ADD,              false, false, false},
    {  823.0,   93.0, 823.0 +   93.0, ADD_ASSIGN,       true,  false, false},
    {  823.0,  -93.0, 823.0 -   93.0, ADD_ASSIGN,       true,  false, true},
    {  823.0, -932.0, 823.0 -  932.0, ADD_ASSIGN,       false, true,  true},
    {  823.0,   93.0,            1.0, ADD_ASSIGN,       false, false, false},

    {  432.0,  285.0, 432.0 -  285.0, SUBTRACT,         true,  false, false},
    {  432.0, -285.0, 432.0 +  285.0, SUBTRACT,         true,  false, true},
    {  432.0,  763.0, 432.0 -  763.0, SUBTRACT,         false, true,  false},
    {  432.0,  285.0,            1.0, SUBTRACT,         false, false, false},
    {  432.0,  431.0,            1.0, SUBTRACT,         true,  false, false},
    {  432.0,  432.0,            0.0, SUBTRACT,         false, true,  false},
    {  823.0,   93.0, 823.0 -   93.0, SUBTRACT_ASSIGN,  true,  false, false},
    {  823.0, -932.0, 823.0 +  932.0, SUBTRACT_ASSIGN,  true,  false, true},
    {  823.0, 1393.0, 823.0 - 1393.0, SUBTRACT_ASSIGN,  false, true,  false},
    {  823.0,   93.0,            1.0, SUBTRACT_ASSIGN,  false, false, false},
    {  823.0,  822.0,            1.0, SUBTRACT_ASSIGN,  true,  false, false},
    {  823.0,  823.0,            0.0, SUBTRACT_ASSIGN,  false, true,  false},

    {   40.0,   12.0,  40.0 *   12.0, MULTIPLY,         true,  false, false},
    {   23.0,   -8.0,  23.0 *   -8.0, MULTIPLY,         false, true,  true},
    {   40.0,   12.0,            1.0, MULTIPLY,         false, false, false},
    {   18.0,    4.0,  18.0 *    4.0, MULTIPLY_ASSIGN,  true,  false, false},
    {    2.0, -232.0,   2.0 * -232.0, MULTIPLY_ASSIGN,  false, true,  true},
    {   18.0,    4.0,            1.0, MULTIPLY_ASSIGN,  false, false, false},

    {  900.0,    5.0, 900.0 /    5.0, DIVIDE,           true,  false, false},
    {  900.0,   -5.0, 900.0 /   -5.0, DIVIDE,           false, true,  true},
    {  900.0,  900.0, 900.0 /  900.0, DIVIDE,           true,  false, false},
    {  900.0,  901.0, 900.1 /  901.0, DIVIDE,           true,  false, false},
    {  900.0,  900.0,            2.0, DIVIDE,           false, false, false},
    {  480.0,   20.0, 480.0 /   20.0, DIVIDE_ASSIGN,    true,  false, false},
    {  480.0,  -20.0, 480.0 /  -20.0, DIVIDE_ASSIGN,    false, true,  true},
    {  480.0,  480.0, 480.0 /  480.0, DIVIDE_ASSIGN,    true,  false, false},
    {  480.0,  481.0, 480.0 /  481.0, DIVIDE_ASSIGN,    true,  false, false},
    {  480.0,  480.0,            2.0, DIVIDE_ASSIGN,    false, false, false},

  };

template<>
const PositiveFloat::value_type NumericTest<PositiveFloat>::error(0.0005);

template<>
const PositiveFloat NumericTest<PositiveFloat>::safedefault(9999.0);