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

#ifndef NETKET_NDM_SPIN_PHASE_HPP
#define NETKET_NDM_SPIN_PHASE_HPP

#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include "Machine/rbm_spin.hpp"
#include "Utils/all_utils.hpp"
#include "Utils/lookup.hpp"
#include "abstract_density_matrix.hpp"

namespace netket {

/** Neural Density Matrix machine class with spin 1/2 hidden units.
This version has real-valued weights and two NDMs parameterizing phase and
amplitude
 *
 */
class NdmSpinPhase : public AbstractDensityMatrix {
  // number of visible units
  int nv_;

  // number of hidden units
  int nh_;

  // number of ancillary units
  int na_;

  // number of parameters
  int npar_;

  // visible units bias
  RealVectorType b1_;
  RealVectorType b2_;

  // hidden units bias
  RealVectorType h1_;
  RealVectorType h2_;

  // ancillary units bias
  RealVectorType d1_;

  // hidden unit weights
  RealMatrixType W1_;
  RealMatrixType W2_;

  // ancillary unit weights
  RealMatrixType U1_;
  RealMatrixType U2_;

  // Caches
  RealVectorType thetas_r1_;
  RealVectorType thetas_r2_;
  RealVectorType thetas_c1_;
  RealVectorType thetas_c2_;
  RealVectorType lnthetas_r1_;
  RealVectorType lnthetas_r2_;
  RealVectorType lnthetas_c1_;
  RealVectorType lnthetas_c2_;
  RealVectorType thetasnew_r1_;
  RealVectorType thetasnew_r2_;
  RealVectorType thetasnew_c1_;
  RealVectorType thetasnew_c2_;
  RealVectorType lnthetasnew_r1_;
  RealVectorType lnthetasnew_r2_;
  RealVectorType lnthetasnew_c1_;
  RealVectorType lnthetasnew_c2_;

  RealVectorType thetas_a1_;
  RealVectorType thetas_a2_;
  RealVectorType thetasnew_a1_;
  RealVectorType thetasnew_a2_;
  VectorType pi_;
  VectorType lnpi_;
  VectorType lnpinew_;

  bool useb_;
  bool useh_;
  bool used_;

  const Complex I_;

 public:
  explicit NdmSpinPhase(std::shared_ptr<const AbstractHilbert> hilbert,
                        int nhidden = 0, int nancilla = 0, int alpha = 0,
                        int beta = 0, bool useb = true, bool useh = true,
                        bool used = true)
      : AbstractDensityMatrix(hilbert),
        nv_(hilbert->Size()),
        useb_(useb),
        useh_(useh),
        used_(used),
        I_(0, 1) {
    nh_ = std::max(nhidden, alpha * nv_);
    na_ = std::max(nancilla, beta * nv_);
    Init();
  }

  void Init() {
    W1_.resize(nv_, nh_);
    U1_.resize(nv_, na_);
    b1_.resize(nv_);
    h1_.resize(nh_);
    d1_.resize(na_);

    W2_.resize(nv_, nh_);
    U2_.resize(nv_, na_);
    b2_.resize(nv_);
    h2_.resize(nh_);

    thetas_r1_.resize(nh_);
    thetas_r2_.resize(nh_);
    thetas_c1_.resize(nh_);
    thetas_c2_.resize(nh_);

    lnthetas_r1_.resize(nh_);
    lnthetas_r2_.resize(nh_);
    lnthetas_c1_.resize(nh_);
    lnthetas_c2_.resize(nh_);

    thetasnew_r1_.resize(nh_);
    thetasnew_r2_.resize(nh_);
    thetasnew_c1_.resize(nh_);
    thetasnew_c2_.resize(nh_);

    lnthetasnew_r1_.resize(nh_);
    lnthetasnew_r2_.resize(nh_);
    lnthetasnew_c1_.resize(nh_);
    lnthetasnew_c2_.resize(nh_);

    thetas_a1_.resize(na_);
    thetas_a2_.resize(na_);
    thetasnew_a1_.resize(na_);
    thetasnew_a2_.resize(na_);
    pi_.resize(na_);
    lnpi_.resize(na_);
    lnpinew_.resize(na_);

    npar_ = 2 * nv_ * (nh_ + na_);

    if (useb_) {
      npar_ += 2 * nv_;
    } else {
      b1_.setZero();
      b2_.setZero();
    }

    if (useh_) {
      npar_ += 2 * nh_;
    } else {
      h1_.setZero();
      h2_.setZero();
    }

    if (used_) {
      npar_ += na_;
    } else {
      d1_.setZero();
    }

    InfoMessage() << "Phase NDM Initizialized with nvisible = " << nv_
                  << " and nhidden  = " << nh_ << " and nancilla = " << na_
                  << std::endl;
    InfoMessage() << "Using visible   bias = " << useb_ << std::endl;
    InfoMessage() << "Using hidden    bias  = " << useh_ << std::endl;
    InfoMessage() << "Using ancillary bias  = " << used_ << std::endl;
  }

  int Nvisible() const override { return nv_; }

  int Nhidden() const { return nh_; }

  int Nancilla() const { return na_; }

  int Npar() const override { return npar_; }

  void InitRandomPars(int seed, double sigma) override {
    RealVectorType par(npar_);

    netket::RandomGaussian(par, seed, sigma);

    SetParameters(VectorType(par));
  }

  void InitLookup(VisibleConstType v, LookupType &lt) override {
    if (lt.VectorSize() == 0) {
      lt.AddVector(h1_.size());  // row 1
      lt.AddVector(h2_.size());  // row 2
      lt.AddVector(h1_.size());  // col 1
      lt.AddVector(h2_.size());  // col 2
      lt.AddVector(d1_.size());  // ancilla modulus
      lt.AddVector(d1_.size());  // ancilla phase
    }
    if (lt.V(0).size() != h1_.size()) {
      lt.V(0).resize(h1_.size());
    }
    if (lt.V(1).size() != h2_.size()) {
      lt.V(1).resize(h2_.size());
    }
    if (lt.V(2).size() != h1_.size()) {
      lt.V(2).resize(h1_.size());
    }
    if (lt.V(3).size() != h2_.size()) {
      lt.V(3).resize(h2_.size());
    }
    if (lt.V(4).size() != d1_.size()) {
      lt.V(4).resize(d1_.size());
    }
    if (lt.V(5).size() != d1_.size()) {
      lt.V(5).resize(d1_.size());
    }

    VisibleConstType vr = v.head(GetHilbertPhysical().Size());
    VisibleConstType vc = v.tail(GetHilbertPhysical().Size());

    lt.V(0) = (W1_.transpose() * vr + h1_);
    lt.V(1) = (W2_.transpose() * vr + h2_);
    lt.V(2) = (W1_.transpose() * vc + h1_);
    lt.V(3) = (W2_.transpose() * vc + h2_);

    lt.V(4) = (0.5 * U1_.transpose() * (vr + vc) + d1_);
    lt.V(5) = (0.5 * U2_.transpose() * (vr - vc));
  }

  void UpdateLookup(VisibleConstType v, const std::vector<int> &tochange,
                    const std::vector<double> &newconf,
                    LookupType &lt) override {
    VisibleConstType vr = v.head(GetHilbertPhysical().Size());
    VisibleConstType vc = v.tail(GetHilbertPhysical().Size());

    if (tochange.size() != 0) {
      for (std::size_t s = 0; s < tochange.size(); s++) {
        const int sf = tochange[s];
        if (sf < Nvisible()) {
          lt.V(0) += W1_.row(sf) * (newconf[s] - vr(sf));
          lt.V(1) += W2_.row(sf) * (newconf[s] - vr(sf));

          lt.V(4) += 0.5 * U1_.row(sf) * (newconf[s] - vr(sf));
          lt.V(5) += 0.5 * U2_.row(sf) * (newconf[s] - vr(sf));
        } else {
          const int sfc = sf - Nvisible();
          lt.V(2) += W1_.row(sfc) * (newconf[s] - vc(sfc));
          lt.V(3) += W2_.row(sfc) * (newconf[s] - vc(sfc));

          lt.V(4) += 0.5 * U1_.row(sfc) * (newconf[s] - vc(sfc));
          lt.V(5) -= 0.5 * U2_.row(sfc) * (newconf[s] - vc(sfc));
        }
      }
    }
  }

  VectorType DerLog(VisibleConstType v) override {
    LookupType ltnew;
    InitLookup(v, ltnew);
    return DerLog(v, ltnew);
  }

  VectorType DerLog(VisibleConstType v, const LookupType &lt) override {
    VisibleConstType vr = v.head(GetHilbertPhysical().Size());
    VisibleConstType vc = v.tail(GetHilbertPhysical().Size());

    VectorType der(npar_);

    const int impar = (npar_ + na_ * used_) / 2;

    if (useb_) {
      der.head(nv_) = 0.5 * (vr + vc);
      der.segment(impar, nv_) = I_ * 0.5 * (vr - vc);
    }

    RbmSpin::tanh(lt.V(0).real(), lnthetas_r1_);
    RbmSpin::tanh(lt.V(1).real(), lnthetas_r2_);
    RbmSpin::tanh(lt.V(2).real(), lnthetas_c1_);
    RbmSpin::tanh(lt.V(3).real(), lnthetas_c2_);

    if (useh_) {
      der.segment(useb_ * nv_, nh_) = 0.5 * (lnthetas_r1_ + lnthetas_c1_);
      der.segment(impar + useb_ * nv_, nh_) =
          I_ * 0.5 * (lnthetas_r2_ - lnthetas_c2_);
    }

    thetas_a1_ = 0.5 * U1_.transpose() * (vr + vc) + d1_;
    thetas_a2_ = 0.5 * U2_.transpose() * (vr - vc);
    RbmSpin::tanh(lt.V(4).real() + I_ * lt.V(5).real(), lnpi_);

    if (used_) {
      der.segment(useb_ * nv_ + useh_ * nh_, na_) = lnpi_;
    }

    const int initw_1 = nv_ * useb_ + nh_ * useh_ + na_ * used_;
    const int initw_2 = nv_ * useb_ + nh_ * useh_;

    MatrixType wder =
        0.5 * (vr * lnthetas_r1_.transpose() + vc * lnthetas_c1_.transpose());
    der.segment(initw_1, nv_ * nh_) =
        Eigen::Map<VectorType>(wder.data(), nv_ * nh_);

    wder = 0.5 * I_ *
           (vr * lnthetas_r2_.transpose() - vc * lnthetas_c2_.transpose());
    der.segment(impar + initw_2, nv_ * nh_) =
        Eigen::Map<VectorType>(wder.data(), nv_ * nh_);

    const int initu_1 = initw_1 + nv_ * nh_;
    const int initu_2 = initw_2 + nv_ * nh_;

    MatrixType uder = 0.5 * (vr + vc) * lnpi_.transpose();
    der.segment(initu_1, nv_ * na_) =
        Eigen::Map<VectorType>(uder.data(), nv_ * na_);

    uder = 0.5 * I_ * (vr - vc) * lnpi_.transpose();
    der.segment(impar + initu_2, nv_ * na_) =
        Eigen::Map<VectorType>(uder.data(), nv_ * na_);

    return der;
  }

  VectorType GetParameters() override {
    VectorType pars(npar_);

    const int impar = (npar_ + na_ * used_) / 2;

    if (useb_) {
      pars.head(nv_) = b1_;
      pars.segment(impar, nv_) = b2_;
    }

    if (useh_) {
      pars.segment(nv_ * useb_, nh_) = h1_;
      pars.segment(impar + nv_ * useb_, nh_) = h2_;
    }

    if (used_) {
      pars.segment(useb_ * nv_ + useh_ * nh_, na_) = d1_;
    }

    const int initw_1 = nv_ * useb_ + nh_ * useh_ + na_ * used_;
    const int initw_2 = nv_ * useb_ + nh_ * useh_;

    pars.segment(initw_1, nv_ * nh_) =
        Eigen::Map<RealVectorType>(W1_.data(), nv_ * nh_);
    pars.segment(impar + initw_2, nv_ * nh_) =
        Eigen::Map<RealVectorType>(W2_.data(), nv_ * nh_);

    const int initu_1 = initw_1 + nv_ * nh_;
    const int initu_2 = initw_2 + nv_ * nh_;

    pars.segment(initu_1, nv_ * na_) =
        Eigen::Map<RealVectorType>(U1_.data(), nv_ * na_);
    pars.segment(impar + initu_2, nv_ * na_) =
        Eigen::Map<RealVectorType>(U2_.data(), nv_ * na_);

    return pars;
  }

  void SetParameters(VectorConstRefType pars) override {
    const int impar = (npar_ + na_ * used_) / 2;

    if (useb_) {
      b1_ = pars.head(nv_).real();
      b2_ = pars.segment(impar, nv_).real();
    }

    if (useh_) {
      h1_ = pars.segment(useb_ * nv_, nh_).real();
      h2_ = pars.segment(impar + useb_ * nv_, nh_).real();
    }

    if (used_) {
      d1_ = pars.segment(useb_ * nv_ + useh_ * nh_, na_).real();
    }

    const int initw_1 = nv_ * useb_ + nh_ * useh_ + na_ * used_;
    const int initw_2 = nv_ * useb_ + nh_ * useh_;

    VectorType Wpars = pars.segment(initw_1, nv_ * nh_);
    W1_ = Eigen::Map<MatrixType>(Wpars.data(), nv_, nh_).real();

    Wpars = pars.segment(impar + initw_2, nv_ * nh_);
    W2_ = Eigen::Map<MatrixType>(Wpars.data(), nv_, nh_).real();

    const int initu_1 = initw_1 + nv_ * nh_;
    const int initu_2 = initw_2 + nv_ * nh_;

    VectorType Upars = pars.segment(initu_1, nv_ * na_);
    U1_ = Eigen::Map<MatrixType>(Upars.data(), nv_, na_).real();

    Upars = pars.segment(impar + initu_2, nv_ * na_);
    U2_ = Eigen::Map<MatrixType>(Upars.data(), nv_, na_).real();
  }

  // Value of the logarithm of the wave-function
  Complex LogVal(VisibleConstType v) override {
    VisibleConstType vr = v.head(GetHilbertPhysical().Size());
    VisibleConstType vc = v.tail(GetHilbertPhysical().Size());

    RbmSpin::lncosh(W1_.transpose() * vr + h1_, lnthetas_r1_);
    RbmSpin::lncosh(W2_.transpose() * vr + h2_, lnthetas_r2_);
    RbmSpin::lncosh(W1_.transpose() * vc + h1_, lnthetas_c1_);
    RbmSpin::lncosh(W2_.transpose() * vc + h2_, lnthetas_c2_);

    thetas_a1_ = 0.5 * U1_.transpose() * (vr + vc) + d1_;
    thetas_a2_ = 0.5 * U2_.transpose() * (vr - vc);
    RbmSpin::lncosh(thetas_a1_ + I_ * thetas_a2_, lnpi_);

    auto gamma_1 =
        0.5 * (lnthetas_r1_.sum() + lnthetas_c1_.sum() + (vr + vc).dot(b1_));

    auto gamma_2 =
        0.5 * (lnthetas_r2_.sum() - lnthetas_c2_.sum() + (vr - vc).dot(b2_));

    return gamma_1 + I_ * gamma_2 + lnpi_.sum();
  }

  // Value of the logarithm of the wave-function
  // using pre-computed look-up tables for efficiency
  Complex LogVal(VisibleConstType v, const LookupType &lt) override {
    VisibleConstType vr = v.head(GetHilbertPhysical().Size());
    VisibleConstType vc = v.tail(GetHilbertPhysical().Size());

    RbmSpin::lncosh(lt.V(0).real(), lnthetas_r1_);
    RbmSpin::lncosh(lt.V(1).real(), lnthetas_r2_);
    RbmSpin::lncosh(lt.V(2).real(), lnthetas_c1_);
    RbmSpin::lncosh(lt.V(3).real(), lnthetas_c2_);
    RbmSpin::lncosh(lt.V(4).real() + I_ * lt.V(5).real(), lnpi_);

    auto gamma_1 =
        0.5 * (lnthetas_r1_.sum() + lnthetas_c1_.sum() + (vr + vc).dot(b1_));
    auto gamma_2 =
        0.5 * (lnthetas_r2_.sum() - lnthetas_c2_.sum() + (vr - vc).dot(b2_));

    return gamma_1 + I_ * gamma_2 + lnpi_.sum();
  }

  // Difference between logarithms of values, when one or more visible variables
  // are being flipped
  VectorType LogValDiff(
      VisibleConstType v, const std::vector<std::vector<int>> &tochange,
      const std::vector<std::vector<double>> &newconf) override {
    VisibleConstType vr = v.head(GetHilbertPhysical().Size());
    VisibleConstType vc = v.tail(GetHilbertPhysical().Size());

    const std::size_t nconn = tochange.size();

    thetas_r1_ = (W1_.transpose() * vr + h1_);
    thetas_r2_ = (W2_.transpose() * vr + h2_);
    thetas_c1_ = (W1_.transpose() * vc + h1_);
    thetas_c2_ = (W2_.transpose() * vc + h2_);

    RbmSpin::lncosh(thetas_r1_, lnthetas_r1_);
    RbmSpin::lncosh(thetas_r2_, lnthetas_r2_);
    RbmSpin::lncosh(thetas_c1_, lnthetas_c1_);
    RbmSpin::lncosh(thetas_c2_, lnthetas_c2_);

    thetas_a1_ = 0.5 * U1_.transpose() * (vr + vc) + d1_;
    thetas_a2_ = 0.5 * U2_.transpose() * (vr - vc);
    RbmSpin::lncosh(thetas_a1_ + I_ * thetas_a2_, lnpi_);

    Complex logtsum = 0.5 * (lnthetas_r1_.sum() + lnthetas_c1_.sum()) +
                      0.5 * I_ * (lnthetas_r2_.sum() - lnthetas_c2_.sum()) +
                      lnpi_.sum();

    VectorType logvaldiffs = VectorType::Zero(nconn);
    for (std::size_t k = 0; k < nconn; k++) {
      if (tochange[k].size() != 0) {
        thetasnew_r1_ = thetas_r1_;
        thetasnew_r2_ = thetas_r2_;
        thetasnew_c1_ = thetas_c1_;
        thetasnew_c2_ = thetas_c2_;

        thetasnew_a1_ = thetas_a1_;
        thetasnew_a2_ = thetas_a2_;

        for (std::size_t s = 0; s < tochange[k].size(); s++) {
          const int sf = tochange[k][s];

          if (sf < Nvisible()) {
            logvaldiffs(k) += 0.5 * b1_(sf) * (newconf[k][s] - vr(sf));
            logvaldiffs(k) += 0.5 * I_ * b2_(sf) * (newconf[k][s] - vr(sf));

            thetasnew_r1_ += W1_.row(sf) * (newconf[k][s] - vr(sf));
            thetasnew_r2_ += W2_.row(sf) * (newconf[k][s] - vr(sf));
            thetasnew_a1_ += 0.5 * U1_.row(sf) * (newconf[k][s] - vr(sf));
            thetasnew_a2_ += 0.5 * U2_.row(sf) * (newconf[k][s] - vr(sf));
          } else {
            const int sfc = tochange[k][s] - Nvisible();
            logvaldiffs(k) += 0.5 * b1_(sfc) * (newconf[k][s] - vc(sfc));
            logvaldiffs(k) -= 0.5 * I_ * b2_(sfc) * (newconf[k][s] - vc(sfc));

            thetasnew_c1_ += W1_.row(sfc) * (newconf[k][s] - vc(sfc));
            thetasnew_c2_ += W2_.row(sfc) * (newconf[k][s] - vc(sfc));
            thetasnew_a1_ += 0.5 * U1_.row(sfc) * (newconf[k][s] - vc(sfc));
            thetasnew_a2_ -= 0.5 * U2_.row(sfc) * (newconf[k][s] - vc(sfc));
          }
        }
        RbmSpin::lncosh(thetasnew_r1_, lnthetasnew_r1_);
        RbmSpin::lncosh(thetasnew_r2_, lnthetasnew_r2_);
        RbmSpin::lncosh(thetasnew_c1_, lnthetasnew_c1_);
        RbmSpin::lncosh(thetasnew_c2_, lnthetasnew_c2_);
        RbmSpin::lncosh(thetasnew_a1_ + I_ * thetasnew_a2_, lnpinew_);

        logvaldiffs(k) +=
            0.5 * (lnthetasnew_r1_.sum() + lnthetasnew_c1_.sum()) +
            0.5 * I_ * (lnthetasnew_r2_.sum() - lnthetasnew_c2_.sum()) +
            lnpinew_.sum() - logtsum;
      }
    }
    return logvaldiffs;
  }

  // Difference between logarithms of values, when one or more visible variables
  // are being flipped Version using pre-computed look-up tables for efficiency
  // on a small number of spin flips
  Complex LogValDiff(VisibleConstType v, const std::vector<int> &tochange,
                     const std::vector<double> &newconf,
                     const LookupType &lt) override {
    VisibleConstType vr = v.head(GetHilbertPhysical().Size());
    VisibleConstType vc = v.tail(GetHilbertPhysical().Size());

    Complex logvaldiff = 0.;

    if (tochange.size() != 0) {
      RbmSpin::lncosh(lt.V(0).real(), lnthetas_r1_);
      RbmSpin::lncosh(lt.V(1).real(), lnthetas_r2_);
      RbmSpin::lncosh(lt.V(2).real(), lnthetas_c1_);
      RbmSpin::lncosh(lt.V(3).real(), lnthetas_c2_);
      RbmSpin::lncosh(lt.V(4).real() + I_ * lt.V(5).real(), lnpi_);

      thetasnew_r1_ = lt.V(0).real();
      thetasnew_r2_ = lt.V(1).real();
      thetasnew_c1_ = lt.V(2).real();
      thetasnew_c2_ = lt.V(3).real();
      thetasnew_a1_ = lt.V(4).real();
      thetasnew_a2_ = lt.V(5).real();

      for (std::size_t s = 0; s < tochange.size(); s++) {
        const int sf = tochange[s];

        if (sf < Nvisible()) {
          logvaldiff += 0.5 * b1_(sf) * (newconf[s] - vr(sf));
          logvaldiff += 0.5 * I_ * b2_(sf) * (newconf[s] - vr(sf));

          thetasnew_r1_ += W1_.row(sf) * (newconf[s] - vr(sf));
          thetasnew_r2_ += W2_.row(sf) * (newconf[s] - vr(sf));
          thetasnew_a1_ += 0.5 * U1_.row(sf) * (newconf[s] - vr(sf));
          thetasnew_a2_ += 0.5 * U2_.row(sf) * (newconf[s] - vr(sf));
        } else {
          const int sfc = tochange[s] - Nvisible();
          logvaldiff += 0.5 * b1_(sfc) * (newconf[s] - vc(sfc));
          logvaldiff -= 0.5 * I_ * b2_(sfc) * (newconf[s] - vc(sfc));

          thetasnew_c1_ += W1_.row(sfc) * (newconf[s] - vc(sfc));
          thetasnew_c2_ += W2_.row(sfc) * (newconf[s] - vc(sfc));
          thetasnew_a1_ += 0.5 * U1_.row(sfc) * (newconf[s] - vc(sfc));
          thetasnew_a2_ -= 0.5 * U2_.row(sfc) * (newconf[s] - vc(sfc));
        }
      }

      RbmSpin::lncosh(thetasnew_r1_, lnthetasnew_r1_);
      RbmSpin::lncosh(thetasnew_r2_, lnthetasnew_r2_);
      RbmSpin::lncosh(thetasnew_c1_, lnthetasnew_c1_);
      RbmSpin::lncosh(thetasnew_c2_, lnthetasnew_c2_);
      RbmSpin::lncosh(thetasnew_a1_ + I_ * thetasnew_a2_, lnpinew_);

      logvaldiff += 0.5 * (lnthetasnew_r1_.sum() + lnthetasnew_c1_.sum());
      logvaldiff -= 0.5 * (lnthetas_r1_.sum() + lnthetas_c1_.sum());
      logvaldiff += 0.5 * I_ * (lnthetasnew_r2_.sum() - lnthetasnew_c2_.sum());
      logvaldiff -= 0.5 * I_ * (lnthetas_r2_.sum() - lnthetas_c2_.sum());
      logvaldiff += (lnpinew_.sum() - lnpi_.sum());
    }
    return logvaldiff;
  }

  inline static double lncosh(double x) {
    const double xp = std::abs(x);
    if (xp <= 12.) {
      return std::log(std::cosh(xp));
    } else {
      const static double log2v = std::log(2.);
      return xp - log2v;
    }
  }

  void Save(const std::string &filename) const override {
    json state;
    state["Name"] = "NdmSpinPhase";
    state["Nvisible"] = nv_;
    state["Nhidden"] = nh_;
    state["Nancilla"] = na_;
    state["UseVisibleBias"] = useb_;
    state["UseHiddenBias"] = useh_;
    state["UseAncillaBias"] = used_;
    state["b1"] = b1_;
    state["h1"] = h1_;
    state["d1"] = d1_;
    state["W1"] = W1_;
    state["U1"] = U1_;

    state["b2"] = b2_;
    state["h2"] = h2_;
    state["W2"] = W2_;
    state["U2"] = U2_;
    WriteJsonToFile(state, filename);
  }

  void Load(const std::string &filename) override {
    auto pars = ReadJsonFromFile(filename);
    std::string name = FieldVal<std::string>(pars, "Name");
    if (name != "NdmSpinPhase") {
      throw InvalidInputError(
          "Error while constructing RbmSpinPhase from input parameters");
    }

    if (FieldExists(pars, "Nvisible")) {
      nv_ = FieldVal<int>(pars, "Nvisible");
    }
    if (nv_ != GetHilbertPhysical().Size()) {
      throw InvalidInputError(
          "Number of visible units is incompatible with given "
          "Hilbert space");
    }

    if (FieldExists(pars, "Nhidden")) {
      nh_ = FieldVal<int>(pars, "Nhidden");
    } else {
      nh_ = nv_ * double(FieldVal<double>(pars, "Alpha"));
    }

    if (FieldExists(pars, "Nancilla")) {
      na_ = FieldVal<int>(pars, "Nancilla");
    } else {
      na_ = nv_ * double(FieldVal<double>(pars, "Beta"));
    }

    useb_ = FieldOrDefaultVal(pars, "UseVisibleBias", true);
    useh_ = FieldOrDefaultVal(pars, "UseHiddenBias", true);
    used_ = FieldOrDefaultVal(pars, "UseAncillaBias", true);

    Init();

    // Loading parameters, if defined in the input
    if (FieldExists(pars, "b1")) {
      b1_ = FieldVal<RealVectorType>(pars, "b1");
      b2_ = FieldVal<RealVectorType>(pars, "b2");
    } else {
      b1_.setZero();
      b2_.setZero();
    }

    if (FieldExists(pars, "h1")) {
      h1_ = FieldVal<RealVectorType>(pars, "h1");
      h2_ = FieldVal<RealVectorType>(pars, "h2");
    } else {
      h1_.setZero();
      h2_.setZero();
    }

    if (FieldExists(pars, "d1")) {
      d1_ = FieldVal<RealVectorType>(pars, "d1");
    } else {
      d1_.setZero();
    }

    if (FieldExists(pars, "W1")) {
      W1_ = FieldVal<RealMatrixType>(pars, "W1");
      W2_ = FieldVal<RealMatrixType>(pars, "W2");
    }

    if (FieldExists(pars, "U1")) {
      U1_ = FieldVal<RealMatrixType>(pars, "U1");
      U2_ = FieldVal<RealMatrixType>(pars, "U2");
    }
  }

  bool IsHolomorphic() const noexcept override { return false; }
};

}  // namespace netket

#endif  // NETKET_NDM_SPIN_PHASE_HPP


#include <iostream>
#include <map>
#include <vector>
#include <utility>
#include <tuple>
#include <regex>

#include <boost/multiprecision/cpp_int.hpp>

using boost::multiprecision::uint128_t;

class Part2 {
public:
    Part2(int max_score);
    std::pair<uint128_t, uint128_t> solve(int p1, int p2, int s1, int s2);

private:
    int max_;

    std::vector<std::tuple<int, int, int>> throws;
    std::map<std::tuple<int, int, int, int>, std::pair<uint128_t, uint128_t>> cache_;
};

Part2::Part2(int max_score) : max_(max_score) {
    for (int d1 = 1; d1 <= 3; ++d1) {
        for (int d2 = 1; d2 <= 3; ++d2) {
            for (int d3 = 1; d3 <= 3; ++d3) {
                throws.emplace_back(d1,d2,d3);
            }
        }
    }
}

std::pair<uint128_t, uint128_t> Part2::solve(int p1, int p2, int s1, int s2) {
    if (s1 >= max_)
        return std::make_pair(1, 0);
    if (s2 >= max_)
        return std::make_pair(0, 1);

    auto q = std::make_tuple(p1, p2, s1, s2);
    auto a = cache_.find(q);
    if (a != cache_.end()) {
        return a->second;
    }
    auto score = std::make_pair<uint128_t,uint128_t>(0, 0);
    for (auto& a : throws) {
        auto [d1, d2, d3] = a;
        int new_p1 = p1 + d1 + d2 + d3;
        new_p1 %= 10;
        if (new_p1 == 0) new_p1 = 10;
        int new_s1 = s1 + new_p1;
        auto [p2_wins, p1_wins] = solve(p2, new_p1, s2, new_s1);
        score.first += p1_wins;
        score.second += p2_wins;
    }

    cache_[q] = score;
    return score;
}

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

    int player_1_start = 0;
    int player_2_start = 0;

    for (int i = 0; i < 2; ++i) {
        std::string input;
        getline(std::cin, input);
        const std::regex re(R"(Player \d starting position: (\d*))");
        std::smatch match;
        if (regex_search(input, match, re)) {
            if (i == 0)
                player_1_start = stoi(match.str(1));
            else
                player_2_start = stoi(match.str(1));
        }
    }

    int player_1 = 0;
    int player_2 = 0;

    int p1 = player_1_start;
    int p2 = player_2_start;

    int dice = 1;
    while (player_1 < 1000 && player_2 < 1000) {
        p1 += 3*dice + 3;
        dice += 3;
        p1 %= 10;
        if (p1 == 0) p1 = 10;
        player_1 += p1;
        if (player_1 >= 1000)
            break;
        p2 += 3*dice + 3;
        dice += 3;
        p2 %= 10;
        if (p2 == 0) p2 = 10;
        player_2 += p2;
    }

    int part_1 = std::min(player_1, player_2) * (dice-1);
    std::cout << part_1 << std::endl;

    Part2 part2(21);
    auto a = part2.solve(player_1_start, player_2_start, 0, 0);
    std::cout << std::max(a.first, a.second) << std::endl;

    return 0;
}


/********************************************************************************
*  This file is part of CinoLib                                                 *
*  Copyright(C) 2016: Marco Livesu                                              *
*                                                                               *
*  The MIT License                                                              *
*                                                                               *
*  Permission is hereby granted, free of charge, to any person obtaining a      *
*  copy of this software and associated documentation files (the "Software"),   *
*  to deal in the Software without restriction, including without limitation    *
*  the rights to use, copy, modify, merge, publish, distribute, sublicense,     *
*  and/or sell copies of the Software, and to permit persons to whom the        *
*  Software is furnished to do so, subject to the following conditions:         *
*                                                                               *
*  The above copyright notice and this permission notice shall be included in   *
*  all copies or substantial portions of the Software.                          *
*                                                                               *
*  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR   *
*  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,     *
*  FITNESS FOR A PARTICULAR PURPOSE AND NON INFRINGEMENT. IN NO EVENT SHALL THE *
*  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER       *
*  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING      *
*  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS *
*  IN THE SOFTWARE.                                                             *
*                                                                               *
*  Author(s):                                                                   *
*                                                                               *
*     Marco Livesu (marco.livesu@gmail.com)                                     *
*     http://pers.ge.imati.cnr.it/livesu/                                       *
*                                                                               *
*     Italian National Research Council (CNR)                                   *
*     Institute for Applied Mathematics and Information Technologies (IMATI)    *
*     Via de Marini, 6                                                          *
*     16149 Genoa,                                                              *
*     Italy                                                                     *
*********************************************************************************/
#include <cinolib/laplacian.h>
#include <cinolib/symbols.h>
#include <Eigen/Sparse>

namespace cinolib
{

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
std::vector<Eigen::Triplet<double>> laplacian_matrix_entries(const AbstractMesh<M,V,E,P> & m,
                                                             const int mode,
                                                             const int n) // diagonally replicate n times
{
    std::vector<Entry> entries;

    unsigned int nv = m.num_verts();
    std::vector<unsigned int> base(n);
    for(int i=0; i<n; ++i) base[i] = nv*i;

    for(unsigned int vid=0; vid<m.num_verts(); ++vid)
    {
        std::vector<std::pair<unsigned int,double>> wgts;
        m.vert_weights(vid, mode, wgts);
        double sum = 0.0;
        for(auto item : wgts)
        {
            for(int i=0; i<n; ++i)
            {
                entries.push_back(Entry(base[i] + vid, base[i] + item.first, item.second));
            }
            sum -= item.second;
        }
        if(sum == 0.0)
        {
            std::cerr << "WARNING: null row in the matrix! (disconnected vertex? I put 1 in the diagonal)" << std::endl;
            sum = 1.0;
        }
        for(int i=0; i<n; ++i)
        {
            entries.push_back(Entry(base[i] + vid, base[i] + vid, sum));
        }
    }

    return entries;
}

//::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::

template<class M, class V, class E, class P>
CINO_INLINE
Eigen::SparseMatrix<double> laplacian(const AbstractMesh<M,V,E,P> & m, const int mode, const int n)
{
    std::vector<Entry> entries = laplacian_matrix_entries(m, mode, n);

    unsigned int nv = n*m.num_verts();
    Eigen::SparseMatrix<double> L(nv,nv);
    L.setFromTriplets(entries.begin(), entries.end());

    return L;
}

}


// Copyright © 2016-2021 Thomas Nagler and Thibault Vatter
//
// This file is part of the vinecopulib library and licensed under the terms of
// the MIT license. For a copy, see the LICENSE file in the root directory of
// vinecopulib or https://vinecopulib.github.io/vinecopulib/.

#pragma once

#include <boost/numeric/odeint.hpp>
#include <functional>

namespace vinecopulib {

namespace tools_integration {

inline double
integrate_zero_to_one(std::function<double(double)> f)
{
  boost::numeric::odeint::runge_kutta_dopri5<double> stepper;
  double lb = 1e-12;
  double ub = 1.0 - lb;
  double x = 0.0;
  auto ifunc = [f](const double /* x */, double& dxdt, const double t) {
    dxdt = f(t);
  };
  integrate_adaptive(boost::numeric::odeint::make_controlled(lb, lb, stepper),
                     ifunc,
                     x,
                     lb,
                     ub,
                     lb);
  return x;
}
}
}


#if !defined(BALSA_GEOMETRY_TRIANGLE_MESH_EARCLIPPING_HPP)
#define BALSA_GEOMETRY_TRIANGLE_MESH_EARCLIPPING_HPP
#include <list>

#include <range/v3/view/subrange.hpp>
#include <range/v3/view/sliding.hpp>
#include <range/v3/algorithm/copy.hpp>
#include <range/v3/view/take_exactly.hpp>
#include <range/v3/view/cycle.hpp>
#include <range/v3/range/conversion.hpp>
#include "balsa/eigen/stl2eigen.hpp"
#include <Eigen/Core>
#include "balsa/geometry/winding_number.hpp"
#include "balsa/geometry/get_angle.hpp"
#include "balsa/eigen/types.hpp"
#include "balsa/eigen/shape_checks.hpp"
#include <numbers>
#include <utility>

namespace balsa::geometry::triangle_mesh {
template<eigen::concepts::Vec2Compatible VDerived, std::forward_iterator BeginIt, std::forward_iterator EndIt>
requires std::is_integral_v<std::decay_t<decltype(*std::declval<BeginIt>())>>
  std::vector<std::array<std::decay_t<decltype(*std::declval<BeginIt>())>, 3>> earclipping_stl(
    const VDerived &V,
    const BeginIt &beginit,
    const EndIt &endit) {
    using Index = std::decay_t<decltype(*std::declval<BeginIt>())>;
    using Face = std::array<int, 3>;
    std::vector<Face> stlF;

    auto range = ranges::subrange(beginit, endit);

    if constexpr (std::is_integral_v<std::decay_t<decltype(*beginit)>>) {

        size_t size = std::distance(beginit, endit);
        stlF.reserve(size - 2);
        double inner_ang_sum = 0;
        double outer_ang_sum = 0;
        auto triplets = range | ranges::views::cycle// pretend range is cyclic
                        | ranges::views::sliding(3)// take sequences of triplets
                        | ranges::views::take_exactly(std::distance(beginit, endit));// we only want to see each cycle once
        for (auto &&triplet : triplets) {
            Face f;
            ranges::copy(triplet, f.begin());
            const auto &[ai, bi, ci] = f;
            auto a = V.col(ai);
            auto b = V.col(bi);
            auto c = V.col(ci);
            double ang = get_positive_clamped_angle(get_angle(c - b, a - b));
            inner_ang_sum += ang;
            outer_ang_sum += 2 * std::numbers::pi_v<double> - ang;
        }

        bool reverse_orientation = outer_ang_sum < inner_ang_sum;
        if (reverse_orientation) {
        }

        std::list<Index> CL(beginit, endit);

        auto is_earclip = [&](const Face &f) -> bool {
            auto a = V.col(f[0]);
            auto b = V.col(f[1]);
            auto c = V.col(f[2]);
            auto cb = c - b;
            auto ab = a - b;
            // balsa::Vec2d n(-ac.y(),ac.x());
            /*
            if(cb.x() * ab.y() -  cb.y() * ab.x() < 1e-12) {
                return false;
            }
            */
            if (cb.x() * ab.y() - cb.y() * ab.x() < 1e-10) {
                return false;
            }
            // double ang = balsa::geometry::trigonometry::angle(cb,ab)(0);
            double ang = get_positive_clamped_angle(get_angle(c - b, a - b));
            if (ang > std::numbers::pi_v<double> || ang < 0) {
                return false;
            }

            for (auto &&i : range) {
                if (i == f[0] || i == f[1] || i == f[2]) {
                    continue;
                }
                auto v = V.col(i);
                if (interior_winding_number(V, f, v)) {
                    return false;
                }
            }
            return true;
        };

        while (CL.size() > 3) {
            bool earclipped = false;

            for (auto it = CL.begin(); it != CL.end(); ++it) {
                auto it1 = it;
                it1++;
                if (it1 == CL.end()) {
                    it1 = CL.begin();
                }
                auto it2 = it1;
                it2++;
                if (it2 == CL.end()) {
                    it2 = CL.begin();
                }
                Face f{ { *it, *it1, *it2 } };
                if (reverse_orientation) {
                    std::swap(f[0], f[2]);
                }
                if (is_earclip(f)) {
                    stlF.push_back(f);
                    CL.erase(it1);
                    earclipped = true;
                    break;
                }
            }
            if (!earclipped) {
                // logging::warn() << "Earclipping failed!";
                auto it = CL.begin();
                auto it1 = it;
                it1++;
                auto it2 = it1;
                it2++;
                Face f{ { *it, *it1, *it2 } };

                stlF.push_back(f);
                CL.erase(it1);
            }
        }

        Face f;
        std::copy(CL.begin(), CL.end(), f.begin());
        auto a = V.col(f[0]);
        auto b = V.col(f[1]);
        auto c = V.col(f[2]);
        auto cb = c - b;
        auto ab = a - b;
        // auto ac = a - c;
        if (cb.x() * ab.y() - cb.y() * ab.x() < 1e-10) {
            std::swap(f[0], f[1]);
        }
        stlF.push_back(f);
        //} else {
        //    size_t size = 0;
        //    for (auto &&c : ranges::subrange(beginit, endit)) {
        //        size += c.size() - 2;
        //    }
        //    stlF.reserve(size);

        //    for (auto &&c : ranges::subrange(beginit, endit)) {
        //        if (c.size() >= 3) {
        //            auto F = earclipping_stl(V, c.begin(), c.end());
        //            stlF.insert(stlF.end(), F.begin(), F.end());
        //        }
        //    }
    }
    return stlF;
}
// both return balsa::eigen::ColVectors<integral_type, 3>
template<eigen::concepts::ColVecs2Compatible VDerived, typename Container>

auto earclipping(const VDerived &V,
                 const Container &C) {
    return eigen::stl2eigen(earclipping_stl(V, C.begin(), C.end()));
}
template<eigen::concepts::ColVecs2Compatible VDerived, typename T>
auto earclipping(const VDerived &V,
                 const std::initializer_list<T> &C) {
    return eigen::stl2eigen(earclipping_stl(V, C.begin(), C.end()));
}
}// namespace balsa::geometry::triangle_mesh
#endif


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved.      *
 *                                                                            *
 * Redistribution and use in source and binary forms, with or without         *
 * modification, are permitted provided that the following conditions         *
 * are met:                                                                   *
 * * Redistributions of source code must retain the above copyright           *
 *   notice, this list of conditions and the following disclaimer.            *
 * * 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.     *
 * * Neither the name of ANU nor the names of its contributors may be         *
 *   used to endorse or promote products derived from this software without   *
 *   specific prior written permission.                                       *
 *                                                                            *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE  *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE 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.                                                               *
 ******************************************************************************/

#include <opengv/sac_problems/relative_pose/MultiCentralRelativePoseSacProblem.hpp>

#include <opengv/relative_pose/methods.hpp>
#include <opengv/triangulation/methods.hpp>
#include <Eigen/NonLinearOptimization>
#include <Eigen/NumericalDiff>

bool
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::computeModelCoefficients(
    const std::vector<std::vector<int> > &indices,
    model_t & outModel) const
{
  std::vector<std::shared_ptr<translations_t> > multiTranslations;
  std::vector<std::shared_ptr<rotations_t> > multiRotations;

  for(size_t pairIndex = 0; pairIndex < _adapter.getNumberPairs(); pairIndex++)
  {
    std::vector<int> serializedIndices;
    for(
        size_t correspondenceIndex = 0;
        correspondenceIndex < indices[pairIndex].size();
        correspondenceIndex++ )
      serializedIndices.push_back(_adapter.convertMultiIndex(
          pairIndex, indices[pairIndex][correspondenceIndex] ));

    essential_t essentialMatrix =
        opengv::relative_pose::eightpt(_adapter,serializedIndices);

    //Decompose the essential matrix into rotations and translations
    std::shared_ptr<translations_t> translations(new translations_t());
    std::shared_ptr<rotations_t> rotations(new rotations_t());

    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    W(0,1) = -1;
    W(1,0) = 1;
    W(2,2) = 1;

    Eigen::JacobiSVD< Eigen::MatrixXd > SVD(
        essentialMatrix,
        Eigen::ComputeFullV | Eigen::ComputeFullU );
    Eigen::VectorXd singularValues = SVD.singularValues();

    // check for bad essential matrix
    if( singularValues[2] > 0.001 ) {};
    // continue; //singularity constraints not applied -> removed because too harsh
    if( singularValues[1] < 0.75 * singularValues[0] ) {};
    // continue; //bad essential matrix -> removed because too harsh

    // maintain scale
    double scale = singularValues[0];

    // get possible rotation and translation vectors
    rotation_t Ra = SVD.matrixU() * W * SVD.matrixV().transpose();
    rotation_t Rb = SVD.matrixU() * W.transpose() * SVD.matrixV().transpose();
    translation_t t = scale*SVD.matrixU().col(2);

    // change sign if det = -1
    if( Ra.determinant() < 0 ) Ra = -Ra;
    if( Rb.determinant() < 0 ) Rb = -Rb;

    //Store the decomposition, and already convert to our convention
    translations->push_back(t);
    translations->push_back(-t); //this is not needed actually!
    rotations->push_back(Ra);
    rotations->push_back(Rb);

    multiTranslations.push_back(translations);
    multiRotations.push_back(rotations);
  }

  for(size_t pairIndex = 0; pairIndex < _adapter.getNumberPairs(); pairIndex++)
  {
    //For each pair, find the right rotation and translation by our critera
    //
    //
    //fill outModel with that (it is a vector of transformations. A
    //transformation is 3x4 with R from frame 2 to 1, and position of 2 in 1)
  }

  return true;
}

void
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::getSelectedDistancesToModel(
    const model_t & model,
    const std::vector<std::vector<int> > & indices,
    std::vector<std::vector<double> > & scores) const
{
  Eigen::Matrix<double,4,1> p_hom;
  p_hom[3] = 1.0;

  for( size_t pairIndex = 0; pairIndex < indices.size(); pairIndex++ )
  {
    translation_t translation = model[pairIndex].col(3);
    rotation_t rotation = model[pairIndex].block<3,3>(0,0);

    for(
        size_t correspondenceIndex = 0;
        correspondenceIndex < indices[pairIndex].size();
        correspondenceIndex++ )
    {
      _adapter.sett12(translation);
      _adapter.setR12(rotation);

      transformation_t inverseSolution;
      inverseSolution.block<3,3>(0,0) = rotation.transpose();
      inverseSolution.col(3) =
          -inverseSolution.block<3,3>(0,0)*translation;

      p_hom.block<3,1>(0,0) =
          opengv::triangulation::triangulate2(
              _adapter,
              _adapter.convertMultiIndex(
              pairIndex, indices[pairIndex][correspondenceIndex] ));

      bearingVector_t reprojection1 = p_hom.block<3,1>(0,0);
      bearingVector_t reprojection2 = inverseSolution * p_hom;
      reprojection1 = reprojection1 / reprojection1.norm();
      reprojection2 = reprojection2 / reprojection2.norm();
      bearingVector_t f1 = _adapter.getBearingVector1(pairIndex,correspondenceIndex);
      bearingVector_t f2 = _adapter.getBearingVector2(pairIndex,correspondenceIndex);

      //bearing-vector based outlier criterium (select threshold accordingly):
      //1-(f1'*f2) = 1-cos(alpha) \in [0:2]
      double reprojError1 = 1.0 - (f1.transpose() * reprojection1);
      double reprojError2 = 1.0 - (f2.transpose() * reprojection2);
      scores[pairIndex].push_back(reprojError1 + reprojError2);
    }
  }
}

void
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::optimizeModelCoefficients(
    const std::vector<std::vector<int> > & inliers,
    const model_t & model,
    model_t & optimized_model)
{
  optimized_model = model; //todo: include non-linear optimization of model
}

std::vector<int>
opengv::sac_problems::
    relative_pose::MultiCentralRelativePoseSacProblem::getSampleSizes() const
{
  std::vector<int> sampleSizes;
  for(size_t pairIndex = 0; pairIndex < _adapter.getNumberPairs(); pairIndex++)
    sampleSizes.push_back(_sampleSize);

  return sampleSizes;
}


#include "util_3drotation_log_exp.h"
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/TriMesh_ArrayKernelT.hh>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <string>
#include <iostream>
#include <pybind11/numpy.h>
#include <pybind11/pybind11.h>
#include <vector>;

namespace py=pybind11;

struct TriTraits : public OpenMesh::DefaultTraits
{
  /// Use double precision points
  typedef OpenMesh::Vec3d Point;
  /// Use double precision Normals
  typedef OpenMesh::Vec3d Normal;
  /// Use double precision TexCood2D
  typedef OpenMesh::Vec2d TexCoord2D;

  /// Use RGBA Color
  typedef OpenMesh::Vec4f Color;

    /// Status
    VertexAttributes(OpenMesh::Attributes::Status);
    FaceAttributes(OpenMesh::Attributes::Status);
    EdgeAttributes(OpenMesh::Attributes::Status);

//    VertexAttributes(
//            OpenMesh::Attributes::
//            )
};

/// Simple Name for Mesh
typedef OpenMesh::TriMesh_ArrayKernelT<TriTraits>  TriMesh;

class DR_feature{
public:
    DR_feature();
	void read_ref_mesh(std::string ref_mesh_name);
	void read_defor_mesh(std::string defor_mesh_name);
	std::vector<double> get_feature(std::string ref_mesh_name, std::string defor_mesh_name);

private:
	TriMesh ref_mesh_;
	TriMesh defor_mesh_;

	OpenMesh::EPropHandleT<double> LB_weights;
	OpenMesh::VPropHandleT<Eigen::Matrix3d> T_matrixs;
private:
	void compute_ref_LB_weight();
    void compute_Ti(TriMesh::VertexHandle v_it,TriMesh::VertexHandle v_to_it);
};

DR_feature::DR_feature(){
	ref_mesh_.add_property(LB_weights);
	defor_mesh_.add_property(T_matrixs);
}

void DR_feature::read_ref_mesh(std::string ref_mesh_name){
	if(!OpenMesh::IO::read_mesh(ref_mesh_, ref_mesh_name)){
        std::cout<<"read_ref_mesh_ : read file wrong!!!"<<std::endl;
        return ;
    }

    compute_ref_LB_weight();
}

void DR_feature::compute_ref_LB_weight(){
	TriMesh::EdgeIter e_it, e_end(ref_mesh_.edges_end());
    TriMesh::HalfedgeHandle    h0, h1, h2;
    TriMesh::VertexHandle      v0, v1;
    TriMesh::Point             p0, p1, p2, d0, d1;
    TriMesh::Scalar w;
    for (e_it=ref_mesh_.edges_begin(); e_it!=e_end; e_it++)
    {
        w  = 0.0;
        if(ref_mesh_.is_boundary(*e_it))
        {
            h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
            if(ref_mesh_.is_boundary(h0))
                h0 = ref_mesh_.opposite_halfedge_handle(h0);

            v0 = ref_mesh_.to_vertex_handle(h0);
            v1 = ref_mesh_.from_vertex_handle(h0);
            p0 = ref_mesh_.point(v0);
            p1 = ref_mesh_.point(v1);
            h1 = ref_mesh_.next_halfedge_handle(h0);
            p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h1));
            d0 = (p0-p2).normalize();
            d1 = (p1-p2).normalize();
            w += 2.0 / tan(acos(std::min(0.99, std::max(-0.99, (d0|d1)))));
            w = std::max(0.0, w);

            if(std::isnan(w))
                std::cout<<"Some weight NAN"<<std::endl;
            ref_mesh_.property(LB_weights,e_it) = w;
            continue;
        }
        h0 = ref_mesh_.halfedge_handle(e_it.handle(),0);
        v0 = ref_mesh_.to_vertex_handle(h0);
        p0 = ref_mesh_.point(v0);

        h1 = ref_mesh_.opposite_halfedge_handle(h0);
        v1 = ref_mesh_.to_vertex_handle(h1);
        p1 = ref_mesh_.point(v1);

        h2 = ref_mesh_.next_halfedge_handle(h0);
        p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
        d0 = (p0 - p2).normalize();
        d1 = (p1 - p2).normalize();
        w += 1.0/ tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0) ))));

        h2 = ref_mesh_.next_halfedge_handle(h1);
        p2 = ref_mesh_.point(ref_mesh_.to_vertex_handle(h2));
        d0 = (p0 - p2).normalize();
        d1 = (p1 - p2).normalize();
        w += 1.0 / tan(acos(std::max(-1.0, std::min(1.0, dot(d1,d0)))));

        if(std::isnan(w))
            std::cout<<"Some weight is NAN"<<std::endl;
        w = std::max(0.0, w);
        ref_mesh_.property(LB_weights,e_it) = w;
    }
}

void DR_feature::read_defor_mesh(std::string _filename){
	if(!OpenMesh::IO::read_mesh(defor_mesh_,_filename))
    {
        std::cout<<"read_defor_mesh : read file wrong"<<std::endl;
        return ;
    }

    TriMesh::FaceIter f_it = defor_mesh_.faces_begin();
    for(;f_it!=defor_mesh_.faces_end();f_it++)
    {
        TriMesh::FaceHandle f_h = *f_it;
        TriMesh::FaceVertexIter fe_it0,fe_it1;
        fe_it0 = ref_mesh_.fv_iter(f_h);
        fe_it1 = defor_mesh_.fv_iter(f_h);
        int v00,v01,v02,v10,v11,v12;
        v00 = (*fe_it0).idx();fe_it0++;
        v01 = (*fe_it0).idx();fe_it0++;
        v02 = (*fe_it0).idx();fe_it0++;

        v10 = (*fe_it1).idx();fe_it1++;
        v11 = (*fe_it1).idx();fe_it1++;
        v12 = (*fe_it1).idx();fe_it1++;

        if(v00 == v10)
        {
            if(v01!=v11||v02!=v12)
            {
                std::cout<<"defor and ref are not compatible!!!"<<std::endl;
                return;
            }
        }
        else if(v00 == v11)
        {
            if(v01!=v12||v02!=v10)
            {
                std::cout<<"defor and ref are not compatible!!!"<<std::endl;
                return;
            }
        }
        else if(v00 == v12)
        {
            if(v01!=v10||v02!=v11)
            {
                std::cout<<"defor and ref are not compatible!!!"<<std::endl;
                return;
            }
        }
        else
        {
            std::cout<<"defor and ref are not compatible!!!"<<std::endl;
            return;
        }
    }

    TriMesh::VertexIter v_it, v_to_it;
    for(v_it=ref_mesh_.vertices_begin(),v_to_it=defor_mesh_.vertices_begin()
        ;v_it!=ref_mesh_.vertices_end()&&v_to_it!=defor_mesh_.vertices_end()
        ;v_it++,v_to_it++)
    {
        if((*v_it).idx()!=(*v_to_it).idx())
            std::cout<<"DR_feature::compute_ref_to_defor_Tmatrixs different topology!!!"<<std::endl;
        compute_Ti(*v_it,*v_to_it);
    }

}

void DR_feature::compute_Ti(TriMesh::VertexHandle v_it, TriMesh::VertexHandle v_to_it){
    if(v_it.idx()!=v_to_it.idx())
    	std::cout<<"compute_Ti correspond is wrong!!!"<<std::endl;
    TriMesh::VertexEdgeIter veiter=ref_mesh_.ve_iter(v_it);
    int v_id = v_it.idx();
    TriMesh::Point p0,p1;
    p0 = ref_mesh_.point(v_it);
    p1 = defor_mesh_.point(v_to_it);
    Eigen::Matrix3d L,RI;
    L.setZero();
    RI.setZero();
    double tolerance = 1.0e-6;
    TriMesh::HalfedgeHandle h_e=ref_mesh_.halfedge_handle(v_it);
    TriMesh::VertexHandle test_v = ref_mesh_.to_vertex_handle(h_e);
    TriMesh::Point tp0,tp1;
    tp0 = ref_mesh_.point(v_it); tp1 = ref_mesh_.point(test_v);
    double scale=1.0;
    if(((tp0[0]-tp1[0])*(tp0[0]-tp1[0])+(tp0[1]-tp1[1])*(tp0[1]-tp1[1])+(tp0[2]-tp1[2])*(tp0[2]-tp1[2]))<0.1)
        scale = 100;

    for(;veiter.is_valid();veiter++)
    {
        double weight = 1.0;
        int to_id;
        TriMesh::VertexHandle to_v=ref_mesh_.to_vertex_handle(ref_mesh_.halfedge_handle(*veiter, 0));
        if(to_v.idx()==v_id)
            to_v = ref_mesh_.from_vertex_handle(ref_mesh_.halfedge_handle(*veiter, 0));
        to_id = to_v.idx();
        Eigen::Vector3d eij0,eij1;
        TriMesh::Point q0,q1;
        q0 = ref_mesh_.point(to_v);
        q1 = defor_mesh_.point(to_v);
        eij0(0) = p0[0]-q0[0];
        eij0(1) = p0[1]-q0[1];
        eij0(2) = p0[2]-q0[2];
        eij0*=weight*scale;

        eij1(0) = p1[0]-q1[0];
        eij1(1) = p1[1]-q1[1];
        eij1(2) = p1[2]-q1[2];
        eij1*=weight*scale;

        L+=eij1*eij0.transpose();
        RI+=eij0*eij0.transpose();
    }
    Eigen::Matrix3d T;
    if(fabs(RI.determinant())>tolerance)
         T = L*RI.inverse();
    else
    {
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(RI, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::Matrix3d U,V;
        U=svd.matrixU();
        V=svd.matrixV();
        Eigen::Matrix3d S_inv(svd.singularValues().asDiagonal());
        for(int i=0;i<3;i++)
        {
            if(fabs(S_inv(i,i))>tolerance)
                S_inv(i,i)=1.0/S_inv(i,i);
            else
                S_inv(i,i)=0.0;
        }
        T = L*V*S_inv*U.transpose();
    }
    defor_mesh_.property(T_matrixs,v_it) = T;
}

std::vector<double> DR_feature::get_feature(std::string ref_mesh_name, std::string defor_mesh_name){
	read_ref_mesh(ref_mesh_name);
	read_defor_mesh(defor_mesh_name);

	std::vector<double> dr_feature;

	TriMesh::VertexIter v_it;
    for(v_it=defor_mesh_.vertices_begin(); v_it!=defor_mesh_.vertices_end(); v_it++)
    {
        Eigen::Matrix3d T = defor_mesh_.property(T_matrixs,*v_it);
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeThinU | Eigen::ComputeThinV);
        Eigen::Matrix3d U,V;
        U=svd.matrixU();
        V=svd.matrixV();
        Eigen::Matrix3d S(svd.singularValues().asDiagonal());
        Eigen::Matrix3d Temp=Eigen::Matrix3d::Identity();
        Temp(2,2) = (U*V.transpose()).determinant();
        Eigen::Matrix3d R=U*Temp*V.transpose();
        Eigen::Matrix3d Scale = V*Temp*S*V.transpose();
        Eigen::Matrix3d logR = rotation_log_exp::log(R);

        dr_feature.push_back(Scale(0, 0));
        dr_feature.push_back(Scale(0, 1));
        dr_feature.push_back(Scale(0, 2));
        dr_feature.push_back(Scale(1, 1));
        dr_feature.push_back(Scale(1, 2));
        dr_feature.push_back(Scale(2, 2));
        dr_feature.push_back(logR(0, 1));
        dr_feature.push_back(logR(0, 2));
        dr_feature.push_back(logR(1, 2));
    }
    return dr_feature;
}

py::array_t<double> get_dr(std::string ref_mesh_name, std::string defor_mesh_name){
	DR_feature feature;
	std::vector<double> temp = feature.get_feature(ref_mesh_name, defor_mesh_name);

    auto result = py::array_t<double>(temp.size());
    auto result_buffer = result.request();
    double *result_ptr = (double *)result_buffer.ptr;

    std::memcpy(result_ptr, temp.data(), temp.size()*sizeof(double));

    return result;

}

 PYBIND11_MODULE(get_dr, m){
    m.doc() = "get dr";
    m.def("get_dr", &get_dr, "get_dr");
 }


#include <stdlib.h>
#include <stdio.h>
#include <vector>
#include <string>
#include <sstream>
#include <iostream>
#include <stdexcept>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/voxel_grid.hpp>
#include <arc_utilities/pretty_print.hpp>
#include <sdf_tools/sdf.hpp>

#ifndef SDF_GENERATION_HPP
#define SDF_GENERATION_HPP

namespace sdf_generation
{
    struct bucket_cell
    {
        double distance_square;
        int32_t update_direction;
        uint32_t location[3];
        uint32_t closest_point[3];
    };

    typedef VoxelGrid::VoxelGrid<bucket_cell> DistanceField;

    inline int GetDirectionNumber(const int dx, const int dy, const int dz)
    {
        return ((dx + 1) * 9) + ((dy + 1) * 3) + (dz + 1);
    }

    inline std::vector<std::vector<std::vector<std::vector<int>>>> MakeNeighborhoods()
    {
        std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods;
        neighborhoods.resize(2);
        for (size_t n = 0; n < neighborhoods.size(); n++)
        {
            neighborhoods[n].resize(27);
            // Loop through the source directions
            for (int dx = -1; dx <= 1; dx++)
            {
                for (int dy = -1; dy <= 1; dy++)
                {
                    for (int dz = -1; dz <= 1; dz++)
                    {
                        int direction_number = GetDirectionNumber(dx, dy, dz);
                        // Loop through the target directions
                        for (int tdx = -1; tdx <= 1; tdx++)
                        {
                            for (int tdy = -1; tdy <= 1; tdy++)
                            {
                                for (int tdz = -1; tdz <= 1; tdz++)
                                {
                                    if (tdx == 0 && tdy == 0 && tdz == 0)
                                    {
                                        continue;
                                    }
                                    if (n >= 1)
                                    {
                                        if ((abs(tdx) + abs(tdy) + abs(tdz)) != 1)
                                        {
                                            continue;
                                        }
                                        if ((dx * tdx) < 0 || (dy * tdy) < 0 || (dz * tdz) < 0)
                                        {
                                            continue;
                                        }
                                    }
                                    std::vector<int> new_point;
                                    new_point.resize(3);
                                    new_point[0] = tdx;
                                    new_point[1] = tdy;
                                    new_point[2] = tdz;
                                    neighborhoods[n][direction_number].push_back(new_point);
                                }
                            }
                        }
                    }
                }
            }
        }
        return neighborhoods;
    }

    inline double ComputeDistanceSquared(const int32_t x1, const int32_t y1, const int32_t z1, const int32_t x2, const int32_t y2, const int32_t z2)
    {
        int32_t dx = x1 - x2;
        int32_t dy = y1 - y2;
        int32_t dz = z1 - z2;
        return double((dx * dx) + (dy * dy) + (dz * dz));
    }

    inline DistanceField BuildDistanceField(const Eigen::Isometry3d& grid_origin_tranform,
                                            const double grid_resolution,
                                            const int64_t grid_num_x_cells,
                                            const int64_t grid_num_y_cells,
                                            const int64_t grid_num_z_cells,
                                            const std::vector<VoxelGrid::GRID_INDEX>& points)
    {
        // Make the DistanceField container
        bucket_cell default_cell;
        default_cell.distance_square = std::numeric_limits<double>::infinity();
        DistanceField distance_field(grid_origin_tranform, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, default_cell);
        // Compute maximum distance square
        long max_distance_square = (distance_field.GetNumXCells() * distance_field.GetNumXCells()) + (distance_field.GetNumYCells() * distance_field.GetNumYCells()) + (distance_field.GetNumZCells() * distance_field.GetNumZCells());
        // Make bucket queue
        std::vector<std::vector<bucket_cell>> bucket_queue(max_distance_square + 1);
        bucket_queue[0].reserve(points.size());
        // Set initial update direction
        int initial_update_direction = GetDirectionNumber(0, 0, 0);
        // Mark all points with distance zero and add to the bucket queue
        for (size_t index = 0; index < points.size(); index++)
        {
            const VoxelGrid::GRID_INDEX& current_index = points[index];
            std::pair<bucket_cell&, bool> query = distance_field.GetMutable(current_index);
            if (query.second)
            {
                query.first.location[0] = current_index.x;
                query.first.location[1] = current_index.y;
                query.first.location[2] = current_index.z;
                query.first.closest_point[0] = current_index.x;
                query.first.closest_point[1] = current_index.y;
                query.first.closest_point[2] = current_index.z;
                query.first.distance_square = 0.0;
                query.first.update_direction = initial_update_direction;
                bucket_queue[0].push_back(query.first);
            }
            // If the point is outside the bounds of the SDF, skip
            else
            {
                throw std::runtime_error("Point for BuildDistanceField out of bounds");
            }
        }
        // Process the bucket queue
        std::vector<std::vector<std::vector<std::vector<int>>>> neighborhoods = MakeNeighborhoods();
        for (size_t bq_idx = 0; bq_idx < bucket_queue.size(); bq_idx++)
        {
            std::vector<bucket_cell>::iterator queue_itr = bucket_queue[bq_idx].begin();
            while (queue_itr != bucket_queue[bq_idx].end())
            {
                // Get the current location
                bucket_cell& cur_cell = *queue_itr;
                double x = cur_cell.location[0];
                double y = cur_cell.location[1];
                double z = cur_cell.location[2];
                // Pick the update direction
                int D = bq_idx;
                if (D > 1)
                {
                    D = 1;
                }
                // Make sure the update direction is valid
                if (cur_cell.update_direction < 0 || cur_cell.update_direction > 26)
                {
                    ++queue_itr;
                    continue;
                }
                // Get the current neighborhood list
                std::vector<std::vector<int>>& neighborhood = neighborhoods[D][cur_cell.update_direction];
                // Update the distance from the neighboring cells
                for (size_t nh_idx = 0; nh_idx < neighborhood.size(); nh_idx++)
                {
                    // Get the direction to check
                    int dx = neighborhood[nh_idx][0];
                    int dy = neighborhood[nh_idx][1];
                    int dz = neighborhood[nh_idx][2];
                    int nx = x + dx;
                    int ny = y + dy;
                    int nz = z + dz;
                    std::pair<bucket_cell&, bool> neighbor_query = distance_field.GetMutable((int64_t)nx, (int64_t)ny, (int64_t)nz);
                    if (!neighbor_query.second)
                    {
                        // "Neighbor" is outside the bounds of the SDF
                        continue;
                    }
                    // Update the neighbor's distance based on the current
                    int new_distance_square = ComputeDistanceSquared(nx, ny, nz, cur_cell.closest_point[0], cur_cell.closest_point[1], cur_cell.closest_point[2]);
                    if (new_distance_square > max_distance_square)
                    {
                        // Skip these cases
                        continue;
                    }
                    if (new_distance_square < neighbor_query.first.distance_square)
                    {
                        // If the distance is better, time to update the neighbor
                        neighbor_query.first.distance_square = new_distance_square;
                        neighbor_query.first.closest_point[0] = cur_cell.closest_point[0];
                        neighbor_query.first.closest_point[1] = cur_cell.closest_point[1];
                        neighbor_query.first.closest_point[2] = cur_cell.closest_point[2];
                        neighbor_query.first.location[0] = nx;
                        neighbor_query.first.location[1] = ny;
                        neighbor_query.first.location[2] = nz;
                        neighbor_query.first.update_direction = GetDirectionNumber(dx, dy, dz);
                        // Add the neighbor into the bucket queue
                        bucket_queue[new_distance_square].push_back(neighbor_query.first);
                    }
                }
                // Increment the queue iterator
                ++queue_itr;
            }
            // Clear the current queue now that we're done with it
            bucket_queue[bq_idx].clear();
        }
        return distance_field;
    }

    template<typename T>
    inline std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const Eigen::Isometry3d& grid_origin_tranform,
                                                                                const double grid_resolution,
                                                                                const int64_t grid_num_x_cells,
                                                                                const int64_t grid_num_y_cells,
                                                                                const int64_t grid_num_z_cells,
                                                                                const std::function<bool(const VoxelGrid::GRID_INDEX&)>& is_filled_fn,
                                                                                const float oob_value,
                                                                                const std::string& frame)
    {
        std::vector<VoxelGrid::GRID_INDEX> filled;
        std::vector<VoxelGrid::GRID_INDEX> free;
        for (int64_t x_index = 0; x_index < grid_num_x_cells; x_index++)
        {
            for (int64_t y_index = 0; y_index < grid_num_y_cells; y_index++)
            {
                for (int64_t z_index = 0; z_index < grid_num_z_cells; z_index++)
                {
                    const VoxelGrid::GRID_INDEX current_index(x_index, y_index, z_index);
                    if (is_filled_fn(current_index))
                    {
                        // Mark as filled
                        filled.push_back(current_index);
                    }
                    else
                    {
                        // Mark as free space
                        free.push_back(current_index);
                    }
                }
            }
        }
        // Make two distance fields (one for distance to filled voxels, one for distance to free voxels
        const DistanceField filled_distance_field = BuildDistanceField(grid_origin_tranform, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, filled);
        const DistanceField free_distance_field = BuildDistanceField(grid_origin_tranform, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, free);
        // Generate the SDF
        sdf_tools::SignedDistanceField new_sdf(grid_origin_tranform, frame, grid_resolution, grid_num_x_cells, grid_num_y_cells, grid_num_z_cells, oob_value);
        double max_distance = -std::numeric_limits<double>::infinity();
        double min_distance = std::numeric_limits<double>::infinity();
        for (int64_t x_index = 0; x_index < new_sdf.GetNumXCells(); x_index++)
        {
            for (int64_t y_index = 0; y_index < new_sdf.GetNumYCells(); y_index++)
            {
                for (int64_t z_index = 0; z_index < new_sdf.GetNumZCells(); z_index++)
                {
                    const double distance1 = std::sqrt(filled_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * new_sdf.GetResolution();
                    const double distance2 = std::sqrt(free_distance_field.GetImmutable(x_index, y_index, z_index).first.distance_square) * new_sdf.GetResolution();
                    const double distance = distance1 - distance2;
                    if (distance > max_distance)
                    {
                        max_distance = distance;
                    }
                    if (distance < min_distance)
                    {
                        min_distance = distance;
                    }
                    new_sdf.SetValue(x_index, y_index, z_index, distance);
                }
            }
        }
        std::pair<double, double> extrema(max_distance, min_distance);
        return std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>>(new_sdf, extrema);
    }

    template<typename T, typename BackingStore=std::vector<T>>
    inline std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const VoxelGrid::VoxelGrid<T, BackingStore>& grid, const std::function<bool(const VoxelGrid::GRID_INDEX&)>& is_filled_fn, const float oob_value, const std::string& frame, const bool add_virtual_border)
    {
      (void)(add_virtual_border);
      const Eigen::Vector3d cell_sizes = grid.GetCellSizes();
      if ((cell_sizes.x() != cell_sizes.y()) || (cell_sizes.x() != cell_sizes.z()))
      {
        throw std::invalid_argument("Grid must have uniform resolution");
      }
      if (add_virtual_border == false)
      {
        // This is the conventional single-pass result
        return ExtractSignedDistanceField<T>(grid.GetOriginTransform(), cell_sizes.x(), grid.GetNumXCells(), grid.GetNumYCells(), grid.GetNumZCells(), is_filled_fn, oob_value, frame);
      }
      else
      {
        const int64_t x_axis_size_offset = (grid.GetNumXCells() > 1) ? (int64_t)2 : (int64_t)0;
        const int64_t x_axis_query_offset = (grid.GetNumXCells() > 1) ? (int64_t)1 : (int64_t)0;
        const int64_t y_axis_size_offset = (grid.GetNumYCells() > 1) ? (int64_t)2 : (int64_t)0;
        const int64_t y_axis_query_offset = (grid.GetNumYCells() > 1) ? (int64_t)1 : (int64_t)0;
        const int64_t z_axis_size_offset = (grid.GetNumZCells() > 1) ? (int64_t)2 : (int64_t)0;
        const int64_t z_axis_query_offset = (grid.GetNumZCells() > 1) ? (int64_t)1 : (int64_t)0;
        // We need to lie about the size of the grid to add a virtual border
        const int64_t num_x_cells = grid.GetNumXCells() + x_axis_size_offset;
        const int64_t num_y_cells = grid.GetNumYCells() + y_axis_size_offset;
        const int64_t num_z_cells = grid.GetNumZCells() + z_axis_size_offset;
        // Make some deceitful helper functions that hide our lies about size
        // For the free space SDF, we lie and say the virtual border is filled
        const std::function<bool(const VoxelGrid::GRID_INDEX&)> free_is_filled_fn
            = [&] (const VoxelGrid::GRID_INDEX& virtual_border_grid_index)
        {
          // Is there a virtual border on our axis?
          if (x_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.x == 0)
                || (virtual_border_grid_index.x == (num_x_cells - 1)))
            {
              return true;
            }
          }
          // Is there a virtual border on our axis?
          if (y_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.y == 0)
                || (virtual_border_grid_index.y == (num_y_cells - 1)))
            {
              return true;
            }
          }
          // Is there a virtual border on our axis?
          if (z_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.z == 0)
                || (virtual_border_grid_index.z == (num_z_cells - 1)))
            {
              return true;
            }
          }
          const VoxelGrid::GRID_INDEX real_grid_index(
                virtual_border_grid_index.x - x_axis_query_offset,
                virtual_border_grid_index.y - y_axis_query_offset,
                virtual_border_grid_index.z - z_axis_query_offset);
          return is_filled_fn(real_grid_index);
        };
        // For the filled space SDF, we lie and say the virtual border is empty
        const std::function<bool(const VoxelGrid::GRID_INDEX&)> filled_is_filled_fn
            = [&] (const VoxelGrid::GRID_INDEX& virtual_border_grid_index)
        {
          // Is there a virtual border on our axis?
          if (x_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.x == 0)
                || (virtual_border_grid_index.x == (num_x_cells - 1)))
            {
              return false;
            }
          }
          // Is there a virtual border on our axis?
          if (y_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.y == 0)
                || (virtual_border_grid_index.y == (num_y_cells - 1)))
            {
              return false;
            }
          }
          // Is there a virtual border on our axis?
          if (z_axis_size_offset > 0)
          {
            // Are we a virtual border cell?
            if ((virtual_border_grid_index.z == 0)
                || (virtual_border_grid_index.z == (num_z_cells - 1)))
            {
              return false;
            }
          }
          const VoxelGrid::GRID_INDEX real_grid_index(
                virtual_border_grid_index.x - x_axis_query_offset,
                virtual_border_grid_index.y - y_axis_query_offset,
                virtual_border_grid_index.z - z_axis_query_offset);
          return is_filled_fn(real_grid_index);
        };
        // Make both SDFs
        auto free_sdf_result = ExtractSignedDistanceField<T>(grid.GetOriginTransform(), cell_sizes.x(), num_x_cells, num_y_cells, num_z_cells, free_is_filled_fn, oob_value, frame);
        auto filled_sdf_result = ExtractSignedDistanceField<T>(grid.GetOriginTransform(), cell_sizes.x(), num_x_cells, num_y_cells, num_z_cells, filled_is_filled_fn, oob_value, frame);
        // Combine to make a single SDF
        sdf_tools::SignedDistanceField combined_sdf(grid.GetOriginTransform(), frame, cell_sizes.x(), grid.GetNumXCells(), grid.GetNumYCells(), grid.GetNumZCells(), oob_value);
        for (int64_t x_idx = 0; x_idx < combined_sdf.GetNumXCells(); x_idx++)
        {
          for (int64_t y_idx = 0; y_idx < combined_sdf.GetNumYCells(); y_idx++)
          {
            for (int64_t z_idx = 0; z_idx < combined_sdf.GetNumZCells(); z_idx++)
            {
              const int64_t query_x_idx = x_idx + x_axis_query_offset;
              const int64_t query_y_idx = y_idx + y_axis_query_offset;
              const int64_t query_z_idx = z_idx + z_axis_query_offset;
              const float free_sdf_value
                  = free_sdf_result.first.GetImmutable(
                      query_x_idx, query_y_idx, query_z_idx).first;
              const float filled_sdf_value
                  = filled_sdf_result.first.GetImmutable(
                      query_x_idx, query_y_idx, query_z_idx).first;
              if (free_sdf_value >= 0.0)
              {
                combined_sdf.SetValue(x_idx, y_idx, z_idx, free_sdf_value);
              }
              else if (filled_sdf_value <= -0.0)
              {
                combined_sdf.SetValue(x_idx, y_idx, z_idx, filled_sdf_value);
              }
              else
              {
                combined_sdf.SetValue(x_idx, y_idx, z_idx, 0.0f);
              }
            }
          }
        }
        // Get the combined max/min values
        const std::pair<double, double> combined_extrema(
              free_sdf_result.second.first, filled_sdf_result.second.second);
        return std::make_pair(combined_sdf, combined_extrema);
      }
    }

    template<typename T, typename BackingStore=std::vector<T>>
    inline std::pair<sdf_tools::SignedDistanceField, std::pair<double, double>> ExtractSignedDistanceField(const VoxelGrid::VoxelGrid<T, BackingStore>& grid, const std::function<bool(const T&)>& is_filled_fn, const float oob_value, const std::string& frame)
    {
        const std::function<bool(const VoxelGrid::GRID_INDEX&)> real_is_filled_fn = [&] (const VoxelGrid::GRID_INDEX& index)
        {
            const T& stored = grid.GetImmutable(index).first;
            // If it matches an object to use OR there are no objects supplied
            if (is_filled_fn(stored))
            {
                // Mark as filled
                return true;
            }
            else
            {
                // Mark as free space
                return false;
            }
        };
        return ExtractSignedDistanceField(grid, real_is_filled_fn, oob_value, frame, false);
    }
}

#endif // SDF_GENERATION_HPP


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C)  2017 Cord Harms

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file localcorrsurface.hpp
    \brief Local Correlation surface derived ....
*/

#include <ql/experimental/termstructures/Helper/ParticleMethodUtils.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/experimental/templatemodels/multiasset/localcorrelationSLVmodel.hpp>
#include <ql/experimental/templatemodels/montecarlo/montecarlomodells.hpp>
#include <ql/experimental/templatemodels/montecarlo/mcpayoffT.hpp>
#include <ql/math/interpolations/linearinterpolation.hpp>
#include <math.h>
#include <boost/math/distributions.hpp>
#include <boost/shared_ptr.hpp>
#include <ql\pricingengines\blackscholescalculator.hpp>

namespace QuantLib {

	void ParticleMethodUtils::calibrateFX(Handle<LocalCorrSurfaceABFFX>& surface, const std::string& kernelIn, unsigned int numberOfPaths, Time maxTime,
		Time deltaT, Time tMin, Real kappa, Real sigmaAVR, Real exponentN, Real gridMinQuantile,
		Real gridMaxQuantile, unsigned int ns1, unsigned int ns2) {

		boost::shared_ptr<KernelInterface> kernel;

		if (kernelIn == "QuarticKernel") {
			kernel = boost::shared_ptr<KernelInterface>(new QuarticKernel());
		}
		else {
			QL_REQUIRE(false, "Kernel not supported. Supported is: QuarticKernel");
		}

		std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>> processes = surface->getProcesses();
		boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>			    processToCal= surface->getProcessToCal();

		//build assetModel for simulation
		boost::shared_ptr<YieldTermStructure> yld(new FlatForward(processToCal->blackVolatility()->referenceDate(),0, processToCal->blackVolatility()->dayCounter()));
		Handle<YieldTermStructure> yldH = Handle<YieldTermStructure>(yld);
		std::vector<std::string> aliases(2);
		aliases[0] = "fx1";
		aliases[1] = "fx2";
		Handle<LocalCorrTermStructure> surfaceGen(surface.currentLink());
		boost::shared_ptr<LocalCorrelationSLVModel> assetModel = boost::shared_ptr<LocalCorrelationSLVModel>((new LocalCorrelationSLVModel(yldH, aliases, processes, surfaceGen)));

		std::vector<Time>& times = surface->getTimes();
		std::vector<std::vector<Real>>& strikes = surface->getStrikes();
		std::vector<std::vector<Real>>& surfaceF = surface->getSurfaceF();

		//time grid from t=0 to maxTime:
		times.resize(1);
		times[0] = 0;
		size_t i = 0;
		while (times[i] < maxTime) {
			times.push_back(times[i]+deltaT);
			i++;
		}
		surface->setInterpolationTime<Linear>();

		RealMCSimulation simulation(assetModel, times, times, numberOfPaths,1,true,true,false);
		simulation.prepareForSlicedSimulation();
		simulation.simulate(0, false);

		//create strike grid.
		//the strike grid depends on simulation results (min and max quantile)
		//the simulation itself depends on local correlation which itself is calibrated using this function.
		//Consequence: strike grid can merely be calculated iteratively during calibration

		size_t numberStrikes;
		Real strikeStep;
		surfaceF.resize(times.size());
		strikes.resize(times.size());
		std::vector<Real> state;
		std::vector<Real> assets(2);
		std::vector<Real> crossFX(numberOfPaths);

		Real vol1;
		Real vol2;
		std::vector<Real> vol3;
		std::vector<Real> eNum;
		std::vector<Real> eDen;
		std::vector<Real> eScale;
		Real a = 0;
		Real b = 0;
		Real kernelV = 0;
		Real bandwidth = 0;
		Real minPosBw;
		Real maxNegBw;
		Real bwIn;
		Real bwRatio;

		//Calculate local correlation successively over time

		for (size_t i = 1; i < surfaceF.size()-1; i++) //iteration over time, for i=0 nothing to do as correlation independent of a,b,f. In last entry, no additional simulation necessary.
		{
			numberStrikes = numberStrikeGrid(times[i],ns1,ns2);
			QL_REQUIRE(numberStrikes>1,"ns1 or ns2 has to be increased, strike grid cannot be calculated.");
			simulation.simulate(i,false);

			//Now strike grid can be calculated

			surfaceF[i].resize(numberStrikes);
			strikes[i].resize(numberStrikes);
			for (size_t k = 0; k < numberOfPaths; k++)
			{
				state = simulation.state(k, times[i]);
				assets[0] = processes[0]->s0()->value() * std::exp(state[0]);
				assets[1] = processes[1]->s0()->value() * std::exp(state[1]);
				crossFX[k] = getCrossFX(assets[0] , assets[1]);
			}

			std::sort(crossFX.begin(),crossFX.end());

			strikes[i][0] = crossFX[(size_t)(numberOfPaths*gridMinQuantile)];
			strikes[i][strikes[i].size()-1] = crossFX[(size_t)(numberOfPaths*gridMaxQuantile)];

			strikeStep = (strikes[i][strikes[i].size() - 1] - strikes[i][0]) / (numberStrikes-1);

			QL_ASSERT(strikeStep > 0, "Error. StrikeStep shouldn't be zero for time " << times[i] << ". Check volatility.");

			bandwidth = ParticleMethodUtils::bandwidth(times[i], getCrossFX(processes[0]->s0()->value(),processes[1]->s0()->value()), kappa, sigmaAVR, tMin, numberOfPaths, exponentN);

			for (size_t j = 1; j < strikes[i].size()-1; j++)
			{
				strikes[i][j] = strikes[i][j-1] + strikeStep;
			}

			//Particle method for that time step

			eNum.resize(strikes[i].size());
			eDen.resize(strikes[i].size());
			eScale.resize(strikes[i].size());
			vol3.resize(strikes[i].size());

			for (size_t j = 0; j < strikes[i].size(); j++)
			{
				eNum[j] = 0;
				eDen[j] = 0;
				eScale[j] = 0;
				vol3[j] = processToCal->localVolatility()->localVol(times[i], strikes[i][j], true);
			}

			for (size_t k = 0; k < numberOfPaths; k++) //particle method: over MC paths
			{
				state = simulation.state(k, times[i]);

				assets[0] = processes[0]->s0()->value() * std::exp(state[0]);
				assets[1] = processes[1]->s0()->value() * std::exp(state[1]);

				state[2] = state[2] < 0 ? 0.0001 : state[2];//heston vol might become negative (dep. on feller constant)
				state[3] = state[3] < 0 ? 0.0001 : state[3];

				vol1 = processes[0]->leverageFct()->localVol(times[i], assets[0], true)*std::sqrt(state[2]); //*ai from Heston
				vol2 = processes[1]->leverageFct()->localVol(times[i], assets[1], true)*std::sqrt(state[3]);

				a = surface->localA(times[i], assets, true);
				b = surface->localB(times[i], assets, true);

				if (vol1 != vol1 || vol2 != vol2)
					QL_FAIL("surface of asset1 oder asset2 not well defined");

				for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
				{

					minPosBw = 10000000;
					maxNegBw = -10000000;

					bwIn = getCrossFX(assets[0], assets[1]) - strikes[i][j];
					bwRatio = bwIn / bandwidth;

					if (bwRatio > maxNegBw && bwRatio <= 0) maxNegBw = bwRatio;
					if (bwRatio < minPosBw && bwRatio >= 0) minPosBw = bwRatio;

					kernelV = assets[1] * ParticleMethodUtils::kernel(bandwidth, bwIn, kernel);

					eNum[j] += (vol1*vol1 + vol2*vol2 + 2 * a*vol1*vol2 / b)*kernelV;
					eDen[j] += vol1*vol2*kernelV / b;
					eScale[j] += kernelV;
				}
			}

			for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
			{
				QL_REQUIRE(eScale[j] != 0, std::string("ParticleMethodUtils::calibrateFX: resulting bandwidth is too small for calibration (support: < ") + std::to_string(maxNegBw)
					+ std::string(" and > ") + std::to_string(minPosBw)
					+ std::string("). Either decrease number of MC paths or increase exponentN or kappa."));
				if (eNum[j] != eNum[j] || vol3 != vol3 || eScale[j] != eScale[j] || eDen[j] != eDen[j] || eDen[j] == 0)
					QL_FAIL("surface not well defined.");
				surfaceF[i][j] = (eNum[j] - vol3[j] * vol3[j]* eScale[j]) / (2 * eDen[j]);
			}
			//set interpolation on new dimension:
			surface->setInterpolationStrike<Linear>(i);
		}
	}

	void ParticleMethodUtils::calibrateIndex(Handle<LocalCorrSurfaceABFIndex>& surface, const std::string& kernelIn, unsigned int numberOfPaths, Time maxTime,
		Time deltaT, Time tMin, Real kappa, Real sigmaAVR, Real exponentN, Real gridMinQuantile,
		Real gridMaxQuantile, unsigned int ns1, unsigned int ns2) {

		boost::shared_ptr<KernelInterface> kernel;

		if (kernelIn == "QuarticKernel") {
			kernel = boost::shared_ptr<KernelInterface>(new QuarticKernel());
		}
		else {
			QL_REQUIRE(false, "Kernel not supported. Supported is: QuarticKernel");
		}

		std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>> processes = surface->getProcesses();
		boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>			    processToCal = surface->getProcessToCal();

		//build assetModel for simulation
		boost::shared_ptr<YieldTermStructure> yld(new FlatForward(processToCal->blackVolatility()->referenceDate(), 0, processToCal->blackVolatility()->dayCounter()));
		Handle<YieldTermStructure> yldH = Handle<YieldTermStructure>(yld);
		std::vector<std::string> aliases(2);
		aliases[0] = "index1";
		aliases[1] = "index2";
		Handle<LocalCorrTermStructure> surfaceGen(surface.currentLink());
		boost::shared_ptr<LocalCorrelationSLVModel> assetModel = boost::shared_ptr<LocalCorrelationSLVModel>((new LocalCorrelationSLVModel(yldH, aliases, processes, surfaceGen)));

		std::vector<Time>& times = surface->getTimes();
		std::vector<std::vector<Real>>& strikes = surface->getStrikes();
		std::vector<std::vector<Real>>& surfaceF = surface->getSurfaceF();

		//time grid from t=0 to maxTime:
		times.resize(1);
		times[0] = 0;
		size_t i = 0;
		while (times[i] < maxTime) {
			times.push_back(times[i] + deltaT);
			i++;
		}
		surface->setInterpolationTime<Linear>();

		RealMCSimulation simulation(assetModel, times, times, numberOfPaths, 1, true, true, false);
		simulation.prepareForSlicedSimulation();
		simulation.simulate(0, false);

		//create strike grid.
		//the strike grid depends on simulation results (min and max quantile)
		//the simulation itself depends on local correlation which itself is calibrated using this function.
		//Consequence: strike grid can merely be calculated iteratively during calibration

		size_t numberStrikes;
		Real strikeStep;
		surfaceF.resize(times.size());
		strikes.resize(times.size());
		std::vector<Real> state;
		std::vector<Real> assets(processes.size());
		std::vector<Real> indexVals(numberOfPaths);

		std::vector<Real> vol(processes.size());
		std::vector<Real> vol3;
		std::vector<Real> eNum;
		std::vector<Real> eDen;
		std::vector<Real> eScale;
		Real a = 0;
		Real b = 0;
		Real kernelV = 0;
		Real bandwidth = 0;
		Real minPosBw;
		Real maxNegBw;
		Real bwIn;
		Real bwRatio;
		double indexStart;

		//Calculate local correlation successively over time

		for (size_t i = 1; i < surfaceF.size() - 1; i++) //iteration over time, for i=0 nothing to do as correlation independent of a,b,f. In last entry, no additional simulation necessary.
		{
			numberStrikes = numberStrikeGrid(times[i], ns1, ns2);
			QL_REQUIRE(numberStrikes>1, "ns1 or ns2 has to be increased, strike grid cannot be calculated.");
			simulation.simulate(i, false);

			//Now strike grid can be calculated

			surfaceF[i].resize(numberStrikes);
			strikes[i].resize(numberStrikes);
			for (size_t k = 0; k < numberOfPaths; k++)
			{
				state = simulation.state(k, times[i]);
				indexVals[k] = surface->localFStrike(times[i],state);
			}

			std::sort(indexVals.begin(), indexVals.end());

			strikes[i][0] = indexVals[(size_t)(numberOfPaths*gridMinQuantile)];
			strikes[i][strikes[i].size() - 1] = indexVals[(size_t)(numberOfPaths*gridMaxQuantile)];

			strikeStep = (strikes[i][strikes[i].size() - 1] - strikes[i][0]) / (numberStrikes - 1);
			QL_ASSERT(strikeStep > 0, "Error. StrikeStep shouldn't be zero for time " << times[i] << ". Check volatility.");
			indexStart = surface->localFStrike(times[i], std::vector<Real>(processes.size(), 0));

			bandwidth = ParticleMethodUtils::bandwidth(times[i], abs(indexStart)>1 ? abs(indexStart) : 1, kappa, sigmaAVR, tMin, numberOfPaths, exponentN);

			for (size_t j = 1; j < strikes[i].size() - 1; j++)
			{
				strikes[i][j] = strikes[i][j - 1] + strikeStep;
			}

			//Particle method for that time step

			eNum.resize(strikes[i].size());
			eDen.resize(strikes[i].size());
			eScale.resize(strikes[i].size());
			vol3.resize(strikes[i].size());

			for (size_t j = 0; j < strikes[i].size(); j++)
			{
				eNum[j] = 0;
				eDen[j] = 0;
				eScale[j] = 0;
				if (surface->possibleNegativeIndex()) {
					vol3[j] = getLocalVolFromPriceFormula(processToCal,strikes[i][j],times[i],surface->getProcessToCalBlackVolShift());
				}
				else
					vol3[j] = processToCal->localVolatility()->localVol(times[i], strikes[i][j], true);
			}

			for (size_t k = 0; k < numberOfPaths; k++) //particle method: over MC paths
			{
				state = simulation.state(k, times[i]);

				for (size_t j = 0; j < assets.size(); j++)
				{
					assets[j] = processes[j]->s0()->value() * std::exp(state[j]);
					state[j+processes.size()] = state[j + processes.size()] < 0 ? 0.0001 : state[j + processes.size()];//heston vol might become negative (dep. on feller constant)
					vol[j] = processes[j]->leverageFct()->localVol(times[i], assets[j], true)*std::sqrt(state[j + processes.size()]); //*ai from Heston
					if (vol[j] != vol[j])
						QL_FAIL(std::string("surface of asset ") + std::to_string(j) + std::string(" not well defined"));
				}

				Real vol0 = surface->getIndexCovariance(LocalCorrSurfaceABFIndex::CTSIndexCovarianceType::CORR0, assets, vol);
				Real vol1 = surface->getIndexCovariance(LocalCorrSurfaceABFIndex::CTSIndexCovarianceType::CORR1, assets, vol);

				a = surface->localA(times[i], assets, true);
				b = surface->localB(times[i], assets, true);

				Real indexVal = surface->localFStrike(times[i], state);

				for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
				{

					minPosBw = 10000000;
					maxNegBw = -10000000;

					bwIn = indexVal - strikes[i][j];
					bwRatio = bwIn / bandwidth;

					if (bwRatio > maxNegBw && bwRatio <= 0) maxNegBw = bwRatio;
					if (bwRatio < minPosBw && bwRatio >= 0) minPosBw = bwRatio;

					kernelV = ParticleMethodUtils::kernel(bandwidth, bwIn, kernel);

					eNum[j] += (vol0 - a*(vol1-vol0) / b)*kernelV;
					eDen[j] += (vol1 - vol0)*kernelV / b;
					eScale[j] += kernelV;
				}
			}

			for (size_t j = 0; j < surfaceF[i].size(); j++) //iteration over strike dimension
			{
				QL_REQUIRE(eScale[j] != 0, std::string("ParticleMethodUtils::calibrateIndex: resulting bandwidth is too small for calibration (support: < ") + std::to_string(maxNegBw)
					+ std::string(" and > ") + std::to_string(minPosBw)
					+ std::string("). Either decrease number of MC paths or increase exponentN or kappa."));
				if (eNum[j] != eNum[j] || vol3[j] != vol3[j] || eScale[j] != eScale[j] || eDen[j] != eDen[j] || eDen[j] == 0)
					QL_FAIL("surface not well defined.");
				if (surface->possibleNegativeIndex()) {
					surfaceF[i][j] = (vol3[j] * eScale[j] - eNum[j]) / (eDen[j]);
				}
				else
					surfaceF[i][j] = ( vol3[j] * vol3[j] * strikes[i][j]* strikes[i][j] * eScale[j]- eNum[j]) / (eDen[j]);
			}
			//set interpolation on new dimension:
			surface->setInterpolationStrike<Linear>(i);
		}
	}

	Real ParticleMethodUtils::bandwidth(Time t, Real s0, Real kappa, Real sigmaAVR, Real tMin, unsigned int numberOfPaths, Real exponentN) {
		Real mult = pow(numberOfPaths, exponentN);
		return kappa*sigmaAVR* s0 * sqrt(t>tMin ? t : tMin) * mult;
	}

	Real ParticleMethodUtils::kernel(Real bandwidth, Real x, boost::shared_ptr<KernelInterface>& kernel) {
		QL_REQUIRE(bandwidth != 0, "Error in ParticleMethodUtils: bandwidth is not allowed to be zero.");
		return kernel->value(x / bandwidth) / bandwidth;
	}
	size_t ParticleMethodUtils::numberStrikeGrid(Time t, unsigned int ns1, unsigned int ns2) {
		Real numberOfStrikes = ns1*sqrt(t);
		return (numberOfStrikes > ns2) ? ((size_t)numberOfStrikes) : (ns2);
	}

	Real ParticleMethodUtils::getCrossFX(Real asset1, Real asset2) {
		return asset1 / asset2;
	}

	Real ParticleMethodUtils::getLocalVolFromPriceFormula(boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>& processToCal, Real strike, Time t, Real indShift) {
		double negIndVol;
		double negIndPriceT;
		double negIndPrice;
		double negIndPriceUp;
		double negIndPriceDn;
		double negIndTimeBmp = 0.0001;
		double negIndAssetBmp = 0.001;
		double negIndAsset;
		double dCdt;
		double dCdKdK;
		double dCdK;

		negIndVol = processToCal->blackVolatility()->blackVol(t, strike, true);
		negIndPrice = BlackScholesCalculator(Option::Type::Call, strike + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t),
			negIndVol*sqrt(t), processToCal->riskFreeRate()->discount(t)).value();

		negIndVol = processToCal->blackVolatility()->blackVol(t + negIndTimeBmp, strike, true);
		negIndPriceT = BlackScholesCalculator(Option::Type::Call, strike + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t + negIndTimeBmp),
			negIndVol*sqrt(t + negIndTimeBmp), processToCal->riskFreeRate()->discount(t + negIndTimeBmp)).value();

		dCdt = (negIndPriceT - negIndPrice) / negIndTimeBmp;

		negIndAsset = strike>1 ? strike * (1 + negIndAssetBmp) : strike + negIndAssetBmp;
		negIndVol = processToCal->blackVolatility()->blackVol(t, negIndAsset, true);
		negIndPriceUp = BlackScholesCalculator(Option::Type::Call, negIndAsset + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t),
			negIndVol*sqrt(t), processToCal->riskFreeRate()->discount(t)).value();

		negIndAsset = strike>1 ? strike * (1 - negIndAssetBmp) : strike - negIndAssetBmp;
		negIndVol = processToCal->blackVolatility()->blackVol(t, negIndAsset, true);
		negIndPriceDn = BlackScholesCalculator(Option::Type::Call, negIndAsset + indShift, processToCal->x0() + indShift, processToCal->dividendYield()->discount(t),
			negIndVol*sqrt(t), processToCal->riskFreeRate()->discount(t)).value();

		negIndAsset = strike>1 ? negIndAssetBmp*strike : negIndAssetBmp;
		dCdKdK = (negIndPriceUp - 2 * negIndPrice + negIndPriceDn) / (negIndAsset * negIndAsset);

		dCdK = (negIndPriceUp - negIndPriceDn) / (2 * negIndAsset);

		Real rate = processToCal->riskFreeRate()->zeroRate(t, Compounding::Continuous);
		Real div = processToCal->dividendYield()->zeroRate(t, Compounding::Continuous);

		QL_REQUIRE(abs(dCdKdK) > QL_EPSILON, "local vol for cross asset delivers NaN (t=" << t << ", strike=" << strike << ")");

		return 2 * (dCdt + strike * dCdK*(rate-div) + div*negIndPrice) / dCdKdK;

	}
}