/******************************************************
 *
 *
 *
 * 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();
}


// Copyright (c) 2017 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE RangeTest
#include <boost/test/unit_test.hpp>
#include <poplin/ConvUtil.hpp>
#include <vector>

namespace std {
ostream &operator<<(ostream &s, const pair<unsigned, unsigned> &p) {
  s << '<' << p.first << ',' << p.second << '>';
  return s;
}
} // namespace std

static poplin::ConvParams makeParams(unsigned stride, unsigned kernelSize,
                                     unsigned paddingLower,
                                     unsigned paddingUpper,
                                     unsigned inputSize) {
  poplin::ConvParams params{
      poplar::FLOAT,            // data type,
      1,                        // batch size
      {inputSize, inputSize},   // input size
      {kernelSize, kernelSize}, // kernel size
      1,                        // input channels
      1,                        // output channels
      1                         // conv groups
  };
  params.inputTransform.paddingLower = {paddingLower, paddingLower};
  params.inputTransform.paddingUpper = {paddingUpper, paddingUpper};
  params.outputTransform.stride = {stride, stride};
  return params;
}

BOOST_AUTO_TEST_CASE(inputRangeTest) {
  // No stride, no padding
  const auto params1 = makeParams(1, 3, 0, 0, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params1), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 1, 0, params1), 1U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params1), 1U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 7, 2, params1), 9U);

  // Stride = 2, no padding
  const auto params2 = makeParams(2, 3, 0, 0, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 1, 0, params2), 2U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 3, 2, params2), 8U);

  // Stride 1, padding lower=1, padding upper=0
  const auto params3 = makeParams(1, 3, 1, 0, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params3), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params3), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params3), 1U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 8, 1, params3), 8U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 8, 2, params3), 9U);

  // Stride 1, padding lower=2, padding upper=0
  const auto params4 = makeParams(1, 3, 2, 0, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params4), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params4), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params4), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 9, 1, params4), 8U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 9, 2, params4), 9U);

  // Stride 2, padding lower=1, padding upper=0
  const auto params5 = makeParams(2, 3, 1, 0, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params5), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params5), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params5), 1U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 4, 1, params5), 8U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 4, 2, params5), 9U);

  // Stride 2, padding lower=2, padding upper=0
  const auto params6 = makeParams(2, 3, 2, 0, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params6), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params6), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params6), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 4, 1, params6), 7U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 4, 2, params6), 8U);

  // Stride 2, padding lower=4, padding upper=0
  const auto params7 = makeParams(2, 3, 4, 0, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params7), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params7), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params7), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 1, 0, params7), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 1, 1, params7), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 1, 2, params7), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 5, 1, params7), 7U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 5, 2, params7), 8U);

  // Stride 1, padding lower=1, padding upper=1
  const auto params8 = makeParams(1, 3, 1, 1, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params8), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params8), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params8), 1U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 9, 1, params8), 9U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 9, 2, params8), ~0U);

  // Stride 2, padding lower=1, padding upper=2
  const auto params9 = makeParams(2, 3, 1, 2, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params9), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params9), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params9), 1U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 4, 1, params9), 8U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 4, 2, params9), 9U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 5, 0, params9), 9U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 5, 1, params9), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 5, 2, params9), ~0U);

  // Stride 3, padding lower=2, padding upper=2
  const auto params10 = makeParams(3, 3, 2, 2, 10);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 0, params10), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 1, params10), ~0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 0, 2, params10), 0U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 3, 1, params10), 8U);
  BOOST_CHECK_EQUAL(poplin::getInputIndex(0, 3, 2, params10), 9U);
}

std::pair<unsigned, unsigned>
getOutputDim(unsigned inDimY, unsigned inDimX, unsigned kernelSizeY,
             unsigned kernelSizeX, const std::vector<unsigned> &stride,
             const std::vector<unsigned> &paddingLower,
             const std::vector<unsigned> &paddingUpper) {
  poplin::ConvParams params{
      poplar::FLOAT,              // data type,
      1,                          // batch size
      {inDimY, inDimX},           // input size
      {kernelSizeY, kernelSizeX}, // kernel size
      1,                          // input channels
      1,                          // output channels
      1                           // conv groups
  };
  params.inputTransform.paddingLower = paddingLower;
  params.inputTransform.paddingUpper = paddingUpper;
  params.outputTransform.stride = stride;
  return {params.getOutputSize(0), params.getOutputSize(1)};
}

BOOST_AUTO_TEST_CASE(outputDimTest) {

  BOOST_CHECK_EQUAL(getOutputDim(10, 10, 3, 3, {1, 1}, {0, 0}, {0, 0}),
                    std::make_pair(8u, 8u));

  BOOST_CHECK_EQUAL(getOutputDim(10, 10, 2, 2, {1, 1}, {0, 0}, {0, 0}),
                    std::make_pair(9u, 9u));

  BOOST_CHECK_EQUAL(getOutputDim(10, 10, 3, 3, {1, 1}, {1, 1}, {0, 0}),
                    std::make_pair(9u, 9u));

  BOOST_CHECK_EQUAL(getOutputDim(10, 10, 3, 3, {1, 1}, {0, 0}, {2, 2}),
                    std::make_pair(10u, 10u));

  BOOST_CHECK_EQUAL(getOutputDim(10, 10, 3, 3, {1, 1}, {3, 3}, {2, 2}),
                    std::make_pair(13u, 13u));

  BOOST_CHECK_EQUAL(getOutputDim(10, 10, 3, 3, {2, 2}, {3, 3}, {2, 2}),
                    std::make_pair(7u, 7u));

  BOOST_CHECK_EQUAL(getOutputDim(10, 10, 3, 3, {1, 2}, {3, 3}, {2, 2}),
                    std::make_pair(13u, 7u));

  BOOST_CHECK_EQUAL(getOutputDim(10, 12, 3, 5, {1, 1}, {0, 1}, {0, 2}),
                    std::make_pair(8u, 11u));

  BOOST_CHECK_EQUAL(getOutputDim(4, 4, 3, 3, {2, 1}, {0, 0}, {1, 0}),
                    std::make_pair(2u, 2u));

  BOOST_CHECK_EQUAL(getOutputDim(4, 4, 3, 3, {3, 3}, {0, 1}, {1, 0}),
                    std::make_pair(1u, 1u));
}


/*
 *  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;
}


#pragma once

#include <Eigen/Dense>

namespace Tools {
	namespace Math {
		template <typename T, int R, int C>
		using Matrix = Eigen::Matrix<T, R, C>;

		template <typename T>
		using Matrix2 = Matrix<T, 2, 2>;
		template <typename T>
		using Matrix3 = Matrix<T, 3, 3>;
		template <typename T>
		using Matrix4 = Matrix<T, 4, 4>;

		template <typename T>
		using MatrixX = Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

		template <typename T, int R>
		using Vector = Matrix<T, R, 1>;

		template <typename T>
		using Vector2 = Vector<T, 2>;
		template <typename T>
		using Vector3 = Vector<T, 3>;
		template <typename T>
		using Vector4 = Vector<T, 4>;

		template <typename T>
		using VectorX = Vector<T, Eigen::Dynamic>;
	}
}


#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 */


//transfer skeleton pose to actuator angle

#include <ros/ros.h>
#include <stdlib.h>
#include <iostream>
#include <vector>
#include <math.h>
#include <ros/package.h>
#include <tf/transform_broadcaster.h>
#include <stdio.h>
#include <geometry_msgs/Point.h>
#include <boost/shared_ptr.hpp>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include "skeleton/JointMsg.h"
#include <xm_arm_msgs/xm_ArmSerialDatagram.h>

using namespace std;
ros::Publisher     arm_serial_pub_;
ros::Subscriber    skeleton_state_sub_;
float joint_angle[4];
float act_angle[4];

void transfromjointtomotor()
{//need change!
    act_angle[0]=1.5*(-joint_angle[0]+joint_angle[1]-joint_angle[2])*180/3.1415+220;
    act_angle[1]=1.5*(-2.1628*joint_angle[0]-joint_angle[1])*180/3.1415+300;
    act_angle[2]=1.5*(joint_angle[0]-joint_angle[1]-joint_angle[2])*180/3.1415+260;
    act_angle[3]=200-120*joint_angle[3];
//act_angle[3]=100;
}

geometry_msgs::Point rotate(float angle,geometry_msgs::Point Point)
{
  geometry_msgs::Point feedback;
  feedback.x=Point.x;
  feedback.y=Point.y*cos(angle)-Point.z*sin(angle);
  feedback.z=Point.y*sin(angle)+Point.z*cos(angle);

  return feedback;
}

void publishArmCommand(const u_int8_t func,
                                         const u_int8_t jnt_id,
                                         const float    jnt_pos)
{
    xm_arm_msgs::xm_ArmSerialDatagramPtr datagram_ptr =
        boost::make_shared<xm_arm_msgs::xm_ArmSerialDatagram>();
    datagram_ptr->sender = 0x03;
    datagram_ptr->receiver = jnt_id + 0x2B;
    datagram_ptr->data.resize(5, 0);
    u_int8_t *data_ptr = datagram_ptr->data.data();
    data_ptr[0] = func;
    *(float *)(data_ptr + 1) = jnt_pos;
    arm_serial_pub_.publish(datagram_ptr);
}

void transfer_skeleton_motor(const skeleton::JointMsg::ConstPtr &msg)

{
   std::vector<geometry_msgs::Point> skeletonPoints;
   skeletonPoints= msg->joints;

  geometry_msgs::Point point1= skeletonPoints[0];
   geometry_msgs::Point point2= skeletonPoints[1];
    geometry_msgs::Point point3= skeletonPoints[2];

//ROS_INFO("I heard: %f \r\n", point1.x);
if (point1.x>5000)
  {
  ROS_INFO("initialize failed ,please check you standing pose!!!");
  return ;
  }
if(point1.x<0)
 {
  ROS_INFO("initialize failed ,please check you standing pose!!!");
  return ;
 }

float x1,y1,z1,x2,y2,z2,x3,y3,z3,l1,l2;
float angle1,angle2,angle3,angle4;
float sum2;
x1=point1.x;
x2=point2.x;
x3=point3.x;
y1=point1.y;
y2=point2.y;
y3=point3.y;
z1=point1.z;
z2=point2.z;
z3=point3.z;

l1=sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1));
l2=sqrt((x3-x2)*(x3-x2)+(y3-y2)*(y3-y2)+(z3-z2)*(z3-z2));

if((x2-x1)<0) {
ROS_INFO("ALERT:　sorry, angle2 out of range, please move your body~~");
return;
}
//angle2 range is ok
angle2=asin((x2-x1)/l1);
//sum2=pow((x2-x1)/l1,2)+pow((z1-z2)/l1,2)+pow((y2-y1)/l1,2);
//printf("sum2: >>>>>>>>>>>>>>>    %f",sum2);
if((y2-y1)<0){
ROS_INFO("ALERT:　sorry again, angle out of range, please move your body~~");
return;
}

if((z1-z2)<0){
ROS_INFO("ALERT:　sorry again again, angle out of range, please move your body~~");
return;
}
//angle1 range is ok
//angle1=acos((y2-y1)/(l1*cos(angle2)));
//need change here
if((z1-z2)>(y2-y1))
{
  angle1=asin((z1-z2)/(l1*cos(angle2)));
}
else angle1=acos((y2-y1)/(l1*cos(angle2)));
//angle1=(asin((z1-z2)/(l1*cos(angle2)))+acos((y2-y1)/(l1*cos(angle2))))/2;
//>>>>>>>>>>>>>>>rotate transmission>>>>>>>>>>>>>>>>>
point1=rotate(angle1,point1);
point2=rotate(angle1,point2);
point3=rotate(angle1,point3);
x1=point1.x;
x2=point2.x;
x3=point3.x;
y1=point1.y;
y2=point2.y;
y3=point3.y;
z1=point1.z;
z2=point2.z;
z3=point3.z;
//angle4 is ready!!!!!
angle4=acos(((x3-x2)*(x2-x1)+(y3-y2)*(y2-y1)+(z3-z2)*(z2-z1))/(l1*l2));

//>>>>>>>>>>>>>>>>>calculate angle3>>>>>>>>>>>>>>>>>>>
float d_long,d_short;
float mark;
d_long=l2*sin(angle4);
d_short=abs(x3*(-cos(angle2))+y3*sin(angle2)+x1*cos(angle2)-y1*sin(angle2));
angle3=asin(d_short/d_long);

mark=-cos(angle2)*(x3-x1)+sin(angle2)*(y3-y1);

if(mark>0) angle3=-angle3;
//angle3 is ready!!!!!

ROS_INFO("I heard joint1:%f ;joint2:%f ; joint3:%f ;joint4:%f  \r\n",angle1,angle2,angle3,angle4);
joint_angle[0]=angle1;
joint_angle[1]=angle2;
joint_angle[2]=angle3;
joint_angle[3]=angle4;

//transmit from joint_angle to motor_angle

transfromjointtomotor();
for(size_t i =0;i<3;i++)
{
  if (act_angle[i]<1) act_angle[i]=1;
  if(act_angle[i]>359)act_angle[i]=359;
}
printf(">>>>>>>>>>act_angle:>>>>>>>>>> motor1:  %f , motor2:  %f  , motor3: %f,motor4: %f", act_angle[0],act_angle[1],act_angle[2],act_angle[3]);

  for(size_t i=0;i<4;i++)
  {
   publishArmCommand(0x01,i, act_angle[i]);
   //delay func
   boost::this_thread::sleep( boost::posix_time::milliseconds(5) ); //单位是毫秒
  }

}

//main func
int main( int argc, char**argv)
{
    ros::init(argc,argv,"skeleton_send_node");
    ros::NodeHandle nh;

for(size_t i=0; i<4; i++)
  {
  joint_angle[i]=0;
  }

    arm_serial_pub_ = nh.advertise<xm_arm_msgs::xm_ArmSerialDatagram>(
        "xm_arm_serial/send_arm_command", 1000);

   skeleton_state_sub_=nh.subscribe("skeleton_msg", 1000,
        transfer_skeleton_motor);

ros::spin();

return 0;

}


/*!
 * @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;
}


#include "InputFileOperator.h"
#include "CommandLineHandlers/SingleCommandLineHandler.h"
#include "NumberOperator.h"
#include <boost/algorithm/string/trim.hpp>

USING_NAMESPACE(std);
USING_NAMESPACE(clu);

class RoundOperator : public NumberOperator
{
public:
	RoundOperator(int value) : m_value(value)
	{
	}

	virtual bool OnNumberRead(double number)
	{
		long roundedNumber = (number > 0.0) ? (long)(number) : (long)(number)-1;

		long reminder = roundedNumber % m_value;
		roundedNumber -= reminder;
		if (abs(reminder) >= (m_value / 2))
		{
			if (number > 0)
				roundedNumber += m_value;
			else
				roundedNumber -= m_value;
		}

		m_OutputHandler->OutputInteger(roundedNumber);
		return true;
	}

protected:
	long m_value;
};

REGISTER_SINGLE_INTEGER("round", RoundOperator)->SetHelp("Round       - Round up to the nearest");


/**
 * @file quasiinterpolation_test.cc
 * @brief NPDE exam QuasiInterpolation code
 * @author Oliver Rietmann
 * @date 15.07.2020
 * @copyright Developed at SAM, ETH Zurich
 */

#include "../quasiinterpolation.h"

#include <gtest/gtest.h>
#include <lf/mesh/test_utils/test_meshes.h>
#include <lf/mesh/utils/utils.h>
#include <lf/uscalfe/uscalfe.h>

#include <Eigen/Core>
#include <memory>
#include <utility>

namespace QuasiInterpolation::test {

TEST(QuasiInterpolation, findKp) {
  std::shared_ptr<const lf::mesh::Mesh> mesh_p =
      lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  lf::mesh::utils::CodimMeshDataSet<
      std::pair<const lf::mesh::Entity *, unsigned int>>
      KpMeshDataSet = findKp(mesh_p);

  unsigned int vertexIndex = 9;       // 8
  unsigned int localVertexIndex = 1;  // 0
  unsigned int triangleIndex = 12;    // 11

  const lf::mesh::Entity *vertex = mesh_p->EntityByIndex(2, vertexIndex);
  std::pair<const lf::mesh::Entity *, unsigned int> Kp = KpMeshDataSet(*vertex);

  EXPECT_EQ(localVertexIndex, Kp.second);
  EXPECT_NE(Kp.first, nullptr);
  if (Kp.first != nullptr) {
    EXPECT_EQ(triangleIndex, mesh_p->Index(*Kp.first));
  }
}

TEST(QuasiInterpolation, quasiInterpolate) {
  std::shared_ptr<const lf::mesh::Mesh> mesh_p =
      lf::mesh::test_utils::GenerateHybrid2DTestMesh(3);
  lf::uscalfe::FeSpaceLagrangeO1<double> fe_space(mesh_p);

  // For polynomials of degree 1, the quasi projection yields the exact result
  auto f = [](Eigen::Vector2d x) -> double {
    return 1.5 * x(0) - 2.0 * x(1) + 0.5;
  };
  lf::mesh::utils::MeshFunctionGlobal mf(f);

  Eigen::VectorXd coefficients =
      QuasiInterpolation::quasiInterpolate(fe_space, mf);
  Eigen::VectorXd coefficients_ref = lf::fe::NodalProjection(fe_space, mf);

  double tol = 1.0e-12;
  double error = (coefficients - coefficients_ref).lpNorm<Eigen::Infinity>();
  ASSERT_NEAR(0.0, error, tol);
}

}  // namespace QuasiInterpolation::test


/*! \file 2d_bezier.cpp
  \brief Simple 2D plot using bezier curves.
  \author Jacob Voytko and Paul A. Bristow
  \date 2007
*/

// Copyright (C) Jacob Voytko 2007
// Copyright Paul A. Bristow 2009

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Example of using SVG plot bezier option showing smoothing effect when using a limited number of data-points.

#include <boost/svg_plot/svg_2d_plot.hpp>
using namespace boost::svg;

#include <map>
 // using std::map;
#include <cmath>
  // using std::sin;
  // using std::cos;

// sin and cos functions.
double f(double x)
{
  return std::sin(x);
}

double g(double x)
{
  return std::cos(x);
}

int main()
{
  std::map<double, double> sin_data;
  std::map<double, double> cos_data;

  const double interval = 3.14159265 / 8.; // pi/8 - between values, too few for a smooth plot.

  for(double i = 0; i <= 10.; i += interval)
  {
    sin_data[i] = f(i);
    cos_data[i] = g(i);
  }

  svg_2d_plot my_plot;

  // Size/scale settings.
  my_plot.size(700, 500)
    .x_range(-.5, 10.5)
    .y_range(-1.1, 1.1);  // Ensure that 1.0 and -1.0 are visible.

  // Text settings.
  my_plot.title("Plot of sin and cos functions")
    .title_font_size(29)
    .x_label("X Axis Units")
    .y_major_labels_side(-1)
    .y_major_grid_on(true);

  // Commands.
  my_plot.legend_on(true) // To show a legend box.
    .plot_window_on(true)
    .x_label_on(true);

  // Color settings.
  my_plot.background_color(svg_color(67, 111, 69))  // Color specified using RGB values.
    .legend_background_color(svg_color(207, 202,167))
    .legend_border_color(svg_color(102, 102, 84))
    .plot_background_color(svg_color(136, 188, 126))
    .title_color(white) // Color specified using SVG named colors.
    .y_major_grid_color(grey);

  // X axis settings.
  my_plot.x_major_interval(2)
    .x_major_tick_length(14)
    .x_major_tick_width(1)
    .x_minor_tick_length(7)
    .x_minor_tick_width(1)
    .x_num_minor_ticks(3)

  // Y axis settings.
    .y_major_interval(25)
    .y_num_minor_ticks(5);

  // legend settings.
  my_plot.legend_title_font_size(15);

  my_plot.plot(sin_data, "Sin(x)").line_on(true)
    .shape(circlet).size(5).fill_color(yellow);

  my_plot.plot(cos_data, "Cos(x)")
    .line_color(blue)
    .bezier_on(true)
    .shape(square).size(5).fill_color(red);

  // Note the slightly smoother curve for the cosine curve using bezier_on, compared to the line_on option.

  my_plot.write("./2d_bezier.svg");

  return 0;
} // int main()


#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);
}


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is 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.
// __END_LICENSE__

#include <gtest/gtest_VW.h>
#include <vw/Math/Functors.h>
#include <boost/random.hpp>

using namespace vw;
using namespace vw::math;

static const double DELTA = 1e-5;

TEST(Accumulators, CDF_cauchy) {
  boost::mt19937 random_gen(42);
  boost::cauchy_distribution<double> cauchy(35,80);
  boost::variate_generator<boost::mt19937&,
    boost::cauchy_distribution<double> > generator(random_gen, cauchy);

  { // Default settings
    CDFAccumulator<double> cdf;
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 35.0, 2.0 );
    EXPECT_NEAR( cdf.first_quartile(), -45, 4.0 );
    EXPECT_NEAR( cdf.third_quartile(), 115, 4.0 );
  }
  { // More quantiles == more precision
    CDFAccumulator<double> cdf(2000,500);
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 35.0, 1.0 );
    EXPECT_NEAR( cdf.first_quartile(), -45, 2.0 );
    EXPECT_NEAR( cdf.third_quartile(), 115, 2.0 );
  }
}

TEST(Accumulators, CDF_triangular) {
  boost::mt19937 random_gen(42);
  boost::triangle_distribution<double> triangular(10,60,80);
  boost::variate_generator<boost::mt19937&,
    boost::triangle_distribution<double> > generator(random_gen, triangular);

  { // Default settings
    CDFAccumulator<double> cdf;
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 51.833, 1.0 );
    EXPECT_NEAR( cdf.first_quartile(), 39.6, 2.0 );
    EXPECT_NEAR( cdf.third_quartile(), 61.3, 2.0 );
    EXPECT_NEAR( cdf.approximate_mean(), 50, 1.0 );
    EXPECT_NEAR( cdf.approximate_mean(0.05), 50, 0.5 );
    EXPECT_NEAR( cdf.approximate_stddev(), 14.7196, 2.0 );
    EXPECT_NEAR( cdf.approximate_stddev(0.05), 14.7196, 1.0 );
  }
  { // More quantiles == more precision
    CDFAccumulator<double> cdf(2000,500);
    for ( uint16 i = 0; i < 50000; i++ )
      cdf( generator() );

    EXPECT_NEAR( cdf.median(), 51.833, 0.5 );
    EXPECT_NEAR( cdf.first_quartile(), 39.6, 1.0 );
    EXPECT_NEAR( cdf.third_quartile(), 61.3, 1.0 );
    EXPECT_NEAR( cdf.approximate_mean(), 50, 0.5 );
    EXPECT_NEAR( cdf.approximate_mean(0.05), 50, 0.25 );
    EXPECT_NEAR( cdf.approximate_stddev(), 14.7196, 3.0 );
    EXPECT_NEAR( cdf.approximate_stddev(0.05), 14.7196, 0.5 );
  }
}

TEST(Accumulators, Median) {
  MedianAccumulator<double> median;

  boost::mt19937 random_gen(42);
  boost::cauchy_distribution<double> cauchy(35,80);
  boost::variate_generator<boost::mt19937&,
    boost::cauchy_distribution<double> > generator(random_gen, cauchy);

  for ( uint16 i = 0; i < 50000; i++ )
    median( generator() );

  EXPECT_NEAR( median.value(), 35.0, 1.5 );
}

TEST(Accumulators, CDF_Merge ) {
  boost::mt19937 random_gen(42);
  boost::normal_distribution<double> norm1(0, 3);
  boost::normal_distribution<double> norm2(5, 3);
  boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > generator1( random_gen, norm1 );
  boost::variate_generator<boost::mt19937&, boost::normal_distribution<double> > generator2( random_gen, norm2 );

  CDFAccumulator<double> cdf1, cdf2, cdf3;
  // 1 will only see gen1 .. but will later be merged with 2.
  // 2 will only see gen2
  // 3 is our control and sees both gen1 and gen2;
  for ( size_t i = 0; i < 50000; i++ ) {
    double sample1 = generator1(), sample2 = generator2();
    cdf1( sample1 );
    cdf2( sample2 );
    cdf3( sample1 );
    cdf3( sample2 );
  }

  cdf1.update();
  cdf2.update();
  cdf3.update();
  cdf1(cdf2);

  EXPECT_NEAR( cdf1.median(), cdf3.median(), 0.01 );
  EXPECT_NEAR( cdf1.first_quartile(), cdf3.first_quartile(), 0.01 );
  EXPECT_NEAR( cdf1.third_quartile(), cdf3.third_quartile(), 0.01 );
  EXPECT_NEAR( cdf1.approximate_mean(),
               cdf3.approximate_mean(), 0.01 );
}


#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