#include "ear/decorrelate.hpp"

#include <boost/math/constants/constants.hpp>
#include <cmath>
#include <complex>
#include <random>
#include "ear/layout.hpp"
#include "kissfft/kissfft.hh"

const double PI = boost::math::constants::pi<double>();

namespace ear {

  std::vector<long> genRandMt19937(int seed, int n) {
    std::mt19937 mtRand(seed);
    std::vector<long> ret;
    for (int i = 0; i < n; ++i) {
      ret.push_back(mtRand());
    }
    return ret;
  }

  std::vector<double> genRandFloat(int seed, int n) {
    std::vector<double> ret;
    for (long randomValue : genRandMt19937(seed, n)) {
      ret.push_back(randomValue / static_cast<double>(0x100000000l));
    }
    return ret;
  }

  /** @brief Design an all-pass random-phase FIR filter.
   *
   * @param decorrelator_id Random seed, to obtain different filters.
   * @param size filter length.
   *
   * @return  Filter coefficients.
   */
  std::vector<double> designDecorrelatorBasic(int decorrelatorId, int size) {
    std::vector<double> rand = genRandFloat(decorrelatorId, size / 2 - 1);
    std::vector<std::complex<double>> freqDomainData(size);
    freqDomainData[0] = std::complex<double>(1.0, 0.0);
    for (size_t i = 0; i < rand.size(); ++i) {
      freqDomainData[i + 1] =
          std::exp(std::complex<double>(0.0, 2.0 * PI * rand[i]));
    }
    freqDomainData[size / 2] = std::complex<double>(1.0, 0.0);
    for (size_t i = 0; i < freqDomainData.size() / 2; ++i) {
      freqDomainData[size / 2 + i] = std::conj(freqDomainData[size / 2 - i]);
    }
    kissfft<double> fft(size, true);
    std::vector<std::complex<double>> timeDomainData(size);
    fft.transform(&freqDomainData[0], &timeDomainData[0]);
    std::vector<double> timeDomainDataReal(size);
    for (size_t i = 0; i < timeDomainData.size(); ++i) {
      timeDomainDataReal[i] = timeDomainData[i].real() / size;
    }
    return timeDomainDataReal;
  }

  const int decorrelator_size = 512;

  template <>
  std::vector<std::vector<double>> designDecorrelators<double>(Layout layout) {
    std::vector<std::string> channelNames = layout.channelNames();
    std::vector<std::string> channelNamesSorted(channelNames);
    std::sort(channelNamesSorted.begin(), channelNamesSorted.end());
    std::vector<std::vector<double>> decorrelators;
    for (auto channelName : channelNames) {
      auto it =
          std::find_if(channelNamesSorted.begin(), channelNamesSorted.end(),
                       [&channelName](const std::string name) -> bool {
                         return channelName == name;
                       });
      int index =
          static_cast<int>(std::distance(channelNamesSorted.begin(), it));
      std::vector<double> coefficients =
          designDecorrelatorBasic(index, decorrelator_size);
      decorrelators.push_back(coefficients);
    }
    return decorrelators;
  }

  template <>
  EAR_EXPORT std::vector<std::vector<float>> designDecorrelators<float>(
      Layout layout) {
    auto decorrelators = designDecorrelators<double>(layout);
    std::vector<std::vector<float>> decorrelators_float;

    for (auto &decorrelator : decorrelators) {
      std::vector<float> decorrelator_float(decorrelator.size());

      for (size_t i = 0; i < decorrelator.size(); i++)
        decorrelator_float[i] = (float)decorrelator[i];

      decorrelators_float.emplace_back(std::move(decorrelator_float));
    }

    return decorrelators_float;
  }

  int decorrelatorCompensationDelay() { return (decorrelator_size - 1) / 2; }
}  // namespace ear


/******************************************************
 *
 *
 *
 * mex -I/usr/include/eigen3 ssimcpp.cpp
 ******************************************************/

#include <iostream>
#include <math.h>
#include <Eigen/Dense>
#include "mex.h"

using namespace Eigen;
using namespace std;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{

  const mwSize *dim;
  dim = mxGetDimensions(prhs[0]);

  double *Aptr, *Bptr;
  Aptr = mxGetPr(prhs[0]);
  Bptr = mxGetPr(prhs[1]);

  MatrixXd A, B, A2, B2;
  A.resize(dim[0],dim[1]);
  A << Map<MatrixXd>(Aptr, dim[0], dim[1]);
  B.resize(dim[0],dim[1]);
  B << Map<MatrixXd>(Bptr, dim[0], dim[1]);

  A2.resize(dim[0],dim[1]);
  B2.resize(dim[0],dim[1]);

  const double mA = A.mean();
  const double mB = B.mean();

  A2 = (A- mA*MatrixXd::Ones(dim[0], dim[1]));
  B2 = (B- mB*MatrixXd::Ones(dim[0], dim[1]));

  const double N = dim[0]*dim[1];
  const double sA = A2.cwiseAbs2().sum()/N;
  const double sB = B2.cwiseAbs2().sum()/N;

  double sAB = 0.0;
  uint i, j;
  for (i=0; i<N; i++){
      for (j=0; j<N; j++){
          if (j>i) sAB = sAB+(A(i)-A(j))*(B(i)-B(j));
      }
  }
  sAB = sAB/(N*N);

  plhs[0] = mxCreateDoubleMatrix(1, 1, mxREAL);
  double *Out;
  Out = mxGetPr(plhs[0]);
  MatrixXd::Map(Out, 1, 1) << (2*mA*mB+0.001)*(2*sAB+0.009)/((mB*mB+mA*mA+0.001)*(sB+sA+0.009));
  return;

}

//EOF


//
// Created by jachu on 05.02.18.
//

#include <iostream>

#include <Eigen/Dense>

#include <EKFPlane.hpp>

#include "Types.hpp"
#include "Misc.hpp"
#include "EKFPlane.hpp"

using namespace std;

EKFPlane::EKFPlane() {}

EKFPlane::EKFPlane(const Eigen::Quaterniond &xq, const Eigen::Matrix4d &Pq) {
    init(xq, Pq);
}

EKFPlane::EKFPlane(const Eigen::Quaterniond &xq, int npts) {
    init(xq, npts);
}

void EKFPlane::init(const Eigen::Quaterniond &xq, const Eigen::Matrix4d &Pq) {
    x = xq;
    Eigen::MatrixXd J = jacob_dom_dq(xq);
    P = J * Pq * J.transpose();
}

void EKFPlane::init(const Eigen::Quaterniond &xq, int npts) {
    x = xq;
    EKFPlane::npts = npts;
}

void EKFPlane::update(const Eigen::Quaterniond &zq, const Eigen::Matrix4d &Rq) {
    // jacobian of transformation from quaternion to log-map of quaternion
    Eigen::MatrixXd J_dom_dq = jacob_dom_dq(zq);
    // covariance in log-map representation
    Eigen::Matrix3d R = J_dom_dq * Rq * J_dom_dq.transpose();

    update(zq, R);
}

void EKFPlane::update(const Eigen::Quaterniond &zq, const Eigen::Matrix3d &R) {
//    cout << endl << "x = " << x.coeffs().transpose() << endl;
//    cout << "P = " << P << endl;
//    cout << "zq = " << zq.coeffs().transpose() << endl;
//    cout << "R = " << R << endl;
    // innovation
    Eigen::Vector3d v = Misc::logMap(zq * x.inverse());
//    cout << "v = " << v.transpose() << endl;
    // innovation covariance
    Eigen::Matrix3d S = P + R;
//    cout << "S = " << S << endl;
    // Kalman gain
    Eigen::Matrix3d K = P * S.inverse();
//    {
//        Eigen::EigenSolver<Eigen::Matrix3d> evd(R);
//
//        Eigen::Matrix3d evecs;
//        Eigen::Vector3d evals;
//        for(int i = 0; i < 3; ++i){
//            evecs.col(i) = evd.eigenvectors().col(2 - i).real();
//            evals(i) = evd.eigenvalues()(2 - i).real();
//        }
////        if(evals(0) > 1.0){
//            cout << endl << "evecs = " << evecs << endl;
//            cout << "evals = " << evals.transpose() << endl;
//
//            cout  << "x = " << x.coeffs().transpose() << endl;
//            cout  << "log(x) = " << Misc::logMap(x).transpose() << endl;
//            cout << "zq = " << zq.coeffs().transpose() << endl;
//            cout  << "log(zq) = " << Misc::logMap(zq).transpose() << endl;
//            cout << "v = " << v.transpose() << endl;
//            cout << "updated x = " << (Misc::expMap(K * v) * x).coeffs().transpose() << endl;
//            cout  << "log(updated x) = " << Misc::logMap(Misc::expMap(K * v) * x).transpose() << endl;
//            cout << "P = " << P << endl;
//            cout << "R = " << R << endl;
//            cout << "S = " << S << endl;
//            cout << "S.inverse() = " << S.inverse() << endl;
//            cout << "K = " << K << endl;
//            cout << "K * v = " << (K * v).transpose() << endl;
//
////            char a;
////            cin >> a;
////        }
//    }
//    cout << "K = " << K << endl;
    // update of state
//    cout << "K * v = " << K * v << endl;
    x = Misc::expMap(K * v) * x;
//    cout << "updated x = " << x.coeffs().transpose() << endl;
    // update of covariance
    P = (Eigen::Matrix3d::Identity() - K) * P;
//    cout << "updated P = " << P << endl;
}

//void EKFPlane::transform(const Eigen::Matrix4d T)
//{
//    Eigen::Matrix4d Tinv = T.inverse();
//    Eigen::Matrix4d Tinvt = Tinv.transpose();
//
//    Eigen::Vector4d planeEq = Tinvt * x.coeffs();
//    Eigen::Matrix4d covarQuat = Tinvt * covarQuat * Tinv;
//}

void EKFPlane::update(const Eigen::Quaterniond &zq, int znpts) {
//    cout << "updating" << endl;
    // innovation
    Eigen::Vector3d v = Misc::logMap(zq * x.inverse());
    x = Misc::expMap((double)znpts/(znpts + npts) * v) * x;

//    Eigen::Vector3d meanLogMap;
//    meanLogMap << 0.0, 0.0, 0.0;
//    int sumPoints = 0;
//    {
//        Eigen::Vector3d z = Misc::logMap(zq);
//        meanLogMap += z * znpts;
//        sumPoints += znpts;
//        cout << "z = " << zq.coeffs().transpose() << endl;
//        cout << "logMap(z) = " << z.transpose() << endl;
//    }
//    {
//        Eigen::Vector3d xu = Misc::logMap(x);
//        meanLogMap += xu * npts;
//        sumPoints += npts;
//        cout << "x = " << x.coeffs().transpose() << endl;
//        cout << "logMap(x) = " << xu.transpose() << endl;
//    }
//    meanLogMap /= sumPoints;
//
//    cout << "meanLogMap = " << meanLogMap.transpose() << endl;

    npts += znpts;
}

double EKFPlane::distance(const Eigen::Quaterniond &xcq) const {
//    Eigen::Matrix3d inf = P.inverse();
//    cout << "P = " << P << endl;
//    cout << "inf = " << inf << endl;
//    Eigen::Vector3d e = Misc::logMap(xcq * x.inverse());
//    cout << "x = " << x.coeffs().transpose() << endl;
//    cout << "xcq = " << xcq.coeffs().transpose() << endl;
//    cout << "diff = " << (xcq * x.inverse()).coeffs().transpose() << endl;

//    return e.transpose() * inf * e;
//    return e.transpose() * e;

    Eigen::Vector4d plEq = x.coeffs();
    plEq /= plEq.head<3>().norm();
    if(plEq(3) < 0){
        plEq = -plEq;
    }
    Eigen::Vector4d plEqc = xcq.coeffs();
    plEqc /= plEqc.head<3>().norm();
    if(plEqc(3) < 0){
        plEqc = -plEqc;
    }
    double dd = plEq(3) - plEqc(3);
    dd = dd * dd;
    double da = acos(plEq.head<3>().dot(plEqc.head<3>()));
    da = da * da;

    double drange = 4;
    double arange = M_PI;

    return (dd * arange + da * drange)/(drange + arange);
}

void EKFPlane::compPlaneEqAndCovar(const Eigen::MatrixXd &pts,
                                   Eigen::Quaterniond &q,
                                   Eigen::Matrix4d &R)
{
//    // Compute mean
//    mean_ = Eigen::Vector4f::Zero ();
//    compute3DCentroid (*input_, *indices_, mean_);
//    // Compute demeanished cloud
//    Eigen::MatrixXf cloud_demean;
//    demeanPointCloud (*input_, *indices_, mean_, cloud_demean);
//    assert (cloud_demean.cols () == int (indices_->size ()));
//    // Compute the product cloud_demean * cloud_demean^T
//    Eigen::Matrix3f alpha = static_cast<Eigen::Matrix3f> (cloud_demean.topRows<3> () * cloud_demean.topRows<3> ().transpose ());
//
//    // Compute eigen vectors and values
//    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> evd (alpha);
//    // Organize eigenvectors and eigenvalues in ascendent order
//    for (int i = 0; i < 3; ++i)
//    {
//        eigenvalues_[i] = evd.eigenvalues () [2-i];
//        eigenvectors_.col (i) = evd.eigenvectors ().col (2-i);
//    }
    Eigen::Vector3d mean = Eigen::Vector3d::Zero();
    for(int i = 0; i < pts.cols(); ++i){
        mean += pts.col(i);
    }
    mean /= pts.cols();

    Eigen::MatrixXd demeanPts = pts;
    for(int i = 0; i < demeanPts.cols(); ++i){
        demeanPts.col(i) -= mean;
    }

    Eigen::Matrix3d covar = demeanPts * demeanPts.transpose();
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> evd(covar);

    Eigen::Matrix3d evecs;
    Eigen::Vector3d evals;
    for(int i = 0; i < 3; ++i){
        evecs.col(i) = evd.eigenvectors().col(2 - i);
        evals(i) = evd.eigenvalues()(2 - i);
    }
    cout << "evals = " << (evals.array()/demeanPts.cols()).sqrt() << endl;

    // the smallest eigenvalue corresponds to the eigenvector that is normal to the plane
    double varD = evals(2) / demeanPts.cols();
    double varX = evals(2) / evals(0);
    double varY = evals(2) / evals(1);

    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.block<3, 3>(0, 0) = evecs;
    T.block<3, 1>(0, 3) = mean;
    Eigen::Matrix4d Tinv = T.inverse();
    Eigen::Matrix4d Tinvt = Tinv.transpose();

    // plane with normal (0, 0, 1) and distance 0;
    Eigen::Vector4d planeEq;
    planeEq << 0.0, 0.0, 1.0, 0.0;
    // transform it to destination pose
    planeEq = Tinvt * planeEq;

    Eigen::Matrix4d covarQuat = Eigen::Matrix4d::Zero();
    covarQuat(0, 0) = varX;
    covarQuat(1, 1) = varY;
    covarQuat(2, 2) = 0;
    covarQuat(3, 3) = varD;
    covarQuat = Tinvt * covarQuat * Tinv;

//    Eigen::Matrix4d J_dqn_dq = jacob_dqn_dq(Eigen::Quaterniond(planeEq(3), planeEq(0), planeEq(1), planeEq(2)));
//    planeEq.normalize();
//    R = J_dqn_dq * covarQuat * J_dqn_dq.transpose();

    double planeEqNorm = planeEq.norm();
    planeEq /= planeEqNorm;
    R = covarQuat / (planeEqNorm * planeEqNorm);

    q.coeffs() = planeEq;
}

//[ (2*acos(qw)*(qy^2 + qz^2))/(qx^2 + qy^2 + qz^2)^(3/2),        -(2*qx*qy*acos(qw))/(qx^2 + qy^2 + qz^2)^(3/2),        -(2*qx*qz*acos(qw))/(qx^2 + qy^2 + qz^2)^(3/2), -(2*qx)/((1 - qw^2)^(1/2)*(qx^2 + qy^2 + qz^2)^(1/2))]
//[        -(2*qx*qy*acos(qw))/(qx^2 + qy^2 + qz^2)^(3/2), (2*acos(qw)*(qx^2 + qz^2))/(qx^2 + qy^2 + qz^2)^(3/2),        -(2*qy*qz*acos(qw))/(qx^2 + qy^2 + qz^2)^(3/2), -(2*qy)/((1 - qw^2)^(1/2)*(qx^2 + qy^2 + qz^2)^(1/2))]
//[        -(2*qx*qz*acos(qw))/(qx^2 + qy^2 + qz^2)^(3/2),        -(2*qy*qz*acos(qw))/(qx^2 + qy^2 + qz^2)^(3/2), (2*acos(qw)*(qx^2 + qy^2))/(qx^2 + qy^2 + qz^2)^(3/2), -(2*qz)/((1 - qw^2)^(1/2)*(qx^2 + qy^2 + qz^2)^(1/2))]

Eigen::MatrixXd EKFPlane::jacob_dom_dq(const Eigen::Quaterniond &q) {
    Eigen::MatrixXd J(3, 4);
    double qx = q.x();
    double qy = q.y();
    double qz = q.z();
    double qw = q.w();
    double sqVecNorm = qx*qx + qy*qy + qz*qz;
    double vecNorm = sqrt(sqVecNorm);
    double den = sqVecNorm * vecNorm;
    J << (2*acos(qw)*(qy*qy + qz*qz))/den,          -(2*qx*qy*acos(qw))/den,          -(2*qx*qz*acos(qw))/den, -(2*qx)/(sqrt(1 - qw*qw)*vecNorm),
        -(2*qx*qy*acos(qw))/den,           (2*acos(qw)*(qx*qx + qz*qz))/den,          -(2*qy*qz*acos(qw))/den, -(2*qy)/(sqrt(1 - qw*qw)*vecNorm),
        -(2*qx*qz*acos(qw))/den,                    -(2*qy*qz*acos(qw))/den, (2*acos(qw)*(qx*qx + qy*qy))/den, -(2*qz)/(sqrt(1 - qw*qw)*vecNorm);

    return J;
}

//[ (qw^2 + qy^2 + qz^2)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qx*qy)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qx*qz)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qw*qx)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2)]
//[             -(qx*qy)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2), (qw^2 + qx^2 + qz^2)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qy*qz)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qw*qy)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2)]
//[             -(qx*qz)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qy*qz)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2), (qw^2 + qx^2 + qy^2)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qw*qz)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2)]
//[             -(qw*qx)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qw*qy)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2),             -(qw*qz)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2), (qx^2 + qy^2 + qz^2)/(qw^2 + qx^2 + qy^2 + qz^2)^(3/2)]
Eigen::MatrixXd EKFPlane::jacob_dqn_dq(const Eigen::Quaterniond &q) {
    Eigen::MatrixXd J(4, 4);
    double qx = q.x();
    double qy = q.y();
    double qz = q.z();
    double qw = q.w();
    double sqVecNorm = qx*qx + qy*qy + qz*qz + qw*qw;
    double vecNorm = sqrt(sqVecNorm);
    double den = sqVecNorm * vecNorm;

    J << (qw*qw + qy*qy + qz*qz)/den,                -(qx*qy)/den,                -(qx*qz)/den,                -(qw*qx)/den,
                        -(qx*qy)/den, (qw*qw + qx*qx + qz*qz)/den,                -(qy*qz)/den,                -(qw*qy)/den,
                        -(qx*qz)/den,                -(qy*qz)/den, (qw*qw + qx*qx + qy*qy)/den,                -(qw*qz)/den,
                        -(qw*qx)/den,                -(qw*qy)/den,                -(qw*qz)/den, (qx*qx + qy*qy + qz*qz)/den;

    return J;
}

const Eigen::Quaterniond &EKFPlane::getX() const {
    return x;
}

const Eigen::Matrix3d &EKFPlane::getP() const {
    return P;
}


#include <array>                    // for std::array
#include <chrono>                   // for std::chrono
#include <iostream>                 // for std::cout
#include <boost/format.hpp>         // for boost::format
#include <boost/numeric/odeint.hpp> // for boost::numeric::odeint

using state_type = std::array<double, 2>;

void rhs(state_type const & y, state_type & dydx, double const x);

int main()
{
    using namespace std::chrono;
    using namespace boost::numeric::odeint;
    using stepper_type = boost::numeric::odeint::bulirsch_stoer<state_type>;

    auto const start = system_clock::now();

    auto const x1 = 0.00001;
    auto const xf = 11.0;
    auto const dx = 0.001;

    state_type y1 = { 0.99998416139571, -1.58175227379914 };

    integrate_const(
        stepper_type(1.0E-15, 1.0E-15),
        [](state_type const & y, state_type & dydx, double const x)
        {
	        dydx[0] = y[1];
	        dydx[1] = y[0] * std::sqrt(y[0] / x);
        },
        y1,
        x1,
        xf,
        dx);
    auto const end = system_clock::now();

    std::cout << boost::format("y[1] = %.14f, y[2] = %.14f\n") % y1[0] % y1[1];
    std::cout << boost::format("計算時間 = %.14f（秒）\n") % duration_cast< duration<double> >(end - start).count();
}


/*
 *  callbond_test.cpp
 *  bondgeek
 *
 *  Created by BART MOSLEY on 6/3/12.
 *  Copyright 2012 BG Research LLC. All rights reserved.
 *
 */

#include <bg/bondgeek.hpp>

#include <iostream>
#include <algorithm>

#include <boost/timer.hpp>

using namespace std;
using namespace QuantLib;
using namespace bondgeek;

#if defined(QL_ENABLE_SESSIONS)
namespace QuantLib {
    Integer sessionId() { return 0; }
}
#endif

boost::shared_ptr<YieldTermStructure>
flatRate(const Date& today,
         const boost::shared_ptr<Quote>& forward,
         const DayCounter& dc,
         const Compounding& compounding,
         const Frequency& frequency) {
    return boost::shared_ptr<YieldTermStructure>(
                                                 new FlatForward(today,
                                                                 Handle<Quote>(forward),
                                                                 dc,
                                                                 compounding,
                                                                 frequency));
}

boost::shared_ptr<YieldTermStructure>
flatRate(const Date& today,
         Rate forward,
         const DayCounter& dc,
         const Compounding &compounding,
         const Frequency &frequency) {
    return flatRate(today,
                    boost::shared_ptr<Quote>(new SimpleQuote(forward)),
                    dc,
                    compounding,
                    frequency);
}

int main (int argc, char * const argv[])
{

    Date today = Date(16,October,2007);
    Settings::instance().evaluationDate() = today;

    cout <<  endl;
    cout << "Pricing a callable fixed rate bond using" << endl;
    cout << "Hull White model w/ reversion parameter = 0.03" << endl;
    cout << "BAC4.65 09/15/12  ISIN: US06060WBJ36" << endl;
    cout << "roughly five year tenor, ";
    cout << "quarterly coupon and call dates" << endl;
    cout << "reference date is : " << today << endl;
    cout << "3 day settle: " << TARGET().advance(today, 3, Days) << endl << endl;

    /* Bloomberg OAS1: "N" model (Hull White)
     varying volatility parameter

     The curve entered into Bloomberg OAS1 is a flat curve,
     at constant yield = 5.5%, semiannual compounding.
     Assume here OAS1 curve uses an ACT/ACT day counter,
     as documented in PFC1 as a "default" in the latter case.
     */

    // set up a flat curve corresponding to Bloomberg flat curve

    Rate bbCurveRate = 0.055;
    DayCounter bbDayCounter = ActualActual(ActualActual::Bond);
    InterestRate bbIR(bbCurveRate,bbDayCounter,Compounded,Semiannual);

    Handle<YieldTermStructure> termStructure(flatRate(today,
                                                      bbIR.rate(),
                                                      bbIR.dayCounter(),
                                                      bbIR.compounding(),
                                                      bbIR.frequency()));

    // set up the call schedule

    CallabilitySchedule callSchedule;
    Real callPrice = 100.;
    Size numberOfCallDates = 19;
    Date callDate = Date(15, December, 2007);

    Date sDate(callDate.serialNumber());
    for (Size i=0; i< numberOfCallDates; i++) {
        Calendar nullCalendar = NullCalendar();

        Callability::Price myPrice(callPrice,
                                   Callability::Price::Clean);
        callSchedule.push_back(
                               boost::shared_ptr<Callability>(
                                                              new Callability(myPrice,
                                                                              Callability::Call,
                                                                              sDate )));
        sDate = nullCalendar.advance(sDate, 3, Months);
    }

    cout << "calls: " << callDate << " to " << sDate << endl;

    // set up the callable bond

    Date dated = Date(16,September,2004);
    Date issue = dated;
    Date maturity = Date(15,September,2012);
    Natural settlementDays = 3;  // Bloomberg OAS1 settle is Oct 19, 2007
    Calendar bondCalendar = UnitedStates(UnitedStates::GovernmentBond);

    //Real coupon = .0465;
    Real coupon = .06;
    Frequency frequency = Quarterly;
    Real redemption = 100.0;
    Real faceAmount = 100.0;

    /* The 30/360 day counter Bloomberg uses for this bond cannot
     reproduce the US Bond/ISMA (constant) cashflows used in PFC1.
     Therefore use ActAct(Bond)
     */
    DayCounter bondDayCounter = ActualActual(ActualActual::Bond);

    // PFC1 shows no indication dates are being adjusted
    // for weekends/holidays for vanilla bonds
    BusinessDayConvention accrualConvention = Unadjusted;
    BusinessDayConvention paymentConvention = Unadjusted;

    Schedule sch(dated, maturity, Period(frequency), bondCalendar,
                 accrualConvention, accrualConvention,
                 DateGeneration::Backward, false);

    Size maxIterations = 1000;
    Real accuracy = 1e-8;
    Integer gridIntervals = 60;
    Real reversionParameter = .03;

    // output price/yield results for varying volatility parameter

    Real sigma = max(0.03, QL_EPSILON); // core dumps if zero on Cygwin

    boost::shared_ptr<ShortRateModel> hw0(
                                          new HullWhite(termStructure,reversionParameter,sigma));

    boost::shared_ptr<PricingEngine> engine0(
                                             new TreeCallableFixedRateBondEngine(hw0,gridIntervals));

    CallableFixedRateBond callableBond(settlementDays, faceAmount, sch,
                                       vector<Rate>(1, coupon),
                                       bondDayCounter, paymentConvention,
                                       redemption, issue, callSchedule);
    callableBond.setPricingEngine(engine0);

    cout << setprecision(5)
    << "Flat Rate: " << bbCurveRate << " | disc factor: " << 100.0*termStructure.currentLink()->discount(10.0) << endl;

    cout << setprecision(2)
    << showpoint
    << fixed
    << "sigma/vol (%) = "
    << 100.*sigma
    << ", mean reversion = " << reversionParameter
    << endl;

    cout << "\nQuantLib price/yld (%)  ";
    cout << callableBond.cleanPrice() << " / "
    << 100. * callableBond.yield(bondDayCounter,
                                 Compounded,
                                 frequency,
                                 accuracy,
                                 maxIterations)
    << endl;

    cout << "\nNow price with a bg curve " << endl;
    string depotenors[] = {"1W", "1M", "3M", "6M", "9M", "1y"};
    double depospots[] = {.055, .055, .055, .055, .055, .055};
    string swaptenors[] = {"2y", "3y", "5y", "10y", "15y", "20y", "30y"};
    double swapspots[] = {.055, .055, .055, .055, .055, .055, .055};

    cout << "Test with new curve " << endl;
    RateHelperCurve acurve = RateHelperCurve(USDLiborCurve("3M"));
    acurve.update(depotenors,
                  depospots,
                  6,
                  swaptenors,
                  swapspots,
                  7,
                  today);

    cout << "Curve: "
    << acurve.discountingTermStructure().currentLink()->referenceDate() << "/"
    << acurve.discountingTermStructure().currentLink()->maxDate() << endl;

    cout << setprecision(3)
    << "quote:  " << io::rate(acurve.tenorquote("10Y")) ;

    cout << setprecision(5)
    << " | disc factor: " << 100.0*acurve.discountingTermStructure().currentLink()->discount(10.0) << endl;

    boost::shared_ptr<ShortRateModel> hw1(
                                          new HullWhite(acurve.discountingTermStructure(),
                                                        reversionParameter,
                                                        sigma));

    boost::shared_ptr<PricingEngine> engine1(
                                             new TreeCallableFixedRateBondEngine(hw1,
                                                                                gridIntervals));

    callableBond.setPricingEngine(engine1);

    cout << "price/yld (%)  ";
    cout << callableBond.cleanPrice() << " / "
    << 100. * callableBond.yield(bondDayCounter,
                                 Compounded,
                                 frequency,
                                 accuracy,
                                 maxIterations)
    << endl;

    cout << "\nbondgeek::CallBond\n";
    cout << setprecision(3)
    << "cpn: " << io::rate(coupon)
    << " mty: " << maturity
    << " call: " << callDate
    << " @ " << callPrice
    << endl;

    CallBond noncallbond(coupon,
                         maturity,
                         dated,
                         bondCalendar,
                         settlementDays,
                         bondDayCounter,
                         frequency,
                         redemption,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );

    CallBond zeronc_bond(0.0,
                         Date(15, February, 2023),
                         Date(29, June, 1995),
                         bondCalendar,
                         settlementDays,
                         Thirty360(Thirty360::BondBasis),
                         Semiannual,
                         redemption,
                         faceAmount,
                         accrualConvention,
                         paymentConvention
                         );

    BulletBond bulletbond(coupon,
                          maturity,
                          dated,
                          bondCalendar,
                          settlementDays,
                          bondDayCounter,
                          frequency,
                          redemption,
                          faceAmount,
                          accrualConvention,
                          paymentConvention
                          );

    CallBond callbnd(coupon,
                     maturity,
                     callDate,
                     callPrice,
                     dated,
                     bondCalendar,
                     settlementDays,
                     bondDayCounter,
                     frequency,
                     frequency,
                     redemption,
                     faceAmount,
                     accrualConvention,
                     paymentConvention);

    cout << "Call Schedule" << endl;
    CallabilitySchedule  callbndsched1 = callableBond.callability();
    CallabilitySchedule  callbndsched2 = callbnd.callability();
    CallabilitySchedule  callbndsched3 = noncallbond.callability();

    vector< boost::shared_ptr<Callability> >::size_type sz1 = callbndsched1.size();
    vector< boost::shared_ptr<Callability> >::size_type sz2 = callbndsched2.size();
    vector< boost::shared_ptr<Callability> >::size_type sz3 = callbndsched3.size();

    cout << "Call sizes: " << endl <<
    "1) " << sz1 << endl <<
    "2) " << sz2 << endl <<
    "3) " << sz3 << endl;

    if (sz1 != sz2) {
        cout << "\nCall schedules not equal!!! " << sz1 << " vs " << sz2 << endl;
        for (int i=0; i<sz1; i++) {
            cout << callbndsched1[i]->date() << " | " << callbndsched1[i]->price().amount() << endl;
        }
        for (int i=0; i<sz2; i++) {
            cout << callbndsched2[i]->date() << " | " << callbndsched2[i]->price().amount() << endl;
        }
        return 1;
    }

    callbnd.setPricingEngine(engine1);
    noncallbond.setPricingEngine(engine1);
    bulletbond.setPricingEngine(engine1);
    cout << "test value: " << callbnd.cleanPrice() << endl << endl;
    cout << "test value (noncall): " << noncallbond.cleanPrice() << endl << endl;
    cout << "test value (bullet): " << bulletbond.cleanPrice() << endl << endl;
    cout << "test value (bullet, toPrice): " << bulletbond.toPrice() << endl << endl;

    cout << "\n\nMuni Schedules " << endl;
    CallabilitySchedule muniSched = createBondCallSchedule(Date(15, December, 2007),
                                                           102.,
                                                           Date(15, December, 2009),
                                                           maturity
                                                           );

    CallBond callbnd_sched(coupon,
                           maturity,
                           muniSched,
                           dated,
                           bondCalendar,
                           settlementDays,
                           bondDayCounter,
                           frequency,
                           redemption,
                           faceAmount,
                           accrualConvention,
                           paymentConvention
                           );

    CallBond callbnd_sched1(coupon,
                           maturity,
                           Date(15, December, 2007),
                           102.,
                           Date(15, December, 2009),
                           dated,
                           bondCalendar,
                           settlementDays,
                           bondDayCounter,
                           frequency,
                           Annual,
                           redemption,
                           faceAmount,
                           accrualConvention,
                           paymentConvention
                           );

    CallabilitySchedule muniSched1 = callbnd_sched1.callability();
    vector< boost::shared_ptr<Callability> >::size_type sz_m = muniSched1.size();

    for (int i=0; i<sz_m; i++)
    {
        cout << muniSched1[i]->date() << " | " << muniSched1[i]->price().amount() << endl;
    }

    callbnd_sched.setPricingEngine(engine1);
    callbnd_sched1.setPricingEngine(engine1);

    cout << "test value: " << callbnd_sched.toPrice() << " | " << callbnd_sched1.toPrice() << endl;
    cout << "yield to worst: " << callbnd_sched.toYield() << " | " << callbnd_sched1.toYield() << endl;
    cout << "price from ytw: " << callbnd_sched.toPrice(callbnd_sched.toYield())
    << " | " << callbnd_sched1.toPrice(callbnd_sched1.toYield()) <<
    endl << endl;

    //TODO: create vol/mean reversion matrix
    cout << "\nHull-White (normal) pricing " << endl;
    Real sig[3] = {0.0, .01, .03};

    cout << "mean reversion = " << reversionParameter << endl;

    for (int i=0; i<3; i++) {
        callbnd.setEngine(acurve, reversionParameter, sig[i], false);
        zeronc_bond.setEngine(acurve, reversionParameter, sig[i], false);
        noncallbond.setEngine(acurve, reversionParameter, sig[i], false);
        bulletbond.setEngine(acurve);

        cout << setprecision(3) << "sigma: " << io::rate(sig[i]);
        cout << setprecision(5) << " | value: " << callbnd.cleanPrice() ;
        cout << " | nc1: " << noncallbond.cleanPrice();
        cout << " bullet: " << bulletbond.cleanPrice();
        cout << " zero: " << zeronc_bond.cleanPrice() << "/"
        << 100. * zeronc_bond.yield(Thirty360(Thirty360::BondBasis),
                                    Compounded,
                                    Semiannual,
                                    accuracy,
                                    maxIterations);
        cout << endl;
    }

    cout << "\nLognormal pricing " << endl;

    Real mr = 0.0;
    Real v[3] = {0.0, .2017, .6763};

    cout << "mean reversion = " << mr
    << endl;

    callbnd.oasEngine(acurve, mr, v[0], 0.0);

    for (int i=0; i<3; i++) {
        noncallbond.setEngine(acurve, mr, v[i]);
        bulletbond.setEngine(acurve);

        cout << setprecision(3) << "vol: " << io::rate(v[i]);
        cout << setprecision(5) << " | value: " << callbnd.oasValue(0.0, v[i], mr);
        cout << " nc1: " << noncallbond.cleanPrice();
        cout << " bullet: " << bulletbond.cleanPrice();

    }

    Real testPx[] = {80., 85., 90., 95., 100., 100.25};

    cout << " vol: " << v[1] << endl << endl;

    boost::timer timer;

    for (int i=0; i<6; i++) {
        Real oasx = callbnd.oasT<Secant>(testPx[i], v[1]);

        cout << fixed << setprecision(5)
        << "OAS: " << oasx ;
        cout << " > value: " << callbnd.oasValue(oasx) << endl;

    }
    Real t1 = timer.elapsed();
    cout << fixed << setprecision(3)
    << t1 << " s\n" << endl;
    Real t0=t1;

    for (int i=0; i<6; i++) {
        Real oasx = callbnd.oasT<Brent>(testPx[i], v[1]);

        cout << fixed << setprecision(5)
        << "OAS: " << oasx ;
        cout << " > value: " << callbnd.oasValue(oasx) << endl;

    }

    t1  = timer.elapsed();
    cout << fixed << setprecision(3)
    << t1-t0 << " s\n" << endl;
    t0=t1;

    Real testspread = 0.01543;
    Real xvol = callbnd.oasImpliedVol(90., testspread);

    cout << "\nImpVol " << xvol
    << " value " << callbnd.oasValue(testspread, xvol, 0.0, true)
    << endl ;

    t1  = timer.elapsed();
    cout << fixed << setprecision(3)
    << t1-t0 << " s\n" << endl;

    return 0;
}


#ifndef DISTRIBUTION_HPP
#define DISTRIBUTION_HPP

#include <assert.h>
#include <memory>
#include <mutex>
#include <Eigen/Core>
#include <Eigen/Eigen>
#include <iostream>

namespace jaco2_utils {
namespace math {
namespace statistic {
template<bool limit_covariance = false>
class Distribution {
public:
    typedef std::shared_ptr<Distribution<limit_covariance>> Ptr;

    typedef Eigen::VectorXd       PointType;
    typedef Eigen::MatrixXd       MatrixType;
    typedef Eigen::MatrixXd       EigenValueSetType;
    typedef Eigen::MatrixXd       EigenVectorSetType;
    typedef Eigen::VectorXcd      ComplexVectorType;
    typedef Eigen::MatrixXcd      ComplexMatrixType;

    static constexpr double sqrt_2_M_PI = std::sqrt(2 * M_PI);
    static constexpr double lambda_ratio = 1e-2;

    Distribution(std::size_t dim = 1) :
        dim_(dim),
        mean(PointType::Zero(dim_)),
        correlated(MatrixType::Zero(dim_, dim_)),
        n(1),
        n_1(0),
        covariance(MatrixType::Zero(dim_, dim_)),
        inverse_covariance(MatrixType::Zero(dim_, dim_)),
        eigen_values(EigenValueSetType::Zero(dim_, dim_)),
        eigen_vectors(EigenVectorSetType::Zero(dim_, dim_)),
        determinant(0.0),
        dirty(false),
        dirty_eigen(false)
    {
    }

    Distribution(const Distribution &other) = default;
    Distribution& operator=(const Distribution &other) = default;

    inline void reset()
    {
        mean = PointType::Zero(dim_);
        covariance = MatrixType::Zero(dim_, dim_);
        correlated = MatrixType::Zero(dim_, dim_);
        n = 1;
        n_1 = 0;
        dirty = true;
        dirty_eigen = true;
    }

    /// Modification
    inline void add(const PointType &_p)
    {
        mean = (mean * n_1 + _p) / n;
        for(std::size_t i = 0 ; i < dim_ ; ++i) {
            for(std::size_t j = i ; j < dim_ ; ++j) {
                correlated(i, j) = (correlated(i, j) * n_1 + _p(i) * _p(j)) / (double) n;
            }
        }
        ++n;
        ++n_1;
        dirty = true;
        dirty_eigen = true;
    }

    inline Distribution& operator+=(const PointType &_p)
    {
        add(_p);
        return *this;
    }

    inline Distribution& operator+=(const Distribution &other)
    {
        std::size_t _n = n_1 + other.n_1;
        PointType   _mean = (mean * n_1 + other.mean * other.n_1) / (double) _n;
        MatrixType  _corr = (correlated * n_1 + other.correlated * other.n_1) / (double) _n;
        n   = _n + 1;
        n_1 = _n;
        mean = _mean;
        correlated = _corr;
        dirty = true;
        dirty_eigen = true;
        return *this;
    }

    /// Distribution properties
    inline std::size_t getN() const
    {
        return n_1;
    }

    inline PointType getMean() const
    {
        return mean;
    }

    inline void getMean(PointType &_mean) const
    {
        _mean = mean;
    }

    inline MatrixType getCovariance() const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            return covariance;
        }
        return MatrixType::Zero(dim_, dim_);
    }

    inline void getCovariance(MatrixType &_covariance) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            _covariance = covariance;
        } else {
            _covariance = MatrixType::Zero();
        }
    }

    inline MatrixType getInformationMatrix() const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            return inverse_covariance;
        }
        return MatrixType::Zero(dim_, dim_);
    }

    inline void getInformationMatrix(MatrixType &_inverse_covariance) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            _inverse_covariance = inverse_covariance;
        } else {
            _inverse_covariance = MatrixType::Zero(dim_, dim_);
        }
    }

    inline EigenValueSetType getEigenValues(const bool _abs = false) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            if(dirty_eigen)
                updateEigen();

            if(_abs)
                return eigen_values.cwiseAbs();
            else
                return eigen_values;
        }
        return EigenValueSetType::Zero(dim_, dim_);
    }

    inline void getEigenValues(EigenValueSetType &_eigen_values,
                               const double _abs = false) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            if(dirty_eigen)
                updateEigen();

            if(_abs)
                _eigen_values = eigen_values.cwiseAbs();
            else
                _eigen_values = eigen_values;
        } else {
            _eigen_values = EigenValueSetType::Zero(dim_, dim_);
        }
    }

    inline EigenVectorSetType getEigenVectors() const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            if(dirty_eigen)
                updateEigen();

            return eigen_vectors;
        }
        return EigenVectorSetType::Zero();
    }

    inline void getEigenVectors(EigenVectorSetType &_eigen_vectors) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            if(dirty_eigen)
                updateEigen();

            _eigen_vectors = eigen_vectors;
        } else {
            _eigen_vectors = EigenVectorSetType::Zero(dim_, dim_);
        }
    }

    /// Evaluation
    inline double sample(const PointType &_p) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            PointType  q = _p - mean;
            double exponent = -0.5 * double(q.transpose() * inverse_covariance * q);
            double denominator = 1.0 / (covariance.determinant() * sqrt_2_M_PI);
            return denominator * exp(exponent);
        }
        return 0.0;
    }

    inline double sample(const PointType &_p,
                         PointType &_q) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            _q = _p - mean;
            double exponent = -0.5 * double(_q.transpose() * inverse_covariance * _q);
            double denominator = 1.0 / (determinant * sqrt_2_M_PI);
            return denominator * exp(exponent);
        }
        return 0.0;
    }

    inline double sampleNonNormalized(const PointType &_p) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();

            PointType  q = _p - mean;
            double exponent = -0.5 * double(q.transpose() * inverse_covariance * q);
            return exp(exponent);
        }
        return 0.0;
    }

    inline double sampleNonNormalized(const PointType &_p,
                                      PointType &_q) const
    {
        if(n_1 >= 2) {
            if(dirty)
                update();
            _q = _p - mean;
            double exponent = -0.5 * double(_q.transpose() * inverse_covariance * _q);
            return exp(exponent);
        }
        return 0.0;
    }

private:
    const std::size_t            dim_;
    PointType                    mean;
    MatrixType                   correlated;
    std::size_t                  n;
    std::size_t                  n_1;            /// actual amount of points in distribution

    mutable MatrixType           covariance;
    mutable MatrixType           inverse_covariance;
    mutable EigenValueSetType    eigen_values;
    mutable EigenVectorSetType   eigen_vectors;
    mutable double               determinant;

    mutable bool                 dirty;
    mutable bool                 dirty_eigen;

    inline void update() const
    {
        double scale = n_1 / (double)(n_1 - 1);
        for(std::size_t i = 0 ; i < dim_ ; ++i) {
            for(std::size_t j = i ; j < dim_ ; ++j) {
                covariance(i, j) = (correlated(i, j) - (mean(i) * mean(j))) * scale;
                covariance(j, i) = covariance(i, j);
            }
        }

        if(limit_covariance) {
            if(dirty_eigen)
                updateEigen();

            double max_lambda = std::numeric_limits<double>::lowest();
            for(std::size_t i = 0 ; i < dim_ ; ++i) {
                if(eigen_values(i) > max_lambda)
                    max_lambda = eigen_values(i);
            }
            MatrixType Lambda = MatrixType::Zero(dim_, dim_);
            double l = max_lambda * lambda_ratio;
            for(std::size_t i = 0 ; i < dim_; ++i) {
                if(fabs(eigen_values(i)) < fabs(l)) {
                    Lambda(i,i) = l;
                } else {
                    Lambda(i,i) = eigen_values(i);
                }
            }
            covariance = eigen_vectors * Lambda * eigen_vectors.transpose();
            inverse_covariance = eigen_vectors * Lambda.inverse() * eigen_vectors.transpose();
        } else {
            inverse_covariance = covariance.inverse();
        }

        determinant = covariance.determinant();
        dirty = false;
        dirty_eigen = true;
    }

    inline void updateEigen() const
    {
        Eigen::EigenSolver<MatrixType> solver;
        solver.compute(covariance);
        eigen_vectors = solver.eigenvectors().real();
        eigen_values  = solver.eigenvalues().real();
        dirty_eigen = false;
    }
};
}
}
}

#endif /* DISTRIBUTION_HPP */


/*!
 * @file diffusion_problem.hpp
 * @brief Contains implementation of the main object.
 * @author Konrad Simon
 * @date August 2019
 */

#ifndef INCLUDE_DIFFUSION_PROBLEM_HPP_
#define INCLUDE_DIFFUSION_PROBLEM_HPP_

// Deal.ii
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/logstream.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>

#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

// STL
#include <cmath>
#include <fstream>
#include <iostream>

// My Headers
#include "matrix_coeff.hpp"
#include "right_hand_side.hpp"
#include "neumann_bc.hpp"
#include "dirichlet_bc.hpp"

/*!
 * @namespace DiffusionProblem
 * @brief Contains implementation of the main object
 * and all functions to solve a
 * Dirichlet-Neumann problem on a unit square.
 */
namespace DiffusionProblem
{
using namespace dealii;

/*!
 * @class DiffusionProblem
 * @brief Main class to solve
 * Dirichlet-Neumann problem on a unit square.
 */
template <int dim>
class DiffusionProblem
{
public:
	DiffusionProblem (unsigned int n_refine);
	void run ();

private:
	void make_grid ();
	void setup_system ();
	void assemble_system ();
	void solve_iterative ();
	void output_results () const;

	Triangulation<dim>   			triangulation;
	FE_Q<dim>            			fe;
	DoFHandler<dim>      			dof_handler;

	AffineConstraints<double> 		constraints;

	SparsityPattern      			sparsity_pattern;
	SparseMatrix<double> 			system_matrix;

	/*!
	 * Current solution. Needed forping.
	 */
	Vector<double>       			solution;

	/*!
	 * Contains all parts of the right-hand side needed to
	 * solve the linear system.
	 */
	Vector<double>       			system_rhs;

	/*!
	 * Number of initial grid refinements.
	 */
	unsigned int n_refine;
};

/*!
 * Default constructor.
 */
template <int dim>
DiffusionProblem<dim>::DiffusionProblem (unsigned int n_refine) :
  fe (1),
  dof_handler (triangulation),
  n_refine (n_refine)
{}

/*!
 * @brief Set up the grid with a certain number of refinements.
 *
 * Generate a triangulation of \f$[0,1]^{\rm{dim}}\f$ with edges/faces
 * numbered form \f$1,\dots,2\rm{dim}\f$.
 */
template <int dim>
void DiffusionProblem<dim>::make_grid ()
{
	GridGenerator::hyper_cube (triangulation, 0, 1, /* colorize */ true);

	triangulation.refine_global (n_refine);

	std::cout << "Number of active cells: "
			<< triangulation.n_active_cells()
			<< std::endl;
}

/*!
 * @brief Setup sparsity pattern and system matrix.
 *
 * Compute sparsity pattern and reserve memory for the sparse system matrix
 * and a number of right-hand side vectors. Also build a constraint object
 * to take care of Dirichlet boundary conditions.
 */
template <int dim>
void DiffusionProblem<dim>::setup_system ()
{
	dof_handler.distribute_dofs (fe);

	std::cout << "Number of active cells: " << triangulation.n_active_cells()
			<< std::endl
			<< "Number of degrees of freedom: " << dof_handler.n_dofs()
			<< std::endl
			<< std::endl;

	constraints.clear();
	DoFTools::make_hanging_node_constraints(dof_handler, constraints);

	/*
	 * Set up Dirichlet boundary conditions.
	 */
	const Coefficients::DirichletBC<dim> dirichlet_bc;
	for (unsigned int i = 0; i<dim; ++i)
	{
		VectorTools::interpolate_boundary_values(dof_handler,
													/*boundary id*/ 2*i, // only even boundary id
													dirichlet_bc,
													constraints);
	}

	constraints.close();

	DynamicSparsityPattern dsp(dof_handler.n_dofs());
	DoFTools::make_sparsity_pattern (dof_handler,
									dsp,
									constraints,
									/*keep_constrained_dofs =*/ true); // forping this is essential to be true

	sparsity_pattern.copy_from(dsp);

	system_matrix.reinit (sparsity_pattern);

	solution.reinit (dof_handler.n_dofs());
	system_rhs.reinit (dof_handler.n_dofs());
}

/*!
 * @brief Assemble the system matrix and the static right hand side.
 *
 * Assembly routine to build the time-independent (static)part.
 * Neumann boundary conditions will be put on edges/faces
 * with odd number. Constraints are not applied here yet.
 */
template <int dim>
void DiffusionProblem<dim>::assemble_system ()
{
	QGauss<dim>  quadrature_formula(fe.degree + 1);
	QGauss<dim - 1> face_quadrature_formula(fe.degree + 1);

	FEValues<dim> 	fe_values (fe, quadrature_formula,
								update_values    |  update_gradients |
								update_quadrature_points  |  update_JxW_values);

	FEFaceValues<dim> 	fe_face_values(fe,
										face_quadrature_formula,
										update_values | update_quadrature_points |
										update_normal_vectors |
										update_JxW_values);

	const unsigned int   	dofs_per_cell = fe.dofs_per_cell;
	const unsigned int   	n_q_points    = quadrature_formula.size();
	const unsigned int 	n_face_q_points = face_quadrature_formula.size();

	FullMatrix<double>   cell_matrix (dofs_per_cell, dofs_per_cell);
	Vector<double>       cell_rhs (dofs_per_cell);

	std::vector<types::global_dof_index> local_dof_indices (dofs_per_cell);

	/*
	 * Matrix coefficient and vector to store the values.
	 */
	const Coefficients::MatrixCoeff<dim> 		matrix_coeff;
	std::vector<Tensor<2,dim>> 	matrix_coeff_values(n_q_points);

	/*
	 * Right hand side and vector to store the values.
	 */
	const Coefficients::RightHandSide<dim> 	right_hand_side;
	std::vector<double>      	rhs_values(n_q_points);

	/*
	 * Neumann BCs and vector to store the values.
	 */
	const Coefficients::NeumannBC<dim> 	neumann_bc;
	std::vector<double>  	neumann_values(n_face_q_points);

	/*
	 * Integration over cells.
	 */
	for (const auto &cell: dof_handler.active_cell_iterators())
	{
		cell_matrix = 0;
		cell_rhs = 0;

		fe_values.reinit (cell);

		// Now actually fill with values.
		matrix_coeff.value_list(fe_values.get_quadrature_points (),
						  	  	  matrix_coeff_values);
		right_hand_side.value_list(fe_values.get_quadrature_points(),
									   rhs_values);

		for (unsigned int q_index=0; q_index<n_q_points; ++q_index)
		{
			for (unsigned int i=0; i<dofs_per_cell; ++i)
			{
				for (unsigned int j=0; j<dofs_per_cell; ++j)
				{

					cell_matrix(i,j) += fe_values.shape_grad(i,q_index) *
										 matrix_coeff_values[q_index] *
										 fe_values.shape_grad(j,q_index) *
										 fe_values.JxW(q_index);
				} // end ++j

				cell_rhs(i) += fe_values.shape_value(i,q_index) *
								   rhs_values[q_index] *
								   fe_values.JxW(q_index);
			} // end ++i
		} // end ++q_index

		/*
		 * Boundary integral for Neumann values for odd boundary_id.
		 */
		for (unsigned int face_number = 0;
			 face_number < GeometryInfo<dim>::faces_per_cell;
			 ++face_number)
		{
			if (cell->face(face_number)->at_boundary() &&
					(
						(cell->face(face_number)->boundary_id() == 1) ||
						(cell->face(face_number)->boundary_id() == 3) ||
						(cell->face(face_number)->boundary_id() == 5)
					)
				)
			{
				fe_face_values.reinit(cell, face_number);

				/*
				 * Fill in values at this particular face.
				 */
				neumann_bc.value_list(fe_face_values.get_quadrature_points(),
										   neumann_values);

				for (unsigned int q_face_point = 0; q_face_point < n_face_q_points; ++q_face_point)
				{
					for (unsigned int i = 0; i < dofs_per_cell; ++i)
					{
						cell_rhs(i) += neumann_values[q_face_point] // g(x_q)
										* fe_face_values.shape_value(i, q_face_point) // phi_i(x_q)
										* fe_face_values.JxW(q_face_point); // dS
					} // end ++i
				} // end ++q_face_point
			} // end if
		} // end ++face_number

		// get global indices
		cell->get_dof_indices (local_dof_indices);
		/*
		 * Now add the cell matrix and rhs to the right spots
		 * in the global matrix and global rhs. Constraints will
		 * be taken care of later.
		 */
		for (unsigned int i = 0; i < dofs_per_cell; ++i)
		{
			for (unsigned int j = 0; j < dofs_per_cell; ++j)
			{
				system_matrix.add(local_dof_indices[i],
							local_dof_indices[j],
							cell_matrix(i, j));
			}
			system_rhs(local_dof_indices[i]) += cell_rhs(i);
		}
	} // end ++cell
}

/*!
 * @brief Iterative solver.
 *
 * CG-based solver with SSOR-preconditioning.
 */
template <int dim>
void DiffusionProblem<dim>::solve_iterative ()
{
	SolverControl           solver_control (1000, 1e-12);
	SolverCG<>              solver (solver_control);

	PreconditionSSOR<> preconditioner;
	preconditioner.initialize(system_matrix, 1.2);

	solver.solve (system_matrix,
				solution,
				system_rhs,
				preconditioner);

	constraints.distribute (solution);

	std::cout << "   " << solver_control.last_step()
			<< " CG iterations needed to obtain convergence."
			<< std::endl;
}

/*!
 * @brief Write results to disk.
 *
 * Write results to disk in vtu-format.
 */
template <int dim>
void DiffusionProblem<dim>::output_results () const
{
	DataOut<dim> data_out;
	data_out.attach_dof_handler (dof_handler);
	data_out.add_data_vector (solution, "solution");
	data_out.build_patches ();

	std::string filename = (dim == 2 ?
								"solution-std_2d" :
								"solution-std_3d" );
	filename += "_refinements-" + Utilities::int_to_string(n_refine, 1)
				+ ".vtu";

	std::ofstream output (filename.c_str());
	data_out.write_vtu (output);
}

/*!
 * @brief Run function of the object.
 *
 * Run the computation after object is built. Implements theping loop.
 */
template <int dim>
void DiffusionProblem<dim>::run ()
{
	std::cout << std::endl
				<< "===========================================" << std::endl;
	std::cout << "Solving problem in " << dim << " space dimensions." << std::endl;

	make_grid ();

	setup_system ();

	assemble_system ();

	// Now solve
	constraints.condense(system_matrix, system_rhs);
	solve_iterative ();

	output_results ();

	std::cout << std::endl
			<< "===========================================" << std::endl;
}

} // end namespace DiffusionProblem

#endif /* INCLUDE_DIFFUSION_PROBLEM_HPP_ */


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Interpolation from panel to sub-panel
// Interpolate linearly surface singularities from panel vertices to sub-panels
//
// Inputs:
// - idP: current panel index
// - bPan: body panels (structure)
// - mu0, mu1, mu2, mu3: doublet at interpolation points (vertices)
// - tau0, tau1, tau2, tau3: sources at interpolation points (vertices)
//
// Output:
// - spis: interpolated singularities (row = sub-panel number; col 0 = doublet, col 1 = source)

#include <iostream>
#include <Eigen/Dense>
#include "interp_sp.h"
#include "interp.h"

using namespace Eigen;

MatrixXd interp_sp(int idP, Network &bPan, Subpanel &sp,
                   double mu0, double mu1, double mu2, double mu3,
                   double tau0, double tau1, double tau2, double tau3) {

    // Temporary variables
    double x0, x1, x2, x3, y0, y1, y2, y3, z0, z1, z2, z3; // vertices coordinates
    double a, b; // interpolation parameters
    double X, Y, Z; // sub-panel center
    MatrixXd spis; // Interpolated singularities
    spis.resize(sp.NS, 2);

    // Copy vertices ton local variables
    x0 = bPan.v0(idP,0);
    x1 = bPan.v1(idP,0);
    x2 = bPan.v2(idP,0);
    x3 = bPan.v3(idP,0);
    y0 = bPan.v0(idP,1);
    y1 = bPan.v1(idP,1);
    y2 = bPan.v2(idP,1);
    y3 = bPan.v3(idP,1);
    z0 = bPan.v0(idP,2);
    z1 = bPan.v1(idP,2);
    z2 = bPan.v2(idP,2);
    z3 = bPan.v3(idP,2);

    // Computation of sub-panel centers in global axes
    int idx = 0, j = 0;
    for (int jj = 0; jj < sp.NSs; jj++) {
        int i = 0;
        for (int ii = 0; ii < sp.NSs; ii++) {
            // Compute weight factors
            a = (double) (i+1)/sp.NSs/2;
            b = (double) (j+1)/sp.NSs/2;
            // Compute center point
            X = (1-b)*((1-a)*x0 + a*x1) + b*(a*x2 + (1-a)*x3);
            Y = (1-b)*((1-a)*y0 + a*y1) + b*(a*y2 + (1-a)*y3);
            Z = (1-b)*((1-a)*z0 + a*z1) + b*(a*z2 + (1-a)*z3);

            // Interpolate singularities
            spis(idx,0) = interp(x0, y0, z0, x1, y1, z1, x2, y2, z2, x3, y3, z3,
                                 mu0, mu1, mu2, mu3,
                                 X, Y, Z);
            spis(idx,1) = interp(x0, y0, z0, x1, y1, z1, x2, y2, z2, x3, y3, z3,
                                 tau0, tau1, tau2, tau3,
                                 X, Y, Z);
            idx++;
            i += 2;
        }
        j += 2;
    }
    return spis;
}


//
//  main.cpp
//  g2obatest
//
//  Created by Seung-Chan Kim on 1/11/17.
//  based on https://github.com/gaoxiang12/g2o_ba_example
//  http://nimbro.net/OP/Doc/html/Localization_8hpp_source.html

#include <iostream>

//#include <Eigen/Core>
//#include <Eigen/StdVector>

// opencv
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>

// g2o
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/cholmod/linear_solver_cholmod.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/types/slam3d/se3quat.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace std;
using namespace g2o;
using namespace cv;

int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 );

double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;

int main(int argc, const char * argv[]) {

    cv::Mat img1;
    cv::Mat img2;

    if (argc != 3)
    {
        img1 =cv::imread("../../../data/set1/1.png");
        img2 =cv::imread("../../../data/set1/2.png");
    }
    else
    {
        img1 = cv::imread( argv[1] );
        img2 = cv::imread( argv[2] );
    }
    cout << "loaded " << img1.size().width << " X " << img1.size().height << " X " << img1.channels() << endl;

    vector<cv::Point2f> pts1, pts2;
    if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
    {
        cout<<"too few feature matches！ # of matches = "<< pts1.size() << endl;
        return 0;
    }
    cout<<"# of matches = "<< pts1.size() << endl;

    g2o::SparseOptimizer    optimizer;

#if 1
    // create the linear solver
    g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new  g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();

    // create the block solver on the top of the linear solver
    // solver for BA/3D SLAM
    // typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;
    g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );

#else
    // create the linear solver
    BlockSolverX::LinearSolverType * linearSolver;
    linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();

    // create the block solver on the top of the linear solver
    // variable size solver
    // typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > BlockSolverX;

    BlockSolverX* block_solver;
    block_solver = new BlockSolverX(linearSolver);

#endif

    //create the algorithm to carry out the optimization
    g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );

    optimizer.setAlgorithm( algorithm );

    for ( int i=0; i<2; i++ )
    {
        // SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential map
        g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
        v->setId(i);
        if ( i == 0)
            v->setFixed( true );

        v->setEstimate( g2o::SE3Quat() );
        optimizer.addVertex( v );
    }
    cout << g2o::SE3Quat()  << endl;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
        v->setId( 2 + i );

        double z = 1;
        double x = ( pts1[i].x - cx ) * z / fx;
        double y = ( pts1[i].y - cy ) * z / fy;
        v->setMarginalized(true);
        v->setEstimate( Eigen::Vector3d(x,y,z) );
        optimizer.addVertex( v );
    }

    g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
    camera->setId(0);
    optimizer.addParameter( camera );

    // First frame
    vector<g2o::EdgeProjectXYZ2UV*> edges;
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(0)) );
        edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0, 0);

        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    // Second frame
    for ( size_t i=0; i<pts2.size(); i++ )
    {
        g2o::EdgeProjectXYZ2UV*  edge = new g2o::EdgeProjectXYZ2UV();
        edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*>   (optimizer.vertex(i+2)) );
        edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*>     (optimizer.vertex(1)) );
        edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
        edge->setInformation( Eigen::Matrix2d::Identity() );
        edge->setParameterId(0,0);

        edge->setRobustKernel( new g2o::RobustKernelHuber() );
        optimizer.addEdge( edge );
        edges.push_back(edge);
    }

    optimizer.setVerbose(true);
    optimizer.initializeOptimization();
    optimizer.optimize(10);

    cout << "Optimization done.." << endl;

    g2o::VertexSE3Expmap* v0 = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(0) );
    Eigen::Isometry3d pose0 = v0->estimate();
    cout<<"Pose (fixed) ="<<endl<<pose0.matrix()<<endl;

    g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
    Eigen::Isometry3d pose = v->estimate();
    cout<<"Pose="<<endl<<pose.matrix()<<endl;

    if(0)
    for ( size_t i=0; i<pts1.size(); i++ )
    {
        g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
        cout<<"vertex id "<<i+2<<", pos = ";
        Eigen::Vector3d pos = v->estimate();
        cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
    }

    int inliers = 0;
    for ( auto e:edges )
    {
        e->computeError();

        if ( e->chi2() > 1 )
        {
            cout<<"error = "<<e->chi2()<<endl;
        }
        else
        {
            inliers++;
        }
    }

    cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
    optimizer.save("ba.g2o");

    imshow("img1", img1);
    imshow("img2", img2);
    cv::waitKey(-1);

    std::cout << "Done !\n";

    // TODO
    // : compute camera poses via epipolar geometry and triangulate the points.
    // : compare the results.
    return 0;
}

int     findCorrespondingPoints( const cv::Mat& img1, const cv::Mat& img2, vector<cv::Point2f>& points1, vector<cv::Point2f>& points2 )
{
    cv::Ptr<cv::FeatureDetector>		detector;
    cv::Ptr<cv::DescriptorExtractor>	extractor;

    detector = cv::ORB::create();
    extractor = cv::ORB::create();

    vector<cv::KeyPoint> kp1, kp2;
    cv::Mat desc1, desc2;

    detector->detect(img1, kp1);
    detector->detect(img2, kp2);

    extractor->compute(img1,kp1,desc1);
    extractor->compute(img2,kp2,desc2);

    //cv::ORB orb;

    cout<<"# of kp= "<<kp1.size()<<" & "<<kp2.size()<<endl;

    cv::Ptr<cv::DescriptorMatcher>  matcher = cv::DescriptorMatcher::create( "BruteForce-Hamming");

    double knn_match_ratio=0.8;
    vector< vector<cv::DMatch> > matches_knn;
    matcher->knnMatch( desc1, desc2, matches_knn, 2 );
    vector< cv::DMatch > matches;
    for ( size_t i=0; i<matches_knn.size(); i++ )
    {
        if (matches_knn[i][0].distance < knn_match_ratio * matches_knn[i][1].distance )
            matches.push_back( matches_knn[i][0] );
    }

    if (matches.size() <= 20)
        return false;

    for ( auto m:matches )
    {
        points1.push_back( kp1[m.queryIdx].pt );
        points2.push_back( kp2[m.trainIdx].pt );
    }

    return true;
}


#pragma once

#ifdef _MSC_VER
#pragma warning( push )
#pragma warning( disable : 4996)
#endif
#include <boost/numeric/ublas/blas.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#ifdef _MSC_VER
#pragma warning( pop )
#endif

namespace dungeng
{
    namespace blas = ::boost::numeric::ublas;

    /*template <size_t N>
    using vec = blas::bounded_vector<double, N>;*/

    template <size_t N>
    class vec : public blas::bounded_vector<double, N>
    {
    private:
        using base = blas::bounded_vector<double, N>;
        using ArrayOfDouble = const double[N];

    public:
        using base::base;

        constexpr vec(ArrayOfDouble& src)
            : base(Initer(src))
        {
        }

    private:
        constexpr static base Initer(ArrayOfDouble& src)
        {
            base result;
            std::copy(std::cbegin(src), std::cend(src), std::begin(result));
            return result;
        }
    };

    /*template <size_t M, size_t N>
    using mat = blas::bounded_matrix<double, M, N>;*/

    template <size_t M, size_t N>
    class mat : public blas::bounded_matrix<double, M, N>
    {
	private:
		using base = blas::bounded_matrix<double, M, N>;
        using ArrayOfDouble = const double[N];
        using ArrayWithArrayOfDouble = const ArrayOfDouble[M];

    public:
        using base::base;

        constexpr mat(ArrayWithArrayOfDouble& src)
            : base(Initer(src))
        {
        }

    private:
        constexpr static base Initer(ArrayWithArrayOfDouble& src)
        {
            base result;
            for (size_t i = 0; i < std::size(src); ++i)
                for (size_t j = 0; j < std::size(src[i]); ++j)
                    result(i, j) = src[i][j];
            return result;
        }
    };

    using vec2 = vec<2>;
    using vec3 = vec<3>;

    using mat33 = mat<3, 3>;

    mat33 rotate_matrix_ccw(double angle);
    mat33 translate_matrix(const vec3& xy);
    mat33 translate_matrix(const vec2& xy);
    mat33 scale_matrix(const vec3& xy);
    mat33 scale_matrix(const vec2& xy);
}


#include <entity.hh>
#include <vector.hh>
#include <pow.hh>
#include <linear_system.hh>
#include <Utils/statistics.hh>
#include "PolygonTriangularization/poly_triang.hh"
#include "Utils/error_handling.hh"

#define JACOBI
#ifdef JACOBI
#pragma warning( push )
#pragma warning( disable : 4714 )
#include <Eigen/Dense>
#endif

#include <vector>

namespace {
bool check_par(double _t) { return _t >= 0 && _t <= 1; }
bool check_par(double _u, double _v)
{
  return _u >= 0 && _v >= 0 && (1 - _u - _v) >= 0;
}
}//namespace

namespace Gen
{
template <class TypeT, size_t DimT, bool Inf1T, bool Inf2T>
bool closest_point(const Segment<TypeT, DimT>& _seg_a, const Segment<TypeT, DimT>& _seg_b,
                   Geo::Vector<TypeT, DimT>* _clsst_pt, double _t[2], double * _dist_sq)
{
  Geo::Vector<TypeT, DimT> a[2] = {
    _seg_a[1] - _seg_a[0],
    _seg_b[0] - _seg_b[1] };
  Eigen::MatrixXd A(a[0].size(), std::size(a));
  Eigen::VectorXd B(a[0].size());
  for (int i = 0; i < a[0].size(); ++i)
  {
    B(i) = _seg_b[0][i] - _seg_a[0][i];
    for (int j = 0; j < std::size(a); ++j)
      A(i, j) = a[j][i];
  }
  Eigen::VectorXd  res = (A.transpose() * A).ldlt().solve(A.transpose() * B);
  if (constexpr(!Inf1T))
  {
    if (!check_par(res(0)))
      return false;
  }
  if (constexpr(!Inf2T))
  {
    if (!check_par(res(1)))
      return false;
  }
  auto pt_a = evaluate(_seg_a, res(0));
  auto pt_b = evaluate(_seg_b, res(1));
  if (_clsst_pt != nullptr)
    *_clsst_pt = (pt_a + pt_b) / 2.;
  if (_dist_sq != nullptr)
    *_dist_sq = Geo::length_square(pt_a - pt_b);
  if (_t != nullptr)
  {
    _t[0] = res(0);
    _t[1] = res(1);
  }
  return true;
}

#define INST_CLOSEST_POINT_SEG_SEG(TYPE, NUM, INF)                        \
template bool closest_point<TYPE, NUM, INF, INF>(                              \
  const Segment<TYPE, NUM>& _seg_a, const Segment<TYPE, NUM>& _seg_b,\
  Geo::Vector<TYPE, NUM>* _clsst_pt, double _t[2], double * _dist);

INST_CLOSEST_POINT_SEG_SEG(double, 2, false)
INST_CLOSEST_POINT_SEG_SEG(double, 2, true)

INST_CLOSEST_POINT_SEG_SEG(double, 3, false)
INST_CLOSEST_POINT_SEG_SEG(double, 3, true)

} // namespace Gen

namespace Geo {

Point evaluate(const Segment& _seg, double _t)
{
  return (1 - _t) * _seg[0] + _t * _seg[1];
}

Point evaluate(const Triangle& _tri, double _u, double _v)
{
  return (1 - _u - _v) * _tri[0] +_u * _tri[1] + _v * _tri[2];
}

namespace {
struct PolygonalFace : public IPolygonalFace
{
  virtual bool triangle(size_t _idx, Triangle& _tri) const
  {
    if (_idx > tris_.size())
      return false;
    _tri = tris_[_idx];
    return true;
  }
  virtual size_t triangle_number() const { return tris_.size(); }

  virtual Point normal() const;

protected:
  virtual size_t make_new_loop() override
  {
    ptss_.emplace_back();
    return ptss_.size() - 1;
  }
  virtual void add_point(const Point& _pt, size_t _loop_num) override
  {
    ptss_[_loop_num].push_back(_pt);
  }
  virtual void compute() override;

private:
  std::vector<Triangle> tris_;
  std::vector<std::vector<Point>> ptss_;
};

void PolygonalFace::compute()
{
  if (ptss_.empty())
    return;
  auto ptg = IPolygonTriangulation::make();
  for (auto& pts : ptss_)
  {
    THROW_IF(pts.size() < 3, "Loop withless than 3 points.");
    ptg->add(pts);
  }
  const auto& tris = ptg->triangles();
  const auto& poly = ptg->polygon();
  for (const auto& tri : tris)
  {
    tris_.push_back({
      poly[tri[0]],
      poly[tri[1]],
      poly[tri[2]] });
  }
}

Point PolygonalFace::normal() const
{
  Point normal{ 0,0,0 };
  for (auto& tri : tris_)
    normal += (tri[1] - tri[0]) % (tri[2] - tri[0]);
  auto len = length(normal);
  if (len > 0)
    normal /= len;
  return normal;
}

}//namespace

std::shared_ptr<IPolygonalFace> IPolygonalFace::make()
{
  return std::make_shared<PolygonalFace>();
}

// Finds u and v such that the distance between pt and
// _tri[0] * u + _tri[1] * v + _tri[2] * (1 - u - v)
// is minimal. u and v must be > 0 and u + v < 1.
// If the constraints are not satisfied, returns false.
bool closest_point(const Triangle& _tri, const Point& _pt,
  Point* _clsst_pt, double * _dist_sq)
{
  double A[2][2], B[2];
  auto v0 = _tri[0] - _tri[2];
  auto v1 = _tri[1] - _tri[2];
  auto dp = _pt - _tri[2];

  A[0][0] = length_square(v0);
  A[1][1] = length_square(v1);
  A[0][1] = A[1][0] = v0 * v1;

  B[0] = dp * v0;
  B[1] = dp * v1;

  double uv[2];
  if (!solve_2x2(A, uv, B))
    return false;
  if (!check_par(uv[0], uv[1]))
    return false;
  auto clsst_pt = _tri[2] + uv[0] * (_tri[0] - _tri[2]) + uv[1] * (_tri[1] - _tri[2]);
  if (_clsst_pt != nullptr)
    *_clsst_pt = clsst_pt;
  if (_dist_sq != nullptr)
    *_dist_sq = length(clsst_pt - _pt);
  return true;
}

bool closest_point(const IPolygonalFace& _face, const Point& _pt,
  Point* _clsst_pt, double * _dist_sq)
{
  Utils::StatisticsT<double> dist_stats;
  Triangle tri;
  Point clsst_pt;
  const auto tri_nmbr = _face.triangle_number();
  for (size_t i = 0; i < tri_nmbr; ++i)
  {
    if (!_face.triangle(i, tri))
      continue;
    double dist_sq = 0;
    if (!closest_point(tri, _pt, &clsst_pt, &dist_sq))
      continue;
    if ((dist_stats.add(dist_sq) & dist_stats.Smallest) && _clsst_pt != nullptr)
      *_clsst_pt = clsst_pt;
  }
  if (dist_stats.count() == 0)
    return false;
  if (_dist_sq != nullptr)
    *_dist_sq = dist_stats.min();
  return true;
}

bool closest_point(const Triangle& _tri, const Segment& _seg,
  Point* _clsst_pt, double * _t, double * _dist_sq)
{
  const auto a = _seg[0] - _tri[0];
  const Point coeff[] =
  { _tri[1] - _tri[0], _tri[2] - _tri[0], _seg[0] - _seg[1] };

  Eigen::MatrixXd A(3, 3);
  Eigen::VectorXd B(3);
  for (int i = 0; i < 3; ++i)
  {
    for (int j = 0; j < 3; ++j)
      A(j, i) = coeff[i][j];
    B(i) = a[i];
  }
#if 1
  Eigen::VectorXd  uvt = (A.transpose() * A).ldlt().solve(A.transpose() * B);
#else
  const auto& jsvd =
    A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV);
  Eigen::VectorXd uvt = jsvd.solve(B);
#endif

  if (!check_par(uvt[0], uvt[1]) || !check_par(uvt[2]))
    return false;
  auto pt_seg = evaluate(_seg, uvt[2]);
  auto pt_tri = evaluate(_tri, uvt[0], uvt[1]);
  if (_clsst_pt!= nullptr)
    *_clsst_pt = (pt_seg + pt_tri) / 2.;
  if (_t != nullptr)
    *_t = uvt[2];
  if (_dist_sq != nullptr)
    *_dist_sq = length_square(pt_seg - pt_tri);
  return true;
}

bool closest_point(const IPolygonalFace& _face, const Segment& _pt,
  Point* _clsst_pt, double * _t, double * _dist_sq)
{
  Utils::StatisticsT<double> dist_stats;
  Triangle tri;
  Point clsst_pt;
  for (size_t i = 0; i < _face.triangle_number(); ++i)
  {
    if (!_face.triangle(i, tri))
      continue;
    double dist_sq = 0, t;
    if (!closest_point(tri, _pt, &clsst_pt, &t, &dist_sq))
      continue;
    if (dist_stats.add(dist_sq) & dist_stats.Smallest)
    {
      if (_clsst_pt != nullptr)
        *_clsst_pt = clsst_pt;
      if (_t != nullptr)
        *_t = t;
    }
  }
  if (dist_stats.count() == 0)
    return false;
  if (_dist_sq != nullptr)
    *_dist_sq = dist_stats.min();
  return true;
}

}//namespace Geo


/*
 * Copyright (c) 2013-2018 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef ODE_QR_HPP
#define ODE_QR_HPP

// ODE using QR Decomposition

#include <iostream>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <kv/interval.hpp>
#include <kv/rdouble.hpp>
#include <kv/interval-vector.hpp>
#include <kv/qr.hpp>
#include <kv/vleq.hpp>
#include <kv/ode.hpp>
#include <kv/ode-autodif.hpp>
#include <kv/ode-param.hpp>
#include <kv/ode-callback.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T, class F>
int
odelong_qr(
	F f,
	ub::vector< interval<T> >& init,
	const interval<T>& start,
	interval<T>& end,
	ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>(),
	ub::matrix< interval<T> >* mat = NULL
) {
	int s = init.size();
	int i, j;

	ub::vector< interval<T> > c;
	ub::vector< interval<T> > fc;
	ub::vector< autodif< interval<T> > > Iad;

	ub::vector< interval<T> > result_i;
	ub::matrix< interval<T> > result_d;

	ub::vector< interval<T> > x, x1;
	interval<T> t, t1;
	ub::matrix< interval<T> > M;
	int ret_ode, ret_ode2;
	int ret_val = 0;
	bool bo;
	bool ret_callback;

	ub::matrix<T> Q, Q2, R, Q2t;
	ub::matrix< interval<T> > AQ, QAQ, Q2i;
	ub::vector< interval<T> > y, y1, y2, tmp;

	ub::vector< psa< interval<T> > > result_psa;

	if (mat != NULL) {
		M = ub::identity_matrix< interval<T> >(s);
	}

	t = start;
	x = init;

	c = mid(x);
	y = x - c;
	Q = ub::identity_matrix<T>(s);

	ode_param<T> p2;

	while (1) {
		x1 = x;
		t1 = end;

		Iad = autodif< interval<T> >::init(x1);
		p2 = p;
		p2.set_autostep(true);
		// NOTICE: below must be autodif version of ode
		ret_ode = ode(f, Iad, t, t1, p2, &result_psa);
		if (ret_ode == 0) break;

		fc = c;
		// Step size should be same as above ode call.
		// Because above ode call is with autodif and interval input and
		// below ode call is without autodif and point input,
		// below ode call is supposed to be easier to succeed than above.
		// If below ode call fails, force success by increasing order.
		p2 = p;
		p2.set_autostep(false);
		while (1) {
			ret_ode2 = ode(f, fc, t, t1, p2);
			if (ret_ode2 != 0) break;
			p2.order++;
			std::cout << "increase order: " << p2.order << "\n";
		}

		autodif< interval<T> >::split(Iad, result_i, result_d);

		#if 0
		// centering result_d
		fc += prod(result_d - mid(result_d), x - c);
		result_d =  mid(result_d);
		#endif

		AQ = prod(result_d, Q);
		bo = qr(mid(AQ), Q2, R);
		if (bo == false) break;
		Q2i = Q2;
		Q2t = trans(Q2);
		// bo = vleq(Q2i, AQ, QAQ);
		bo = vleq(Q2i, AQ, QAQ, &Q2t);
		if (bo == false) break;
		Q2i = Q2;
		y1 = prod(QAQ, y);
		c = mid(fc);
		tmp = fc - c;
		// bo = vleq(Q2i, tmp, y2);
		bo = vleq(Q2i, tmp, y2, &Q2t);
		if (bo == false) break;
		y = y1 + y2;
		x1 = prod(Q2, y) + c;

		// below seems to have some efficiency.
		// we comment out below because we have not study it
		// theoretically yet.

		// x1 = intersect(x1, result_i);

		Q = Q2;

		ret_val = 1;

		if (mat != NULL) M = prod(result_d, M);

		if (p.verbose == 1) {
			std::cout << "t: " << t1 << "\n";
			std::cout << x1 << "\n";
		}

		ret_callback = callback(t, t1, x, x1, result_psa);

		if (ret_callback == false) {
			ret_val = 3;
			break;
		}

		if (ret_ode == 2) {
			ret_val = 2;
			break;
		}

		t = t1;
		x = x1;
	}

	if (ret_val >= 1) {
		init = x1;
		if (mat != NULL) *mat = M;
	}
	if (ret_val == 1) {
		end = t;
	}
	if (ret_val == 3) {
		end = t1;
	}

	return ret_val;
}

template <class T, class F>
int
odelong_qr(
	F f,
	ub::vector< autodif< interval<T> > >& init,
	const interval<T>& start,
	interval<T>& end, ode_param<T> p = ode_param<T>(),
	const ode_callback<T>& callback = ode_callback<T>()
) {
	int s = init.size();
	int i, j;
	ub::vector< interval<T> > x;
	ub::matrix< interval<T> > M, M_tmp;
	int r;

	autodif< interval<T> >::split(init, x, M);
	int s2 = M.size2();

	r = odelong_qr(f, x, start, end, p, callback, &M_tmp);

	if (r == 0) return 0;

	M = prod(M_tmp, M);

	for (i=0; i<s; i++) {
		init(i).v = x(i);
		init(i).d.resize(s2);
		for (j=0; j<s2; j++) {
			init(i).d(j) = M(i, j);
		}
	}

	return r;
}

} // namespace kv

#endif // ODE_QR_HPP


#pragma once

#include "GaussianDistribution.hpp"

#include <Eigen/Dense>
#include <Eigen/Cholesky>

#include <array>

namespace icarus {
    template<typename T, size_t N>
    struct MerweScaledSigmaPoints
    {
        explicit MerweScaledSigmaPoints(T alpha, T beta = 2, T kappa = 3 - T(N))
        {
            mLambda = alpha * alpha * (T(N) + kappa) - T(N);
            mFirstMeanWeight = mLambda / (T(N) + mLambda);
            mCovarianceWeight = mFirstMeanWeight + 1 - alpha * alpha + beta;
            mNextWeights = T(1) / (2 * (T(N) + mLambda));
        }

        static constexpr size_t size()
        {
            return 2 * N + 1;
        }

        std::array<Eigen::Matrix<T, N, 1>, size()> operator()(GaussianDistribution<T, N> const & distribution) const
        {
            Eigen::Matrix<T, N, N> offsets = ((T(N) + mLambda) * distribution.covariance).llt().matrixL();

            std::array<Eigen::Matrix<T, N, 1>, size()> ret;
            ret[0] = distribution.mean;

            for (int i = 0; i < N; ++i) {
                ret[1 + 2 * i + 0] = distribution.mean + offsets.col(i);
                ret[1 + 2 * i + 1] = distribution.mean - offsets.col(i);
            }

            return ret;
        }

        template<int M>
        GaussianDistribution<T, M> unscentedTransform(std::array<Eigen::Matrix<T, M, 1>, 2 * N + 1> const & points) const
        {
            GaussianDistribution<T, M> ret;

            ret.mean = mFirstMeanWeight * points[0];

            for (int i = 1; i < 2 * N + 1; ++i) {
                ret.mean += mNextWeights * points[i];
            }

            auto difference = (points[0] - ret.mean).eval();
            ret.covariance.template triangularView<Eigen::Lower>() = mCovarianceWeight * difference * difference.transpose();

            for (int i = 1; i < 2 * N + 1; ++i) {
                difference = (points[i] - ret.mean).eval();
                ret.covariance.template selfadjointView<Eigen::Lower>().rankUpdate(difference, mNextWeights);
            }

            return ret;
        }

        T covarianceWeight(size_t index) const
        {
            if (index == 0) {
                return mCovarianceWeight;
            } else {
                return mNextWeights;
            }
        }
    private:
        T mLambda;
        T mFirstMeanWeight;
        T mCovarianceWeight;
        T mNextWeights;
    };
}


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

/*
 Copyright (C) 2010 Sebastian Schlenkrich

*/

/*! \file integratorsT.hpp
    \brief provide template functions for numerical integration
*/

#ifndef quantlib_templateintegrators_hpp
#define quantlib_templateintegrators_hpp

#include <boost/function.hpp>
//#include <boost/math/special_functions/erf.hpp>
//#include <ql/experimental/template/auxilliaries/MinimADVariable2.hpp>

namespace TemplateAuxilliaries {

    // evaluate \int_a^b v(t) f(t) dt = \sum v_i [F(t_i) - F(t_i-1)] with
    // v(t) piece-wise left-constant,
    // F'(t) = f(t)
    template <typename PassiveType, typename ActiveType, typename FuncType>
    class PieceWiseConstantIntegral {
    private:
        std::vector<PassiveType> t_;
        std::vector<ActiveType> v_;
        FuncType F_;
    public:
        PieceWiseConstantIntegral(const std::vector<PassiveType>& t, const std::vector<ActiveType>& v, const FuncType& F) : t_(t), v_(v), F_(F) {}
        ActiveType operator()(PassiveType startTime, PassiveType endTime) {
            int sgn = 1;
            if (startTime>endTime) {  // we want to ensure startTime <= endTime
                PassiveType t = startTime;
                startTime = endTime;
                endTime = t;
                sgn = -1;
            }
            // organising indices
            size_t idx_min  = 0;
            size_t idx_max  = std::min(t_.size(),v_.size())-1;
            size_t idx_last = idx_max;
            // enforce a < t_min <= t_max < b or special treatment
            while ((startTime>=t_[idx_min])&&(idx_min<idx_last)) ++idx_min;
            while ((endTime  <=t_[idx_max])&&(idx_max>0       )) --idx_max;
            ActiveType tmp = sgn * ( F_(endTime) - F_(startTime) );
            if (endTime<=t_[0])    return v_[0]        * tmp;  // short end
            if (idx_min==idx_last) return v_[idx_last] * tmp;  // long end
            if (idx_min> idx_max)  return v_[idx_min]  * tmp;  // integration within grid interval
            // integral a ... x_min
            tmp = v_[idx_min] * ( F_(t_[idx_min]) - F_(startTime) );
            // integral x_min ... x_max
            for (size_t i=idx_min; i<idx_max; ++i) tmp += v_[i+1] * ( F_(t_[i+1]) - F_(t_[i]) );
            // integral x_max ... b
            if (idx_max<idx_last) tmp += v_[idx_max+1] * ( F_(endTime) - F_(t_[idx_max]) );
            else                  tmp += v_[idx_max]   * ( F_(endTime) - F_(t_[idx_max]) );
            // finished
            return sgn * tmp;
        }
    };

    // evaluate \int_x(0)^x(n) v(x) f(x) dx via trapezoidal rule
    // v(x) interpolated values on variable x-grid
    // f(x) scalar function as functor
    class TrapezoidalIntegral {
    public:
        template <typename PassiveType, typename ActiveType, typename FuncType>
        ActiveType operator()(const std::vector<PassiveType>& x, const std::vector<ActiveType>& v, const FuncType& f) {
            size_t n=std::min(x.size(),v.size());
            if (n<2) return (ActiveType)0.0;
            ActiveType sum=0;
            for (size_t i=0; i<n-1; ++i) sum += 0.5*(v[i]*f(x[i]) + v[i+1]*f(x[i+1]))*(x[i+1] - x[i]);
            return sum;
        }
    };

    // evaluate \int_x[0]^x[n-1] v(x) f(x) dx via Gau�-Tschebyschow-Integration
    // x[0] left boundary, x[n-1] right boundary
    // x[1], ..., x[n-2] Gauss-Tschebyschow grid points
    // v(x) interpolated values on x-grid
    // f(x) scalar function as functor
    class GaussTschebyschowIntegral {
    public:
        template <typename PassiveType>
        std::vector<PassiveType> getGrid(PassiveType a, PassiveType b, size_t n) {
            std::vector<PassiveType> x(n);
            if (n==0) return x;
            if (n==1) { x[0] = 0.5*(a+b); return x; }
            x[0] = a;
            x[n-1] = b;
            if (n==2) return x;
            // n>2
            for (size_t k=1; k<n-1; ++k) {
                x[k] = -cos( (2.0*k-1.0) / (2.0*(n-2.0)) * M_PI );      // x \in (-1, 1)
                x[k] = 0.5*(b-a)*(x[k]+1) + a;                 // x \in ( a, b)
            }
            return x;
        }
        template <typename PassiveType, typename ActiveType, typename FuncType>
        ActiveType operator()(const std::vector<PassiveType>& x, const std::vector<ActiveType>& v, const FuncType& f) {
            size_t n=std::min(x.size(),v.size());
            if (n<3) return TrapezoidalIntegral()(x,v,f);
            ActiveType sum=0;
            for (size_t k=1; k<n-1; ++k) {
                sum += v[k] * f(x[k]) * sin( (2*k-1)/(2*(n-2))*M_PI );
            }
            sum *= M_PI / (n-2);
            return sum;
        }
    };

    // 4th order Runge Kutta step for y' = f(t,y)
    // via y1   = y0 + b^T k dt
    //     k_i  = f(t+c_i dt, y0 + a_i^T k dt)
    template <typename DateType, typename ActiveType>
    void rungeKuttaStep( const std::vector<ActiveType>&                                                                      y0,
                         const DateType                                                                                t,
                         const boost::function< void (const DateType, const std::vector<ActiveType>&, std::vector<ActiveType>&) >& f,
                         const DateType                                                                                dt,
                         std::vector<ActiveType>&                                                                      y1  ) {
        std::vector<ActiveType> k1(y0.size()), k2(y0.size()), k3(y0.size()), k4(y0.size());
        y1 = y0;
        // we add an epsilon in case we are at a boundary of piecewise constant parameters
        DateType eps = 1.0e-8*dt;
        // k1 = f(t,y0)
        f( t+eps,    y0, k1);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/6.0 * dt * k1[i];
        // k2 = f(t + 0.5dt, y0 + 0.5 k1)
        for (size_t i=0; i<y0.size(); ++i) k1[i] = y0[i] + 0.5 * dt * k1[i];
        f( t+0.5*dt, k1, k2);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/3.0 * dt * k2[i];
        // k3 = f(t + 0.5dt, y0 + 0.5 k2)
        for (size_t i=0; i<y0.size(); ++i) k2[i] = y0[i] + 0.5 * dt * k2[i];
        f( t+0.5*dt, k2, k3);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/3.0 * dt * k3[i];
        // k4 = f(t + dt, y0 + k3)
        for (size_t i=0; i<y0.size(); ++i) k3[i] = y0[i] + dt * k3[i];
        f( t+1.0*dt-eps, k3, k4);
        for (size_t i=0; i<y0.size(); ++i) y1[i] += 1.0/6.0 * dt * k4[i];
        return;
    }

}

#endif  /* quantlib_templateintegrators_hpp */


/*
 *  mgbvtd.cpp
 *  vortrac
 *
 *  Created by Xiaowen Tang on 5/28/13.
 *  Copyright 2013 University Corporation for Atmospheric Research.
 *  All rights reserved.
 *
 */
#include <vector>
#include <algorithm>
#include <cmath>
#include <armadillo>
#include "mgbvtd.h"

MGBVTD::MGBVTD(float x0, float y0, float hgt, float rmw, GriddedData& cappi):
m_cappi(cappi)
{
	m_centerx = x0;
	m_centery = y0;
	m_centerz = hgt;
	m_rmw     = rmw;
}

float MGBVTD::computeCrossBeamWind(float guessMax, QString& velField, GBVTD* gbvtd, Hvvp* hvvp)
{
	const float Rt = sqrt(m_centerx*m_centerx+m_centery*m_centery);

	//1. calculate Vt profile first
	std::vector<float> vt;
	std::vector<float> vt_rng;
	//1. compute the radial profile of symmetric tangential wind
	for(float rng=m_rmw*1.2; rng<=.6*Rt; rng+=1.){
		m_cappi.setCartesianReferencePoint(m_centerx, m_centery, m_centerz);
		int numData = m_cappi.getCylindricalAzimuthLength(rng, m_centerz);
		float* ringData = new float[numData];
		float* ringAzi  = new float[numData];
		m_cappi.getCylindricalAzimuthData(velField, numData, rng, m_centerz, ringData);
        m_cappi.getCylindricalAzimuthPosition(numData, rng, m_centerz, ringAzi);
		Coefficient* coeff = new Coefficient[20];
		float vtdDev;
		if(gbvtd->analyzeRing(m_centerx, m_centery, rng, m_centerz, numData, ringData, ringAzi, coeff, vtdDev)){
			if(coeff[0].getParameter()=="VTC0"){
				vt.push_back(coeff[0].getValue());
				vt_rng.push_back(rng);
			}
		}
		delete[] ringAzi;
		delete[] ringData;
		delete[] coeff;
	}
	if(vt.size()<15) {
		// std::cout<<std::endl;
		return 0.f;
	}

	//Compute hvvp first
	float cc0, cc6, vt_std, hvvp_std;
	if(!hvvp->computeCrossBeamWind(m_centerz, cc0, cc6, hvvp_std)) {
		// std::cout<<std::endl;
		return 0.f;
	}
	// printf("cc0=%5.2f, cc6=%5.2f, ", cc0, cc6);

	//Iterate through all possible values of cross-beam wind
	arma::fmat A(vt.size(), 2);
	for(int ii=0; ii<vt.size(); ++ii){
		A(ii,0) = log(Rt/vt_rng[ii]);
		A(ii,1) = 1;
	}

	std::vector<float> guessWinds;
	for(float currentWind=-fabs(guessMax); currentWind<fabs(guessMax)+1.; currentWind+=1.)
		guessWinds.push_back(currentWind);

	std::vector<bool> flag(guessWinds.size(), false);
	arma::fmat B(vt.size(), guessWinds.size());
	B.fill(0.f);
	arma::fvec b(vt.size());
	for(std::vector<float>::iterator it=guessWinds.begin(); it!=guessWinds.end(); ++it){
		int idx = std::distance(guessWinds.begin(), it);
		for(int ii=0; ii<vt.size(); ++ii)
			b(ii) = vt[ii]-*it*vt_rng[ii]/Rt;
		if( b.min()>0.f){
			B.col(idx) = arma::log(b);
			flag[idx]  = true;
		}
	}
	arma::fmat X=arma::solve(A, B);
	arma::fmat E=(A*X-B);
	vt_std = sqrt(arma::accu(arma::square(E))/E.size());
	// printf("min_Xt=%5.2f, max_Xt=%5.2f, ", arma::min(X.row(0)), arma::max(X.row(0)));

	//Compare results and find the best one
	float curDev=999., tmpBest=0., tmpXt=999.;
	for(std::vector<float>::iterator it=guessWinds.begin(); it!=guessWinds.end(); ++it){
		int idx = std::distance(guessWinds.begin(), it);
		if(!flag[idx] || X(0,idx)<=0.f ) continue;
		float hvvp_vm = cc0-Rt*cc6/(X(0,idx)+1.);
		if(fabs(*it-hvvp_vm)<curDev){
			curDev  = fabs(*it-hvvp_vm);
			tmpBest = *it;
			tmpXt   = X(0,idx);
		}
	}

	// printf("num_vt_fit=%3d, vt_std=%5.2f, hvvp_std=%5.2f, tmpXt=%5.2f, vm=%5.2f, dev=%5.2f\n", vt.size(), vt_std, hvvp_std, tmpXt, tmpBest, curDev);
	if(curDev>4.0f)
		return 0.0f;
	else
		return tmpBest;
}


#pragma once

#include "utils.hpp"
#include <Eigen/Core>

template <typename MI, typename Tree>
std::vector<Eigen::VectorXd> FD(const MI& info, Tree& tree, const std::vector<Eigen::VectorXd>& Tau)
{
    constexpr int ord = Tree::order;
    const auto& mb = info.model.mb;
    const auto& pred = mb.predecessors();
    const auto& succ = mb.successors();

    std::vector<Eigen::MatrixXd> C(mb.nrBodies());
    std::vector<Eigen::MatrixXd> CD(mb.nrBodies());
    std::vector<Eigen::VectorXd> PA(mb.nrBodies());
    std::vector<Eigen::MatrixXd> IA(mb.nrBodies());
    std::vector<Eigen::MatrixXd> G(mb.nrBodies());
    std::vector<Eigen::MatrixXd> U(mb.nrBodies());
    std::vector<Eigen::MatrixXd> UD(mb.nrBodies());
    std::vector<Eigen::MatrixXd> D(mb.nrBodies());
    std::vector<Eigen::VectorXd> T(mb.nrBodies());
    std::vector<Eigen::VectorXd> y(mb.nrBodies());

    for (int i = 0; i < mb.nrBodies(); ++i) {
        C[i] = tree.joints[i].inverse().template matrix<ord>();
        CD[i] = tree.joints[i].template dualMatrix<ord>();
        G[i] = makeDiag<ord>(mb.joint(i).motionSubspace());
        IA[i].setZero(6 * ord, 6 * ord);
        PA[i].setZero(6 * ord);
    }

    for (int i = mb.nrBodies() - 1; i >= 0; --i) {
        IA[i] += makeDiag<ord>(mb.body(i).inertia().matrix());
        U[i] = IA[i] * G[i];
        UD[i] = G[i].transpose() * IA[i];
        D[i] = G[i].transpose() * U[i];

        y[i] = D[i].inverse() * (Tau[i] - G[i].transpose() * PA[i]);
        if (pred[i] != -1) {
            auto tmp1 = IA[i] - U[i] * D[i].inverse() * UD[i];
            IA[pred[i]] += CD[i] * tmp1 * C[i];
            auto tmp2 = PA[i] + U[i] * y[i];
            PA[pred[i]] += CD[i] * tmp2;
        }
    }

    for (int i = 0; i < mb.nrJoints(); ++i) {
        int dof = mb.joint(i).dof();
        if (pred[i] != -1) {
            y[i] -= D[i].inverse() * UD[i] * C[i] * T[pred[i]];
        }
        T[i] = G[i] * y[i];
        if (pred[i] != -1) {
            T[i] += C[i] * T[pred[i]];
        }
    }

    return y;
}

template <typename MI, typename Tree>
Eigen::VectorXd standard_FD(const MI& info, Tree& tree, const Eigen::VectorXd& tau)
{
    constexpr int ord = Tree::order;
    const auto& mb = info.model.mb;
    const auto& pred = mb.predecessors();
    const auto& succ = mb.successors();
    const auto& jpd = mb.jointsPosInDof();

    std::vector<Eigen::Vector6d> PA(mb.nrBodies());
    std::vector<Eigen::Matrix6d> IA(mb.nrBodies());
    std::vector<Eigen::Matrix6d> X(mb.nrBodies());
    std::vector<Eigen::Vector6d> T(mb.nrBodies());
    std::vector<Eigen::MatrixXd> U(mb.nrBodies());
    std::vector<Eigen::MatrixXd> D(mb.nrBodies());
    Eigen::VectorXd y(tau.size());

    for (int i = 0; i < mb.nrBodies(); ++i) {
        IA[i].setZero();
        PA[i].setZero();
        X[i] = tree.joints[i].transform().inverse().matrix();
    }

    for (int i = mb.nrBodies() - 1; i >= 0; --i) {
        const auto& S = mb.joint(i).motionSubspace();
        int dof = mb.joint(i).dof();
        IA[i] += mb.body(i).inertia().matrix();
        U[i] = IA[i] * S;
        D[i] = S.transpose() * U[i];

        y.segment(jpd[i], dof) = D[i].inverse() * (tau.segment(jpd[i], dof) - S.transpose() * PA[i]);
        if (pred[i] != -1) {
            auto tmp1 = IA[i] - U[i] * D[i].inverse() * U[i].transpose();
            IA[pred[i]] += X[i].transpose() * tmp1 * X[i];
            auto tmp2 = PA[i] + U[i] * y.segment(jpd[i], dof);
            PA[pred[i]] += X[i].transpose() * tmp2;
        }
    }

    for (int i = 0; i < mb.nrJoints(); ++i) {
        int dof = mb.joint(i).dof();
        if (pred[i] != -1) {
            y.segment(jpd[i], dof) -= D[i].inverse() * U[i].transpose() * X[i] * T[pred[i]];
        }
        T[i] = mb.joint(i).motionSubspace() * y.segment(jpd[i], dof);
        if (pred[i] != -1) {
            T[i] += X[i] * T[pred[i]];
        }
    }

    return y;
}