// Copyright Tom Brinkman 2008. 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 _sections_hpp_
#define _sections_hpp_

#include <boost/function.hpp>
#include <boost/gil/gil_all.hpp>

struct sections
{
	struct section
	{
		int pos;
		section* next;
	};

	section* head;
	section* current;

	operator bool (){return current != NULL;}
	operator section * () {return head;}
	double operator *(){return current->pos;}
	void operator ++(int){current = current->next;}
	void operator ++(){current = current->next;}
	void reset() {current = head;}

	~sections()
	{
		while (head)
		{
			section* tmp = head->next;
			free(head);
			head = tmp;
		}

		head = NULL;
	}

	sections(int width, int intervals) : head(NULL), current(NULL)
	{
		BOOST_ASSERT(intervals);

		int adj = width / intervals;
		int extra = width-adj*intervals;
		int pos = 0;
		int ppos = 0;

		head = NULL;
		current = NULL;

		while (pos < width)
		{
			int add = adj;
			if (extra-- > 0)
				add++;

			section* tmp  = (section*)malloc(sizeof(section));
			pos += add;
			tmp->pos = pos - ppos;
			tmp->next = NULL;
			ppos = pos;

			if (head == NULL)
				head = tmp;
			else
				current->next = tmp;

			current = tmp;
		}

		current = head;
	}
};

namespace layer
{

template <typename view_t>
struct columns
{
	typedef boost::function<void (view_t&)> layer_t;
	typedef std::pair<double, layer_t> pair_t;

	std::vector<pair_t> layers;
	int margin;

	columns(pair_t* p, int size, int margin = 0) : margin(margin)
	{
		for (int n = 0; n < size; ++n)
			layers.push_back(p[n]);
	}

	void operator()(view_t& view)
	{
		int zcount = 0;
		int swidth = 0;
		for (int n = 0; n < layers.size(); ++n)
		{
			BOOST_ASSERT(layers[n].first >= 0);
			if (layers[n].first < 1 && layers[n].first > 0)
				layers[n].first = (int)(layers[n].first * view.width());

			swidth += (int)(layers[n].first);
			zcount += layers[n].first == 0 ? 1 : 0;
		}

		sections curr(view.width()-swidth+margin,zcount);
		int x = 0;
		for (int n = 0; n < layers.size(); ++n)
		{
			if (!layers[n].first)
			{
				layers[n].first = *curr;
				curr++;
			}

			int xwidth = (int)(layers[n].first);
			xwidth -= margin;

			view_t v = boost::gil::subimage_view(view,x,0,xwidth,view.height());
			layers[n].second(v);
			x += xwidth + margin;
		}
	}
};

template <typename view_t>
struct rows
{
	typedef boost::function<void (view_t&)> layer_t;
	typedef std::pair<double, layer_t> pair_t;
	typedef std::vector<std::pair<double,layer_t> > layers_t;

	int margin;
	layers_t layers;

	rows(pair_t* p, int size, int margin = 0) : margin(margin)
	{
		for (int n = 0; n < size; ++n)
			layers.push_back(p[n]);
	}

	void operator()(view_t& view)
	{
		int zcount = 0;
		int sheight = 0;
		for (int n = 0; n < layers.size(); ++n)
		{
			BOOST_ASSERT(layers[n].first >= 0);
			if (layers[n].first < 1 && layers[n].first > 0)
				layers[n].first = (int)(layers[n].first * view.height());

			sheight += (int)(layers[n].first);
			zcount += layers[n].first == 0 ? 1 : 0;
		}

		sections curr(view.height()-sheight+margin,zcount);
		int y = 0;
		for (int n = 0; n < layers.size(); ++n)
		{
			if (!layers[n].first)
			{
				layers[n].first = *curr;
				curr++;
			}

			int yheight = (int)(layers[n].first);
			yheight -= margin;

			view_t v = boost::gil::subimage_view(view,0,y,view.width(),yheight);
			layers[n].second(v);
			y += yheight+margin;
		}
	}
};

};

#endif


/*
 * ext.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <dials/algorithms/integration/bayes/bayesian_integrator.h>
#include <dials/algorithms/shoebox/mask_code.h>

namespace dials { namespace algorithms { namespace boost_python {

  using namespace boost::python;

  template <typename FloatType>
  void bayesian_integrator_wrapper(const char *name) {
    typedef BayesianIntegrator<FloatType> BayesianIntegratorType;

    class_<BayesianIntegratorType>(name, no_init)
      .def(init<const af::const_ref<FloatType> &,
                const af::const_ref<FloatType> &,
                const af::const_ref<int> &>(
        (arg("signal"), arg("background"), arg("mask"))))
      .def(init<const af::const_ref<FloatType, af::c_grid<2> > &,
                const af::const_ref<FloatType, af::c_grid<2> > &,
                const af::const_ref<int, af::c_grid<2> > &>(
        (arg("signal"), arg("background"), arg("mask"))))
      .def(init<const af::const_ref<FloatType, af::c_grid<3> > &,
                const af::const_ref<FloatType, af::c_grid<3> > &,
                const af::const_ref<int, af::c_grid<3> > &>(
        (arg("signal"), arg("background"), arg("mask"))))
      .def("intensity", &BayesianIntegratorType::intensity)
      .def("variance", &BayesianIntegratorType::variance)
      .def("background", &BayesianIntegratorType::background)
      .def("background_variance", &BayesianIntegratorType::background_variance)
      .def("n_signal", &BayesianIntegratorType::n_signal)
      .def("n_background", &BayesianIntegratorType::n_background)
      .def("success", &BayesianIntegratorType::success);
  }

  template <typename FloatType>
  BayesianIntegrator<FloatType> make_bayesian_integrator_1d(
    const af::const_ref<FloatType> &image,
    const af::const_ref<FloatType> &background,
    const af::const_ref<int> &mask) {
    return BayesianIntegrator<FloatType>(image, background, mask);
  }

  template <typename FloatType>
  BayesianIntegrator<FloatType> make_bayesian_integrator_2d(
    const af::const_ref<FloatType, af::c_grid<2> > &image,
    const af::const_ref<FloatType, af::c_grid<2> > &background,
    const af::const_ref<int, af::c_grid<2> > &mask) {
    return BayesianIntegrator<FloatType>(image, background, mask);
  }

  template <typename FloatType>
  BayesianIntegrator<FloatType> make_bayesian_integrator_3d(
    const af::const_ref<FloatType, af::c_grid<3> > &image,
    const af::const_ref<FloatType, af::c_grid<3> > &background,
    const af::const_ref<int, af::c_grid<3> > &mask) {
    return BayesianIntegrator<FloatType>(image, background, mask);
  }

  template <typename FloatType>
  void bayesian_integrator_suite() {
    def("integrate_by_bayesian_integrator",
        &make_bayesian_integrator_1d<FloatType>,
        (arg("image"), arg("background"), arg("mask")));

    def("integrate_by_bayesian_integrator",
        &make_bayesian_integrator_2d<FloatType>,
        (arg("image"), arg("background"), arg("mask")));

    def("integrate_by_bayesian_integrator",
        &make_bayesian_integrator_3d<FloatType>,
        (arg("image"), arg("background"), arg("mask")));
  }

  BOOST_PYTHON_MODULE(dials_algorithms_integration_bayes_ext) {
    bayesian_integrator_wrapper<float>("BayesianIntegratorFloat");
    bayesian_integrator_wrapper<double>("BayesianIntegratorDouble");

    bayesian_integrator_suite<float>();
    bayesian_integrator_suite<double>();
  }

}}}  // namespace dials::algorithms::boost_python


//
// CTRHMM.cpp
//
// Copyright (c) 2020 Shion Hosoda
//
// This software is released under the MIT License.
// http://opensource.org/licenses/mit-license.php
//

//include{{{
#include <stdlib.h>
#include <boost/program_options.hpp>
#include "include/CTRHMMVariationalEstimator.hpp"
//}}}

using namespace std;
using namespace Eigen;

MatrixXd attachBiasTerm(const MatrixXd &x){//{{{
    MatrixXd xWithBias(MatrixXd::Ones(x.rows()+1, x.cols()));
    xWithBias.block(0, 0, x.rows(), x.cols()) = x;
    return xWithBias;
}//}}}

void drawOutNeededColumns(const vector<vector<string> > &metadataMatrix, const vector<string> &metadataColumnNames, vector<double> &timepoints, vector<string> &subjects){//{{{
    for(int i=0; i<metadataMatrix.size(); i++){
        for(int j=0; j<metadataMatrix[i].size(); j++){
            if(metadataColumnNames[j]=="timepoint"){
                timepoints.push_back(stod(metadataMatrix[i][j]));
            }else if(metadataColumnNames[j]=="subjectID"){
                subjects.push_back(metadataMatrix[i][j]);
            }
        }
    }
}//}}}

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

    //Options{{{
    boost::program_options::options_description opt("Options");
    opt.add_options()
    ("help,h", "show help")
    ("output,o", boost::program_options::value<string>()->default_value("./"), "Directory name for output")
    ("threshold-convergence,a", boost::program_options::value<double>()->default_value(0.0), "Threshold of convergence")
    ("threshold-cluster-shrinkage,b", boost::program_options::value<double>()->default_value(0.0), "Threshold of shrinkage of pi")
    ("iteration-number,n", boost::program_options::value<int>()->default_value(100), "The number of iteration")
    ("number-of-clusters,k", boost::program_options::value<int>()->default_value(10), "The initial number of clusters")
    ("test-x,x", boost::program_options::value<string>()->default_value(""), "Test x data filename (optional)")
    ("test-y,y", boost::program_options::value<string>()->default_value(""), "Test y data filename (optional)")
    ("test-y-variance,v", boost::program_options::value<string>()->default_value(""), "Test y variance data filename (optional)")
    ("test-metadata,m", boost::program_options::value<string>()->default_value(""), "Test metadata filename (optional)")
    ;

    boost::program_options::positional_options_description pd;
    pd.add("xfile", 1);
    pd.add("yfile", 1);
    pd.add("yvariancefile", 1);
    pd.add("metadatafile", 1);

    boost::program_options::options_description hidden("hidden");
    hidden.add_options()
        ("xfile", boost::program_options::value<string>(), "hidden")
        ("yfile", boost::program_options::value<string>(), "hidden")
        ("yvariancefile", boost::program_options::value<string>(), "hidden")
        ("metadatafile", boost::program_options::value<string>(), "hidden")
        ;
    boost::program_options::options_description cmdline_options;
    cmdline_options.add(opt).add(hidden);

    boost::program_options::variables_map vm;
    try{
        boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(cmdline_options).positional(pd).run(), vm);
    }catch(const boost::program_options::error_with_option_name& e){
        cout<<e.what()<<endl;
    }
    boost::program_options::notify(vm);
    string xFilename, yFilename, yVarianceFilename, metadataFilename;
    string testXFilename, testYFilename, testYVarianceFilename, testMetadataFilename;
    double thresholdOfConvergence, thresholdOfClusterShrinkage;
    int iterNum;
    int K;
    string outputDirectory;
    if (vm.count("help") || !vm.count("xfile") || !vm.count("yfile") || !vm.count("yvariancefile")  || !vm.count("metadatafile")){
        cout<<"Usage:\n CTRHMM [x file] [y file] [y variance file] [metadata file] [-options] "<<endl;
        cout<<endl;
        cout<<opt<<endl;
        exit(1);
    }else{
        xFilename = vm["xfile"].as<string>();
        yFilename = vm["yfile"].as<string>();
        yVarianceFilename = vm["yvariancefile"].as<string>();
        metadataFilename = vm["metadatafile"].as<string>();
        outputDirectory = vm["output"].as<string>();
        if(outputDirectory[outputDirectory.size()-1] != '/')outputDirectory.push_back('/');
        thresholdOfConvergence = vm["threshold-convergence"].as<double>();
        thresholdOfClusterShrinkage = vm["threshold-cluster-shrinkage"].as<double>();
        iterNum = vm["iteration-number"].as<int>();
        K = vm["number-of-clusters"].as<int>();
        testXFilename = vm["test-x"].as<string>();
        testYFilename = vm["test-y"].as<string>();
        testYVarianceFilename = vm["test-y-variance"].as<string>();
        testMetadataFilename = vm["test-metadata"].as<string>();
    }
    //}}}

    //process metadata{{{
    vector<string> metadataColumnNames;
    vector<vector<string> > metadataMatrix;
    readCSV<string>(metadataFilename, metadataMatrix, nullptr, &metadataColumnNames);
    vector<double> timepoints;
    vector<string> subjects;
    drawOutNeededColumns(metadataMatrix, metadataColumnNames, timepoints, subjects);
    MatrixXd x, y, yVariance;
    vector<string> rowNames, columnNames;
    parseCSV2Eigen(xFilename, x, &rowNames, &columnNames);
    x = attachBiasTerm(x);
    parseCSV2Eigen(yFilename, y, &rowNames, &columnNames);
    parseCSV2Eigen(yVarianceFilename, yVariance, &rowNames, &columnNames);
    //}}}

//estimation{{{
    CTRHMMVariationalEstimator *estimator;

    estimator = new CTRHMMVariationalEstimator(x, y, yVariance, timepoints, subjects, K, thresholdOfClusterShrinkage, thresholdOfConvergence, iterNum);
    estimator->runIteraions();
    estimator->writeParameters(outputDirectory);
    estimator->writeObjective(outputDirectory);

    //{{{test
    if(testXFilename != "" && testYFilename != "" && testYVarianceFilename != "" && testMetadataFilename != ""){
        vector<string> testMetadataColumnNames;
        vector<vector<string> > testMetadataMatrix;
        readCSV<string>(testMetadataFilename, testMetadataMatrix, nullptr, &testMetadataColumnNames);
        vector<double> testTimepoints;
        vector<string> testSubjects;
        drawOutNeededColumns(testMetadataMatrix, testMetadataColumnNames, testTimepoints, testSubjects);
        MatrixXd testX, testY, testYVariance;
        parseCSV2Eigen(testXFilename, testX, &rowNames, &columnNames);
        testX = attachBiasTerm(testX);
        parseCSV2Eigen(testYFilename, testY, &rowNames, &columnNames);
        parseCSV2Eigen(testYVarianceFilename, testYVariance, &rowNames, &columnNames);
        estimator->calcAndWriteTestLogLikelihood(testX, testY, testYVariance, testTimepoints, testSubjects, outputDirectory);
    }
    //}}}

    delete estimator;
//}}}
    return 0;
}


/*******************************************************************************
*
*  Filename    : CompareKeys.cc
*  Description : Main control flow of Keys PDF comparison
*  Author      : Yi-Mu "Enoch" Chen [ ensc@hep1.phys.ntu.edu.tw ]
*
*******************************************************************************/
#include "TstarAnalysis/MassRecoCompare/interface/Common.hpp"
#include "TstarAnalysis/MassRecoCompare/interface/KeysCompMgr.hpp"

#include "ManagerUtils/SysUtils/interface/PathUtils.hpp"

#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <iostream>

using namespace std;
namespace opt = boost::program_options;

int
main( int argc, char* argv[] )
{
  /*******************************************************************************
  *   Options definition
  *******************************************************************************/
  opt::options_description desc( "Options for comparing Keys Pdf results" );
  desc.add_options()
    ( "mass,m", opt::value<int>(), "Masspoint to run" )
    ( "compare,x", opt::value<string>(), "Which reconstruction methods to use" )
  ;

  reconamer.AddOptions( desc );
  reconamer.SetNamingOptions( "mass", "compare" );
  const int run = reconamer.ParseOptions( argc, argv );
  if( run == mgr::OptNamer::PARSE_ERROR ){ return 1; }
  if( run == mgr::OptNamer::PARSE_HELP  ){ return 0; }

  /*******************************************************************************
  *   Making KeysCompMgr
  *******************************************************************************/
  KeysCompMgr::InitStaticVars();

  const string name    = reconamer.GetInput<string>( "compare" );
  const string ltxname = reconamer.ExtQuery<string>( "reco", name, "Root Name" );
  KeysCompMgr* compmgr = new KeysCompMgr( name, ltxname );

  /*******************************************************************************
  *   Filling and plotting
  *******************************************************************************/
  boost::format globformat( "/wk_cms2/yichen/TstarAnalysis/EDMStore_New/recocomp/%s/*%d*.root" );
  const string globq = str( globformat % reconamer.GetInput<string>( "channel" ) % reconamer.GetInput<int>( "mass" ) );

  cout << globq << endl;

  for( const auto& file : mgr::Glob( globq ) ){
    cout << file << endl;
    compmgr->FillDataSet( file );
  }

  compmgr->MakeKeysPdf();
  compmgr->MakeGauss();
  compmgr->MakeConv();
  MakeKeysPlots( compmgr );
  MakeGaussPlots( compmgr );
  MakeConvPlots( compmgr );

  /*******************************************************************************
  *   Clearing up
  *******************************************************************************/
  delete compmgr;

  return 0;
}


#include "../checkpoint/checkpoint.h"
#include "goexit/goexit.h"
#ifdef HAVE_SSE2
	#include "myrandom/myrandsfmt.h"
#else
	#include "myrandom/myrand.h"
#endif
#include <array>                       	// for std::array
#include <cstdint>  	               	// for std::uint32_t
#include <functional>                   // for std::hash
#include <iomanip>		               	// for std::setiosflags, std::setprecision
#include <iostream> 	               	// for std::cout
#include <string>                      	// for std::string
#include <utility>                      // for std::move
#ifdef _CHECK_PARALELL_PERFORM
    #include <vector>   	            // for std::vector
#endif
#include <boost/container/flat_map.hpp>	// for boost::container::flat_map
#include <tbb/concurrent_hash_map.h>    // for tbb::concurrent_hash_map
#include <tbb/concurrent_vector.h>     	// for tbb::concurrent_vector
#include <tbb/parallel_for.h>           // for tbb::parallel_for

namespace {
    //! A global variable (constant expression).
    /*!
        モンテカルロシミュレーションの試行回数
    */
    static auto constexpr MCMAX = 1000000U;

    //! A global variable (constant expression).
    /*!
        UかDの文字列の長さ
    */
    static auto constexpr RANDNUMTABLELEN = 100U;

    //! A global variable (constant).
    /*!
        UとDの文字列の可能な集合の配列
    */
    static std::array<std::string, 8U> const udarray = { "DDD", "DDU", "DUD", "DUU", "UDD", "UDU", "UUD", "UUU" };

    //! A typedef.
    /*!
        文字列とその文字列に対応する出現回数のstd::map
    */
    using mymap = boost::container::flat_map<std::string, std::uint32_t>;

    //! A typedef.
    /*!
        文字列のペア
    */
    using strpair = std::pair<std::string, std::string>;

    //! A struct.
    /*!
        strpairを扱うハッシュと比較操作を定義する構造体
    */
    struct MyHashCompare final {
        //! A public static member function.
        /*!
            strpairからハッシュを生成する
            \param sp 文字列のペア
            \return 与えられたstrpairのハッシュ
        */
        static std::size_t hash(strpair const & sp)
        {
            auto const h1 = std::hash<std::string>()(sp.first);
            auto const h2 = std::hash<std::string>()(sp.second);
            return h1 ^ (h2 << 1);
        }

        //! A public static member function.
        /*!
            二つのstrpairを比較する
            \param lhs 左辺のstrpair
            \param rhs 右辺のstrpair
            \return 文字列のペアが等しい場合にはtrue
        */
        static bool equal(strpair const & lhs, strpair const & rhs)
        {
            return lhs == rhs;
        }
    };

    //! A typedef.
    /*!
        各文字列の順列に対応する勝利回数の結果を格納するtbb::concurrent_hash_map
    */
    using myhashmap = tbb::concurrent_hash_map<strpair, std::uint32_t, MyHashCompare>;

    //! A typedef.
    /*!
        文字列のペアと、どちらの文字列が勝ったかのstd::pair
    */
    using mymap2 = boost::container::flat_map<strpair, bool>;

    //! A typedef.
    /*!
        文字列のペアと、文字列の勝利数のstd::pair
    */
    using mymap3 = boost::container::flat_map<strpair, std::uint32_t>;

    //! A function.
    /*!
        文字列の可能な順列を列挙する
        \return 文字列の可能な順列を列挙したstd::array
    */
    std::array<strpair, 56U> makecombination();

    //! A global variable (constant).
    /*!
        udarrayから二つを抽出したときの可能な順列の配列
    */
    static std::array<strpair, 56U> const cbarray = makecombination();

#ifdef _CHECK_PARALELL_PERFORM
    //! A function.
    /*!
        文字列のペアの、前者が勝利した回数を集計する
        \param mcresultwinningavg 文字列のペアと、どちらの文字列が勝ったかの結果が格納された連想配列の可変長配列
        \return 文字列のペアの、前者が勝利した回数が格納された連想配列
    */
    mymap3 aggregateWinningAvg(std::vector<mymap2> const & mcresultwinningavg);
#endif

    //! A function.
    /*!
        文字列のペアの、前者が勝利した回数を集計する
        \param mcresultwinningavg 文字列のペアと、どちらの文字列が勝ったかの結果が格納された連想配列の可変長配列
        \return 文字列のペアの、前者が勝利した回数が格納された連想配列
    */
    mymap3 aggregateWinningAvg(tbb::concurrent_vector<mymap2> const & mcresultwinningavg);

    template <typename T>
    //! A template function.
    /*!
        UDのランダム文字列を生成する
        \param mr 自作乱数クラスのオブジェクト
        \return UDのランダム文字列を格納したstd::string
    */
    inline auto makerandomudstr(T & mr);

#ifdef _CHECK_PARALELL_PERFORM
    //! A function.
    /*!
        モンテカルロ・シミュレーションを行う
        \return 期待値と、どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果のstd::pair
    */
    std::pair<std::vector<mymap>, std::vector<mymap2> > montecarlo();
#endif

    //! A function.
    /*!
        モンテカルロ・シミュレーションをTBBで並列化して行う
        \return 期待値と、どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果のstd::pair
    */
    std::pair< tbb::concurrent_vector<mymap>, tbb::concurrent_vector<mymap2> > montecarloTBB();

    template <typename T>
    //! A template function.
    /*!
        期待値に対するモンテカルロ・シミュレーションの実装
        \param mr 自作乱数クラスのオブジェクト
        \return 期待値に対するモンテカルロ・シミュレーションの結果が格納された連想配列
    */
    mymap montecarloImplAvg(T & mr);

    template <typename T>
    //! A template function.
    /*!
        文字列のペアのうち、どちらの文字列が先に出現したかのモンテカルロ・シミュレーションの実装
        \param mr 自作乱数クラスのオブジェクト
        \return 文字列のペアのうち、どちらの文字列が先に出現したかのモンテカルロ・シミュレーションの結果が格納された連想配列
    */
    mymap2 montecarloImplWinningAvg(T & mr);

    //! A function.
    /*!
        UとDのランダム文字列から与えられた文字列の位置を検索し、文字列の末尾の位置を与える
        \param str 検索する文字列
        \param udstr UとDのランダム文字列
        \return 検索された文字列の末尾の位置
    */
    inline std::uint32_t myfind(std::string const & str, std::string const & udstr);

    //! A template function.
    /*!
        期待値に対するモンテカルロ・シミュレーションの和を計算する
        \return 期待値に対するモンテカルロ・シミュレーションの結果の和の連想配列
    */
    template <typename T>
    mymap sumMontecarloAvg(T const & mcresultavg);
}

int main()
{
    checkpoint::CheckPoint cp;

    cp.checkpoint("処理開始", __LINE__);

#ifdef _CHECK_PARALELL_PERFORM
    {
        // モンテカルロ・シミュレーションの結果を代入
        auto const mcresult(montecarlo());

        // 各文字列のペアに対する勝率を計算する
        auto const trialwinningavg(aggregateWinningAvg(mcresult.second));
    }

    cp.checkpoint("並列化無効", __LINE__);
#endif

    // モンテカルロ・シミュレーションの結果を代入
    auto const mcresultTBB(montecarloTBB());

    // 各文字列のペアに対する勝率を計算する
    auto const trialwinningavg(aggregateWinningAvg(mcresultTBB.second));

    cp.checkpoint("並列化有効", __LINE__);

    // 期待値に対するモンテカルロ・シミュレーションの結果の和を計算する
    auto const trialavg(sumMontecarloAvg(mcresultTBB.first));

    // 各文字列に対する期待値の表示
    std::cout << std::setprecision(1) << std::setiosflags(std::ios::fixed);
    for (auto && itr = trialavg.begin(); itr != trialavg.end(); ++itr) {
        std::cout << itr->first
                  << " が出るまでの期待値: "
                  << static_cast<double>(itr->second) / static_cast<double>(MCMAX)
                  << "回\n";
    }

    // 各文字列のペアに対する勝率の表示
    std::cout << "\n    ";
    for (auto i = 0; i < 8; i++) {
        std::cout << udarray[i] << "  ";
    }
    std::cout << '\n';

    auto && itr = trialwinningavg.begin();
    auto const len = udarray.size();
    for (auto i = 0U; i < len; i++) {
        std::cout << itr->first.first << ' ';
        for (auto j = 0U; j < len; j++) {
            if (i == j) {
                std::cout << "     ";
            }
            else {
                std::cout << static_cast<double>(itr->second) / static_cast<double>(MCMAX) * 100.0
                          << ' ';
                ++itr;
            }
        }
        std::cout << '\n';
    }

    cp.checkpoint("それ以外の処理", __LINE__);

    cp.checkpoint_print();

	goexit::goexit();

    return 0;
}

namespace {
#ifdef _CHECK_PARALELL_PERFORM
    mymap3 aggregateWinningAvg(std::vector<mymap2> const & mcresultwinningavg)
    {
        // 各文字列の順列に対応する勝利回数の結果を格納するboost::container::flat_map
        mymap3 trialwinningavg;

        // tbb::concurrent_hash_mapの初期化
        for (auto const & sp : cbarray) {
            trialwinningavg[sp] = 0U;
        }

        // 試行回数分繰り返す
        for (auto const & mcr : mcresultwinningavg) {
            for (auto && itr = mcr.begin(); itr != mcr.end(); ++itr) {
                if (itr->second) {
                    trialwinningavg[itr->first]++;
                }
            }
        }

        return trialwinningavg;
    }
#endif

    mymap3 aggregateWinningAvg(tbb::concurrent_vector<mymap2> const & mcresultwinningavg)
    {
        // 各文字列の順列に対応する勝利回数の結果を格納するtbb::concurrent_hash_map
        myhashmap trial;

        // tbb::concurrent_hash_mapの初期化
        for (auto const & sp : cbarray) {
            myhashmap::accessor a;
            trial.insert(a, sp);
            a->second = 0U;
        }

        // MCMAX回のループを並列化して実行
        tbb::parallel_for(
            tbb::blocked_range<std::uint32_t>(0U, MCMAX),
            [&mcresultwinningavg, &trial](auto const & range) {
            for (auto && i = range.begin(); i != range.end(); ++i) {
                auto const mcr = mcresultwinningavg[i];
                for (auto && itr = mcr.begin(); itr != mcr.end(); ++itr) {
                    if (itr->second) {
                        myhashmap::accessor a;
                        trial.insert(a, itr->first);
                        a->second++;
                    }
                }
            }
        });

        // boost::container::flat_mapに計算結果を複写
        boost::container::flat_map<strpair, std::uint32_t> trialwinningavg;
        for (auto && res : trial) {
            trialwinningavg[res.first] = res.second;
        }

        return trialwinningavg;
    }

    std::array<strpair, 56U> makecombination()
    {
        // 全ての可能な順列を収納する配列
        std::array<strpair, 56U> cb;

        // カウンタ
        auto cnt = 0;

        // 全ての可能な順列を列挙
        auto const len = udarray.size();
        for (auto i = 0U; i < len; i++) {
            for (auto j = 0U; j < len; j++) {
                if (i != j) {
                    cb[cnt++] = std::make_pair(udarray[i], udarray[j]);
                }
            }
        }

        return cb;
    }

    template <typename T>
    auto makerandomudstr(T & mr)
    {
        // UDのランダム文字列を格納するstd::string
        std::string udstring(RANDNUMTABLELEN, '\0');

        // UDのランダム文字列を格納
        for (auto && c : udstring) {
            c = mr.myrand() > 3 ? 'U' : 'D';
        }

		// UDのランダム文字列を返す
        return udstring;
    }

#ifdef _CHECK_PARALELL_PERFORM
    std::pair<std::vector<mymap>, std::vector<mymap2> > montecarlo()
    {
        // 期待値に対するモンテカルロ・シミュレーションの結果を格納するための可変長配列
        std::vector<mymap> mcresultavg;

        // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を格納するための可変長配列
        std::vector<mymap2> mcresultwinningavg;

        // MCMAX個の容量を確保
        mcresultavg.reserve(MCMAX);
        mcresultwinningavg.reserve(MCMAX);

#ifdef HAVE_SSE2
		// 自作乱数クラスを初期化
		myrandom::MyRandSfmt mr(1, 6);
#else
		// 自作乱数クラスを初期化
		myrandom::MyRand mr(1, 6);
#endif

        // 試行回数分繰り返す
        for (auto i = 0U; i < MCMAX; i++) {
            // 期待値に対するモンテカルロ・シミュレーションの結果を代入
            mcresultavg.emplace_back(montecarloImplAvg(mr));

            // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を代入
            mcresultwinningavg.emplace_back(montecarloImplWinningAvg(mr));
        }

        return std::make_pair(std::move(mcresultavg), std::move(mcresultwinningavg));
    }
#endif

    std::pair<tbb::concurrent_vector<mymap>, tbb::concurrent_vector<mymap2> > montecarloTBB()
    {
        // 期待値に対するモンテカルロ・シミュレーションの結果を格納するための可変長配列
        tbb::concurrent_vector<mymap> mcresultavg;

        // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を格納するための可変長配列
        tbb::concurrent_vector<mymap2> mcresultwinningavg;

        // MCMAX個の容量を確保
        mcresultavg.reserve(MCMAX);
        mcresultwinningavg.reserve(MCMAX);

        // MCMAX回のループを並列化して実行
        tbb::parallel_for(
            0U,
            MCMAX,
            1U,
            [&](auto) {

#ifdef HAVE_SSE2
		        // 自作乱数クラスを初期化
		        myrandom::MyRandSfmt mr(1, 6);
#else
		        // 自作乱数クラスを初期化
		        myrandom::MyRand mr(1, 6);
#endif

                // 期待値に対するモンテカルロ・シミュレーションの結果を代入
                mcresultavg.emplace_back(montecarloImplAvg(mr));

                // どちらの文字列が先に出現したかどうかのモンテカルロ・シミュレーションの結果を代入
                mcresultwinningavg.emplace_back(montecarloImplWinningAvg(mr));
        });

        return std::make_pair(std::move(mcresultavg), std::move(mcresultwinningavg));
    }

    template <typename T>
    mymap montecarloImplAvg(T & mr)
    {
        // UDのランダム文字列
        auto const udstr(makerandomudstr(mr));

        // 検索結果のstd::map
        mymap result;

        // 文字列が最初に出現するのは何文字目かを検索し結果を代入
        for (auto const & str : udarray) {
            result.insert(std::make_pair(str, myfind(str, udstr)));
        }

        return result;
    }

    template <typename T>
    mymap2 montecarloImplWinningAvg(T & mr)
    {
        // UDのランダム文字列
        auto const udstr(makerandomudstr(mr));

        // 検索結果のstd::map
        mymap2 result;

        // どちらの文字列が先に出現したかの結果を代入
        for (auto const & sp : cbarray) {
            result.insert(std::make_pair(sp, myfind(sp.first, udstr) < myfind(sp.second, udstr)));
        }

        // 検索結果を返す
        return result;
    }

    std::uint32_t myfind(std::string const & str, std::string const & udstr)
    {
        // 文字列の位置を検索
        auto const pos = udstr.find(str);

        // posをその文字列の末尾の位置に変換
        // もし文字列が見つかっていなかった場合はRANDNUMTABLELENに変換
        return pos != std::string::npos ? static_cast<std::uint32_t>(pos + 3) : RANDNUMTABLELEN;
    }

    template <typename T>
    mymap sumMontecarloAvg(T const & mcresultavg)
    {
        // 各文字列に対して、期待値に対するモンテカルロ・シミュレーションの結果の和を格納するstd::map
        mymap trial;

        // std::mapの初期化
        for (auto const & str : udarray) {
            trial[str] = 0;
        }

        // 試行回数分繰り返す
        for (auto const & mcr : mcresultavg) {
            for (auto && itr = mcr.begin(); itr != mcr.end(); ++itr) {
                trial[itr->first] += itr->second;
            }
        }

        return trial;
    }
}


#include <stdio.h>
#include <string.h>
#include <iostream>
#include <iterator>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <urdf/model.h>
#include <kdl_parser/kdl_parser.hpp>
#include "ros/ros.h"

#include "snoptProblem.hpp"

using namespace std;
using namespace Eigen;

const string prefix     = "j2s7s300";
const double pi         = 3.1415926535897;

const int T             = 10.0;         // planning horizon
const double dt         = 0.5;          // timestep
const int dofRobot      = 3;            // num dofs of robot
const int dofEnv        = 1;            // num dofs of environment
const int dofLamb       =  1;           // dimension of contact forces

const double goal[] = {-0.4, 0.5};
const double obsLow[]   = {-1.0, 0.3};
const double obsHigh[]  = {1.0, 0.3};

// --------- HELPER FUNCTIONS --------- //

VectorXf config2DTo3D(VectorXf& q2D){
  VectorXf q3D(10);
  q3D << q2D(0), pi/2.0, pi/2.0, q2D(1), pi, q2D(2), pi, 0, 0, 0;
  return q3D;
}

VectorXf config3DTo2D(VectorXf& q3D){
  VectorXf q2D(3);
  q2D << q3D(0), q3D(3), q3D(5);
  return q2D;
}

VectorXf vel2DTo3D(VectorXf& dq2D){
  VectorXf dq3D(10);
  dq3D << dq2D(0), 0, 0, dq2D(1), 0, dq2D(2), 0, 0, 0, 0;
  return dq3D;
}

VectorXf vel3DTo2D(VectorXf& dq3D){
  VectorXf dq2D(3);
  dq2D << dq3D(0), dq3D(3), dq3D(5);
  return dq2D;
}

VectorXf control2DTo3D(VectorXf& u2D){
  VectorXf u3D(10);
  u3D << u2D(0), 0, 0, u2D(1), 0, u2D(2), 0, 0, 0, 0;
  return u3D;
}

VectorXf control3DTo2D(VectorXf& u3D){
  VectorXf u2D(3);
  u2D << u3D(0), u3D(3), u3D(5);
  return u2D;
}

VectorXf acc2DTo3D(VectorXf& ddq2D){
  VectorXf ddq3D(10);
  ddq3D << ddq2D(0), 0, 0, ddq2D(1), 0, ddq2D(2), 0, 0, 0, 0;
  return ddq3D;
}

VectorXf acc3DTo2D(VectorXf& ddq3D){
  VectorXf ddq2D(3);
  ddq2D << ddq3D(0), ddq3D(3), ddq3D(5);
  return ddq2D;
}

// --------- GETTER FUNCTIONS --------- //

double getFinalCost(VectorXf& q, VectorXf& dq){
  VectorXf q3D = config2DTo3D(q);
  //string linkName = strcat(prefix, "_link_7");
  //vector<double> eePose = getTransform(robot, q3d, linkName);
  // VectorXf goal(2);
  // goalPose << goal[0], goal[1];
  //double gf = 30*(goalPose - eePose).norm();
  double gf = 0.0;
  return gf;
}

double getRunningCost(VectorXf& q, VectorXf& dq, VectorXf& u){
  return 0.0;
}

VectorXf getConfigRobot(double *x, int t){
  VectorXf qRobot(dofRobot);
  int lowOffset = 2*(dofRobot + dofEnv)*t + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    qRobot(ii-lowOffset) = x[ii];
  return qRobot;
}

VectorXf getVelRobot(double *x, int t){
  VectorXf dqRobot(dofRobot);
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    dqRobot(ii-lowOffset) = x[ii];
  return dqRobot;
}

VectorXf getConfigEnv(double *x, int t){
  VectorXf qEnv(dofEnv); //= new double[dofEnv];
  int lowOffset = 2*(dofRobot + dofEnv)*t + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot + dofEnv;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    qEnv(ii-lowOffset) = x[ii];
  return qEnv;
}

VectorXf getVelEnv(double *x, int t){
  VectorXf dqEnv(dofEnv);
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + 2*(dofRobot + dofEnv);
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    dqEnv(ii-lowOffset) = x[ii];
  return dqEnv;
}

VectorXf getU(double *x, int t){
  VectorXf u(dofRobot);
  int nx = 2*(dofRobot+dofEnv)*(T+1);
  int lowOffset = nx + 1 + dofRobot*(t - 1);
  int upOffset  = nx + 1 + dofRobot*t - 1;
  for(size_t ii=lowOffset; ii <= upOffset; ii++)    // TODO check if should be inclusive
    u(ii-lowOffset) = x[ii];
  return u;
}
// ------------------------------------ //

// --------- SETTER FUNCTIONS --------- //

void setConfigRobot(double *x, int t, double *q){
  int lowOffset = 2*(dofRobot + dofEnv)*t + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot;
  copy(x + lowOffset, x + upOffset, q);
}

void setConfigEnv(double *x, int t, double *q){
  int lowOffset = 2*(dofRobot + dofEnv)*t + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + dofRobot + dofEnv;
  copy(x + lowOffset, x + upOffset, q);
}

void setVelRobot(double *x, int t, double *dq){
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot;
  copy(x + lowOffset, x + upOffset, dq);
}

void setVelEnv(double *x, int t, double *dq){
  int lowOffset = 2*(dofRobot + dofEnv)*t + (dofRobot + dofEnv) + dofRobot + 1;
  int upOffset  = 2*(dofRobot + dofEnv)*t + 2*(dofRobot + dofEnv);
  copy(x + lowOffset, x + upOffset, dq);
}

void setU(double *x, int t, double *u, int nx){
  int uStart    = nx + 1;
  int lowOffset = uStart + dofRobot*(t-1);
  int upOffset  = uStart + dofRobot*t - 1;
  copy(x + lowOffset, x + upOffset, u);
}

void setLambda(double *x, int t, double *lambda, int nx, int nu){
  int lambStart = nx + nu + 1;
  int lowOffset = lambStart + dofLamb*(t-1);
  int upOffset  = lambStart + dofLamb*t - 1;
  copy(x + lowOffset, x + upOffset, lambda);
}

// ------------------------------------ //

void userfun(int    *Status, int *n,    double x[],
       int    *needF,  int *neF,  double F[],
       int    *needG,  int *neG,  double G[],
       char      *cu,  int *lencu,
       int    iu[],    int *leniu,
       double ru[],    int *lenru) {

  // TODO IMPLEMENT ME!
  VectorXf qRobot_T   = getConfigRobot(x, T);
  VectorXf dqRobot_T  = getVelRobot(x, T);
  double obj          = getFinalCost(qRobot_T, dqRobot_T);

  int kinLen  = (dofRobot + dofEnv)*T;
  int dynLen  = (dofRobot + dofEnv)*T;
  int collLen = T;

  // Compute kinematic, dynamics, and collision constraints
  double *kinConstraints = new double[kinLen];
  double *dynConstraints = new double[dynLen];
  double *collConstraints = new double[collLen];

  for(int t=0; t<T-1; t++){
    VectorXf qRobot_t  = getConfigRobot(x, t);
    VectorXf qRobot_t1 = getConfigRobot(x, t+1);

    VectorXf dqRobot_t   = getVelRobot(x, t);
    VectorXf dqRobot_t1  = getVelRobot(x, t+1);

    VectorXf dqEnv_t   = getVelEnv(x, t);
    VectorXf dqEnv_t1  = getVelEnv(x, t+1);

    VectorXf qEnv_t  = getConfigEnv(x, t);
    VectorXf qEnv_t1 = getConfigEnv(x, t+1);

    VectorXf ddqRobot_t1  = (dqRobot_t1 - dqRobot_t)/dt;
    VectorXf ddqEnv_t1    = (dqEnv_t1 - dqEnv_t)/dt;

    VectorXf qRobot3D_t1  = config2DTo3D(qRobot_t1);
    VectorXf dqRobot3D_t1 = vel2DTo3D(qRobot_t1);

    VectorXf u_t1   = getU(x, t+1);
    VectorXf u3D_t1 = control2DTo3D(u_t1);

    //VectorXf ddqRobot3D_t1_fwd  = forwardDynamics(robot, qRobot3D_t1, dqRobot3D_t1, u3D_t1);
    //VectorXf ddqRobot_t1_fwd    = acc3DTo2D(ddqRobot3D_t1_fwd);

    obj += dt*getRunningCost(qRobot_t, dqRobot_t, u_t1);
  }

  int kinStart  = 1;
  int dynStart  = kinStart + kinLen + 1;
  int collStart = dynStart + dynLen + 1;

  // Set up the constraint vector
  F[0] = obj;
  copy(F + kinStart, F + kinStart + kinLen, kinConstraints);
  copy(F + dynStart, F + dynStart + dynLen, dynConstraints);
  copy(F + collStart, F + collStart + collLen, collConstraints);
}

int main(int argc, char **argv) {
  snoptProblemA ToyProb;

  string urdf_file = "../urdf/jaco_dynamics.urdf";

  /*urdf::Model robot;
  if (!robot.initString(urdf_file)) {
    ROS_FATAL("Could not initialize robot model");
    return -1;
  }

  KDL::Tree tree;
  if (!kdl_parser::treeFromFile(urdf_file, tree)){
    ROS_ERROR("Failed to construct kdl tree");
    return false;
  }*/

  // Allocate and initialize;
  int nx        =  2*(dofRobot+dofEnv)*(T+1); // num states over time horizon
  int nu        =  dofRobot*T;        // num controls over time horizon
  int nlamb     =  T;

  int neF       =  1 + 2*(dofRobot+dofEnv)*T+T; // num constraints
  int n         =  nx + nu + nlamb;

  int nS = 0, nInf;
  double sInf;

  double *x      = new double[n];
  double *xlow   = new double[n];
  double *xupp   = new double[n];
  double *xmul   = new double[n];
  int    *xstate = new    int[n];

  double *F      = new double[neF];
  double *Flow   = new double[neF];
  double *Fupp   = new double[neF];
  double *Fmul   = new double[neF];
  int    *Fstate = new int[neF];    // vector of inittial states of F(x)

  int    ObjRow  = 0;   // constant value added to the objective
  double ObjAdd  = 0;   // row of F(x) containing the objective

  int Cold = 0, Basis = 1, Warm = 2;
  int signedDistLinkStart = 1 + 2*(dofRobot + dofEnv)*T;

  double *qRobotInit    = new double[dofRobot];
  double *qRobotMin     = new double[dofRobot]; // all zeros
  double *qRobotMax     = new double[dofRobot];

  double *qEnvInit      = new double[dofEnv];   // all zeros
  double *qEnvMin       = new double[dofEnv];   // all zeros
  double *qEnvMax       = new double[dofEnv];

  double *dqRobotInit   = new double[dofRobot]; // all zeros
  double *dqRobotMin    = new double[dofRobot]; // all zeros
  double *dqRobotMax    = new double[dofRobot];

  double *dqEnvInit     = new double[dofEnv];   // all zeros
  double *dqEnvMin      = new double[dofEnv];   // all zeros
  double *dqEnvMax      = new double[dofEnv];

  double *lambMin       = new double[dofLamb];  // all zeros
  double *lambMax       = new double[dofLamb];

  double *uMin          = new double[dofRobot];
  double *uMax          = new double[dofRobot];

  // --------- Set up init, max and min  --------- //

  fill(qRobotInit, qRobotInit + dofRobot, pi);
  fill(qRobotMax, qRobotMax + dofRobot, 2.0*pi);

  fill(dqRobotMin, dqRobotMin + dofRobot, -0.35);
  fill(dqRobotMax, dqRobotMax + dofRobot, 0.35);

  fill(qEnvMax, qEnvMax + dofEnv, 100.0);
  fill(dqEnvMax, dqEnvMax + dofEnv, 10.0);

  fill(uMin, uMin + dofRobot, -100.0);
  fill(uMax, uMax + dofRobot, 100.0);

  fill(lambMax, lambMax + dofLamb, 1000.0);

  // --------------------------------------------- //

  // --------- Set up the constraints on x --------- //

  for(int t = 0;t < T;t++){
    if(t == 0){
      // Constrain initial config and velocity
      setConfigRobot(xlow, t, qRobotInit);
      setConfigRobot(xupp, t, qRobotInit);

      setVelRobot(xlow, t, dqRobotInit);
      setVelRobot(xupp, t, dqRobotInit);

      setConfigEnv(xlow, t, qEnvInit);
      setConfigEnv(xupp, t, qEnvInit);

      setVelEnv(xlow, t, dqEnvInit);
      setVelEnv(xupp, t, dqEnvInit);

    }else{
      // Constrain joint limits for rest of trajectory
      setConfigRobot(xlow, t, qRobotMin);
      setConfigRobot(xupp, t, qRobotMax);

      setVelRobot(xlow, t, dqRobotMin);
      setVelRobot(xupp, t, dqRobotMax);

      setConfigEnv(xlow, t, qEnvMin);
      setConfigEnv(xupp, t, qEnvMax);

      setVelEnv(xlow, t, dqEnvMin);
      setVelEnv(xupp, t, dqEnvMax);
    }

    if(t > 0){
      // Set torque limits
      setU(xlow, t, uMin, nx);
      setU(xupp, t, uMax, nx);

      // Set lambda constraint forces
      setLambda(xlow, t, lambMin, nx, nu);
      setLambda(xupp, t, lambMax, nx, nu);
    }
  }

  // ----------------------------------------------- //

  // Set bounds on the objective
  Flow[0] = -1e3;
  Fupp[0] =  1e3;
  fill(Fupp + signedDistLinkStart, Fupp + signedDistLinkStart + T, 1000.0);

  // Load the data for planContact ...
  ToyProb.initialize    ("", 1);          // no print file; summary on
  ToyProb.setPrintFile  ("Contact.out");  // oh wait, i want a print file
  ToyProb.setProbName   ("Contact");

  // snopta will compute the Jacobian by finite-differences.
  // snJac will be called  to define the
  // coordinate arrays (iAfun,jAvar,A) and (iGfun, jGvar).
  ToyProb.setIntParameter("Derivative option", 0);
  ToyProb.setIntParameter("Verify level ", 3);

  // Solve the problem.
  // snJac is called implicitly in this case to compute the Jacobian.
  ToyProb.solve(Cold, neF, n, ObjAdd, ObjRow, userfun,
		xlow, xupp, Flow, Fupp,
		x, xstate, xmul, F, Fstate, Fmul,
		nS, nInf, sInf);

  for (int i = 0; i < n; i++){
    cout << "x = " << x[i] << " xstate = " << xstate[i] << endl;
  }

  delete []x;      delete []xlow;   delete []xupp;
  delete []xmul;   delete []xstate;

  delete []F;      delete []Flow;   delete []Fupp;
  delete []Fmul;   delete []Fstate;
}


// Copyright (c) 2020
// Commonwealth Scientific and Industrial Research Organisation (CSIRO)
// ABN 41 687 119 230
//
// Author: Kazys Stepanas, Jason Williams
#include "OhmTestConfig.h"

#include <ohm/Key.h>
#include <ohm/NdtMap.h>
#include <ohm/OccupancyMap.h>
#include <ohm/Trace.h>
#include <ohm/VoxelData.h>

#include <gtest/gtest.h>
#include "ohmtestcommon/CovarianceTestUtil.h"
#include "ohmtestcommon/OhmTestUtil.h"

#include <chrono>
#include <random>
#include <unordered_map>

#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtx/norm.hpp>
#include <glm/mat3x3.hpp>
#include <glm/vec3.hpp>

#include <3esservermacros.h>

#include <Eigen/Dense>

using namespace ohm;
using namespace ohmtestutil;

namespace ndttests
{
/// Default tolerance for checking NDT probability changes.
///
/// We use random generators with fixed seeds to create our ray set for each run. This is deterministic for a given
/// platform, but not beteen platforms as the random generator code may differ. We set a tight, default tolerance,
/// but may tune larger values for specific cases.
const float default_ndt_tolerance = 0.001f;

/// Helper for testing covariance update via @p NdtMap .
///
/// We add each sample to the map as if the sensor were at the origin. The changes to the target voxels are
/// compared against a reference code implementation. Samples may affect any number of voxels.
///
/// @param samples The set of sample points to integrate into the map.
/// @param voxel_resolution The voxel size for the @c OccupancyMap
void testNdtHits(const std::vector<glm::dvec3> &samples, const std::vector<glm::dvec3> &sensors, double resolution)
{
  std::unordered_map<ohm::Key, CovTestVoxel> reference_voxels;

  ohm::OccupancyMap map(resolution, ohm::MapFlag::kVoxelMean);
  ohm::NdtMap ndt(&map, true);

  // Simulate a sensor at the origin. Not used.
  const glm::dvec3 sensor(0.0);

  ohm::Voxel<const ohm::VoxelMean> mean(&map, map.layout().meanLayer());
  ohm::Voxel<const ohm::CovarianceVoxel> covariance(&map, map.layout().covarianceLayer());
  ASSERT_TRUE(mean.isLayerValid());
  ASSERT_TRUE(covariance.isLayerValid());

  // Build a a set of samples into the NDT map and into a reference data set.
  for (size_t i = 0; i < samples.size(); ++i)
  {
    const glm::dvec3 &sample = samples[i];
    const glm::dvec3 &sensor = sensors[i];

    ohm::Key key = map.voxelKey(sample);

    ohm::integrateNdtHit(ndt, key, sensor, sample);
    setVoxelKey(key, mean, covariance);

    ASSERT_TRUE(mean.isValid());
    ASSERT_TRUE(covariance.isValid());
    ohm::CovarianceVoxel cov_voxel;
    ohm::VoxelMean voxel_mean;
    covariance.read(&cov_voxel);
    mean.read(&voxel_mean);

    // Update the reference voxel as well.
    auto ref = reference_voxels.find(key);
    if (ref == reference_voxels.end())
    {
      // New voxel. Initialise.
      CovTestVoxel ref_voxel;
      initialiseTestVoxel(&ref_voxel, float(ndt.map().resolution()));
      ref = reference_voxels.insert(std::make_pair(key, ref_voxel)).first;
    }

    // Update the reference algorithm.
    updateHit(&ref->second, sample);

    // Progressive validation
    EXPECT_TRUE(validate(positionSafe(mean), voxel_mean.count, cov_voxel, ref->second));
  }

  // Finalise validation
  // Reset the voxel references.
  mean = ohm::Voxel<const ohm::VoxelMean>(&map, map.layout().meanLayer());
  covariance = ohm::Voxel<const ohm::CovarianceVoxel>(&map, map.layout().covarianceLayer());
  for (const auto &ref : reference_voxels)
  {
    // Lookup the target voxel.
    setVoxelKey(ref.first, mean, covariance);
    ASSERT_TRUE(mean.isValid());
    ASSERT_TRUE(covariance.isValid());
    ohm::CovarianceVoxel cov_voxel;
    ohm::VoxelMean voxel_mean;
    covariance.read(&cov_voxel);
    mean.read(&voxel_mean);
    EXPECT_TRUE(validate(positionSafe(mean), voxel_mean.count, cov_voxel, ref.second));
  }

  mean.reset();
  covariance.reset();
}

/// Helper for testing NDT based voxel miss logic.
///
/// We first build an @c NdtMap with the given set of @p samples . While we allow for these to be traced from
/// @p sensor only modify the voxel for each sample rather than integrating the full ray into the map. We assume that
/// all @p samples fall with in the same voxel.
///
/// Once the sample voxel is populated, we trace all the @p test_rays through that voxel and compare the *change* in
/// probability against the @p expected_prob_and_tolerance .
///
/// @param sensor The simulated sensor position for all the @p samples .
/// @param samples The set of sample points to integrate into the map. These should all fall in the same voxel.
/// @param voxel_resolution The voxel size for the @c OccupancyMap
/// @param sensor_noise Sensor noise value for the @c NdtMap
/// @param map_origin Origin for the @p OccupancyMap
/// @param test_rays Set of rays to test against the sample voxel. These are expected to all pass through, but not
///   end in the sample voxel. There are two elements per ray in this array making ray origin, end pairs.
/// @param expected_prob_and_tolerance Expected probability adjustments for each of the @p test_rays . This array has
///   half the elements of @p test_rays .
void testNdtMiss(const glm::dvec3 &sensor, const std::vector<glm::dvec3> samples, double voxel_resolution,
                 float sensor_noise, const glm::dvec3 &map_origin, const std::vector<glm::dvec3> &test_rays,
                 const std::vector<std::pair<float, float>> &expected_prob_and_tolerance)
{
  (void)sensor;  // Only used for 3es debugging.
  ohm::OccupancyMap map(voxel_resolution, ohm::MapFlag::kVoxelMean);
  ohm::NdtMap ndt(&map, true);

  map.setOrigin(map_origin);
  // Set explicit probabilities
  map.setHitProbability(0.55f);
  map.setMissProbability(0.45f);
  ndt.setAdaptationRate(1.0f);
  ndt.setSensorNoise(sensor_noise);

  // Integrate all the samples into the map to build voxel covariance and mean. We expect all samples to fall in one
  // voxel and test rays to pass through this voxel. While this assumption is not enforced by the integrateHit() calls
  // below, but we do cache the corresponding voxel key in target_key.
  ohm::Key target_key;
  TES_STMT(std::vector<tes::Vector3d> lines);
  for (size_t i = 0; i < samples.size(); ++i)
  {
    const glm::dvec3 &sample = samples[i];

    ohm::Key key = map.voxelKey(sample);
    target_key = key;
    ohm::integrateNdtHit(ndt, key, sensor, sample);
    TES_LINE(ohm::g_tes, TES_COLOUR(Yellow), tes::Id(), glm::value_ptr(sensor), glm::value_ptr(sample));
    TES_STMT(lines.emplace_back(tes::Vector3d(glm::value_ptr(sensor))));
    TES_STMT(lines.emplace_back(tes::Vector3d(glm::value_ptr(sample))));
  }
  TES_LINES(ohm::g_tes, TES_COLOUR(Yellow), tes::Id(), tes::DataBuffer(lines));
  TES_SERVER_UPDATE(ohm::g_tes, 0.0f);

  // Fetch the target_voxel in which we expect all samples to fall.
  Voxel<float> occupancy(&map, map.layout().occupancyLayer(), target_key);
  ASSERT_TRUE(occupancy.isValid());
  float initial_value;
  occupancy.read(&initial_value);
  ndt.setTrace(true);  // For 3es debugging

  // Now trace all the test_rays through the target_voxel. For each we will restore the initial voxel value, so
  // each test is independent. We then integrate the ray miss/passthrough and validate the probability adjustment
  // against the expected result.
  for (size_t i = 0; i < test_rays.size(); i += 2)
  {
    occupancy.write(initial_value);
    TES_LINE(ohm::g_tes, TES_COLOUR(Cornsilk), tes::Id(&sensor), glm::value_ptr(test_rays[i]),
             glm::value_ptr(test_rays[i + 1]));
    TES_SERVER_UPDATE(ohm::g_tes, 0.0f);
    ohm::integrateNdtMiss(ndt, target_key, test_rays[i], test_rays[i + 1]);
    TES_LINES_END(ohm::g_tes, tes::Id(&sensor));
    // Calculate the value adjustment.
    float adjusted_value;
    occupancy.read(&adjusted_value);
    float value_adjustment = adjusted_value - initial_value;
    // Convert to probability.
    float ray_probability = ohm::valueToProbability(value_adjustment);
    // Validate
    // Note: there is some variance here across platforms as the random generators differ even though we use
    // a fixed seed.
    EXPECT_NEAR(ray_probability, expected_prob_and_tolerance[i / 2].first, expected_prob_and_tolerance[i / 2].second);
  }
  occupancy.reset();
}

TEST(Ndt, Hit)
{
  // Use a fixed seed for test repeatability.
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::normal_distribution<double> gaussian(0.0, 1.0);
  // Generate points within a 2m cube.
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);
  std::vector<glm::dvec3> sensors;
  sensors.reserve(sample_count);

  // First generate a sample surface to target. We have to make sure this falls within a single voxel.
  glm::dvec3 mean(0);
  glm::dmat3 cov(0);

  // Sensor position from which measurements were taken
  glm::dvec3 sensor(0);

  double num_pt = 0;
  while (num_pt < 4)
  {
    // glm::dvec3 pt = 0.5 * randomVector3();
    glm::dvec3 pt;

    pt[0] = uniform(rng);
    pt[1] = uniform(rng);
    pt[2] = uniform(rng);

    const glm::dvec3 diff = pt - mean;
    const double one_on_num_pt_plus_one = 1.0 / (num_pt + 1.0);
    mean = (num_pt * mean + pt) * one_on_num_pt_plus_one;
    cov = (num_pt * cov + glm::dot((num_pt * one_on_num_pt_plus_one) * diff, diff)) * one_on_num_pt_plus_one;
    num_pt += 1.0;
  }

  {
    // TODO: this is the only part we need Eigen for. Find an alternative Eigen decomposition solution.
    Eigen::Matrix3d cov_eigen;
    cov_eigen << glm::row(cov, 0)[0], glm::row(cov, 0)[1], glm::row(cov, 0)[2],  //
      glm::row(cov, 1)[0], glm::row(cov, 1)[1], glm::row(cov, 1)[1],             //
      glm::row(cov, 2)[0], glm::row(cov, 2)[1], glm::row(cov, 2)[1];
    Eigen::LDLT<Eigen::Matrix3d, Eigen::Lower> cov_d(cov_eigen);
    const Eigen::Vector3d &vD = cov_d.vectorD();

    const Eigen::Vector3d mean_eigen(mean.x, mean.y, mean.z);
    for (size_t i = 0; i < sample_count; ++i)
    {
      Eigen::Vector3d pt;
      for (size_t i = 0; i < 3; ++i)
      {
        pt[i] = sqrt(vD[i]) * gaussian(rng);
      }

      pt = mean_eigen + cov_d.matrixL() * pt;
      samples.emplace_back(glm::dvec3(pt.x(), pt.y(), pt.z()));
      sensors.push_back(sensor);
    }
  }

  testNdtHits(samples, sensors, 2.0);
}

TEST(Ndt, MissPlanar)
{
  ohm::Trace trace("ndt-miss-planar.3es");

  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  std::uniform_real_distribution<double> uniform(0.01, 1.99);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(1, 1, 5);

  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = 1.0;
    // sample.z += 0.025 * uniform(rng);
    samples.emplace_back(sample);
  }

  // Setup to trace a ray through the voxel, but not ending in the voxel.
  std::vector<std::pair<float, float>> expected_prob_and_tolerance;
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(1, 1, -5));
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Ray parallel to the voxel ellipsoid - expected to miss.
  rays.emplace_back(glm::dvec3(-5, 1, 0.25));
  rays.emplace_back(glm::dvec3(5, 1, 0.25));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  // Ray parallel to the voxel ellipsoid, but near the centre.
  // Note: we build a perfect plane above in the centre of the voxel. However, the quantisation of the mean
  // can offset the plane. Running exactly on the plane on which we generated the points will not result in the same
  // effect in testing. We must offset the ray a little to ensure we are a bit off the plane either when using
  // voxel mean quantisation or not.
  rays.emplace_back(glm::dvec3(1, 5, 1.01));
  rays.emplace_back(glm::dvec3(1, -5, 1.01));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  // Ray running across the voxel, down towards the ellipsoid, but not crossing. This simulates rays running near
  // parallel a ground plane as it approaches the sample position.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 1));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  // Ray running across the voxel, and through the ellipsoid.
  rays.emplace_back(glm::dvec3(-5, 1, 2));
  rays.emplace_back(glm::dvec3(5, 1, 0.5));
  expected_prob_and_tolerance.emplace_back(0.23f, 0.02f);

  testNdtMiss(sensor, samples, voxel_resolution, 0.05f, glm::dvec3(0), rays, expected_prob_and_tolerance);
}

TEST(Ndt, MissCylindrical)
{
  ohm::Trace trace("ndt-miss-cylindrical.3es");

  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;

  const double cylinder_radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(cylinder_radius - sensor_noise, cylinder_radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);

  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    // Create a random sample in the voxel
    sample.x = uniform(rng);
    sample.y = uniform(rng);
    sample.z = uniform(rng);
    // Convert into a cylinder.
    // Normalise in X/Y, create a random radius (to the limit) and scale by this radius.
    double length_xy = std::sqrt(sample.x * sample.x + sample.y * sample.y);
    if (length_xy > 1e-6)
    {
      const double radius = uniform_radius(rng);
      sample.x = radius * sample.x / length_xy;
      sample.y = radius * sample.y / length_xy;
    }
    samples.emplace_back(sample);
  }

  // Setup rays through the voxel, but not ending in the voxel.
  std::vector<std::pair<float, float>> expected_prob_and_tolerance;
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Ray running parallel to the cylinder near the edge. Should be a near hit.
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, 5));
  rays.emplace_back(glm::dvec3(cylinder_radius, cylinder_radius, -5));
  expected_prob_and_tolerance.emplace_back(0.425f, 0.01f);
  // Ray running parallel to the cylinder, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * cylinder_radius, 1.5 * cylinder_radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * cylinder_radius, 2.0 * cylinder_radius, 5));
  expected_prob_and_tolerance.emplace_back(0.499f, default_ndt_tolerance);
  // Ray across the middle of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0));
  expected_prob_and_tolerance.emplace_back(0.312f, 0.005f);
  // Ray across the end (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, -cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, -cylinder_radius, 0.85));
  expected_prob_and_tolerance.emplace_back(0.436f, 0.003f);
  // Ray across the voxel, missing the cylinder (top) of the cylinder.
  rays.emplace_back(glm::dvec3(2, 2.0 * cylinder_radius, 0.85));
  rays.emplace_back(glm::dvec3(-2, 2.0 * cylinder_radius, 0.85));
  expected_prob_and_tolerance.emplace_back(0.497f, default_ndt_tolerance);
  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, glm::dvec3(-0.5 * voxel_resolution), rays,
              expected_prob_and_tolerance);
}

TEST(Ndt, MissSpherical)
{
  ohm::Trace trace("ndt-miss-spherical.3es");

  // Generate points within a 2m cube.
  const double voxel_resolution = 2.0;
  const float sensor_noise = 0.05f;

  const double radius = 0.3;
  uint32_t seed = 1153297050u;
  std::default_random_engine rng(seed);
  std::uniform_real_distribution<double> uniform(-0.99, 0.99);
  std::uniform_real_distribution<double> uniform_radius(radius - sensor_noise, radius + sensor_noise);
  const size_t sample_count = 10000;
  std::vector<glm::dvec3> samples;
  samples.reserve(sample_count);

  // Create samples which populate a single voxel and define a plane.
  const glm::dvec3 sensor(0, 0, 5);

  // Setup rays through the voxel, but not ending in the voxel.
  std::vector<std::pair<float, float>> expected_prob_and_tolerance;
  std::vector<glm::dvec3> rays;
  // Ray straight down through the voxel
  rays.emplace_back(sensor);
  rays.emplace_back(glm::dvec3(0, 0, -5));
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Reverse the ray above
  rays.emplace_back(rays[1]);
  rays.emplace_back(rays[0]);
  expected_prob_and_tolerance.emplace_back(0.004f, default_ndt_tolerance);
  // Edge of the sphere..
  rays.emplace_back(glm::dvec3(radius, radius, 5));
  rays.emplace_back(glm::dvec3(radius, radius, -5));
  expected_prob_and_tolerance.emplace_back(0.469f, 0.006f);
  // Near the edge of the sphere, but should miss.
  rays.emplace_back(glm::dvec3(1.5 * radius, 1.5 * radius, -5));
  rays.emplace_back(glm::dvec3(2.0 * radius, 2.0 * radius, 5));
  expected_prob_and_tolerance.emplace_back(0.5f, default_ndt_tolerance);
  for (size_t i = 0; i < sample_count; ++i)
  {
    glm::dvec3 sample;
    double sample_len2 = 0;
    // Keep trying while we have a near zero vector.
    while (sample_len2 < 1e-6)
    {
      // Create a random sample in the voxel
      sample.x = uniform(rng);
      sample.y = uniform(rng);
      sample.z = uniform(rng);
      sample_len2 = glm::length2(sample);
    }
    sample = glm::normalize(sample);
    sample *= uniform_radius(rng);
    samples.emplace_back(sample);
  }

  testNdtMiss(sensor, samples, voxel_resolution, sensor_noise, glm::dvec3(-0.5 * voxel_resolution), rays,
              expected_prob_and_tolerance);
}
}  // namespace ndttests


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

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2006 Marco Bianchetti
 Copyright (C) 2007 StatPro Italia srl
 Copyright (C) 2014 Ferdinando Ametrano
 Copyright (C) 2016 Peter Caspers

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

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

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

/*! \file swaption.hpp
    \brief Swaption class
*/

#ifndef quantlib_instruments_swaption_hpp
#define quantlib_instruments_swaption_hpp

#include <ql/option.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/volatility/volatilitytype.hpp>

namespace QuantLib {

    //! %settlement information
    struct Settlement {
        enum Type { Physical, Cash };
    };

    std::ostream& operator<<(std::ostream& out,
                             Settlement::Type type);

    //! %Swaption class
    /*! \ingroup instruments

        \test
        - the correctness of the returned value is tested by checking
          that the price of a payer (resp. receiver) swaption
          decreases (resp. increases) with the strike.
        - the correctness of the returned value is tested by checking
          that the price of a payer (resp. receiver) swaption
          increases (resp. decreases) with the spread.
        - the correctness of the returned value is tested by checking
          it against that of a swaption on a swap with no spread and a
          correspondingly adjusted fixed rate.
        - the correctness of the returned value is tested by checking
          it against a known good value.
        - the correctness of the returned value of cash settled swaptions
          is tested by checking the modified annuity against a value
          calculated without using the Swaption class.

        \todo add greeks and explicit exercise lag
    */
    class Swaption : public Option {
      public:
        class arguments;
        class engine;
        Swaption(const boost::shared_ptr<VanillaSwap>& swap,
                 const boost::shared_ptr<Exercise>& exercise,
                 Settlement::Type delivery = Settlement::Physical);
        //! \name Instrument interface
        //@{
        bool isExpired() const;
        void setupArguments(PricingEngine::arguments*) const;
        //@}
        //! \name Inspectors
        //@{
        Settlement::Type settlementType() const { return settlementType_; }
        VanillaSwap::Type type() const { return swap_->type(); }
        const boost::shared_ptr<VanillaSwap>& underlyingSwap() const {
            return swap_;
        }
        //@}
        //! implied volatility
        Volatility impliedVolatility(
                              Real price,
                              const Handle<YieldTermStructure>& discountCurve,
                              Volatility guess,
                              Real accuracy = 1.0e-4,
                              Natural maxEvaluations = 100,
                              Volatility minVol = 1.0e-7,
                              Volatility maxVol = 4.0,
                              VolatilityType type = ShiftedLognormal,
                              Real displacement = 0.0) const;
      private:
        // arguments
        boost::shared_ptr<VanillaSwap> swap_;
        //Handle<YieldTermStructure> termStructure_;
        Settlement::Type settlementType_;
    };

    //! %Arguments for swaption calculation
    class Swaption::arguments : public VanillaSwap::arguments,
                                public Option::arguments {
      public:
        arguments() : settlementType(Settlement::Physical) {}
        boost::shared_ptr<VanillaSwap> swap;
        Settlement::Type settlementType;
        void validate() const;
    };

    //! base class for swaption engines
    class Swaption::engine
        : public GenericEngine<Swaption::arguments, Swaption::results> {};

}

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

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2006 Cristina Duminuco
 Copyright (C) 2006 Marco Bianchetti
 Copyright (C) 2007 StatPro Italia srl
 Copyright (C) 2014 Ferdinando Ametrano
 Copyright (C) 2016 Peter Caspers

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

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

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

#include <ql/pricingengines/swaption/blackswaptionengine.hpp>
#include <ql/math/solvers1d/newtonsafe.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/exercise.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

    namespace {

        class ImpliedSwaptionVolHelper {
          public:
            ImpliedSwaptionVolHelper(const Swaption&,
                                     const Handle<YieldTermStructure>& discountCurve,
                                     Real targetValue,
                                     Real displacement,
                                     VolatilityType type);
            Real operator()(Volatility x) const;
            Real derivative(Volatility x) const;
          private:
            boost::shared_ptr<PricingEngine> engine_;
            Handle<YieldTermStructure> discountCurve_;
            Real targetValue_;
            boost::shared_ptr<SimpleQuote> vol_;
            const Instrument::results* results_;
        };

    inline     ImpliedSwaptionVolHelper::ImpliedSwaptionVolHelper(
                              const Swaption& swaption,
                              const Handle<YieldTermStructure>& discountCurve,
                              Real targetValue,
                              Real displacement,
                              VolatilityType type)
        : discountCurve_(discountCurve), targetValue_(targetValue) {

            // set an implausible value, so that calculation is forced
            // at first ImpliedSwaptionVolHelper::operator()(Volatility x) call
            vol_ = boost::shared_ptr<SimpleQuote>(new SimpleQuote(-1.0));
            Handle<Quote> h(vol_);

            switch (type) {
            case ShiftedLognormal:
                engine_ = boost::make_shared<BlackSwaptionEngine>(
                    discountCurve_, h, Actual365Fixed(), displacement);
                break;
            case Normal:
                engine_ = boost::make_shared<BachelierSwaptionEngine>(
                    discountCurve_, h, Actual365Fixed());
                break;
            default:
                QL_FAIL("unknown VolatilityType (" << type << ")");
                break;
            }
            swaption.setupArguments(engine_->getArguments());
            results_ = dynamic_cast<const Instrument::results *>(
                engine_->getResults());
        }

    inline     Real ImpliedSwaptionVolHelper::operator()(Volatility x) const {
            if (x!=vol_->value()) {
                vol_->setValue(x);
                engine_->calculate();
            }
            return results_->value-targetValue_;
        }

    inline     Real ImpliedSwaptionVolHelper::derivative(Volatility x) const {
            if (x!=vol_->value()) {
                vol_->setValue(x);
                engine_->calculate();
            }
            std::map<std::string,boost::any>::const_iterator vega_ =
                results_->additionalResults.find("vega");
            QL_REQUIRE(vega_ != results_->additionalResults.end(),
                       "vega not provided");
            return boost::any_cast<Real>(vega_->second);
        }
    }

    inline std::ostream& operator<<(std::ostream& out,
                             Settlement::Type t) {
        switch (t) {
          case Settlement::Physical:
            return out << "Delivery";
          case Settlement::Cash:
            return out << "Cash";
          default:
            QL_FAIL("unknown Settlement::Type(" << Integer(t) << ")");
        }
    }

    inline Swaption::Swaption(const boost::shared_ptr<VanillaSwap>& swap,
                       const boost::shared_ptr<Exercise>& exercise,
                       Settlement::Type delivery)
    : Option(boost::shared_ptr<Payoff>(), exercise), swap_(swap),
      settlementType_(delivery) {
        registerWith(swap_);
        registerWithObservables(swap_);
    }

    inline bool Swaption::isExpired() const {
        return detail::simple_event(exercise_->dates().back()).hasOccurred();
    }

    inline void Swaption::setupArguments(PricingEngine::arguments* args) const {

        swap_->setupArguments(args);

        Swaption::arguments* arguments =
            dynamic_cast<Swaption::arguments*>(args);

        QL_REQUIRE(arguments != 0, "wrong argument type");

        arguments->swap = swap_;
        arguments->settlementType = settlementType_;
        arguments->exercise = exercise_;
    }

    inline void Swaption::arguments::validate() const {
        VanillaSwap::arguments::validate();
        QL_REQUIRE(swap, "vanilla swap not set");
        QL_REQUIRE(exercise, "exercise not set");
    }

    inline Volatility Swaption::impliedVolatility(Real targetValue,
                                           const Handle<YieldTermStructure>& d,
                                           Volatility guess,
                                           Real accuracy,
                                           Natural maxEvaluations,
                                           Volatility minVol,
                                           Volatility maxVol,
                                           VolatilityType type,
                                           Real displacement) const {
        //calculate();
        QL_REQUIRE(!isExpired(), "instrument expired");

        ImpliedSwaptionVolHelper f(*this, d, targetValue, displacement, type);
        //Brent solver;
        NewtonSafe solver;
        solver.setMaxEvaluations(maxEvaluations);
        return solver.solve(f, accuracy, guess, minVol, maxVol);
    }

}

#endif


#include <boost/geometry/strategies/cartesian/buffer_end_round.hpp>


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

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

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

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

#include <ored/portfolio/builders/capfloor.hpp>
#include <ored/portfolio/builders/cpicapfloor.hpp>
#include <ored/portfolio/builders/swap.hpp>
#include <ored/portfolio/builders/yoycapfloor.hpp>
#include <ored/portfolio/capfloor.hpp>
#include <ored/portfolio/durationadjustedcmslegdata.hpp>
#include <ored/portfolio/fixingdates.hpp>
#include <ored/portfolio/legdata.hpp>
#include <ored/utilities/log.hpp>

#include <ql/instruments/capfloor.hpp>
#include <ql/instruments/compositeinstrument.hpp>
#include <ql/instruments/cpicapfloor.hpp>
#include <ql/instruments/inflationcapfloor.hpp>
#include <qle/indexes/inflationindexwrapper.hpp>

#include <boost/make_shared.hpp>

using namespace QuantLib;

namespace ore {
namespace data {

void CapFloor::build(const boost::shared_ptr<EngineFactory>& engineFactory) {

    DLOG("CapFloor::build() called for trade " << id() << ", leg type is " << legData_.legType());

    QL_REQUIRE((legData_.legType() == "Floating") || (legData_.legType() == "CMS") ||
                   (legData_.legType() == "DurationAdjustedCMS") || (legData_.legType() == "CPI") ||
                   (legData_.legType() == "YY"),
               "CapFloor build error, LegType must be Floating, CMS, DurationAdjustedCMS, CPI or YY");

    QL_REQUIRE(caps_.size() > 0 || floors_.size() > 0, "CapFloor build error, no cap rates or floor rates provided");
    QuantLib::CapFloor::Type capFloorType;
    if (floors_.size() == 0) {
        capFloorType = QuantLib::CapFloor::Cap;
    } else if (caps_.size() == 0) {
        capFloorType = QuantLib::CapFloor::Floor;
    } else {
        capFloorType = QuantLib::CapFloor::Collar;
    }

    legs_.clear();
    boost::shared_ptr<EngineBuilder> builder;
    std::string underlyingIndex, qlIndexName;
    boost::shared_ptr<QuantLib::Instrument> qlInstrument;

    // Account for long / short multiplier. In the following we expect the qlInstrument to be set up
    // as a long cap resp. a long floor resp. as a collar which by definition is a long cap + short floor
    // (this is opposite to the definition of a leg with naked option = true!)
    // The isPayer flag in the leg data is ignored.
    Real multiplier = (parsePositionType(longShort_) == Position::Long ? 1.0 : -1.0);

    if (legData_.legType() == "Floating") {

        boost::shared_ptr<FloatingLegData> floatData =
            boost::dynamic_pointer_cast<FloatingLegData>(legData_.concreteLegData());
        QL_REQUIRE(floatData, "Wrong LegType, expected Floating, got " << legData_.legType());
        underlyingIndex = floatData->index();
        Handle<IborIndex> hIndex =
            engineFactory->market()->iborIndex(underlyingIndex, engineFactory->configuration(MarketContext::pricing));
        QL_REQUIRE(!hIndex.empty(), "Could not find ibor index " << underlyingIndex << " in market.");
        boost::shared_ptr<IborIndex> index = hIndex.currentLink();
        qlIndexName = index->name();

        QL_REQUIRE(floatData->caps().empty() && floatData->floors().empty(),
                   "CapFloor build error, Floating leg section must not have caps and floors");

        bool isBmaIndex = boost::dynamic_pointer_cast<QuantExt::BMAIndexWrapper>(index) != nullptr;

        if (!isBmaIndex && !floatData->hasSubPeriods()) {
            // For the cases where we support caps and floors in the regular way, we build a floating leg with
            // the nakedOption flag set to true, this avoids maintaining all features in legs with associated
            // coupon pricers and at the same time in the QuaantLib::CapFloor instrument and pricing engine.
            // Supported cases are
            // - Ibor coupon without sub periods (hasSubPeriods = false)
            // - compounded ON coupon, averaged ON coupon
            // The other cases are handled below.
            LegData tmpLegData = legData_;
            boost::shared_ptr<FloatingLegData> tmpFloatData = boost::make_shared<FloatingLegData>(*floatData);
            tmpFloatData->floors() = floors_;
            tmpFloatData->caps() = caps_;
            tmpFloatData->nakedOption() = true;
            tmpLegData.concreteLegData() = tmpFloatData;
            legs_.push_back(engineFactory->legBuilder(tmpLegData.legType())
                                ->buildLeg(tmpLegData, engineFactory, requiredFixings_,
                                           engineFactory->configuration(MarketContext::pricing)));
            // if both caps and floors are given, we have to use a payer leg, since in this case
            // the StrippedCappedFlooredCoupon used to extract the naked options assumes a long floor
            // and a short cap while we have documented a collar to be a short floor and long cap
            qlInstrument =
                boost::make_shared<QuantLib::Swap>(legs_, std::vector<bool>{!floors_.empty() && !caps_.empty()});
            qlInstrument->setPricingEngine(
                boost::make_shared<DiscountingSwapEngine>(engineFactory->market()->discountCurve(legData_.currency())));
            maturity_ = CashFlows::maturityDate(legs_.front());
        } else {
            // For the cases where we don't have regular cap / floor support we treat the index approximately as an Ibor
            // index and build an QuantLib::CapFloor with associated pricing engine. These cases comprise:
            // - BMA coupons
            // - Ibor coupons with sub periods (hasSubPeriods = true)

            ALOG("CapFloor trade " << id()
                                   << " on a) BMA or b) sub periods Ibor or c) averaged ON underlying (index = '"
                                   << underlyingIndex
                                   << "') built, will treat the index approximately as an ibor index");
            builder = engineFactory->builder(tradeType_);
            legs_.push_back(makeIborLeg(legData_, index, engineFactory));

            // If a vector of cap/floor rates are provided, ensure they align with the number of schedule periods
            if (floors_.size() > 1) {
                QL_REQUIRE(floors_.size() == legs_[0].size(),
                           "The number of floor rates provided does not match the number of schedule periods");
            }

            if (caps_.size() > 1) {
                QL_REQUIRE(caps_.size() == legs_[0].size(),
                           "The number of cap rates provided does not match the number of schedule periods");
            }

            // If one cap/floor rate is given, extend the vector to align with the number of schedule periods
            if (floors_.size() == 1)
                floors_.resize(legs_[0].size(), floors_[0]);

            if (caps_.size() == 1)
                caps_.resize(legs_[0].size(), caps_[0]);

            // Create QL CapFloor instrument
            qlInstrument = boost::make_shared<QuantLib::CapFloor>(capFloorType, legs_[0], caps_, floors_);

            boost::shared_ptr<CapFloorEngineBuilder> capFloorBuilder =
                boost::dynamic_pointer_cast<CapFloorEngineBuilder>(builder);
            qlInstrument->setPricingEngine(capFloorBuilder->engine(parseCurrency(legData_.currency())));

            maturity_ = boost::dynamic_pointer_cast<QuantLib::CapFloor>(qlInstrument)->maturityDate();
        }

    } else if (legData_.legType() == "CMS") {
        builder = engineFactory->builder("Swap");

        boost::shared_ptr<CMSLegData> cmsData = boost::dynamic_pointer_cast<CMSLegData>(legData_.concreteLegData());
        QL_REQUIRE(cmsData, "Wrong LegType, expected CMS");

        underlyingIndex = cmsData->swapIndex();
        Handle<SwapIndex> hIndex =
            engineFactory->market()->swapIndex(underlyingIndex, builder->configuration(MarketContext::pricing));
        QL_REQUIRE(!hIndex.empty(), "Could not find swap index " << underlyingIndex << " in market.");

        boost::shared_ptr<SwapIndex> index = hIndex.currentLink();
        qlIndexName = index->name();

        vector<bool> legPayers;
        if (capFloorType == QuantLib::CapFloor::Collar)
            // long cap, short floor
            legPayers = {true, false};
        else if(capFloorType == QuantLib::CapFloor::Cap)
            // long cap
            legPayers = {true, false};
        else if(capFloorType == QuantLib::CapFloor::Floor)
            // long floor
            legPayers = {false, true};
        legs_.push_back(makeCMSLeg(legData_, index, engineFactory, caps_, floors_));
        legs_.push_back(makeCMSLeg(legData_, index, engineFactory));

        qlInstrument = boost::make_shared<QuantLib::Swap>(legs_, legPayers);
        boost::shared_ptr<SwapEngineBuilderBase> cmsCapFloorBuilder =
            boost::dynamic_pointer_cast<SwapEngineBuilderBase>(builder);
        qlInstrument->setPricingEngine(cmsCapFloorBuilder->engine(parseCurrency(legData_.currency())));

        maturity_ = boost::dynamic_pointer_cast<QuantLib::Swap>(qlInstrument)->maturityDate();

    } else if (legData_.legType() == "DurationAdjustedCMS") {
        auto cmsData = boost::dynamic_pointer_cast<DurationAdjustedCmsLegData>(legData_.concreteLegData());
        QL_REQUIRE(cmsData, "Wrong LegType, expected DurationAdjustedCmsLegData");
        LegData tmpLegData = legData_;
        auto tmpCmsData = boost::make_shared<DurationAdjustedCmsLegData>(*cmsData);
        tmpCmsData->floors() = floors_;
        tmpCmsData->caps() = caps_;
        tmpCmsData->nakedOption() = true;
        tmpLegData.concreteLegData() = tmpCmsData;
        legs_.push_back(engineFactory->legBuilder(tmpLegData.legType())
                            ->buildLeg(tmpLegData, engineFactory, requiredFixings_,
                                       engineFactory->configuration(MarketContext::pricing)));
        // if both caps and floors are given, we have to use a payer leg, since in this case
        // the StrippedCappedFlooredCoupon used to extract the naked options assumes a long floor
        // and a short cap while we have documented a collar to be a short floor and long cap
        qlInstrument = boost::make_shared<QuantLib::Swap>(legs_, std::vector<bool>{!floors_.empty() && !caps_.empty()});
        qlInstrument->setPricingEngine(
            boost::make_shared<DiscountingSwapEngine>(engineFactory->market()->discountCurve(legData_.currency())));
        maturity_ = CashFlows::maturityDate(legs_.front());
    } else if (legData_.legType() == "CPI") {
        DLOG("CPI CapFloor Type " << capFloorType << " ID " << id());

        builder = engineFactory->builder("CpiCapFloor");

        boost::shared_ptr<CPILegData> cpiData = boost::dynamic_pointer_cast<CPILegData>(legData_.concreteLegData());
        QL_REQUIRE(cpiData, "Wrong LegType, expected CPI");

        underlyingIndex = cpiData->index();
        Handle<ZeroInflationIndex> zeroIndex = engineFactory->market()->zeroInflationIndex(
            underlyingIndex, builder->configuration(MarketContext::pricing));
        qlIndexName = zeroIndex->name();

        // the cpi leg uses the first schedule date as the start date, which only makes sense if there are at least
        // two dates in the schedule, otherwise the only date in the schedule is the pay date of the cf and a a separate
        // start date is expected; if both the separate start date and a schedule with more than one date is given
        Date startDate;
        Schedule schedule = makeSchedule(legData_.schedule());

        const string& start = cpiData->startDate();
        if (schedule.size() < 2) {
            QL_REQUIRE(!start.empty(), "Only one schedule date, a 'StartDate' must be given.");
            startDate = parseDate(start);
        } else if (!start.empty()) {
            DLOG("Schedule with more than 2 dates was provided. The first schedule date " <<
                io::iso_date(schedule.dates().front()) << " is used as the start date. The 'StartDate' of " <<
                start << " is not used.");
            startDate = schedule.dates().front();
        }

        Real baseCPI = cpiData->baseCPI();
        Period observationLag = parsePeriod(cpiData->observationLag());
        CPI::InterpolationType interpolation = parseObservationInterpolation(cpiData->interpolation());
        Calendar cal = zeroIndex->fixingCalendar();
        BusinessDayConvention conv = Unadjusted; // not used in the CPI CapFloor engine

        QL_REQUIRE(!zeroIndex.empty(), "Zero Inflation Index is empty");

        legs_.push_back(makeCPILeg(legData_, zeroIndex.currentLink(), engineFactory));

        // If a vector of cap/floor rates are provided, ensure they align with the number of schedule periods
        if (floors_.size() > 1) {
            QL_REQUIRE(floors_.size() == legs_[0].size(),
                       "The number of floor rates provided does not match the number of schedule periods");
        }

        if (caps_.size() > 1) {
            QL_REQUIRE(caps_.size() == legs_[0].size(),
                       "The number of cap rates provided does not match the number of schedule periods");
        }

        // If one cap/floor rate is given, extend the vector to align with the number of schedule periods
        if (floors_.size() == 1)
            floors_.resize(legs_[0].size(), floors_[0]);

        if (caps_.size() == 1)
            caps_.resize(legs_[0].size(), caps_[0]);

        boost::shared_ptr<CpiCapFloorEngineBuilder> capFloorBuilder =
            boost::dynamic_pointer_cast<CpiCapFloorEngineBuilder>(builder);

        // Create QL CPI CapFloor instruments and add to a composite
        qlInstrument = boost::make_shared<CompositeInstrument>();
        maturity_ = Date::minDate();
        for (Size i = 0; i < legs_[0].size(); ++i) {
            DLOG("Create composite " << i);
            Real nominal, gearing;
            Date paymentDate;
            boost::shared_ptr<CPICoupon> coupon = boost::dynamic_pointer_cast<CPICoupon>(legs_[0][i]);
            boost::shared_ptr<CPICashFlow> cashflow = boost::dynamic_pointer_cast<CPICashFlow>(legs_[0][i]);
            if (coupon) {
                nominal = coupon->nominal();
                gearing = coupon->fixedRate() * coupon->accrualPeriod();
                paymentDate = coupon->date();
            } else if (cashflow) {
                nominal = cashflow->notional();
                gearing = 1.0; // no gearing here
                paymentDate = cashflow->date();
            } else {
                QL_FAIL("Failed to interprete CPI flow");
            }

            if (capFloorType == QuantLib::CapFloor::Cap || capFloorType == QuantLib::CapFloor::Collar) {
                boost::shared_ptr<CPICapFloor> capfloor =
                    boost::make_shared<CPICapFloor>(Option::Call, nominal, startDate, baseCPI, paymentDate, cal, conv,
                                                    cal, conv, caps_[i], zeroIndex, observationLag, interpolation);
                capfloor->setPricingEngine(capFloorBuilder->engine(underlyingIndex));
                boost::dynamic_pointer_cast<QuantLib::CompositeInstrument>(qlInstrument)->add(capfloor, gearing);
                maturity_ = std::max(maturity_, capfloor->payDate());
            }

            if (capFloorType == QuantLib::CapFloor::Floor || capFloorType == QuantLib::CapFloor::Collar) {
                // for collars we want a long cap, short floor
                Real sign = capFloorType == QuantLib::CapFloor::Floor ? 1.0 : -1.0;
                boost::shared_ptr<CPICapFloor> capfloor =
                    boost::make_shared<CPICapFloor>(Option::Put, nominal, startDate, baseCPI, paymentDate, cal, conv,
                                                    cal, conv, floors_[i], zeroIndex, observationLag, interpolation);
                capfloor->setPricingEngine(capFloorBuilder->engine(underlyingIndex));
                boost::dynamic_pointer_cast<QuantLib::CompositeInstrument>(qlInstrument)->add(capfloor, sign * gearing);
                maturity_ = std::max(maturity_, capfloor->payDate());
            }
        }

    } else if (legData_.legType() == "YY") {
        builder = engineFactory->builder("YYCapFloor");

        boost::shared_ptr<YoYLegData> yyData = boost::dynamic_pointer_cast<YoYLegData>(legData_.concreteLegData());
        QL_REQUIRE(yyData, "Wrong LegType, expected YY");

        underlyingIndex = yyData->index();
        Handle<YoYInflationIndex> yoyIndex;
        // look for yoy inflation index
        yoyIndex =
            engineFactory->market()->yoyInflationIndex(underlyingIndex, builder->configuration(MarketContext::pricing));
        qlIndexName = yoyIndex->name();

        // we must have either an yoy or a zero inflation index in the market, if no yoy curve, get teh zero
        // and create a yoy index from it
        if (yoyIndex.empty()) {
            Handle<ZeroInflationIndex> zeroIndex = engineFactory->market()->zeroInflationIndex(
                underlyingIndex, builder->configuration(MarketContext::pricing));
            QL_REQUIRE(!zeroIndex.empty(), "Could not find inflation index (of type either zero or yoy) "
                                               << underlyingIndex << " in market.");
            yoyIndex = Handle<YoYInflationIndex>(boost::make_shared<QuantExt::YoYInflationIndexWrapper>(
                zeroIndex.currentLink(), zeroIndex->interpolated()));
        }

        legs_.push_back(makeYoYLeg(legData_, yoyIndex.currentLink(), engineFactory));

        // If a vector of cap/floor rates are provided, ensure they align with the number of schedule periods
        if (floors_.size() > 1) {
            QL_REQUIRE(floors_.size() == legs_[0].size(),
                       "The number of floor rates provided does not match the number of schedule periods");
        }

        if (caps_.size() > 1) {
            QL_REQUIRE(caps_.size() == legs_[0].size(),
                       "The number of cap rates provided does not match the number of schedule periods");
        }

        // If one cap/floor rate is given, extend the vector to align with the number of schedule periods
        if (floors_.size() == 1)
            floors_.resize(legs_[0].size(), floors_[0]);

        if (caps_.size() == 1)
            caps_.resize(legs_[0].size(), caps_[0]);

        // Create QL YoY Inflation CapFloor instrument
        if (capFloorType == QuantLib::CapFloor::Cap) {
            qlInstrument = boost::shared_ptr<YoYInflationCapFloor>(new YoYInflationCap(legs_[0], caps_));
        } else if (capFloorType == QuantLib::CapFloor::Floor) {
            qlInstrument = boost::shared_ptr<YoYInflationCapFloor>(new YoYInflationFloor(legs_[0], floors_));
        } else if (capFloorType == QuantLib::CapFloor::Collar) {
            qlInstrument = boost::shared_ptr<YoYInflationCapFloor>(
                new YoYInflationCapFloor(QuantLib::YoYInflationCapFloor::Collar, legs_[0], caps_, floors_));
        } else {
            QL_FAIL("unknown YoYInflation cap/floor type");
        }

        boost::shared_ptr<YoYCapFloorEngineBuilder> capFloorBuilder =
            boost::dynamic_pointer_cast<YoYCapFloorEngineBuilder>(builder);
        qlInstrument->setPricingEngine(capFloorBuilder->engine(underlyingIndex));

        // Wrap the QL instrument in a vanilla instrument

        maturity_ = boost::dynamic_pointer_cast<QuantLib::YoYInflationCapFloor>(qlInstrument)->maturityDate();
    } else {
        QL_FAIL("Invalid legType " << legData_.legType() << " for CapFloor");
    }

    std::vector<boost::shared_ptr<Instrument>> additionalInstruments;
    std::vector<Real> additionalMultipliers;
    addPremiums(additionalInstruments, additionalMultipliers, 1.0, premiumData_, -multiplier,
                parseCurrency(legData_.currency()), engineFactory,
                engineFactory->configuration(MarketContext::pricing));

    // set instrument
    instrument_ =
        boost::make_shared<VanillaInstrument>(qlInstrument, multiplier, additionalInstruments, additionalMultipliers);

    // add required fixings
    if (!qlIndexName.empty() && !underlyingIndex.empty()) {
        std::map<string, string> indexNameMapper = {{qlIndexName, underlyingIndex}};
        if (engineFactory->iborFallbackConfig().isIndexReplaced(underlyingIndex)) {
            string rfrIndexName = engineFactory->iborFallbackConfig().fallbackData(underlyingIndex).rfrIndex;
            // we don't support convention based rfr fallback indices, with ore ticket 1758 this might change
            indexNameMapper[parseIborIndex(rfrIndexName)->name()] = rfrIndexName;
        }
        auto fdg = boost::make_shared<FixingDateGetter>(requiredFixings_, indexNameMapper);
        for (auto const& l : legs_)
            addToRequiredFixings(l, fdg);
    }

    // Fill in remaining Trade member data
    legCurrencies_.push_back(legData_.currency());
    legPayers_.push_back(false); // already accounted for via the instrument multiplier
    npvCurrency_ = legData_.currency();
    notionalCurrency_ = legData_.currency();
    notional_ = currentNotional(legs_[0]);
}

void CapFloor::fromXML(XMLNode* node) {
    Trade::fromXML(node);
    XMLNode* capFloorNode = XMLUtils::getChildNode(node, "CapFloorData");
    longShort_ = XMLUtils::getChildValue(capFloorNode, "LongShort", true);
    legData_.fromXML(XMLUtils::getChildNode(capFloorNode, "LegData"));
    caps_ = XMLUtils::getChildrenValuesAsDoubles(capFloorNode, "Caps", "Cap");
    floors_ = XMLUtils::getChildrenValuesAsDoubles(capFloorNode, "Floors", "Floor");
    premiumData_.fromXML(capFloorNode);
}

XMLNode* CapFloor::toXML(XMLDocument& doc) {
    XMLNode* node = Trade::toXML(doc);
    XMLNode* capFloorNode = doc.allocNode("CapFloorData");
    XMLUtils::appendNode(node, capFloorNode);
    XMLUtils::addChild(doc, capFloorNode, "LongShort", longShort_);
    XMLUtils::appendNode(capFloorNode, legData_.toXML(doc));
    XMLUtils::addChildren(doc, capFloorNode, "Caps", "Cap", caps_);
    XMLUtils::addChildren(doc, capFloorNode, "Floors", "Floor", floors_);
    XMLUtils::appendNode(capFloorNode, premiumData_.toXML(doc));
    return node;
}
} // namespace data
} // namespace ore