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

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

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

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

/*! \file normalsabrinterpolation.hpp
    \brief normal SABR interpolation interpolation between discrete points
*/

#pragma once

#include <qle/models/normalsabr.hpp>

#include <ql/math/interpolations/xabrinterpolation.hpp>
#include <ql/termstructures/volatility/sabr.hpp>

#include <boost/assign/list_of.hpp>
#include <boost/make_shared.hpp>

namespace QuantExt {
using namespace QuantLib;

namespace detail {

class NormalSABRWrapper {
public:
    NormalSABRWrapper(const Time t, const Real& forward, const std::vector<Real>& params,
                      const std::vector<Real>& addParams)
        : t_(t), forward_(forward), params_(params) {
        // validateSabrParameters(params[0], 0.0, params[1], params[2]);
    }
    const std::vector<Real>& params() const { return params_; }
    Real volatility(const Real x) { return normalSabrVolatility(x, forward_, t_, params_[0], params_[1], params_[2]); }

private:
    const Real t_, &forward_;
    const std::vector<Real> params_;
};

struct NormalSABRSpecs {
    Size dimension() { return 3; }
    void defaultValues(std::vector<Real>& params, std::vector<bool>&, const Real& forward, const Real expiryTime,
                       const std::vector<Real>& addParams) {
        if (params[0] == Null<Real>())
            params[0] = 0.0040;
        if (params[1] == Null<Real>())
            params[1] = std::sqrt(0.4);
        if (params[2] == Null<Real>())
            params[2] = 0.0;
    }
    void guess(Array& values, const std::vector<bool>& paramIsFixed, const Real& forward, const Real expiryTime,
               const std::vector<Real>& r, const std::vector<Real>& addParams) {
        Size j = 0;
        if (!paramIsFixed[0]) {
            values[0] = (0.01 - 2E-6) * r[j++] + 1E-6; // normal vol guess
        }
        if (!paramIsFixed[1])
            values[1] = 1.5 * r[j++] + 1E-6;
        if (!paramIsFixed[2])
            values[2] = (2.0 * r[j++] - 1.0) * (1.0 - 1E-6);
    }
    Real eps1() { return .0000001; }
    Real eps2() { return .9999; }
    Real dilationFactor() { return 0.001; }
    Array inverse(const Array& y, const std::vector<bool>&, const std::vector<Real>&, const Real) {
        Array x(3);
        x[0] = std::tan(y[0] * M_PI / 0.02 - M_PI_2);
        x[1] = std::tan(y[1] * M_PI / 5.00 - M_PI_2);
        x[2] = std::tan((y[2] + 1.0) * M_PI / 2.0 - M_PI_2);
        return x;
    }
    Array direct(const Array& x, const std::vector<bool>&, const std::vector<Real>&, const Real) {
        Array y(3);
        y[0] = 0.02 * (std::atan(x[0]) + M_PI_2) / M_PI;
        y[1] = 5.00 * (std::atan(x[1]) + M_PI_2) / M_PI;
        y[2] = 2.0 * (std::atan(x[2]) + M_PI_2) / M_PI - 1.0;
        return y;
    }
    Real weight(const Real strike, const Real forward, const Real stdDev, const std::vector<Real>& addParams) {
        return bachelierBlackFormulaStdDevDerivative(strike, forward, stdDev, 1.0);
    }
    typedef NormalSABRWrapper type;
    boost::shared_ptr<type> instance(const Time t, const Real& forward, const std::vector<Real>& params,
                                     const std::vector<Real>& addParams) {
        std::vector<Real> updatedParams(params);
        if (!addParams.empty()) {
            updatedParams[0] = normalSabrAlphaFromAtmVol(forward, t, addParams.front(), params[1], params[2]);
        }
        return boost::make_shared<type>(t, forward, updatedParams, addParams);
    }
};
} // namespace detail

//! %SABR smile interpolation between discrete volatility points.
/*! \ingroup interpolations */
class NormalSABRInterpolation : public Interpolation {
public:
    template <class I1, class I2>
    NormalSABRInterpolation(
        const I1& xBegin, // x = strikes
        const I1& xEnd,
        const I2& yBegin, // y = volatilities
        Time t,           // option expiry
        const Real& forward, Real alpha, Real nu, Real rho, bool alphaIsFixed, bool nuIsFixed, bool rhoIsFixed,
        bool vegaWeighted = true, const Size atmStrikeIndex = Null<Size>(), const bool implyAlphaFromAtmVol = false,
        const boost::shared_ptr<EndCriteria>& endCriteria = boost::shared_ptr<EndCriteria>(),
        const boost::shared_ptr<OptimizationMethod>& optMethod = boost::shared_ptr<OptimizationMethod>(),
        const Real errorAccept = 0.0002, const bool useMaxError = false, const Size maxGuesses = 50) {

        QL_REQUIRE(
            !implyAlphaFromAtmVol || atmStrikeIndex != Null<Real>(),
            "NormalSABRInterpolation: imply alpha from atm vol implies a) that the atm strike index must be given");

        std::vector<Real> addParams;
        if (implyAlphaFromAtmVol) {
            addParams.push_back(*std::next(yBegin, atmStrikeIndex));
        }

        impl_ = boost::shared_ptr<Interpolation::Impl>(
            new QuantLib::detail::XABRInterpolationImpl<I1, I2, detail::NormalSABRSpecs>(
                xBegin, xEnd, yBegin, t, forward, boost::assign::list_of(alpha)(nu)(rho),
                boost::assign::list_of(alphaIsFixed)(nuIsFixed)(rhoIsFixed), vegaWeighted, endCriteria, optMethod,
                errorAccept, useMaxError, maxGuesses, addParams));
        coeffs_ = boost::dynamic_pointer_cast<QuantLib::detail::XABRCoeffHolder<detail::NormalSABRSpecs>>(impl_);
    }
    Real expiry() const { return coeffs_->t_; }
    Real forward() const { return coeffs_->forward_; }
    Real alpha() const { return coeffs_->modelInstance_->params()[0]; }
    Real nu() const { return coeffs_->modelInstance_->params()[1]; }
    Real rho() const { return coeffs_->modelInstance_->params()[2]; }
    Real rmsError() const { return coeffs_->error_; }
    Real maxError() const { return coeffs_->maxError_; }
    const std::vector<Real>& interpolationWeights() const { return coeffs_->weights_; }
    EndCriteria::Type endCriteria() { return coeffs_->XABREndCriteria_; }

private:
    boost::shared_ptr<QuantLib::detail::XABRCoeffHolder<detail::NormalSABRSpecs>> coeffs_;
};

//! %SABR interpolation factory and traits
/*! \ingroup interpolations */
class NormalSABR {
public:
    NormalSABR(Time t, Real forward, Real alpha, Real nu, Real rho, bool alphaIsFixed, bool nuIsFixed, bool rhoIsFixed,
               bool vegaWeighted = false,
               const boost::shared_ptr<EndCriteria> endCriteria = boost::shared_ptr<EndCriteria>(),
               const boost::shared_ptr<OptimizationMethod> optMethod = boost::shared_ptr<OptimizationMethod>(),
               const Real errorAccept = 0.0002, const bool useMaxError = false, const Size maxGuesses = 50)
        : t_(t), forward_(forward), alpha_(alpha), nu_(nu), rho_(rho), alphaIsFixed_(alphaIsFixed),
          nuIsFixed_(nuIsFixed), rhoIsFixed_(rhoIsFixed), vegaWeighted_(vegaWeighted), endCriteria_(endCriteria),
          optMethod_(optMethod), errorAccept_(errorAccept), useMaxError_(useMaxError), maxGuesses_(maxGuesses) {}
    template <class I1, class I2> Interpolation interpolate(const I1& xBegin, const I1& xEnd, const I2& yBegin) const {
        return SABRInterpolation(xBegin, xEnd, yBegin, t_, forward_, alpha_, nu_, rho_, alphaIsFixed_, nuIsFixed_,
                                 rhoIsFixed_, vegaWeighted_, endCriteria_, optMethod_, errorAccept_, useMaxError_,
                                 maxGuesses_);
    }
    static const bool global = true;

private:
    Time t_;
    Real forward_;
    Real alpha_, nu_, rho_;
    bool alphaIsFixed_, nuIsFixed_, rhoIsFixed_;
    bool vegaWeighted_;
    const boost::shared_ptr<EndCriteria> endCriteria_;
    const boost::shared_ptr<OptimizationMethod> optMethod_;
    const Real errorAccept_;
    const bool useMaxError_;
    const Size maxGuesses_;
};
} // namespace QuantExt


#include <iostream>
#include <vector>
#include <string>
#include <sstream>
#include <fstream>
#include <numeric>

#include <boost/filesystem.hpp>
#include <glog/logging.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <opencv2/opencv.hpp>
#include <opencv2/core/eigen.hpp>

#include "YamlFileIO.h"

Eigen::Matrix4d loadCameraPose(const std::string &file)
{
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    std::ifstream fs(file);
    if (!fs.is_open())
    {
        std::cerr << "Fail to open camera pose file " << file << std::endl;
        return T;
    }

    std::string line;
    int i = 0;
    while (!fs.eof())
    {
        line.clear();
        getline(fs, line);
        if (line.empty())
            continue;

        std::stringstream ss(line);
        std::string dummy_str;
        if (i < 4)
        {
            ss >> T(i, 0) >> dummy_str >> T(i, 1) >> dummy_str >> T(i, 2) >> dummy_str >> T(i, 3);
            i++;
        }
        else
        {
            std::cerr << "Invalid camera pose format!\n";
            break;
        }
    }
    fs.close();

    return T;
}

void evaluateBackpackExt(const std::vector<Eigen::Matrix4d> &v_extrinsics, const std::string &eval_pattern){
    Eigen::Matrix4d T_c0_c1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c1_vec = Eigen::AngleAxisd(-90*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c1(-0.03543, 0, -0.03543);
    T_c0_c1_gt.block<3, 3>(0, 0) = c0_c1_vec.matrix();
    T_c0_c1_gt.block<3, 1>(0, 3) = t_c0_c1;
    Eigen::Matrix4d T_c0_c2_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c2_vec = Eigen::AngleAxisd(-180*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c2(0, 0, -0.07086);
    T_c0_c2_gt.block<3, 3>(0, 0) = c0_c2_vec.matrix();
    T_c0_c2_gt.block<3, 1>(0, 3) = t_c0_c2;
    Eigen::Matrix4d T_c0_c3_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c3_vec = Eigen::AngleAxisd(90*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c3(0.03543, 0, -0.03543);
    T_c0_c3_gt.block<3, 3>(0, 0) = c0_c3_vec.matrix();
    T_c0_c3_gt.block<3, 1>(0, 3) = t_c0_c3;

    Eigen::Matrix4d T_l0_c0_gt = Eigen::Matrix4d::Identity();
    Eigen::Matrix4d T_l1_c0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd l1_c0_vec1 = Eigen::AngleAxisd(-15*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::AngleAxisd l1_c0_vec2 = Eigen::AngleAxisd(90*M_PI/180.0, Eigen::Vector3d(0,0,1));
    Eigen::Matrix3d l1_c0_vec =  l1_c0_vec1.matrix()*l1_c0_vec2.matrix();
    Eigen::Vector3d t_l1_c0(0.58192, 0.0, -0.1954);
    T_l1_c0_gt.block<3,3>(0, 0) = l1_c0_vec;
    T_l1_c0_gt.block<3, 1>(0, 3) = l1_c0_vec* t_l1_c0;
    std::cout << "T_l1_c0_gt:\n" << T_l1_c0_gt << "\n";

    Eigen::Matrix4d T_l0_l1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd l0_l1_vec1 = Eigen::AngleAxisd(-30*M_PI/180.0, Eigen::Vector3d(0,0,1));
    Eigen::AngleAxisd l0_l1_vec2 = Eigen::AngleAxisd(-75*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::Matrix3d l0_l1_vec =  l0_l1_vec1.matrix()*l0_l1_vec2.matrix();
    Eigen::Vector3d t_l0_l1(-0.2604, 0, -0.34922);
    T_l0_l1_gt.block<3,3>(0, 0) = l0_l1_vec;
    T_l0_l1_gt.block<3, 1>(0, 3) = l0_l1_vec1.matrix() * t_l0_l1;

    Eigen::Matrix4d T_gt = Eigen::Matrix4d::Identity();
    if (eval_pattern == "camera0_to_camera1")
        T_gt = T_c0_c1_gt;
    if (eval_pattern == "camera0_to_camera2")
        T_gt = T_c0_c2_gt;
    if (eval_pattern == "camera0_to_camera3")
        T_gt = T_c0_c3_gt;
    if (eval_pattern == "lidar0_to_camera0")
        T_gt = T_l0_c0_gt;
    if (eval_pattern == "lidar1_to_camera0")
        T_gt = T_l1_c0_gt;
    if (eval_pattern == "lidar0_to_lidar1")
        T_gt = T_l0_l1_gt;

    double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0;
    double avg_tx = 0, avg_ty = 0, avg_tz = 0.0;
    for(auto &T_ext : v_extrinsics){
        Eigen::Matrix4d T_error = T_gt.inverse() * T_ext;

        std::cout << "T_ext:\n" << T_ext << "\n";
        Eigen::Matrix3d R = T_ext.block<3, 3>(0, 0);
        Eigen::Vector3d v_angles = R.eulerAngles(2,1,0) * 180.0/M_PI;
        avg_yaw += (std::abs(v_angles[0]) > 90) ? (180 - std::abs(v_angles[0])) : std::abs(v_angles[0]);
        avg_pitch += (std::abs(v_angles[1]) > 90) ? (180 - std::abs(v_angles[1])) : std::abs(v_angles[1]);
        avg_roll += (std::abs(v_angles[2]) > 90) ? (180 - std::abs(v_angles[2])) : std::abs(v_angles[2]);
        LOG(INFO) << eval_pattern << " euler angles(ZYX): " << v_angles.transpose() << "\n";

        Eigen::Vector3d t = T_ext.block<3,1>(0, 3);
        avg_tx += t[0]; avg_ty += t[1]; avg_tz += t[2];
        LOG(INFO) << eval_pattern << " trans vector: " << t.transpose() << "\n";

        Eigen::Matrix3d err_R = T_error.block<3, 3>(0, 0);
        Eigen::Vector3d err_t = T_error.block<3, 1>(0, 3);
        Eigen::AngleAxisd delta_R_vec(err_R);
        LOG(INFO) << " Rotation difference angle "
                    << delta_R_vec.angle() * 180.0 / M_PI << "\n";
        LOG(INFO) << " Translation difference norm is "
                    << err_t.norm() << "\n";
    }
    int size = v_extrinsics.size();
    LOG(INFO) << "Evaluate " << size  << " extrinsic parameters.";
    LOG(INFO) << "Avg euler angles: (" << avg_roll/size << ", " << avg_pitch/size << ", " << avg_yaw/size << ")";
    LOG(INFO) << "Avg trans: (" << avg_tx/size << ", " << avg_ty/size << ", " << avg_tz/size << ")";
}

void evaluateAntmanExt(const std::vector<Eigen::Matrix4d> &v_extrinsics, const std::string &eval_pattern){
    Eigen::Matrix4d T_c0_c1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c1_vec = Eigen::AngleAxisd(-90*M_PI/180.0, Eigen::Vector3d(0,-1,0));
    Eigen::Vector3d t_c0_c1(0.05647, 0, -0.05647);
    T_c0_c1_gt.block<3, 3>(0, 0) = c0_c1_vec.matrix();
    T_c0_c1_gt.block<3, 1>(0, 3) = t_c0_c1;
    Eigen::Matrix4d T_c0_c2_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c2_vec = Eigen::AngleAxisd(180*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::Vector3d t_c0_c2(0, 0, -0.11294);
    T_c0_c2_gt.block<3, 3>(0, 0) = c0_c2_vec.matrix();
    T_c0_c2_gt.block<3, 1>(0, 3) = t_c0_c2;
    Eigen::Matrix4d T_c0_c3_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c3_vec = Eigen::AngleAxisd(-90*M_PI/180.0, Eigen::Vector3d(0,1,0));
    Eigen::Vector3d t_c0_c3(-0.05647, 0, -0.05647);
    T_c0_c3_gt.block<3, 3>(0, 0) = c0_c3_vec.matrix();
    T_c0_c3_gt.block<3, 1>(0, 3) = t_c0_c3;

    Eigen::Matrix4d T_c3_l0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c3_l0_vec1 = Eigen::AngleAxisd(-M_PI/4.0, -Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd c3_l0_vec2 = Eigen::AngleAxisd(M_PI/2.0, Eigen::Vector3d::UnitX());
    Eigen::Matrix3d c3_l0_vec = c3_l0_vec2.matrix() * c3_l0_vec1.matrix();
    Eigen::Vector3d t_c3_l0(-0.03887, -0.03887, 0.0593);
    T_c3_l0_gt.block<3,3>(0, 0) = c3_l0_vec;
    T_c3_l0_gt.block<3,1>(0, 3) = t_c3_l0;

    Eigen::Matrix4d T_gt = Eigen::Matrix4d::Identity();
    if (eval_pattern == "camera0_to_camera1")
        T_gt = T_c0_c1_gt;
    if (eval_pattern == "camera0_to_camera2")
        T_gt = T_c0_c2_gt;
    if (eval_pattern == "camera0_to_camera3")
        T_gt = T_c0_c3_gt;
    // it is lidar0-camera0 actually
    if (eval_pattern == "camera0_to_lidar0")
        T_gt = T_c3_l0_gt;

    Eigen::Matrix3d R_gt = T_gt.block<3, 3>(0, 0);
    Eigen::Vector3d v_gt_angles = R_gt.eulerAngles(2,1,0) * 180.0/M_PI;
    LOG(INFO) << "GT Euler angles(ZYX): " << v_gt_angles.transpose() << "\n";

    double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0;
    double avg_tx = 0, avg_ty = 0, avg_tz = 0.0;
    for(size_t i = 0; i < v_extrinsics.size(); i++){
        Eigen::Matrix4d T_ext = v_extrinsics[i];

        Eigen::Matrix4d T_error = T_gt.inverse() * T_ext;

        Eigen::Matrix3d R = T_ext.block<3, 3>(0, 0);
        Eigen::Vector3d v_angles = R.eulerAngles(2,1,0) * 180.0/M_PI;
        avg_yaw += (std::abs(v_angles[0]) > 90) ? (180 - std::abs(v_angles[0])) : std::abs(v_angles[0]);
        avg_pitch += (std::abs(v_angles[1]) > 90) ? (180 - std::abs(v_angles[1])) : std::abs(v_angles[1]);
        avg_roll += (std::abs(v_angles[2]) > 90) ? (180 - std::abs(v_angles[2])) : std::abs(v_angles[2]);
        LOG(INFO) << eval_pattern << " euler angles(ZYX): " << v_angles.transpose() << "\n";

        Eigen::Vector3d t = T_ext.block<3,1>(0, 3);
        avg_tx += t[0]; avg_ty += t[1]; avg_tz += t[2];
        LOG(INFO) << eval_pattern << " trans vector: " << t.transpose() << "\n";

        Eigen::Matrix3d err_R = T_error.block<3, 3>(0, 0);
        Eigen::Vector3d err_t = T_error.block<3, 1>(0, 3);
        Eigen::AngleAxisd delta_R_vec(err_R);
        LOG(INFO) << " Rotation difference angle "
                    << delta_R_vec.angle() * 180.0 / M_PI << "\n";
        LOG(INFO) << " Translation difference norm is "
                    << err_t.norm() << "\n";
    }

    int size = v_extrinsics.size();
    LOG(INFO) << "Evaluate " << size  << " extrinsic parameters.";
    LOG(INFO) << "Avg euler angles: (" << avg_roll/size << ", " << avg_pitch/size << ", " << avg_yaw/size << ")";
    LOG(INFO) << "Avg trans: (" << avg_tx/size << ", " << avg_ty/size << ", " << avg_tz/size << ")";
}

void evaluateKaleidoExt(const std::vector<Eigen::Matrix4d> &v_extrinsics, const std::string &eval_pattern){
    Eigen::Matrix4d T_c0_c1_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_c1_vec = Eigen::AngleAxisd(28*M_PI/180.0, Eigen::Vector3d(1,0,0));
    Eigen::Vector3d t_c0_c1(0, 0.028, 0);
    T_c0_c1_gt.block<3, 3>(0, 0) = c0_c1_vec.matrix();
    T_c0_c1_gt.block<3, 1>(0, 3) = t_c0_c1;

    Eigen::Matrix4d T_c0_l0_gt = Eigen::Matrix4d::Identity();
    Eigen::AngleAxisd c0_l0_vec = Eigen::AngleAxisd(-14*M_PI/180.0, Eigen::Vector3d(1,0,0));
    Eigen::Vector3d t_c0_l0(0, -0.014, 0.020);
    T_c0_l0_gt.block<3, 3>(0, 0) = c0_l0_vec.matrix();
    T_c0_l0_gt.block<3, 1>(0, 3) = t_c0_l0;

    Eigen::Matrix4d T_gt = Eigen::Matrix4d::Identity();
    if (eval_pattern == "camera0_to_camera1")
        T_gt = T_c0_c1_gt;
    if (eval_pattern == "camera0_to_lidar0")
        T_gt = T_c0_l0_gt;

    double avg_roll = 0.0, avg_pitch = 0.0, avg_yaw = 0.0;
    double avg_tx = 0, avg_ty = 0, avg_tz = 0.0;
    for(auto &T_ext : v_extrinsics){
        Eigen::Matrix4d T_error = T_gt.inverse() * T_ext;

        Eigen::Matrix3d R = T_ext.block<3, 3>(0, 0);
        Eigen::Vector3d v_angles = R.eulerAngles(2,1,0) * 180.0/M_PI;
        avg_yaw += (std::abs(v_angles[0]) > 90) ? (180 - std::abs(v_angles[0])) : std::abs(v_angles[0]);
        avg_pitch += (std::abs(v_angles[1]) > 90) ? (180 - std::abs(v_angles[1])) : std::abs(v_angles[1]);
        avg_roll += (std::abs(v_angles[2]) > 90) ? (180 - std::abs(v_angles[2])) : std::abs(v_angles[2]);
        LOG(INFO) << eval_pattern << " euler angles: " << v_angles.transpose() << "\n";

        Eigen::Vector3d t = T_ext.block<3,1>(0, 3);
        avg_tx += t[0]; avg_ty += t[1]; avg_tz += t[2];
        LOG(INFO) << eval_pattern << " trans vector: " << t.transpose() << "\n";
        Eigen::Matrix3d err_R = T_error.block<3, 3>(0, 0);
        Eigen::Vector3d err_t = T_error.block<3, 1>(0, 3);
        Eigen::AngleAxisd delta_R_vec(err_R);
        LOG(INFO) << " Rotation difference angle "
                    << delta_R_vec.angle() * 180.0 / M_PI << "\n";
        LOG(INFO) << " Translation difference norm is "
                    << err_t.norm() << "\n";
    }
    int size = v_extrinsics.size();
    LOG(INFO) << "Evaluate " << size  << " extrinsic parameters.";
    LOG(INFO) << "Avg euler angles: (" << avg_roll/size << ", " << avg_pitch/size << ", " << avg_yaw/size << ")";
    LOG(INFO) << "Avg trans: (" << avg_tx/size << ", " << avg_ty/size << ", " << avg_tz/size << ")";
}

// compare several camera poses or extrinsic params, to evaluate params consistency
int main(int argc, char** argv){

    // This exe is used to analyze the exytrinsic calibration repeatability
    if(argc < 2){
        std::cerr <<"Usage: ./test_eval_extrinsics [input_folder] [file_prefix]\n";
        std::cout << "[input_folder]: contains many extrinsic calibration result.\n";
        std::cout << "[file_prefix]: extrinsic file prefix, different sensor configuration results in different extrinsic file.\n";
        return -1;
    }

    std::string dataset_folder(argv[1]);
    std::string file_prefix(argv[2]);

    std::vector<Eigen::Matrix4d> v_extrinsics;

    bool b_eval_backpack = false, b_eval_antman = false, b_eval_kaleido = false;
    if (dataset_folder.find("backpack") != std::string::npos)
        b_eval_backpack = true;
    if (dataset_folder.find("antman") != std::string::npos)
        b_eval_antman = true;
    if (dataset_folder.find("kaleido") != std::string::npos)
        b_eval_kaleido = true;

    for (const auto & entry : boost::filesystem::directory_iterator(dataset_folder)){
        if(!boost::filesystem::is_directory(entry))
            continue;

        std::string extrin_filepath = entry.path().string() + "/" +file_prefix + ".yml";
        Eigen::Matrix4d T_ext = Eigen::Matrix4d::Identity();
        if (common::loadExtFileOpencv(extrin_filepath, T_ext)){
            LOG(INFO) << "Read extrinsic parameters from " << extrin_filepath;
        }else{
            LOG(ERROR) << "Fail to load " << extrin_filepath;
            continue;
        }
        v_extrinsics.emplace_back(T_ext);
    }

    if (b_eval_backpack){
    evaluateBackpackExt(v_extrinsics, file_prefix);
    }else{
        if (b_eval_antman){
            evaluateAntmanExt(v_extrinsics, file_prefix);
        }else{
            evaluateKaleidoExt(v_extrinsics, file_prefix);
        }
    }
    return 0;
}


#include <Eigen/Dense>
#include <fcl/math/constants.h>
#include <fcl/narrowphase/collision.h>
#include <fcl/narrowphase/collision_object.h>
#include <functional>
#include <map>
#include <string>
#include <utility>
#include <vector>

using fcl::AngleAxis;
using fcl::Transform3;
using fcl::Vector3;
using std::map;
using std::pair;
using std::string;
using std::vector;

// Simple specification for defining a box collision object. Specifies the
// dimensions and pose of the box in some frame F (X_FB). For an explanation
// of the notation X_FB, see:
// http://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html
template <typename S> struct BoxSpecification {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Vector3<S> size;
  Transform3<S> X_FB;
};

int main(int argc, char **argv) {
  const double pi = fcl::constants<double>::pi();
  const double size_1 = 1;
  BoxSpecification<double> box_spec_1{
      Vector3<double>{size_1, size_1, size_1},
      Transform3<double>{
          AngleAxis<double>(pi / 4, fcl::Vector3<double>::UnitY())}};

  const double size_2 = 3;
  BoxSpecification<double> box_spec_2{
      fcl::Vector3<double>{size_2, size_2, size_2},
      fcl::Transform3<double>{
          fcl::Translation3<double>(fcl::Vector3<double>(0, 0, -size_2 / 2))}};

  fcl::Vector3<double> expected_normal{0, 0, -1};
  double expected_depth = size_1 * sqrt(2) / 2;

  // Initialize isomorphic rotations of box 2.
  map<string, fcl::Transform3<double>, std::less<string>,
      Eigen::aligned_allocator<
          std::pair<const string, fcl::Transform3<double>>>>
      iso_poses;
  iso_poses["top"] = Transform3<double>::Identity();
  iso_poses["bottom"] =
      Transform3<double>{AngleAxis<double>(pi, Vector3<double>::UnitX())};
  iso_poses["back"] =
      Transform3<double>{AngleAxis<double>(pi / 2, Vector3<double>::UnitX())};
  iso_poses["front"] = Transform3<double>{
      AngleAxis<double>(3 * pi / 2, Vector3<double>::UnitX())};
  iso_poses["left"] =
      Transform3<double>{AngleAxis<double>(pi / 2, Vector3<double>::UnitY())};
  iso_poses["right"] = Transform3<double>{
      AngleAxis<double>(3 * pi / 2, Vector3<double>::UnitY())};

  fcl::Contact<double> expected_contact;
  expected_contact.penetration_depth = expected_depth;

  for (const auto &reorient_pair : iso_poses) {
    const std::string &top_face = reorient_pair.first;
    const fcl::Transform3<double> &pre_pose = reorient_pair.second;

    BoxSpecification<double> box_2_posed{box_spec_2.size,
                                         box_spec_2.X_FB * pre_pose};

    // Collide (1, 2)
    expected_contact.normal = expected_normal;
    {
      using CollisionGeometryPtr_t =
          std::shared_ptr<fcl::CollisionGeometry<double>>;
      CollisionGeometryPtr_t box_geometry_A(
          new fcl::Box<double>(box_spec_1.size));
      CollisionGeometryPtr_t box_geometry_B(
          new fcl::Box<double>(box_spec_2.size));

      fcl::CollisionObject<double> box_A(box_geometry_A, box_spec_1.X_FB);
      fcl::CollisionObject<double> box_B(box_geometry_B, box_spec_2.X_FB);

      // Compute collision - single contact and enable contact.
      fcl::CollisionRequest<double> collisionRequest(1, true);
      collisionRequest.gjk_solver_type = fcl::GST_LIBCCD;
      fcl::CollisionResult<double> collisionResult;
      fcl::collide(&box_A, &box_B, collisionRequest, collisionResult);
      std::vector<fcl::Contact<double>> contacts;
      collisionResult.getContacts(contacts);

      const fcl::Contact<double> &contact = contacts[0];
    }

    // Collide (2, 1)
    expected_contact.normal = -expected_normal;
    {
      using CollisionGeometryPtr_t =
          std::shared_ptr<fcl::CollisionGeometry<double>>;
      CollisionGeometryPtr_t box_geometry_A(
          new fcl::Box<double>(box_spec_2.size));
      CollisionGeometryPtr_t box_geometry_B(
          new fcl::Box<double>(box_spec_1.size));

      fcl::CollisionObject<double> box_A(box_geometry_A, box_spec_2.X_FB);
      fcl::CollisionObject<double> box_B(box_geometry_B, box_spec_1.X_FB);

      // Compute collision - single contact and enable contact.
      fcl::CollisionRequest<double> collisionRequest(1, true);
      collisionRequest.gjk_solver_type = fcl::GST_LIBCCD;
      fcl::CollisionResult<double> collisionResult;
      fcl::collide(&box_A, &box_B, collisionRequest, collisionResult);
      std::vector<fcl::Contact<double>> contacts;
      collisionResult.getContacts(contacts);

      const fcl::Contact<double> &contact = contacts[0];
    }

  }
}


// Copyright 2008 Gunter Winkler <guwi17@gmx.de>
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

// switch automatic singular check off
#define BOOST_UBLAS_TYPE_CHECK 0

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/cstdlib.hpp>

#include "common/testhelper.hpp"

#include <iostream>
#include <sstream>

using namespace boost::numeric::ublas;
using std::string;

static const string matrix_IN = "[3,3]((1,2,2),(2,3,3),(3,4,6))\0";
static const string matrix_LU = "[3,3]((3,4,6),(3.33333343e-01,6.66666627e-01,0),(6.66666687e-01,4.99999911e-01,-1))\0";
static const string matrix_INV= "[3,3]((-3,2,-7.94728621e-08),(1.50000012,0,-5.00000060e-01),(4.99999911e-01,-1,5.00000060e-01))\0";
static const string matrix_PM = "[3](2,2,2)";

int main () {

  typedef float TYPE;

  typedef matrix<TYPE> MATRIX;

  MATRIX A;
  MATRIX LU;
  MATRIX INV;

  {
    std::istringstream is(matrix_IN);
    is >> A;
  }
  {
    std::istringstream is(matrix_LU);
    is >> LU;
  }
  {
    std::istringstream is(matrix_INV);
    is >> INV;
  }
  permutation_matrix<>::vector_type temp;
  {
    std::istringstream is(matrix_PM);
    is >> temp;
  }
  permutation_matrix<> PM(temp);

  permutation_matrix<> pm(3);

  int result = lu_factorize<MATRIX, permutation_matrix<> >(A, pm);

  assertTrue("factorization completed: ", 0 == result);
  assertTrue("LU factors are correct: ", compare(A, LU));
  assertTrue("permutation is correct: ", compare(pm, PM));

  MATRIX B = identity_matrix<TYPE>(A.size2());

  lu_substitute(A, pm, B);

  assertTrue("inverse is correct: ", compare(B, INV));

  return (getResults().second > 0) ? boost::exit_failure : boost::exit_success;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2020 Digvijay Janartha, Hamirpur, India.

// Use, modification and distribution is 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)

#include <iostream>

#include <geometry_test_common.hpp>

#include <boost/core/ignore_unused.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/append.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/multi_polygon.hpp>
#include <boost/geometry/geometries/concepts/multi_polygon_concept.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>
#include <boost/geometry/io/dsv/write.hpp>

#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)
BOOST_GEOMETRY_REGISTER_BOOST_TUPLE_CS(cs::cartesian)

#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
#include <initializer_list>
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST

template <typename P>
bg::model::polygon<P> create_polygon()
{
    bg::model::polygon<P> pl1;
    P p1;
    P p2;
    P p3;
    bg::assign_values(p1, 1, 2);
    bg::assign_values(p2, 2, 0);
    bg::assign_values(p3, 0, 0);

    bg::append(pl1, p1);
    bg::append(pl1, p2);
    bg::append(pl1, p3);
    bg::append(pl1, p1);
    return pl1;
}

template <typename P, typename PL>
bg::model::multi_polygon<PL> create_multi_polygon()
{
    bg::model::multi_polygon<PL> mpl1;
    PL pl1(create_polygon<P>());
    mpl1.push_back(pl1);
    mpl1.push_back(pl1);
    return mpl1;
}

template <typename MPL, typename PL>
void check_multi_polygon(MPL& to_check, PL pl1)
{
    MPL cur;
    cur.push_back(pl1);
    cur.push_back(pl1);

    std::ostringstream out1, out2;
    out1 << bg::dsv(to_check);
    out2 << bg::dsv(cur);
    BOOST_CHECK_EQUAL(out1.str(), out2.str());
}

template <typename P, typename PL>
void test_default_constructor()
{
    bg::model::multi_polygon<PL> mpl1(create_multi_polygon<P, PL>());
    check_multi_polygon(mpl1, PL(create_polygon<P>()));
}

template <typename P, typename PL>
void test_copy_constructor()
{
    bg::model::multi_polygon<PL> mpl1 = create_multi_polygon<P, PL>();
    check_multi_polygon(mpl1, PL(create_polygon<P>()));
}

template <typename P, typename PL>
void test_copy_assignment()
{
    bg::model::multi_polygon<PL> mpl1(create_multi_polygon<P, PL>()), mpl2;
    mpl2 = mpl1;
    check_multi_polygon(mpl2, PL(create_polygon<P>()));
}

template <typename PL>
void test_concept()
{
    typedef bg::model::multi_polygon<PL> MPL;

    BOOST_CONCEPT_ASSERT( (bg::concepts::ConstMultiPolygon<MPL>) );
    BOOST_CONCEPT_ASSERT( (bg::concepts::MultiPolygon<MPL>) );

    typedef typename bg::coordinate_type<MPL>::type T;
    typedef typename bg::point_type<MPL>::type PMPL;
    boost::ignore_unused<T, PMPL>();
}

template <typename P>
void test_all()
{
    typedef bg::model::polygon<P> PL;

    test_default_constructor<P, PL>();
    test_copy_constructor<P, PL>();
    test_copy_assignment<P, PL>();
    test_concept<PL>();
}

template <typename P>
void test_custom_multi_polygon(bg::model::polygon<P> IL)
{
    typedef bg::model::polygon<P> PL;

    std::initializer_list<PL> PIL = {IL};
    bg::model::multi_polygon<PL> mpl1(PIL);
    std::ostringstream out;
    out << bg::dsv(mpl1);
    BOOST_CHECK_EQUAL(out.str(), "((((3, 3), (3, 0), (0, 0), (0, 3), (3, 3))))");
}

template <typename P>
void test_custom()
{
#ifdef BOOST_NO_CXX11_HDR_INITIALIZER_LIST
    std::initializer_list<P> IL = {P(3, 3), P(3, 0), P(0, 0), P(0, 3), P(3, 3)};
    bg::model::ring<P> r1(IL);
    std::initializer_list<bg::model::ring<P> > RIL = {r1};
    test_custom_multi_polygon<P>(RIL);
#endif//BOOST_NO_CXX11_HDR_INITIALIZER_LIST
}

template <typename CS>
void test_cs()
{
    test_all<bg::model::point<int, 2, CS> >();
    test_all<bg::model::point<float, 2, CS> >();
    test_all<bg::model::point<double, 2, CS> >();

    test_custom<bg::model::point<double, 2, CS> >();
}

int test_main(int, char* [])
{
    test_cs<bg::cs::cartesian>();
    test_cs<bg::cs::spherical<bg::degree> >();
    test_cs<bg::cs::spherical_equatorial<bg::degree> >();
    test_cs<bg::cs::geographic<bg::degree> >();

    test_custom<bg::model::d2::point_xy<double> >();

    return 0;
}


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

//[tiernan_girth_circumference
#include <iostream>

#include <boost/graph/directed_graph.hpp>
#include <boost/graph/tiernan_all_cycles.hpp>

#include "helper.hpp"

using namespace std;
using namespace boost;

// Declare the graph type and its vertex and edge types.
typedef directed_graph<> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

int
main(int argc, char *argv[])
{
    // Create the graph and read it from standard input.
    Graph g;
    read_graph(g, cin);

    // Compute the girth and circumference simulataneously
    size_t girth, circ;
    boost::tie(girth, circ) = tiernan_girth_and_circumference(g);

    // Print the result
    cout << "girth: " << girth << endl;
    cout << "circumference: " << circ << endl;

    return 0;
}
//]


#include <basix/e-lagrange.h>
#include <basix/quadrature.h>
#include <boost/program_options.hpp>
#include <cmath>
#include <dolfinx.h>
#include <dolfinx/io/XDMFFile.h>
#include <iostream>

#include <cublas_v2.h>
#include <cuda_profiler_api.h>

// Helper functions
#include "precompute.hpp"
#include <cuda/allocator.hpp>
#include <cuda/array.hpp>
#include <cuda/la.hpp>
#include <cuda/scatter.hpp>
#include <cuda/transform.hpp>
#include <cuda/utils.hpp>

using namespace dolfinx;
namespace po = boost::program_options;

void assert_cublas(cudaError_t e) {
  if (e != cudaSuccess)
    throw std::runtime_error(" Unable to allocate memoy - cublas error");
}
int main(int argc, char* argv[]) {

  po::options_description desc("Allowed options");
  desc.add_options()("help,h", "print usage message")(
      "size", po::value<std::size_t>()->default_value(32))(
      "degree", po::value<int>()->default_value(1));

  po::variables_map vm;
  po::store(po::command_line_parser(argc, argv).options(desc).allow_unregistered().run(),
            vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 0;
  }

  const std::size_t Nx = vm["size"].as<std::size_t>();
  const int degree = vm["degree"].as<int>();

  common::subsystem::init_logging(argc, argv);
  common::subsystem::init_mpi(argc, argv);
  {
    // MPI
    MPI_Comm mpi_comm{MPI_COMM_WORLD};
    int rank = utils::set_device(mpi_comm);

    // Create cublas handle
    cublasHandle_t handle;
    cublasCreate(&handle);

    // Read mesh and mesh tags
    std::array<std::array<double, 3>, 2> p = {{{0.0, 0.0, 0.0}, {1.0, 1.0, 1.0}}};
    std::array<std::size_t, 3> n = {Nx, Nx, Nx};
    auto mesh = std::make_shared<mesh::Mesh>(mesh::create_box(
        mpi_comm, p, n, mesh::CellType::hexahedron, mesh::GhostMode::none));

    // Create a Basix continuous Lagrange element of given degree
    basix::FiniteElement e = basix::element::create_lagrange(
        mesh::cell_type_to_basix_type(mesh::CellType::hexahedron), degree,
        basix::element::lagrange_variant::equispaced, false);

    // Create a scalar function space
    std::shared_ptr<fem::FunctionSpace> V
        = std::make_shared<fem::FunctionSpace>(fem::create_functionspace(mesh, e, 1));
    auto idxmap = V->dofmap()->index_map;

    int ncells = mesh->topology().index_map(3)->size_local();
    int ndofs = e.dim();

    fem::Function<double> u(V);
    // Interpolate sin(2 \pi x[0]) in the scalar Lagrange finite element space
    constexpr double PI = xt::numeric_constants<double>::PI;
    u.interpolate([PI](auto&& x) { return xt::sin(2 * PI * xt::row(x, 0)); });

    CUDA::allocator<double> allocator{};
    la::Vector<double, decltype(allocator)> x(idxmap, 1, allocator);
    la::Vector<double, decltype(allocator)> y(idxmap, 1, allocator);

    auto uarray = u.x()->array();
    std::copy(uarray.begin(), uarray.end(), x.mutable_array().begin());

    linalg::prefetch(0, x);
    linalg::prefetch(0, y);

    // =====================================
    // Tabulate basis functions at quadrature points
    // 1 - Tabulate quadrature points and weights
    int q = 2 * degree; // Quadrature degree
    auto cell = basix::cell::type::hexahedron;
    auto quad = basix::quadrature::type::gauss_jacobi;
    auto [points, weights] = basix::quadrature::make_quadrature(quad, cell, q);

    std::int32_t nquads = weights.size();
    std::int32_t Ne = ncells * ndofs;
    std::int32_t Nq = ncells * nquads;

    // 2 - Tabulate basis functions
    xt::xtensor<double, 4> basis = e.tabulate(0, points);
    xt::xtensor<double, 2> _phi = xt::view(basis, 0, xt::all(), xt::all(), 0);
    cuda::array<double> phi(ndofs * nquads);
    phi.set(_phi);
    _phi = xt::transpose(_phi);
    cuda::array<double> phiT(ndofs * nquads);
    phiT.set(_phi);

    // =====================================
    // Get dofmap for the gather/scatter
    // Copy dofmap data to device
    cuda::array<std::int32_t> dofmap(Ne);
    const std::vector<std::int32_t>& dof_array = V->dofmap()->list().array();
    dofmap.set(dof_array);

    // =====================================
    // Compute determinant of jacobian at quadrature points
    xt::xtensor<double, 4> J = compute_jacobian(mesh, points);
    xt::xtensor<double, 2> _detJ = compute_jacobian_determinant(J);
    for (std::size_t i = 0; i < _detJ.shape(0); i++)
      for (std::size_t j = 0; j < _detJ.shape(1); j++)
        _detJ(i, j) = _detJ(i, j) * weights[j];
    cuda::array<double> detJ(Nq);
    detJ.set(_detJ);

    // Allocate memory for working arrays on device
    cuda::array<double> ue(Ne);
    cuda::array<double> uq(Nq);
    cuda::array<double> xe(Ne);

    double alpha = 1;
    double beta = 0;

    double t = MPI_Wtime();
    // =====================================
    // Apply gather operator Ue = G u
    // Ue <- u[dofmap]
    // From global dof vector to element based dof vector
    gather(ue.size(), dofmap.data(), x.array().data(), ue.data(), 512);

    // =====================================
    // Apply operator B^T D B to Ue
    // Uq^ = B Ue^T
    cublasDgemm(handle, CUBLAS_OP_N, CUBLAS_OP_N, nquads, ncells, ndofs, &alpha,
                phiT.data(), nquads, ue.data(), ndofs, &beta, uq.data(), nquads);
    // Uq = detJ .* Uq
    transform1(Ne, uq.data(), detJ.data(), uq.data(), 512);
    // Xe^T = B^T Uq^T
    cublasDgemm(handle, CUBLAS_OP_N, CUBLAS_OP_N, ndofs, ncells, nquads, &alpha,
                phi.data(), ndofs, uq.data(), nquads, &beta, xe.data(), ndofs);
    // =====================================
    // Apply scatter operator
    // x[dofmap] <- Xe
    // From element based dof vector to global dof vector
    scatter(xe.size(), dofmap.data(), xe.data(), y.mutable_array().data(), 512);
    cudaDeviceSynchronize();
    t = MPI_Wtime() - t;

    std::cout << y.norm() << std::endl;

    std::cout << "Number of cells: " << ncells;
    std::cout << "\nNumber of dofs: " << ndofs;
    std::cout << "\nNumber of quads: " << nquads;
    double ops = 4 * ncells * nquads * ndofs + ncells * nquads;
    std::cout << "\n#Elapsed Time: " << t << std::endl;
    std::cout << "\nDOF/s: " << V->dofmap()->index_map->size_local() / t;
    std::cout << std::endl;
  }

  common::subsystem::finalize_mpi();
  return 0;
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#include <libint2/initialize.h>
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE aomatrix_test

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/aomatrix.h"
#include "votca/xtp/orbitals.h"
#include <votca/tools/eigenio_matrixmarket.h>

using namespace votca::xtp;
using namespace votca;
using namespace std;

BOOST_AUTO_TEST_SUITE(aomatrix_test)

QMMolecule Methane() {

  QMMolecule mol(" ", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) +
                   "/aomatrix/molecule.xyz");
  return mol;
}

BOOST_AUTO_TEST_CASE(aomatrices_test) {
  libint2::initialize();
  QMMolecule mol = Methane();
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, mol);
  AOOverlap overlap;
  overlap.Fill(aobasis);
  Eigen::MatrixXd overlap_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/overlap_ref.mm");

  bool check_overlap = overlap.Matrix().isApprox(overlap_ref, 0.0001);
  BOOST_CHECK_EQUAL(check_overlap, 1);
  if (!check_overlap) {
    cout << "ref" << endl;
    cout << overlap_ref << endl;
    cout << "result" << endl;
    cout << overlap.Matrix() << endl;
  }

  AOKinetic kinetic;
  kinetic.Fill(aobasis);
  Eigen::MatrixXd kinetic_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/kinetic_ref.mm");

  bool check_kinetic = kinetic.Matrix().isApprox(kinetic_ref, 0.00001);
  BOOST_CHECK_EQUAL(check_kinetic, 1);
  if (!check_kinetic) {
    cout << "ref" << endl;
    cout << kinetic_ref << endl;
    cout << "result" << endl;
    cout << kinetic.Matrix() << endl;
  }

  AOCoulomb coulomb;
  coulomb.Fill(aobasis);
  Eigen::MatrixXd coulomb_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/coulomb_ref.mm");
  bool check_coulomb = coulomb.Matrix().isApprox(coulomb_ref, 0.00001);
  BOOST_CHECK_EQUAL(check_coulomb, 1);
  if (!check_coulomb) {
    cout << "ref" << endl;
    cout << coulomb_ref << endl;
    cout << "result" << endl;
    cout << coulomb.Matrix() << endl;
  }

  Eigen::MatrixXd ps_invSqrt = coulomb.Pseudo_InvSqrt(1e-7);
  Eigen::MatrixXd coulombinvsqrt_ref =
      votca::tools::EigenIO_MatrixMarket::ReadMatrix(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/aomatrix/coulombinvsqrt_ref.mm");

  bool check_coulombinvsqrt = ps_invSqrt.isApprox(coulombinvsqrt_ref, 0.00001);
  BOOST_CHECK_EQUAL(check_coulombinvsqrt, 1);
  if (!check_coulombinvsqrt) {
    cout << "ref" << endl;
    cout << coulombinvsqrt_ref << endl;
    cout << "result" << endl;
    cout << ps_invSqrt << endl;
  }

  Eigen::MatrixXd ps_invSqrtgw = coulomb.Pseudo_InvSqrt_GWBSE(overlap, 1e-7);
  Eigen::MatrixXd coulombinvsqrtgw_ref =
      votca::tools::EigenIO_MatrixMarket::ReadMatrix(
          std::string(XTP_TEST_DATA_FOLDER) +
          "/aomatrix/coulombinvsqrtgw_ref.mm");

  bool check_coulombinvsqrtgw =
      ps_invSqrtgw.isApprox(coulombinvsqrtgw_ref, 0.00001);

  BOOST_CHECK_EQUAL(check_coulombinvsqrtgw, 1);
  if (!check_coulombinvsqrtgw) {
    cout << "ref" << endl;
    cout << coulombinvsqrtgw_ref << endl;
    cout << "result" << endl;
    cout << ps_invSqrtgw << endl;
  }
  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(aomatrices_contracted_test) {
  libint2::initialize();
  QMMolecule mol("C", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/C.xyz");
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/contracted.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, mol);
  AOOverlap overlap;
  overlap.Fill(aobasis);
  Eigen::MatrixXd overlap_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) +
      "/aomatrix/overlap_ref_contracted.mm");

  bool check_overlap = overlap.Matrix().isApprox(overlap_ref, 0.0001);
  if (!check_overlap) {
    std::cout << std::endl;
    std::cout << "Ref" << std::endl;
    std::cout << overlap_ref << std::endl;
    std::cout << "Result" << std::endl;
    std::cout << overlap.Matrix() << std::endl;
  }
  BOOST_CHECK_EQUAL(check_overlap, 1);
  libint2::finalize();
}

BOOST_AUTO_TEST_CASE(aocoulomb_inv_test) {
  libint2::initialize();
  QMMolecule mol = Methane();
  BasisSet basis;
  basis.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/3-21G.xml");
  AOBasis aobasis;
  aobasis.Fill(basis, mol);

  AOCoulomb cou;
  cou.Fill(aobasis);

  Eigen::MatrixXd PseudoInvSqrt = cou.Pseudo_InvSqrt(1e-7);

  Eigen::MatrixXd Reformed = PseudoInvSqrt * PseudoInvSqrt * cou.Matrix();

  bool check_inv = Reformed.isApprox(Eigen::MatrixXd::Identity(17, 17), 0.0001);
  if (!check_inv) {
    std::cout << "reformed" << endl;
    std::cout << Reformed << endl;
  }
  BOOST_CHECK_EQUAL(check_inv, 1);
  libint2::finalize();
}

/*BOOST_AUTO_TEST_CASE(large_l_test) {

  QMMolecule mol("C", 0);
  mol.LoadFromFile(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/C2.xyz");

  BasisSet basisset;
  basisset.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/G.xml");

  BasisSet auxbasisset;
  auxbasisset.Load(std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/I.xml");
  AOBasis dftbasis;
  dftbasis.Fill(basisset, mol);

  AOBasis auxbasis;
  auxbasis.Fill(auxbasisset, mol);

  AOOverlap overlap;
  overlap.Fill(auxbasis);

  Eigen::MatrixXd overlap_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/overlap_ref_gi.mm");

  bool check_overlap = overlap.Matrix().isApprox(overlap_ref, 0.00001);

  BOOST_CHECK_EQUAL(check_overlap, 1);
  if (!check_overlap) {
    cout << "ref" << endl;
    cout << overlap_ref << endl;
    cout << "result" << endl;
    cout << overlap.Matrix() << endl;
  }

  AOCoulomb coulomb;
  coulomb.Fill(auxbasis);
  Eigen::MatrixXd coulomb_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/coulomb_ref_gi.mm");

  bool check_coulomb = coulomb.Matrix().isApprox(coulomb_ref, 0.00001);
  BOOST_CHECK_EQUAL(check_coulomb, 1);
  if (!check_coulomb) {
    cout << "ref" << endl;
    cout << coulomb_ref << endl;
    cout << "result" << endl;
    cout << coulomb.Matrix() << endl;
  }

  AOKinetic kinetic;
  kinetic.Fill(dftbasis);

  Eigen::MatrixXd kinetic_ref = votca::tools::EigenIO_MatrixMarket::ReadMatrix(
      std::string(XTP_TEST_DATA_FOLDER) + "/aomatrix/kinetic_ref_gi.mm");

  bool check_kinetic = kinetic.Matrix().isApprox(kinetic_ref, 0.00001);

  BOOST_CHECK_EQUAL(check_kinetic, 1);
  if (!check_kinetic) {
    cout << "ref" << endl;
    cout << kinetic_ref << endl;
    cout << "result" << endl;
    cout << kinetic.Matrix() << endl;
  }
} */

BOOST_AUTO_TEST_SUITE_END()


/*
In this tutorial we will learn how to use the pcl::MomentOfInertiaEstimation class
in order to obtain descriptors based on eccentricity and moment of inertia.
This class also allows to extract axis aligned and oriented bounding boxes of the cloud.
But keep in mind that extracted OBB is not the minimal possible bounding box.
*/

#include "stdafx.h"
#include <iostream>
#include <boost/thread/thread.hpp>
#include <pcl/point_types.h>
#include <pcl/features/moment_of_inertia_estimation.h>
#include <pcl/visualization/pcl_visualizer.h>
#include "KinnectGrabber.h"

int main()
{
	pcl::io::OpenNI2Grabber grabber;
	KinnectGrabber<pcl::PointXYZRGBA> v(grabber);
	v.run();

	pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud(new pcl::PointCloud<pcl::PointXYZRGBA>);
	cloud = v.getLatestCloud();

	if (cloud == nullptr)
	{
		std::cout << "Get cloud failed!" << std::endl;
	}
	else
	{
		// create moment estimator and extratct
		pcl::MomentOfInertiaEstimation<pcl::PointXYZRGBA> feature_extractor;
		feature_extractor.setInputCloud(cloud);
		feature_extractor.compute();

		// define moment and eccentricity etc variable
		std::vector<float> moment_of_intertia;
		std::vector<float> eccentricity;
		pcl::PointXYZRGBA min_point_AABB;
		pcl::PointXYZRGBA max_point_AABB;
		pcl::PointXYZRGBA min_point_OBB;
		pcl::PointXYZRGBA max_point_OBB;
		pcl::PointXYZRGBA position_OBB;
		Eigen::Matrix3f  rotational_matrix_OBB;
		float major_value, middle_value, minor_value;
		Eigen::Vector3f major_vector, middle_vector, minor_vector;
		Eigen::Vector3f mass_center;

		// get moment and eccentricity
		feature_extractor.getMomentOfInertia(moment_of_intertia);
		feature_extractor.getEccentricity(eccentricity);
		feature_extractor.getAABB(min_point_AABB, max_point_AABB);
		feature_extractor.getOBB(min_point_OBB, max_point_OBB, position_OBB, rotational_matrix_OBB);
		feature_extractor.getEigenValues(major_value, middle_value, minor_value);
		feature_extractor.getEigenVectors(major_vector, middle_vector, minor_vector);
		feature_extractor.getMassCenter(mass_center);

		boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
		viewer->setBackgroundColor(0, 0, 0);
		viewer->addCoordinateSystem(1.0);
		viewer->initCameraParameters();
		viewer->addPointCloud<pcl::PointXYZRGBA>(cloud, "sample cloud");
		viewer->addCube(min_point_AABB.x, max_point_AABB.x, min_point_AABB.y, max_point_AABB.y, min_point_AABB.z, max_point_AABB.z, 1.0, 1.0, 0.0, "AABB");

		Eigen::Vector3f position(position_OBB.x, position_OBB.y, position_OBB.z);
		Eigen::Quaternionf quat(rotational_matrix_OBB);
		viewer->addCube(position, quat, max_point_OBB.x - min_point_OBB.x, max_point_OBB.y - min_point_OBB.y, max_point_OBB.z - min_point_OBB.z, "OBB");

		pcl::PointXYZ center(mass_center(0), mass_center(1), mass_center(2));
		pcl::PointXYZ x_axis(major_vector(0) + mass_center(0), major_vector(1) + mass_center(1), major_vector(2) + mass_center(2));
		pcl::PointXYZ y_axis(middle_vector(0) + mass_center(0), middle_vector(1) + mass_center(1), middle_vector(2) + mass_center(2));
		pcl::PointXYZ z_axis(minor_vector(0) + mass_center(0), minor_vector(1) + mass_center(1), minor_vector(2) + mass_center(2));
		viewer->addLine(center, x_axis, 1.0f, 0.0f, 0.0f, "major eigen vector");
		viewer->addLine(center, y_axis, 0.0f, 1.0f, 0.0f, "middle eigen vector");
		viewer->addLine(center, z_axis, 0.0f, 0.0f, 1.0f, "minor eigen vector");

		while (!viewer->wasStopped())
		{
			viewer->spinOnce(100);
			boost::this_thread::sleep(boost::posix_time::microseconds(100000));
		}
	}

	return 0;
}


#include "Utilities/DBC.hh"
#include <Eigen/Dense>
#include <iostream>

namespace Spheral {

//------------------------------------------------------------------------------
// Default constructor
//------------------------------------------------------------------------------
inline
BiQuadraticInterpolator::BiQuadraticInterpolator():
  mnx1(),
  mny1(),
  mxmin(),
  mxmax(),
  mymin(),
  mymax(),
  mxstep(),
  mystep(),
  mcoeffs() {
}

//------------------------------------------------------------------------------
// Construct with tabulated data
//------------------------------------------------------------------------------
template<typename Func>
BiQuadraticInterpolator::BiQuadraticInterpolator(const double xmin,
                                                 const double xmax,
                                                 const double ymin,
                                                 const double ymax,
                                                 const size_t nx,
                                                 const size_t ny,
                                                 const Func& F):
  mnx1(),
  mny1(),
  mxmin(),
  mxmax(),
  mxstep(),
  mcoeffs() {
  this->initialize(xmin, xmax, ymin, ymax, nx, ny, F);
}

//------------------------------------------------------------------------------
// Destructor
//------------------------------------------------------------------------------
inline
BiQuadraticInterpolator::~BiQuadraticInterpolator() {
}

//------------------------------------------------------------------------------
// Initialize the interpolation to fit the given data
//------------------------------------------------------------------------------
template<typename Func>
inline
void
BiQuadraticInterpolator::initialize(const double xmin,
                                    const double xmax,
                                    const double ymin,
                                    const double ymax,
                                    const size_t nx,
                                    const size_t ny,
                                    const Func& F) {

  // Size stuff up.
  REQUIRE(nx > 2u);
  REQUIRE(ny > 2u);
  mnx1 = nx - 2u;
  mny1 = ny - 2u;
  mcoeffs.resize(6*mnx1*mny1);

  // Figure out the sampling steps.
  mxmin = xmin;
  mxmax = xmax;
  mymin = ymin;
  mymax = ymax;
  mxstep = (xmax - xmin)/(nx - 1u);
  mystep = (ymax - ymin)/(ny - 1u);

  // Fit the coefficients
  Eigen::Vector2d x00, x01, x02, x10, x11, x12, x20, x21, x22;
  Eigen::MatrixXd A(9, 6);
  Eigen::VectorXd b(9), c(9);
  for (auto i = 0u; i < mnx1; ++i) {
    for (auto j = 0u; j < mny1; ++j) {
      x00 = {xmin + i      *mxstep, ymin + j      *mystep};
      x10 = {xmin + (i + 1)*mxstep, ymin + j      *mystep};
      x20 = {xmin + (i + 2)*mxstep, ymin + j      *mystep};
      x01 = {xmin + i      *mxstep, ymin + (j + 1)*mystep};
      x11 = {xmin + (i + 1)*mxstep, ymin + (j + 1)*mystep};
      x21 = {xmin + (i + 2)*mxstep, ymin + (j + 1)*mystep};
      x02 = {xmin + i      *mxstep, ymin + (j + 2)*mystep};
      x12 = {xmin + (i + 1)*mxstep, ymin + (j + 2)*mystep};
      x22 = {xmin + (i + 2)*mxstep, ymin + (j + 2)*mystep};
      A << 1.0, x00[0], x00[1], x00[0]*x00[1], x00[0]*x00[0], x00[1]*x00[1],
           1.0, x01[0], x01[1], x01[0]*x01[1], x01[0]*x01[0], x01[1]*x01[1],
           1.0, x02[0], x02[1], x02[0]*x02[1], x02[0]*x02[0], x02[1]*x02[1],
           1.0, x10[0], x10[1], x10[0]*x10[1], x10[0]*x10[0], x10[1]*x10[1],
           1.0, x11[0], x11[1], x11[0]*x11[1], x11[0]*x11[0], x11[1]*x11[1],
           1.0, x12[0], x12[1], x12[0]*x12[1], x12[0]*x12[0], x12[1]*x12[1],
           1.0, x20[0], x20[1], x20[0]*x20[1], x20[0]*x20[0], x20[1]*x20[1],
           1.0, x21[0], x21[1], x21[0]*x21[1], x21[0]*x21[0], x21[1]*x21[1],
           1.0, x22[0], x22[1], x22[0]*x22[1], x22[0]*x22[0], x22[1]*x22[1];
      b << F(x00[0], x00[1]),
           F(x01[0], x01[1]),
           F(x02[0], x02[1]),
           F(x10[0], x10[1]),
           F(x11[0], x11[1]),
           F(x12[0], x12[1]),
           F(x20[0], x20[1]),
           F(x21[0], x21[1]),
           F(x22[0], x22[1]);
      CHECK2(b == b, "BiQuadraticInterpoolator function return error: \n"
             << x00 << " : " << b[0] << "\n"
             << x01 << " : " << b[1] << "\n"
             << x02 << " : " << b[2] << "\n"
             << x10 << " : " << b[3] << "\n"
             << x11 << " : " << b[4] << "\n"
             << x12 << " : " << b[5] << "\n"
             << x20 << " : " << b[6] << "\n"
             << x21 << " : " << b[7] << "\n"
             << x22 << " : " << b[8] << "\n");
      c = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
      // std::cerr << "------------------------------------------------------------------------------\n"
      //           << "x00: " << x00 << "\n"
      //           << "x10: " << x10 << "\n"
      //           << "x20: " << x20 << "\n"
      //           << "x01: " << x01 << "\n"
      //           << "x11: " << x11 << "\n"
      //           << "x21: " << x21 << "\n"
      //           << "x02: " << x02 << "\n"
      //           << "x12: " << x12 << "\n"
      //           << "x22: " << x22 << "\n"
      //           << "A:\n" << A << "\n"
      //           << "b:\n" << b << "\n"
      //           << "c:\n" << c << "\n";
      const auto k = 6*(i + j*mnx1);
      mcoeffs[k    ] = c(0);
      mcoeffs[k + 1] = c(1);
      mcoeffs[k + 2] = c(2);
      mcoeffs[k + 3] = c(3);
      mcoeffs[k + 4] = c(4);
      mcoeffs[k + 5] = c(5);
    }
  }
}

//------------------------------------------------------------------------------
// Interpolate for the given coordinate.
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::operator()(const double xi,
                                    const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  // std::cerr << "================================================================================\n"
  //           << "mxlog, mylog : " << mxlog << " " << mylog << "\n"
  //           << "pos   : " << pos << "\n"
  //           << "(x,y) : " << x << " " << y << "\n"
  //           << "i0    : " << i0 << "\n"
  //           << "coeffs: " << mcoeffs[i0] << " " << mcoeffs[i0 + 1] << " " << mcoeffs[i0 + 2] << " " << mcoeffs[i0 + 3] << " " << mcoeffs[i0 + 4] << " " << mcoeffs[i0 + 5] << "\n"
  //           << "F(x,y): " << mcoeffs[i0] + mcoeffs[i0 + 1]*x + mcoeffs[i0 + 2]*y + mcoeffs[i0 + 3]*x*y + mcoeffs[i0 + 4]*x*x + mcoeffs[i0 + 5]*y*y << "\n";
  return mcoeffs[i0] + mcoeffs[i0 + 1]*x + mcoeffs[i0 + 2]*y + mcoeffs[i0 + 3]*x*y + mcoeffs[i0 + 4]*x*x + mcoeffs[i0 + 5]*y*y;
}

//------------------------------------------------------------------------------
// Interpolate for the gradient (x)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime_x(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 1] + mcoeffs[i0 + 3]*y + 2.0*mcoeffs[i0 + 4]*x;
}

//------------------------------------------------------------------------------
// Interpolate for the gradient (y)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime_y(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 2] + mcoeffs[i0 + 3]*x + 2.0*mcoeffs[i0 + 5]*y;
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (xx)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_xx(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return 2.0*mcoeffs[i0 + 4];
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (xy)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_xy(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 3];
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (yx)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_yx(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return mcoeffs[i0 + 3];
}

//------------------------------------------------------------------------------
// Interpolate for the gradient2 (yy)
//------------------------------------------------------------------------------
inline
double
BiQuadraticInterpolator::prime2_yy(const double xi, const double yi) const {
  const auto x = std::max(mxmin, std::min(mxmax, xi));
  const auto y = std::max(mymin, std::min(mymax, yi));
  const auto i0 = lowerBound(x, y);
  return 2.0*mcoeffs[i0 + 5];
}

//------------------------------------------------------------------------------
// Return the lower bound entry in the table for the given x coordinate
//------------------------------------------------------------------------------
inline
size_t
BiQuadraticInterpolator::lowerBound(const double x, const double y) const {
  const auto result = 6u*(mnx1*std::min(mny1 - 1u, size_t(std::max(0.0, y - mymin)/mystep)) +
                               std::min(mnx1 - 1u, size_t(std::max(0.0, x - mxmin)/mxstep)));
  ENSURE(result <= 6u*mnx1*mny1);
  return result;
}

//------------------------------------------------------------------------------
// Data accessors
//------------------------------------------------------------------------------
inline
size_t
BiQuadraticInterpolator::size() const {
  return mcoeffs.size();
}

inline
double
BiQuadraticInterpolator::xmin() const {
  return mxmin;
}

inline
double
BiQuadraticInterpolator::xmax() const {
  return mxmax;
}

inline
double
BiQuadraticInterpolator::ymin() const {
  return mymin;
}

inline
double
BiQuadraticInterpolator::ymax() const {
  return mymax;
}

inline
double
BiQuadraticInterpolator::xstep() const {
  return mxstep;
}

inline
double
BiQuadraticInterpolator::ystep() const {
  return mystep;
}

inline
const std::vector<double>&
BiQuadraticInterpolator::coeffs() const {
  return mcoeffs;
}

}


/* boost test_uniform_int.ipp
 *
 * Copyright Jens Maurer 2000
 * Copyright Steven Watanabe 2011
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * $Id$
 */

#include <numeric>
#include <sstream>
#include <vector>
#include <boost/config.hpp>
#include <boost/cstdint.hpp>
#include <boost/limits.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/lagged_fibonacci.hpp>
#include <boost/random/variate_generator.hpp>
#include "chi_squared_test.hpp"

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

template<class Generator>
void check_uniform_int(Generator & gen, int iter)
{
    int range = (gen.max)()-(gen.min)()+1;
    std::vector<int> bucket(range);
    for(int j = 0; j < iter; j++) {
        int result = gen();
        BOOST_CHECK_GE(result, (gen.min)());
        BOOST_CHECK_LE(result, (gen.max)());
        if(result >= (gen.min)() && result <= (gen.max)()) {
            bucket[result-(gen.min)()]++;
        }
    }
    int sum = std::accumulate(bucket.begin(), bucket.end(), 0);
    std::vector<double> expected(range, 1.0 / range);
    BOOST_CHECK_LT(chi_squared_test(bucket, expected, sum), 0.99);
}

BOOST_AUTO_TEST_CASE(test_uniform_int)
{
    boost::random::mt19937 gen;
    typedef BOOST_RANDOM_UNIFORM_INT<int> int_gen;

    // large range => small range (modulo case)
    typedef boost::random::variate_generator<boost::random::mt19937&, int_gen> level_one;

    level_one uint12(gen, int_gen(1,2));
    BOOST_CHECK((uint12.distribution().min)() == 1);
    BOOST_CHECK((uint12.distribution().max)() == 2);
    check_uniform_int(uint12, 100000);
    level_one uint16(gen, int_gen(1,6));
    check_uniform_int(uint16, 100000);

    // test chaining to get all cases in operator()

    // identity map
    typedef boost::random::variate_generator<level_one&, int_gen> level_two;
    level_two uint01(uint12, int_gen(0, 1));
    check_uniform_int(uint01, 100000);

    // small range => larger range
    level_two uint05(uint12, int_gen(-3, 2));
    check_uniform_int(uint05, 100000);

    // small range => larger range
    level_two uint099(uint12, int_gen(0, 99));
    check_uniform_int(uint099, 100000);

    // larger => small range, rejection case
    typedef boost::random::variate_generator<level_two&, int_gen> level_three;
    level_three uint1_4(uint05, int_gen(1, 4));
    check_uniform_int(uint1_4, 100000);

    typedef BOOST_RANDOM_UNIFORM_INT<boost::uint8_t> int8_gen;
    typedef boost::random::variate_generator<boost::random::mt19937&, int8_gen> gen8_t;

    gen8_t gen8_03(gen, int8_gen(0, 3));

    // use the full range of the type, where the destination
    // range is a power of the source range
    typedef boost::random::variate_generator<gen8_t, int8_gen> uniform_uint8;
    uniform_uint8 uint8_0255(gen8_03, int8_gen(0, 255));
    check_uniform_int(uint8_0255, 100000);

    // use the full range, but a generator whose range is not
    // a root of the destination range.
    gen8_t gen8_02(gen, int8_gen(0, 2));
    uniform_uint8 uint8_0255_2(gen8_02, int8_gen(0, 255));
    check_uniform_int(uint8_0255_2, 100000);

    // expand the range to a larger type.
    typedef boost::random::variate_generator<gen8_t, int_gen> uniform_uint_from8;
    uniform_uint_from8 uint0300(gen8_03, int_gen(0, 300));
    check_uniform_int(uint0300, 100000);
}

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)

// testcase by Mario Rutti
class ruetti_gen
{
public:
    ruetti_gen() : state((max)() - 1) {}
    typedef boost::uint64_t result_type;
    result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
    result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return std::numeric_limits<result_type>::max BOOST_PREVENT_MACRO_SUBSTITUTION (); }
    result_type operator()() { return state--; }
private:
    result_type state;
};

BOOST_AUTO_TEST_CASE(test_overflow_range)
{
    ruetti_gen gen;
    BOOST_RANDOM_DISTRIBUTION dist(0, 10);
    for (int i=0;i<10;i++) {
        dist(gen);
    }
}

#endif

BOOST_AUTO_TEST_CASE(test_misc)
{
    // bug report from Ken Mahler:  This used to lead to an endless loop.
    typedef BOOST_RANDOM_UNIFORM_INT<unsigned int> uint_dist;
    boost::minstd_rand mr;
    boost::variate_generator<boost::minstd_rand, uint_dist> r2(mr,
                                                            uint_dist(0, 0xffffffff));
    r2();
    r2();

    // bug report from Fernando Cacciola:  This used to lead to an endless loop.
    // also from Douglas Gregor
    boost::variate_generator<boost::minstd_rand, BOOST_RANDOM_DISTRIBUTION > x(mr, BOOST_RANDOM_DISTRIBUTION(0, 8361));
    x();

    // bug report from Alan Stokes and others: this throws an assertion
    boost::variate_generator<boost::minstd_rand, BOOST_RANDOM_DISTRIBUTION > y(mr, BOOST_RANDOM_DISTRIBUTION(1,1));
    y();
    y();
    y();
}


// ChiSquaredDistribution.hpp
//
// (C) Datasim Education BV  2009

#pragma once

#include <boost/math/distributions.hpp>

using namespace System;

namespace Wrapper
{
	// Wrapper for the boost::math::chi_squared_distribution class
	// We use the .NET naming conventions instead of the original C++ name
	public ref class ChiSquaredDistribution
	{
	private:
		// The wrapped native class (only pointers to native classes can be a C++/CLI class datamember)
		boost::math::chi_squared_distribution<>* m_distribution;

	public:
		// Default constructor
		ChiSquaredDistribution();

		// Constructor with value
		ChiSquaredDistribution(double value);

		// Finaliser (called by garbage collector or destructor)
		!ChiSquaredDistribution();

		// Destructor (Dispose)
		~ChiSquaredDistribution();

		// Get the native object
		boost::math::chi_squared_distribution<>* GetNative();
	};
}

windows/CsForFinancialMarketsPart2/Chapters20+21+22+23/Demos - CLI-CS Interop with Excel/CLI Interop Test (Chi-Squared)/Wrapper/ChiSquaredDistribution.hpp


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <algorithm>
#include <cmath>
#include <string>

#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

typedef mtl::dense2D<int> matrix_type;
using mtl::irange; using mtl::iall;

matrix_type inline my_row(int i, int, matrix_type& A)
{
    return A[irange(i, i+1)][iall];
}

matrix_type inline my_col(int, int j, matrix_type& A)
{
    return A[iall][irange(j, j+1)];
}

matrix_type inline my_block(int i, int j, matrix_type& A)
{
    int ib= i/3*3, jb= j/3*3;
    return A[irange(ib, ib+3)][irange(jb, jb+3)];
}

int inline nzero(const matrix_type& A)
{
    int n= 0;
    for (unsigned i= 0; i < num_rows(A); i++)
	for (unsigned j= 0; j < num_cols(A); j++)
	    if (A[i][j]) n++;
    return n;
}

// My relevant sub-matrices
struct my_sub_t
{
    my_sub_t(int i, int j, matrix_type& A)
	: i(i), j(j), mr(my_row(i, j, A)), mc(my_col(i, j, A)), mb(my_block(i, j, A))
    {}

    int         i, j;
    matrix_type mr, mc, mb;
};

bool inline conflict(int v, const matrix_type& A)
{
    for (unsigned i= 0; i < num_rows(A); i++)
	for (unsigned j= 0; j < num_cols(A); j++)
	    if (A[i][j] == v) return true;
    return false;
}

bool inline conflict(int v, const my_sub_t& sub)
{
    return conflict(v, sub.mr) || conflict(v, sub.mc) || conflict(v, sub.mb);
}

struct entry
{
    entry(int i, int j, matrix_type& A)
	: i(i), j(j), nnz(nzero(my_row(i, j, A)) + nzero(my_col(i, j, A)) + nzero(my_block(i, j, A)))
    {}

    friend inline std::ostream& operator<< (std::ostream& stream, const entry& e)
    {
	return stream << "[" << e.i << ", " << e.j << " = " << e.nnz << "]";
    }

    bool operator<(const entry& other) const { return nnz > other.nnz; } // to sort

    int         i, j, nnz;
};

template <typename T>
inline std::ostream& operator<< (std::ostream& stream, const std::vector<T>& v)
{
    stream << "(";
    for (typename std::vector<T>::const_iterator it= v.begin(); it != v.end(); ++it)
	stream << *it << ", ";
    return stream << ")";
}

// void inline wait() { char c; std::cin >> c; }

void solve(const char* file_name)
{
    mtl::dense2D<int> A;
    mtl::io::matrix_market_istream(file_name) >> A;
    cout << "Read from " << file_name <<  " is \n"  << A;

    std::vector<entry> to_fill;
    for (int i= 0; i < 9; i++)
	for (int j= 0; j < 9; j++)
	    if (A[i][j] == 0)
		to_fill.push_back(entry(i, j, A));
    sort(to_fill.begin(), to_fill.end());

    for (unsigned pos= 0;;) {
	int i= to_fill[pos].i, j= to_fill[pos].j;
	my_sub_t my_sub(i, j, A);
	int v= A[i][j] + 1;
	while (v < 10 && conflict(v, my_sub))
	    v++;

	// cout << "Matrix is now: \n" << A;
	if (v == 10) {// nothing works -> back track
	    if (pos-- == 0)
		throw "No solution found";
	    A[i][j]= 0;
	} else {
	    A[i][j]= v;
	    if (++pos == to_fill.size()) {
		cout << "Solution is:\n" << A;
		return;
	    }
	}
    }

}

int main(int argc, char* argv[])
{
    solve(argc > 1 ? argv[1] : "matrix_market/sudoku_easy.mtx");
    return 0;
}


//
// OpenTissue, A toolbox for physical based simulation and animation.
// Copyright (C) 2007 Department of Computer Science, University of Copenhagen
//
#include <OpenTissue/configuration.h>

#include <OpenTissue/core/math/big/big_types.h>
#include <OpenTissue/core/math/big/io/big_read_DLM.h>

#define BOOST_AUTO_TEST_MAIN
#include <OpenTissue/utility/utility_push_boost_filter.h>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>
#include <OpenTissue/utility/utility_pop_boost_filter.h>

BOOST_AUTO_TEST_SUITE(opentissue_math_big_read_dlm);

BOOST_AUTO_TEST_CASE(my_test_case)
{
  typedef ublas::compressed_matrix<double> matrix_type;
  typedef ublas::vector<double>            vector_type;

  std::string data_path = "dlm";

  matrix_type A;
  vector_type x;

  {
    std::string filename = "do_not_exist.wrong_type_extention";
    BOOST_CHECK_THROW( OpenTissue::math::big::read_dlm_matrix( filename, A), std::logic_error );
  }
  {
    std::string filename = "do_not_exist.wrong_type_extention";
    BOOST_CHECK_THROW( OpenTissue::math::big::read_dlm_vector( filename, x), std::logic_error );
  }
  double tolerance = 0.01;
  {
    std::string filename = data_path + "/4/A.dlm";
    bool success = OpenTissue::math::big::read_dlm_matrix( filename, A);
    BOOST_CHECK( success);
    BOOST_CHECK( A.size1() == 4 );
    BOOST_CHECK( A.size2() == 4 );
    BOOST_CHECK_CLOSE( double( A(0,0) ), 1.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(0,1) ), 2.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(0,2) ), 3.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(0,3) ), 4.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(1,0) ), 5.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(1,1) ), 6.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(1,2) ), 7.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(1,3) ), 8.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(2,0) ), 9.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(2,1) ), 10.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(2,2) ), 11.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(2,3) ), 12.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(3,0) ), 13.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(3,1) ), 14.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(3,2) ), 15.0, tolerance);
    BOOST_CHECK_CLOSE( double( A(3,3) ), 16.0, tolerance);
  }
  {
    std::string filename = data_path + "/4/b.dlm";
    bool success = OpenTissue::math::big::read_dlm_vector( filename, x);
    BOOST_CHECK( success);

    BOOST_CHECK( x.size() == 4 );
    BOOST_CHECK_CLOSE( double( x(0) ), 1.0, tolerance);
    BOOST_CHECK_CLOSE( double( x(1) ), 2.0, tolerance);
    BOOST_CHECK_CLOSE( double( x(2) ), 3.0, tolerance);
    BOOST_CHECK_CLOSE( double( x(3) ), 4.0, tolerance);
  }

}

BOOST_AUTO_TEST_SUITE_END();


///////////////////////////////////////////////////////////////////////////////
// independence.hpp                                                          //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_PEARSON_CHISQ_INDEPENDENCE_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_INCLUDE_PEARSON_CHISQ_INDEPENDENCE_HPP_ER_2010

#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/df.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/lost_df.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/statistic.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/pearson_chisq/independence/tag.hpp>

#endif


/* Copyright (C) 2011 Ion Torrent Systems, Inc. All Rights Reserved */

#include <deque>
#include <vector>
#include <armadillo>
#include <Rcpp.h>
#include "mixed.h"
#include "bivariate_gaussian.h"

using namespace std;
using namespace arma;

RcppExport SEXP percentPositive(SEXP RIonoGram, SEXP RCutoff)
{
	vector<double> ionoGram = Rcpp::as<vector<double> >(RIonoGram);
	double cutoff = Rcpp::as<double>(RCutoff);
	double ppf    = percent_positive(ionoGram.begin(), ionoGram.end(), cutoff);

	return Rcpp::List::create(Rcpp::Named("ppf") = ppf);
}

RcppExport SEXP sumFractionalPart(SEXP RIonoGram)
{
	vector<double> ionoGram = Rcpp::as<vector<double> >(RIonoGram);
	double ssq = sum_fractional_part(ionoGram.begin(), ionoGram.end());

	return Rcpp::List::create(Rcpp::Named("ssq") = ssq);
}

RcppExport SEXP fitNormals(SEXP RPPF, SEXP RSSQ)
{
	deque<float> ppf = Rcpp::as<deque<float> >(RPPF);
	deque<float> ssq = Rcpp::as<deque<float> >(RSSQ);

	//vec2  mean[2];
	//mat22 sigma[2];
	//vec2  prior;
	vec mean[2];
	mat sigma[2];
	vec prior;
    for(int i=0; i<2; ++i){
        mean[i].set_size(2);
        sigma[i].set_size(2,2);
    }

// I assume here (possibly falsely) that the default options are good
  PolyclonalFilterOpts local_opts;

	bool converged = fit_normals(mean, sigma, prior, ppf, ssq, local_opts);

	// (Wrapping the results would be much simpler with RcppArmadillo.)
	Rcpp::NumericVector RCloneMean  = Rcpp::wrap(mean[0]);
	Rcpp::NumericVector RMixedMean  = Rcpp::wrap(mean[1]);
	Rcpp::NumericVector RPrior      = Rcpp::wrap(prior);

	Rcpp::NumericMatrix RCloneSigma(2,2);
	Rcpp::NumericMatrix RMixedSigma(2,2);
	for(int r=0; r<2; ++r){
		for(int c=0; c<2; ++c){
			RCloneSigma(r,c) = sigma[0](r,c);
			RMixedSigma(r,c) = sigma[1](r,c);
		}
	}

	return Rcpp::List::create(Rcpp::Named("converged")  = converged,
                              Rcpp::Named("cloneMean")  = RCloneMean,
                              Rcpp::Named("mixedMean")  = RMixedMean,
                              Rcpp::Named("cloneSigma") = RCloneSigma,
                              Rcpp::Named("mixedSigma") = RMixedSigma,
                              Rcpp::Named("prior")      = RPrior);

}

RcppExport SEXP distanceFromMean(SEXP RMean, SEXP RSigma, SEXP RX)
{
	Rcpp::NumericVector tmpMean(RMean);
	Rcpp::NumericMatrix tmpSigma(RSigma);
	Rcpp::NumericVector tmpX(RX);

	//vec2  mean;
	//mat22 sigma;
	//vec2  x;
	vec mean(2);
	mat sigma(2,2);
	vec x(2);

	mean  << tmpMean(0)    << tmpMean(1);
	sigma << tmpSigma(0,0) << tmpSigma(0,1) << endr
	      << tmpSigma(1,0) << tmpSigma(1,1) << endr;
	x     << tmpX(0)       << tmpX(1);

	bivariate_gaussian g(mean, sigma);

	return Rcpp::wrap(g.sd(x));
}


#include <cstdlib>
#include <list>
#include <Eigen/Core>
#include <moihgp/moihgp_online.h>
#include <moihgp/matern32ss.h>
#include <iostream>
#include <time.h>

int main()
{

    double dt = 0.1;
    size_t num_output = 2;
    size_t num_latent = 1;
    size_t windowsize = 1;
    double gamma = 0.9;
    bool threading = false;
    Eigen::MatrixXd H(2, 2);
    H << 0.7, 0.3, -0.3, 0.7;
    std::list<Eigen::VectorXd> data;
    double t = 0.0;
    while (t < 2 * M_PI)
    {
        Eigen::VectorXd x(num_latent);
        x << sin(t), sin(4*t);
        data.push_back(H * x + 0.1 * Eigen::VectorXd(num_output).setRandom());
        t += dt;
    }

    moihgp::MOIHGPOnlineLearning<moihgp::Matern32StateSpace> gp(dt, num_output, num_latent, gamma, windowsize, threading);
    std::list<Eigen::VectorXd> yhat;
    for (std::list<Eigen::VectorXd>::iterator y = data.begin(); y != data.end(); y++)
    {
        clock_t tic = clock();
        yhat.push_back(gp.step(*y));
        clock_t toc = clock();
        std::cout << "Elapsed time per step:" << double(toc - tic) / 1000.0 << "ms" << std::endl;
    }

    return 0;
}


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

#include "utilities.hpp"
#include "toplevelfixture.hpp"
#include <boost/test/unit_test.hpp>
#include <ql/experimental/callablebonds/callablebond.hpp>
#include <ql/indexes/ibor/euribor.hpp>
#include <ql/instruments/callabilityschedule.hpp>
#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/math/randomnumbers/rngtraits.hpp>
#include <ql/math/statistics/incrementalstatistics.hpp>
#include <ql/methods/montecarlo/multipathgenerator.hpp>
#include <ql/methods/montecarlo/pathgenerator.hpp>
#include <ql/quantlib.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/termstructures/credit/flathazardrate.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/target.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <ql/time/daycounters/actualactual.hpp>
#include <ql/time/daycounters/thirty360.hpp>

#include <qle/instruments/cdsoption.hpp>
#include <qle/instruments/creditdefaultswap.hpp>
#include <qle/methods/multipathgeneratorbase.hpp>
#include <qle/models/crossassetmodel.hpp>
#include <qle/models/cdsoptionhelper.hpp>
#include <qle/pricingengines/midpointcdsengine.hpp>

#include <qle/models/crcirpp.hpp>
#include <qle/models/cirppconstantfellerparametrization.hpp>
#include <qle/models/crossassetmodel.hpp>
#include <qle/processes/crcirppstateprocess.hpp>

#include <boost/make_shared.hpp>

#include <fstream>
#include <iostream>

// fix for boost 1.64, see https://lists.boost.org/Archives/boost/2016/11/231756.php
#if BOOST_VERSION >= 106400
#include <boost/serialization/array_wrapper.hpp>
#endif
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/covariance.hpp>
#include <boost/accumulators/statistics/density.hpp>
#include <boost/accumulators/statistics/error_of_mean.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variates/covariate.hpp>
#include <boost/make_shared.hpp>

using namespace QuantLib;
using namespace QuantExt;

using namespace boost::accumulators;

namespace {
struct CreditModelTestData_flat {
    CreditModelTestData_flat()
        : referenceDate(29, July, 2017), dts(boost::make_shared<FlatHazardRate>(referenceDate, 0.04, ActualActual())),
          yts(boost::make_shared<FlatForward>(referenceDate, 0.02, ActualActual())) {

        Settings::instance().evaluationDate() = referenceDate;

        kappa = 0.206;
        theta = 0.04;
        sigma = sqrt(2 * kappa * theta) - 1E-10;
        y0 = theta;
        shifted = true;

        recoveryRate = 0.4;
        // QL_REQUIRE(2 * eurKappa * eurTheta / eurSigma / eurSigma > 1 , "Feller Condition is not satisfied!");
        cirParametrization = boost::make_shared<CrCirppConstantWithFellerParametrization>(
            EURCurrency(), dts, kappa, theta, sigma, y0, shifted);
        QL_REQUIRE(cirParametrization != NULL, "CrCirppConstantWithFellerParametrization has null pointer!");

        model = boost::make_shared<CrCirpp>(cirParametrization);
        BOOST_TEST_MESSAGE("CIR++ parameters: ");
        BOOST_TEST_MESSAGE("Kappa: \t" << model->parametrization()->kappa(0));
        BOOST_TEST_MESSAGE("Theta: \t" << model->parametrization()->theta(0));
        BOOST_TEST_MESSAGE("Sigma: \t" << model->parametrization()->sigma(0));
        BOOST_TEST_MESSAGE("y0: \t" << model->parametrization()->y0(0));
        BOOST_TEST_MESSAGE("Feller condition is (>1 ok) "
                           << 2.0 * model->parametrization()->kappa(0) * model->parametrization()->theta(0) /
                                  model->parametrization()->sigma(0) / model->parametrization()->sigma(0));
    }

    SavedSettings backup;
    Date referenceDate;
    Handle<DefaultProbabilityTermStructure> dts;
    Handle<YieldTermStructure> yts;
    Real kappa, theta, sigma, y0;
    bool shifted;
    Real recoveryRate;
    boost::shared_ptr<CrCirppConstantWithFellerParametrization> cirParametrization;
    boost::shared_ptr<CrCirpp> model;
}; // IrTSModelTestData
} // namespace

BOOST_FIXTURE_TEST_SUITE(CrCirppModelTest, CreditModelTestData_flat)

BOOST_AUTO_TEST_CASE(testMartingaleProperty) {

    BOOST_TEST_MESSAGE("Testing martingale property in credit-CIR++ model for Brigo-Alfonsi discretizations...");

    boost::shared_ptr<StochasticProcess> process = model->stateProcess();
    QL_REQUIRE(process != NULL, "process has null pointer!");

    // CIRplusplusStateProcess::Discretization discType = CIRplusplusStateProcess::Discretization::Reflection;
    // CIRplusplusStateProcess::Discretization discType = CIRplusplusStateProcess::Discretization::PartialTruncation;
    // CIRplusplusStateProcess::Discretization discType = CIRplusplusStateProcess::Discretization::FullTruncation;
    // boost::shared_ptr<StochasticProcess> process = boost::make_shared<CIRplusplusStateProcess>(model.get(),
    // discType);  BOOST_TEST_MESSAGE("Simulation type of negative variance process "<<discType);
    Size n = 10000; // number of paths
    Size seed = 42; // rng seed
    // Time T = 25.0;                          // maturity of payoff
    // Time T2 = 40.0;                         // zerobond maturity
    Time T = 10.0;  // maturity of payoff
    Time T2 = 20.0; // zerobond maturity
    // QL_REQUIRE(T2 == model->maxSimulationHorizon(), "Forward measure horizon must be equalt to the maturity of the
    // zero bond");
    Size steps = static_cast<Size>(T * 52); // number of steps taken (euler)

    TimeGrid grid(T, steps);
    // LowDiscrepancy::rsg_type sg = LowDiscrepancy::make_sequence_generator( 9 * steps, seed);
    // MultiPathGenerator<LowDiscrepancy::rsg_type> pg(process, grid, sg, false);
    MultiPathGeneratorMersenneTwister pg(process, grid, seed, true);
    accumulator_set<double, stats<tag::mean, tag::error_of<tag::mean>>> meanTest_y;
    accumulator_set<double, stats<tag::variance, tag::error_of<tag::mean>>> varTest_y;

    accumulator_set<double, stats<tag::mean, tag::error_of<tag::mean>>> sp;
    accumulator_set<double, stats<tag::mean, tag::error_of<tag::mean>>> numeraire;

    accumulator_set<double, stats<tag::density>> histXAcc(tag::density::cache_size = 10000,
                                                          tag::density::num_bins = 50);

    // typedef boost::iterator_range<std::vector<std::pair<double, double>>::iterator> histogram_type;

    // //create an accumulator
    // acc histXAcc( tag::density::num_bins = 20, tag::density::cache_size = 10);

    std::ofstream of;
    // of.open("y_paths.txt");
    for (Size j = 0; j < n; ++j) {
        Sample<MultiPath> path = pg.next();
        Size l = path.value[0].length() - 1;
        Real y = path.value[0][l];
        Real num = path.value[1][l];
        sp(model->survivalProbability(T, T2, y) * num);
        numeraire(num);
        meanTest_y(y);
        varTest_y(y);
        histXAcc(y);
    }
    // histogram_type histX = density(histXAcc);
    // double total = 0.0;
    // std::ofstream ofHistX;
    // ofHistX.open("y_hist.txt");

    // Real binSize = histX[1].first - histX[0].first;
    // for (Size i = 0; i < histX.size(); i++) {
    //     ofHistX << histX[i].first << "," << histX[i].second / binSize << std::endl;
    //     total += histX[i].second;
    // }
    // BOOST_TEST_MESSAGE("Total cdf(x): " << total); // should be 1 (and it is)

    // total = 0.0;
    // std::ofstream ofPDFX;
    // ofPDFX.open("y_pdf.txt");
    // // for (Real x = - 5.0; x <= 7.0; x += 0.1)
    // for (Real x = 0.00018; x <= 0.01; x += 0.001) {
    //     ofPDFX << x << "," << model->density(x, T) << std::endl;
    //     total += model->density(x, T);
    // }
    // BOOST_TEST_MESSAGE("Total cdf(x): " << total); // should be 1 (and it is)

    // of.close();
    // ofPDFX.close();
    // ofHistX.close();

    // Real kappa = model->parametrization()->kappa(0);
    // Real theta = model->parametrization()->theta(0);
    // Real sigma = model->parametrization()->sigma(0);
    // Real y0 = model->parametrization()->y0(0);
    // Real sigma2 = sigma * sigma;

    BOOST_TEST_MESSAGE("\nBrigo-Alfonsi:");
    // BOOST_TEST_MESSAGE("Mean = " << mean(meanTest_y) << " +- " << error_of<tag::mean>(meanTest_y) << " analytic "
    // <<y0 * exp(-kappa*T) + theta * (1- exp(-kappa*T))); BOOST_TEST_MESSAGE("Variance = " << variance(varTest_y) << "
    // +- " << error_of<tag::mean>(varTest_y)<<" analytic " <<y0*sigma2/theta*(exp(-kappa*T) - exp(-2*kappa*T)) +
    // theta*sigma2/2/kappa*(1- exp(-2*kappa *T))*(1- exp(-2*kappa *T)) );
    BOOST_TEST_MESSAGE("SP = " << mean(sp) << " +- " << error_of<tag::mean>(sp) << " vs analytical "
                               << dts->survivalProbability(T2));
    BOOST_TEST_MESSAGE("Num = " << mean(numeraire) << " +- " << error_of<tag::mean>(numeraire) << " vs analytical "
                                << dts->survivalProbability(T));

    Real tol2 = 12.0E-4;
    Real expectedSP = dts->survivalProbability(T);
    Real expectedCondSP = dts->survivalProbability(T2);
    if (std::abs(mean(numeraire) - expectedSP) > tol2)
        BOOST_FAIL("Martingale test failed for SP(t) (Brigo-Alfonsi discr.), excpected "
                   << expectedSP << ", got " << mean(numeraire) << ", tolerance " << tol2);
    if (std::abs(mean(sp) - expectedCondSP) > tol2)
        BOOST_FAIL("Martingale test failed for  SP(t,T) (Brigo-Alfonsi discr.), excpected "
                   << expectedCondSP << ", got " << mean(sp) << ", tolerance " << tol2);

} // testIrTSMartingaleProperty

BOOST_AUTO_TEST_SUITE_END()