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

/*!
 Copyright (C) 2008 Allen Kuo

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

/* This example sets up a callable fixed rate bond with a Hull White pricing
   engine and compares to Bloomberg's Hull White price/yield calculations.
*/

#include <ql/experimental/callablebonds/callablebond.hpp>
#include <ql/experimental/callablebonds/treecallablebondengine.hpp>
#include <ql/models/shortrate/onefactormodels/hullwhite.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/unitedstates.hpp>
#include <ql/time/daycounters/actualactual.hpp>

#ifdef BOOST_MSVC
/* Uncomment the following lines to unmask floating-point
   exceptions. Warning: unpredictable results can arise...

   See http://www.wilmott.com/messageview.cfm?catid=10&threadid=9481
   Is there anyone with a definitive word about this?
*/
// #include <float.h>
// namespace { unsigned int u = _controlfp(_EM_INEXACT, _MCW_EM); }
#endif

#include <vector>
#include <cmath>
#include <iomanip>
#include <iostream>
#include <boost/timer.hpp>

using namespace std;
using namespace QuantLib;

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

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

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

int main(int, char* [])
{
    try {

        boost::timer timer;

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

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

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

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

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

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

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

        // set up the call schedule

        CallabilitySchedule callSchedule;
        Real callPrice = 100.;
        Size numberOfCallDates = 24;
        Date callDate = Date(15,September,2006);

        for (Size i=0; i< numberOfCallDates; i++) {
            Calendar nullCalendar = NullCalendar();

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

        // set up the callable bond

        Date dated = Date(16,September,2004);
        Date issue = dated;
        Date maturity = Date(15,September,2012);
        Natural settlementDays = 3;  // Bloomberg OAS1 settle is Oct 19, 2007
        Calendar bondCalendar = UnitedStates(UnitedStates::GovernmentBond);
        Real coupon = .0465;
        Frequency frequency = Quarterly;
        Real redemption = 100.0;
        Real faceAmount = 100.0;

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

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

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

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

        // output price/yield results for varying volatility parameter

        Real sigma = QL_EPSILON; // core dumps if zero on Cygwin

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

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

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

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

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

        cout << "Bloomberg price/yld (%) ";
        cout << "96.50 / 5.47"
             << endl
             << endl;

        sigma = .01;

        cout << "sigma/vol (%) = " << 100.*sigma << endl;

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

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

        callableBond.setPricingEngine(engine1);

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

        cout << "Bloomberg price/yld (%) ";
        cout << "95.68 / 5.66"
             << endl
             << endl;

        ////////////////////

        sigma = .03;

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

        boost::shared_ptr<PricingEngine> engine2(
                      new TreeCallableFixedRateBondEngine(hw2,gridIntervals));

        callableBond.setPricingEngine(engine2);

        cout << "sigma/vol (%) = "
             << 100.*sigma
             << endl;

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

        cout << "Bloomberg price/yld (%) ";
        cout << "92.34 / 6.49"
             << endl
             << endl;

        ////////////////////////////

        sigma = .06;

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

        boost::shared_ptr<PricingEngine> engine3(
                      new TreeCallableFixedRateBondEngine(hw3,gridIntervals));

        callableBond.setPricingEngine(engine3);

        cout << "sigma/vol (%) = "
             << 100.*sigma
             << endl;

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

        cout << "Bloomberg price/yld (%) ";
        cout << "87.16 / 7.83"
             << endl
             << endl;

        /////////////////////////

        sigma = .12;

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

        boost::shared_ptr<PricingEngine> engine4(
                      new TreeCallableFixedRateBondEngine(hw4,gridIntervals));

        callableBond.setPricingEngine(engine4);

        cout << "sigma/vol (%) = "
             << 100.*sigma
             << endl;

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

        cout << "Bloomberg price/yld (%) ";
        cout << "77.31 / 10.65"
             << endl
             << endl;

        double seconds = timer.elapsed();
        Integer hours = int(seconds/3600);
        seconds -= hours * 3600;
        Integer minutes = int(seconds/60);
        seconds -= minutes * 60;
        cout << " \nRun completed in ";
        if (hours > 0)
            cout << hours << " h ";
        if (hours > 0 || minutes > 0)
            cout << minutes << " m ";
        cout << fixed << setprecision(0)
             << seconds << " s\n" << endl;

        return 0;

    } catch (std::exception& e) {
        std::cerr << e.what() << std::endl;
        return 1;
    } catch (...) {
        std::cerr << "unknown error" << std::endl;
        return 1;
    }
}


#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>

//#include <iostream>

namespace ublas = boost::numeric::ublas;

int main(int argc, char ** argv)
{
    ublas::matrix<double> M(3, 3, 1);
    ublas::identity_matrix<double> I(3);
    ublas::zero_matrix<double> Z(3);
    ublas::scalar_matrix<double> S(2, 3);

    for (size_t idxRow = 0; idxRow < M.size1(); idxRow++)
    {
        for (size_t idxCol = 0; idxCol < M.size2(); idxCol++)
        {
            M(idxRow, idxCol) = 3 * idxRow + idxCol;
        }
    }

    std::cout << "M: " << M << std::endl;
    std::cout << ublas::row(M, 0) << std::endl;
    std::cout << ublas::inner_prod(ublas::row(M, 0), ublas::row(M, 1)) << std::endl;
    std::cout << ublas::project(ublas::row(M, 1), ublas::range(0, 2)) << std::endl;

    getchar();

    return 0;
}


/* Hector -- A Simple Climate Model
   Copyright (C) 2014-2015  Battelle Memorial Institute

   Please see the accompanying file LICENSE.md for additional licensing
   information.
*/
// ocean_csys_class.cpp : Defines the entry point for the console application.
/*  Ocean Carbon Chemistry CODE File:
 *
 *  Created by Corinne Hartin  1/30/13.

 *  This code translated from MATLAB code
 *  Reference: Richard E. Zeebe and Dieter A. Wolf-Gladrow

 *	Alfred Wegener Institute for
 *	Polar and Marine Research
 *	P.O. Box 12 01 61
 *   D-27515 Bremerhaven
 *	Germany
 *	e-mail: rzeebe@awi-bremerhaven.de   wolf@awi-bremerhaven.de
 *
 *   based on the book by Zeebe and Wolf-Gladrow (2001) CO2 in seawater: equilibrium, kintetics, isotopes. 346 p Amsterdam: Elsevier
 *   http://www.soest.hawaii.edu/oceanography/faculty/zeebe_files/CO2_System_in_Seawater/csys.html
 */

#include <math.h>
#include <boost/math/tools/polynomial.hpp>
#include <boost/math/tools/roots.hpp>

#include "h_exception.hpp"
#include "ocean_csys.hpp"

namespace Hector {

using namespace std;

//------------------------------------------------------------------------------
/*! \brief new oceanbox logger
 *  oceanbox logger may or may not be defined and therefore we check before logging
 */
#define CS_LOG(log, level)  \
if( log != NULL ) H_LOG( (*log), level )

//------------------------------------------------------------------------------
/*! \brief constructor
 */
oceancsys::oceancsys() : ncoeffs(6), m_a(ncoeffs) {
	logger = NULL;
	S = alk = As = Ks = 0.0;
}

//------------------------------------------------------------------------------
/*! \brief A helper functor class used to evauluate a polynomial and its derivative.
 *  \details This helper class gives an interface that is callable from boost's numerical
 *           solvers which need to evaluate a function and it's derivative.  This class
 *           wrapps a polynomical by taking an array of coefficients in ascending order of
 *           the degree of the term they are associated.
 */
class PolyDerivFunctor {
    public:
        PolyDerivFunctor(const double* coefs, const int degree) {
            using namespace boost::math::tools;
            const int size = degree + 1;
            mPoly = polynomial<double>(coefs, degree);
            double* derivCoef = new double[size-1];
            for(int i = 1; i < size; ++i) {
                derivCoef[i - 1] = coefs[i] * static_cast<double>(i);
            }
            mPolyDeriv = polynomial<double>(derivCoef, degree-1);
            delete[] derivCoef;
        }

        pair<double, double> operator()(const double x) {
            return pair<double, double>(mPoly.evaluate(x), mPolyDeriv.evaluate(x));
        }

    private:
        //! The representation of the polynomial to calculate.
        boost::math::tools::polynomial<double> mPoly;

        //! The representation of the derivative of the polynomial to calculate.
        boost::math::tools::polynomial<double> mPolyDeriv;
};

//------------------------------------------------------------------------------
/*! \brief Find the largest real root, using GSL or appropriate algorithms
 *  \param ncoeff   Number of coefficients
 *  \param *a       Coefficients
 *  \return         Largest real root (H+ ion)
 */
double find_largest_root( const int ncoeffs, double* a ) {

    using namespace boost::math::tools;
    const int degree = ncoeffs-1;
    PolyDerivFunctor polyFunctor(a, degree);
    // Use Fujiwara's method to find an upper bound for the roots of the polynomial
    double max = pow(std::abs(a[0] / ( 2.0 * a[degree])), 1.0 / degree);
    for(int i = 1; i < degree; ++i) {
        max = std::max(max, pow(std::abs(a[i]/a[degree]), 1.0 / static_cast<double>(degree - i)));
    }
    max *= 2.0;
    // Use Newton's method to find the largest real root starting from the Fujiwara upper bound
    // arbitrarily solve unil 60% of the digits are correct.
    const int digits = numeric_limits<double>::digits;
    int get_digits = static_cast<int>(digits * 0.6);
    double h = newton_raphson_iterate(polyFunctor, max-0.001, 0.0, max, get_digits);

	return h;
}

//------------------------------------------------------------------------------
/*! \brief Run Ocean csys
 *
 * DIC and ALK calculate pH, pCO2, omega Ar, omega Ca
 * (from Zeebe and Wolfe-Gladrow 2001)
 * pCO2 is used to calculate ocean-atmosphere fluxes
 * (from Takahashi et al, 2009, eq. 7 & 8)
 */
void oceancsys::ocean_csys_run( unitval tbox, unitval carbon )
{

	double tmp, tmp1, tmp2, tmp3;

    // Convert carbon to dic value and temperature to K
    const double dic = convertToDIC( carbon ).value( U_UMOL_KG )/1e6;   // back to mol/kg
    const double Tc = tbox.value( U_DEGC );
    const double Tk = Tc + 273.15;

	// Check that all is OK with input data
	H_ASSERT( Tk > 265 && Tk < 308, "bad Tk value" ); // Kelvin
    H_ASSERT( dic > 1000e-6 && dic < 3700e-6, "bad dic value" );  // mol/kg

    // alk should be constant once spinup is done, but check anyway
    H_ASSERT( alk >= 2000e-6 && alk <= 2750e-6, "bad alk value" );  // mol/kg

	/*---------------------------------------------------------------
     This section calculates the constants K0, Sc, K1, K2, Ksp, Ksi etc.
     ---------------------------------------------------------------*/

	// --------------------- K0 -----------------------------------
	// solubility of CO2 calculated from Weiss 1974 (mol * L-1 * atm-1)
	// used to calculate CO2 fluxes
	tmp1 = -58.0931 + 90.5069* ( 100/Tk ) + 22.2940 * log( Tk/100 );
	tmp2 = S * ( 0.027766 - 0.025888 * ( Tk/100 ) + 0.0050578 * ( ( Tk/100 ) * ( Tk/100 ) ) );
	const double lnK0 =  tmp1 + tmp2;
	K0.set( exp( lnK0 ), U_MOL_L_ATM );

	//---------------------Sc------------------------------------------
	// Schmidt Number from Wanninkhof 1992
	const double Sc = 2073.1 - ( 125.62 * Tc ) + (3.6276 * Tc * Tc) - ( 0.043219 * Tc * Tc * Tc );

	// --------------------- Kwater -----------------------------------
	// table 1.1 in Part1: Seawater carbonate chemistry Andrew Dickson
	// Millero (1995)(in Dickson and Goyet (1994, Chapter 5, p.18))
	tmp1 = -13847.26/Tk + 148.96502 - 23.6521 * log( Tk );
	tmp2 = + (118.67/Tk - 5.977 + 1.0495*log( Tk ) ) * sqrt( S ) - 0.01615 * S;
	const double lnKw =  tmp1 + tmp2;
	Kw.set( exp(lnKw), U_MOL_KG);

	//---------------------- Kh (K Henry) ----------------------------
	// solubility of CO2 calculated from Weiss 1974 (mol*kg-1*atm-1)
	// Kh and K0 are identical equations with differing constants resulting in different units
	// used to calculate pCO2
	tmp = 9345.17 / Tk - 60.2409 + 23.3585 * log( Tk/100 );
	const double nKhwe74 = tmp + S * ( 0.023517-0.00023656 * Tk + 0.0047036e-4 * Tk * Tk );
	Kh.set( exp( nKhwe74 ), U_MOL_KG_ATM);

	// --------------------- K1 ---------------------------------------
	//   Mehrbach et al (1973) refit by Lueker et al. (2000).
	const double pK1mehr = 3633.86/Tk - 61.2172 + 9.6777*log( Tk ) - 0.011555 * S + 0.0001152 * S * S;
	const unitval K1( pow( 10, -pK1mehr ), U_MOL_KG);

	// --------------------- K2 ----------------------------------------
	//   Mehrbach et al. (1973) refit by Lueker et al. (2000).
	const double pK2mehr = 471.78/Tk + 25.9290 - 3.16967 * log( Tk ) - 0.01781 * S + 0.0001122 * S * S;
	const unitval K2( pow( 10.0, -pK2mehr ), U_MOL_KG);

	// --------------------- Kb  --------------------------------------------
	// boric acid DOE 1994
	tmp1 =  ( -8966.90-2890.53 * sqrt( S ) - 77.942 * S+ 1.728*pow( S,( 3.0/2.0 ) ) - 0.0996 * S * S )/Tk;
	tmp2 =   +148.0248+137.1942 * sqrt( S ) + 1.62142 * S;
	tmp3 = +(-24.4344-25.085 * sqrt( S )-0.2474 * S ) * log( Tk ) + 0.053105 * sqrt( S ) * Tk;
	const double lnKb = tmp1 + tmp2 + tmp3;
	const unitval Kb( exp(lnKb), U_MOL_KG);

	// --------------------- Kspc (calcite) ----------------------------
	// Mucci, Alphonso, Amer. J. of Science 283:781-799, 1983
	tmp1 = -171.9065-0.077993 * Tk + 2839.319/Tk + 71.595 * log10( Tk );
	tmp2 = +( -0.77712+0.0028426 * Tk + 178.34/Tk ) * sqrt( S );
	tmp3 = -0.07711 * S + 0.0041249 * pow( S, 1.5 );
	const double log10Kspc = tmp1 + tmp2 + tmp3;
	const double Kspc = pow( 10.0, log10Kspc ); // mol/kg

	// --------------------- Kspa (aragonite) ----------------------------
	// Mucci, Alphonso, Amer. J. of Science 283:781-799, 1983
	tmp1 = -171.945 - 0.077993 * Tk + 2903.293 / Tk + 71.595 * log10( Tk );
	tmp2 = +( -0.068393+0.0017276 * Tk + 88.135/Tk ) * sqrt( S );
	tmp3 = -0.10018 * S + 0.0059415 * pow( S, 1.5 );
	const double log10Kspa = tmp1 + tmp2 + tmp3;
	const double Kspa = pow( 10.0, log10Kspa ); // mol/kg

	//------------------------- boron --------------------------------------
	// total boron concentration
	// DOE 1994
	const double bor = 1 * ( 416.0 * ( S/35.0 ) ) * 1.e-6;   // (mol/kg), DOE94

	/* ---------------------------------------
     ALK and DIC given solve for pH and pCO2
     ------------------------------------------*/

    const double Kb_val = Kb.value( U_MOL_KG );     // for convenience in eqns below
    const double K1_val = K1.value( U_MOL_KG );
    const double K2_val = K2.value( U_MOL_KG );
    const double Kw_val = Kw.value( U_MOL_KG );

	const double p5 = -1.0;
	const double p4 = -alk - Kb_val - K1_val;
	const double p3 = dic * K1_val - alk * ( Kb_val + K1_val )
        + Kb_val * bor + Kw_val - Kb_val
        * K1_val - K1_val * K2_val;
	tmp = dic * ( Kb_val * K1_val + 2.0 * K1_val * K2_val )
        -alk * (Kb_val * K1_val + K1_val * K2_val)
        + Kb_val * bor * K1_val;
	const double p2 = tmp + ( Kw_val * Kb_val + Kw_val * K1_val - Kb_val * K1_val * K2_val );
	tmp = 2.0 * dic * Kb_val * K1_val * K2_val
        - alk * Kb_val * K1_val * K2_val + Kb_val
        * bor * K1_val * K2_val;
	const double p1 = tmp + ( Kw_val * Kb_val * K1_val + Kw_val * K1_val * K2_val );
	const double p0 = Kw_val * Kb_val * K1_val * K2_val;

	m_a[ 0 ] = p0;
	m_a[ 1 ] = p1;
	m_a[ 2 ] = p2;
	m_a[ 3 ] = p3;
	m_a[ 4 ] = p4;
	m_a[ 5 ] = p5;

	const double h      = find_largest_root( ncoeffs, &m_a[0] );

	const double co2st      = dic/( 1.0 + K1_val / h + K1_val * K2_val / h / h ); // co2st = CO2*
	const double hco3   = dic/( 1.0 + h / K1_val + K2_val / h );
	const double co3    = dic/( 1.0 + h / K2_val + h * h / K1_val / K2_val ); // mol/kg

	const double million = 1e6; // unit conversion

	// Output (all variables beginning with capital letter below)
	TCO2o.set( co2st * million, U_UMOL_KG );
	HCO3.set( hco3 * million, U_UMOL_KG );
	CO3.set( co3 * million, U_UMOL_KG );
	PCO2o.set ( co2st * million/Kh.value( U_MOL_KG_ATM ), U_UATM );
	pH.set (-log10( h ), U_PH);

    // ----------------------------------------------------------------------------
    /*! \brief calculate air-sea flux of carbon
     * based on Takahashi et al, 2009 Deep Sea Research
     * Uses K0 (solubility), Sc (Schmidt number) , U (wind stress), PCO2atm, PCO2o
     */

	Tr.set( ( 0.585 * K0.value( U_MOL_L_ATM )
             * pow( Sc, -0.5 ) * U * U ), U_gC_m2_month_uatm );  // units : gC m-2 month-1 uatm-1.
	// 0.585 is a unit conversion factor. See Takahashi et al, 2009 page 568
	// unit conversion * solubility * Schmidt number * wind speed^2

    //------------------------------------------------------------------------
    /*! \brief calculate Omega of Ca/Ar
     * Uses Ksp of Ca and Ar, CO3, S, and pH
     */

	// this is 0.010285*S/35
	const double calcium = 0.02128/40.087 * ( S/1.80655 ); //mol/kg Riley, and Tongudai, Chemical Geology 2:263-269, 1967
	OmegaCa.set( ( ( co3 * calcium ) / Kspc ), U_UNITLESS );
	OmegaAr.set( ( ( co3 * calcium ) / Kspa ), U_UNITLESS );
}

//-------------------------------------------------------------------------------
/*! \brief Calculate the (monthly) atmosphere-surface box flux
 *  \param Ca           Atmospheric CO2
 *  \param cpoolscale   Scale the box C pool by this amount (1.0=none)
 *  \return             Monthly atmospheric C flux, gC/m2/month
 */
double oceancsys::calc_monthly_surface_flux( const unitval& Ca, const double cpoolscale ) const {
	return ( ( Ca.value( U_PPMV_CO2 ) - PCO2o.value( U_UATM ) * cpoolscale ) * Tr.value( U_gC_m2_month_uatm ) ); // units : gC m-2 month-1
}

//-------------------------------------------------------------------------------
/*! \brief Calculate the (annualized) atmosphere-surface box flux
 *  \param Ca           Atmospheric CO2
 *  \param cpoolscale   Scale the box C pool by this amount (1.0=none)
 *  \return             Annual atmospheric C flux, Pg C/yr
 */
unitval oceancsys::calc_annual_surface_flux( const unitval& Ca, const double cpoolscale ) const {
    return unitval( ( calc_monthly_surface_flux( Ca, cpoolscale ) * As * 12.0 ) / 1e15, U_PGC_YR );
}

//-------------------------------------------------------------------------------
/*! \brief Convert the total carbon pool (PgC) to DIC
 *  \param carbon       Carbon value to convert (Pg C)
 * Uses carbon pool, mass of carbon, density of seawater and volume of the box
 */
unitval oceancsys::convertToDIC( const unitval carbon ) {
	const double dic = ( ( carbon.value( U_PGC ) * 1e15 ) * ( 1.0/12.01 ) * (1.0/1027.0 ) * ( 1.0/volumeofbox ) ); // mol/kg
	return unitval( dic * 1e6, U_UMOL_KG );
}

}


#include "CppUnitTest.h"
#include "UnitTestAux.h"
#include "../BVP/Utils/AuxUtils.h"
#include "..\BVP\Problems\NonAutonomousTroeschProblem.h"
#include "..\BVP\Problems\NonAutonomousOscillatingProblem.h"
#include "..\BVP\FunctionApproximation\PointSimple.h"
#include "..\BVP\MultipleShooting\HybridMultipleShootingComponent.h"
#include <boost/multiprecision/cpp_dec_float.hpp>
#include "../BVP/FunctionApproximation/InitialCondition.h"

using namespace auxutils;

using namespace UnitTestAux;
using namespace Microsoft::VisualStudio::CppUnitTestFramework;

typedef float_50_noet numTypeMp;
typedef double numType;

namespace GeneralTest
{
	TEST_CLASS(NonAutonomousProblemsTest)
	{
	public:

		//TEST_METHOD(NonAutonomousTroeschProblemDoubleMultimpeShooting)
		//{
		//	NonAutonomousTroeschProblem<numType> tpf(20);
		//	PointSimple<numType> ptLeft;
		//	ptLeft.Argument  = 0;
		//	ptLeft.Value  = 0;

		//	PointSimple<numType> ptRight;
		//	ptRight.Argument  = 1;
		//	ptRight.Value  = 1;

		//	HybridMultipleShootingComponent<numType> HMSComp(tpf);

		//	bool succeeded;
		//	std::vector<InitCondition<numType>> solution = HMSComp.Run(ptLeft, ptRight, 0.0001, succeeded, 0.1);

		//	auxutils::SaveToMapleFile(solution, "f:\\NonAutoTroeschProblem.txt", true);

		//}

		TEST_METHOD(StandardNonAutonomousOscilatingProblemMultimpeShootingDouble)
		{
			 NonAutonomousOscillatingProblem<numType> problem;
   			 StandardOscillatinProblemMultipleShoothingTest<numType, NonAutonomousOscillatingProblem<numType>>(problem);
		}
	};
}


#include <iostream>
#include <vector>
#include <cassert>
#include <map>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/max_cardinality_matching.hpp>

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> Graph;

void testcase()
{
  int n, c, f;
  std::cin >> n >> c >> f;
  assert(n >= 2 && n <= 400 && n % 2 == 0);
  assert(c >= 1 && c <= 100);
  assert(f >= 0);

  std::map<std::string, int> ids_by_c;
  std::vector<std::vector<int>> cs_by_student(n);
  std::vector<std::vector<bool>> student_masks_by_c;
  for (int i = 0; i < n; i++)
  {
    std::vector<int> &cs_this_student = cs_by_student.at(i);
    for (int j = 0; j < c; j++)
    {
      std::string c_name;
      std::cin >> c_name;
      assert(c_name.size() <= 20);
      if (ids_by_c.count(c_name) == 0)
      {
        ids_by_c.insert(std::make_pair(c_name, ids_by_c.size()));
        student_masks_by_c.emplace_back(std::vector<bool>(n, false));
      }
      int c_id = ids_by_c.at(c_name);

      cs_this_student.push_back(c_id);
      student_masks_by_c.at(c_id).at(i) = true;
    }
  }

  Graph G(n);
  for (int i = 0; i < n; i++)
  {
    for (int j = i + 1; j < n; j++)
    {
      int overlap_count_lower_bound = 0;
      for (auto c_id : cs_by_student.at(i))
      {
        if (student_masks_by_c.at(c_id).at(j))
        {
          overlap_count_lower_bound++;
        }
        if (overlap_count_lower_bound > f)
        {
          break;
        }
      }

      if (overlap_count_lower_bound > f)
      {
        boost::add_edge(i, j, G);
      }
    }
  }

  std::vector<Graph::vertex_descriptor> mate(n);
  bool success = boost::checked_edmonds_maximum_cardinality_matching(G, &mate[0]);
  assert(success);

  bool all_matched = true;
  for (auto its = boost::vertices(G); its.first != its.second; its.first++)
  {
    if (mate[*its.first] == Graph::null_vertex())
    {
      all_matched = false;
    }
  }

  std::cout << (all_matched ? "not optimal" : "optimal") << "\n";
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


#include "pgo_toy_example.h"

#include <ros/ros.h>

#include <geometry_msgs/Point.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <Eigen/Dense>

#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/optimizable_graph.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/solver.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/types/slam3d/types_slam3d.h>

#include <iostream>
#include <vector>

// prev poses (gray sphere).
visualization_msgs::MarkerArray prev_nodes;

// prev edges (gray line).
visualization_msgs::MarkerArray prev_edges;

// optimized poses (black sphere).
visualization_msgs::MarkerArray opt_nodes;

// optimized edges (black line).
visualization_msgs::MarkerArray opt_edges;

// text arrays.
visualization_msgs::MarkerArray texts;

// Position of the first node.
double xinit, yinit, zinit;

/// \brief Set the pose graph nodes for ROS visualization.
/// \param[in] toy Pointer to PGOToyExample instance.
/// \param[out] nodes Marker array of pose graph nodes.
/// \param[in] ns Namespace.
/// \param[in] id Unique node id.
/// \param[in] rgba Color information.
/// \param[in] is_opt_node Boolean of optimized nodes.
void SetNodeForROS(PGOToyExample* toy, visualization_msgs::MarkerArray& nodes,
              std::string ns, int id, Vec4 rgba, bool is_opt_node)
{
  visualization_msgs::Marker node;
  node.header.frame_id = "world";
  node.header.stamp = ros::Time::now();
  node.ns = ns;
  node.id = id;
  node.type = visualization_msgs::Marker::SPHERE;
  node.scale.x = node.scale.y = node.scale.z = 0.2;
  node.color.r = rgba[0]; node.color.g = rgba[1]; node.color.b = rgba[2]; node.color.a = rgba[3];

  if(!is_opt_node) {
    Quaternion q = (Quaternion)toy->GetOriginalPoses()[id].rotation();

    node.pose.position.x = toy->GetOriginalPoses()[id].translation()[0];
    node.pose.position.y = toy->GetOriginalPoses()[id].translation()[1];
    node.pose.position.z = toy->GetOriginalPoses()[id].translation()[2];
    node.pose.orientation.x = q.x();
    node.pose.orientation.y = q.y();
    node.pose.orientation.z = q.z();
    node.pose.orientation.w = q.w();

    if(node.pose.orientation.w < 0) {
      node.pose.orientation.x *= -1;
      node.pose.orientation.y *= -1;
      node.pose.orientation.z *= -1;
      node.pose.orientation.w *= -1;
    }
    nodes.markers.push_back(node);
  }
  else {
    g2o::VertexSE3Expmap* vtx = static_cast<g2o::VertexSE3Expmap*>(toy->GetOptimizer()->vertex(id));
    Isometry opt_poses = vtx->estimate();
    Quaternion q = (Quaternion)opt_poses.rotation();

    // First node.
    if(id==0) {
      xinit = opt_poses.translation()[0];
      yinit = opt_poses.translation()[1];
      zinit = opt_poses.translation()[2];

      node.pose.position.x = 0;
      node.pose.position.y = 0;
      node.pose.position.z = 0;
    }

    node.pose.position.x = opt_poses.translation()[0]-xinit;
    node.pose.position.y = opt_poses.translation()[1]-yinit;
    node.pose.position.z = opt_poses.translation()[2]-zinit;

    node.pose.orientation.x = q.x();
    node.pose.orientation.y = q.y();
    node.pose.orientation.z = q.z();
    node.pose.orientation.w = q.w();

    if(node.pose.orientation.w < 0) {
      node.pose.orientation.x *= -1;
      node.pose.orientation.y *= -1;
      node.pose.orientation.z *= -1;
      node.pose.orientation.w *= -1;
    }
    nodes.markers.push_back(node);

    visualization_msgs::Marker opt_node_text;
    opt_node_text.header.frame_id = "world";
    opt_node_text.header.stamp = ros::Time();
    opt_node_text.ns = "opt_node_text";
    opt_node_text.id = id;
    opt_node_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    opt_node_text.action = visualization_msgs::Marker::ADD;
    opt_node_text.scale.z = 0.1;
    opt_node_text.color.r =0.0; opt_node_text.color.g = 0.0; opt_node_text.color.b = 0.0; opt_node_text.color.a = 1.0;
    opt_node_text.pose.position.x = node.pose.position.x;
    opt_node_text.pose.position.y = node.pose.position.y;
    opt_node_text.pose.position.z = node.pose.position.z+0.2;
    std::string txt = "x" + std::to_string(id);
    opt_node_text.text = txt;
    texts.markers.push_back(opt_node_text);
  }
}

/// \brief Set the pose graph edge for ROS visualization.
/// \param[in] toy Pointer to PGOToyExample instance.
/// \param[out] edges Marker array of pose graph edges.
/// \param[in] ns Namespace.
/// \param[in] id Unique edge id.
/// \param[in] start Start pose node.
/// \param[in] end End pose node.
/// \param[in] rgba Color information.
/// \param[in] is_opt_edge Boolean of optimized edges.
void SetEdgeForROS(PGOToyExample* toy, visualization_msgs::MarkerArray& edges,
             std::string ns, int id, int start, int end, Vec4 rgba, bool is_opt_edge)
{
  visualization_msgs::Marker edge;
  edge.header.frame_id = "world";
  edge.header.stamp = ros::Time::now();
  edge.ns = ns;
  edge.id = id;
  edge.type = visualization_msgs::Marker::LINE_LIST;
  edge.color.r = rgba[0]; edge.color.g = rgba[1]; edge.color.b = rgba[2]; edge.color.a = rgba[3];
  edge.pose.orientation.w = 1.0;
  edge.scale.x = 0.01;

  if(!is_opt_edge) {
    geometry_msgs::Point p1;
    p1.x = toy->GetOriginalPoses()[start].translation()[0];
    p1.y = toy->GetOriginalPoses()[start].translation()[1];
    p1.z = toy->GetOriginalPoses()[start].translation()[2];
    edge.points.push_back(p1);

    geometry_msgs::Point p2;
    p2.x = toy->GetOriginalPoses()[end].translation()[0];
    p2.y = toy->GetOriginalPoses()[end].translation()[1];
    p2.z = toy->GetOriginalPoses()[end].translation()[2];
    edge.points.push_back(p2);

    edges.markers.push_back(edge);

    visualization_msgs::Marker prev_edge_text;
    prev_edge_text.header.frame_id = "world";
    prev_edge_text.header.stamp = ros::Time();
    prev_edge_text.ns = "prev_edge_text";
    prev_edge_text.id = id;
    prev_edge_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    prev_edge_text.action = visualization_msgs::Marker::ADD;
    prev_edge_text.scale.z = 0.1;
    prev_edge_text.color.r = rgba[0]; prev_edge_text.color.g = rgba[1]; prev_edge_text.color.b = rgba[2]; prev_edge_text.color.a = rgba[3];
    prev_edge_text.pose.position.x = (p1.x+p2.x)/2.;
    prev_edge_text.pose.position.y = (p1.y+p2.y)/2.;
    prev_edge_text.pose.position.z = (p1.z+p2.z)/2. + 0.1;
    std::string txt = "zhat" + std::to_string(start) + std::to_string(end);
    prev_edge_text.text = txt;
    texts.markers.push_back(prev_edge_text);
  }
  else {
    g2o::VertexSE3Expmap* prev_vtx = static_cast<g2o::VertexSE3Expmap*>(toy->GetOptimizer()->vertex(start));
    g2o::VertexSE3Expmap* curr_vtx = static_cast<g2o::VertexSE3Expmap*>(toy->GetOptimizer()->vertex(end));
    Isometry prev_opt_poses = prev_vtx->estimate();
    Isometry curr_opt_poses = curr_vtx->estimate();

    // First opt_node.
    if(start == 0) {
      xinit = prev_opt_poses.translation()[0];
      yinit = prev_opt_poses.translation()[1];
      zinit = prev_opt_poses.translation()[2];
    }

    geometry_msgs::Point p1;
    p1.x = prev_opt_poses.translation()[0] -xinit;
    p1.y = prev_opt_poses.translation()[1] -yinit;
    p1.z = prev_opt_poses.translation()[2] -zinit;
    edge.points.push_back(p1);

    geometry_msgs::Point p2;
    p2.x = curr_opt_poses.translation()[0] -xinit;
    p2.y = curr_opt_poses.translation()[1] -yinit;
    p2.z = curr_opt_poses.translation()[2] -zinit;
    edge.points.push_back(p2);

    edges.markers.push_back(edge);

    visualization_msgs::Marker opt_edge_text;
    opt_edge_text.header.frame_id = "world";
    opt_edge_text.header.stamp = ros::Time();
    opt_edge_text.ns = "opt_edge_text";
    opt_edge_text.id = id;
    opt_edge_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    opt_edge_text.action = visualization_msgs::Marker::ADD;
    opt_edge_text.scale.z = 0.1;
    opt_edge_text.color.r = 0.0; opt_edge_text.color.g = 0.0; opt_edge_text.color.b = 0.0; opt_edge_text.color.a = 1.0;
    opt_edge_text.pose.position.x = (p1.x+p2.x)/2.;
    opt_edge_text.pose.position.y = (p1.y+p2.y)/2.;
    opt_edge_text.pose.position.z = (p1.z+p2.z)/2. + 0.1;
    std::string txt = "z" + std::to_string(start) + std::to_string(end);
    opt_edge_text.text = txt;
    texts.markers.push_back(opt_edge_text);
  }

}

int main(int argc, char **argv) {
  ros::init(argc,argv,"pgo_toy_example");
  ros::NodeHandle nh, priv_nh("~");

  // Set the publishers.
  ros::Publisher opt_node_pub = nh.advertise<visualization_msgs::MarkerArray>("opt_nodes",1);
  ros::Publisher opt_edge_pub = nh.advertise<visualization_msgs::MarkerArray>("opt_edges",1);
  ros::Publisher prev_node_pub = nh.advertise<visualization_msgs::MarkerArray>("prev_nodes",1);
  ros::Publisher prev_edge_pub = nh.advertise<visualization_msgs::MarkerArray>("prev_edges",1);
  ros::Publisher text_pub = nh.advertise<visualization_msgs::MarkerArray>("texts",1);

  int _rate, _iter;
  priv_nh.param("loop_rate", _rate, 5);  // loop rate [Hz].
  priv_nh.param("iteration", _iter, 20); // iteration of optimization.

  // Create a PGO toy example instance.
  PGOToyExample* toy = new PGOToyExample(true);

  while(ros::ok()) {
    // Set the loop speed [hz].
    ros::Rate loop_rate(_rate);

    // Reset the instance every iteration finished.
    toy->Reset();
    int num_poses = toy->GetOriginalPosesSize();
    int count = 0;

    while(count < _iter) {
      // Set previous nodes and edges.
      for(int i=0; i<num_poses; i++) {
        SetNodeForROS(toy, prev_nodes, "prev_nodes", i, Vec4(0.0, 0.0, 0.0, 0.25), false);
      }
      for(int i=1; i<num_poses; i++) {
        SetEdgeForROS(toy, prev_edges, "prev_edges", i, i-1, i, Vec4(0.0, 0.0, 0.0, 0.25), false);
      }
      SetEdgeForROS(toy, opt_edges, "prev_edges", 16, 5, 11, Vec4(0.0, 0.0, 0.0, 0.25), false);
      SetEdgeForROS(toy, opt_edges, "prev_edges", 17, 3, 14, Vec4(0.0, 0.0, 0.0, 0.25), false);

      // Set optimized poses and edges.
      for(int i=0; i<num_poses; i++) {
        SetNodeForROS(toy, prev_nodes, "opt_nodes", i, Vec4(0.0, 0.0, 0.0, 1.0), true);
      }
      for(int i=1; i<num_poses; i++) {
        SetEdgeForROS(toy, opt_edges, "opt_edges", i, i-1, i, Vec4(0.0, 0.0, 0.0, 1), true);
      }
      SetEdgeForROS(toy, opt_edges, "opt_edges", 16, 5, 11, Vec4(0.0, 0.0, 0.0, 1), true);
      SetEdgeForROS(toy, opt_edges, "opt_edges", 17, 3, 14, Vec4(0.0, 0.0, 0.0, 1), true);

      // Set the text message marker.
      visualization_msgs::Marker text;
      text.header.frame_id = "world";
      text.header.stamp = ros::Time();
      text.ns = "text";
      text.id = 0;
      text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
      text.action = visualization_msgs::Marker::ADD;
      text.scale.z = 0.1;
      text.color.r = 0.0; text.color.g = 0.0; text.color.b = 0.0; text.color.a = 1.0;
      text.pose.position.y = 0.5;
      text.pose.position.z = 1.0;
      std::string comment = "[PGO toy example using g2o]\n\nPose: SE3Quat\nVertex: VertexSE3Expmap\nEdge: EdgeSE3Expmap\nInformation Matrix: Random\n\nBlack: Current Estimated Poses\nGray: Previous Poses";
      text.text = comment;
      texts.markers.push_back(text);

      // Set the count message marker.
      visualization_msgs::Marker count_text;
      count_text.header.frame_id = "world";
      count_text.header.stamp = ros::Time();
      count_text.ns = "count_text";
      count_text.id = 0;
      count_text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
      count_text.action = visualization_msgs::Marker::ADD;
      count_text.scale.z = 0.1;
      count_text.color.r = 0.0; count_text.color.g = 0.0; count_text.color.b = 0.0; count_text.color.a = 1.0;
      count_text.pose.position.y = 0.5;
      count_text.pose.position.z = 2.0;
      std::string counttxt = "Iteration: " + std::to_string(count);
      count_text.text = counttxt;
      texts.markers.push_back(count_text);

      // Publish to /texts
      text_pub.publish(texts);

      // Publish to /prev_edges
      prev_edge_pub.publish(prev_edges);

      // Publish to /opt_nodes
      opt_node_pub.publish(opt_nodes);

      // Publish to /opt_edges
      opt_edge_pub.publish(opt_edges);

      // Publish to /prev_nodes
      prev_node_pub.publish(prev_nodes);

      // Control the loop speed.
      loop_rate.sleep();

      // Do pose graph optimization (one time per each loop for visualization).
      toy->GetOptimizer()->optimize(1);

      // Add plus one count.
      count += 1;
    }

    std::cout << std::endl;

    // Clear pose graph.
    prev_edges.markers.clear();
    opt_nodes.markers.clear();
    prev_nodes.markers.clear();
    texts.markers.clear();
  }

  return 0;
}


#include <boost/numeric/odeint/stepper/adams_bashforth_moulton.hpp>


/**
 * @file hpt_test.cpp
 *
 * Tests for the hyper-parameter tuning module.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

#include <mlpack/core/cv/metrics/mse.hpp>
#include <mlpack/core/cv/metrics/accuracy.hpp>
#include <mlpack/core/cv/simple_cv.hpp>
#include <mlpack/core/hpt/cv_function.hpp>
#include <mlpack/core/hpt/fixed.hpp>
#include <mlpack/core/hpt/hpt.hpp>
#include <mlpack/core/optimizers/grid_search/grid_search.hpp>
#include <mlpack/core/optimizers/gradient_descent/gradient_descent.cpp>
#include <mlpack/methods/lars/lars.hpp>
#include <mlpack/methods/logistic_regression/logistic_regression.hpp>

#include <boost/test/unit_test.hpp>

using namespace mlpack::cv;
using namespace mlpack::data;
using namespace mlpack::hpt;
using namespace mlpack::optimization;
using namespace mlpack::regression;

BOOST_AUTO_TEST_SUITE(HPTTest);

/**
 * Test CVFunction runs cross-validation in according with specified fixed
 * arguments and passed parameters.
 */
BOOST_AUTO_TEST_CASE(CVFunctionTest)
{
  arma::mat xs = arma::randn(5, 100);
  arma::vec beta = arma::randn(5, 1);
  arma::rowvec ys = beta.t() * xs + 0.1 * arma::randn(1, 100);

  SimpleCV<LARS, MSE> cv(0.2, xs, ys);

  bool transposeData = true;
  bool useCholesky = false;
  double lambda1 = 1.0;
  double lambda2 = 2.0;

  FixedArg<bool, 1> fixedUseCholesky{useCholesky};
  FixedArg<double, 3> fixedLambda1{lambda2};
  CVFunction<decltype(cv), LARS, 4, FixedArg<bool, 1>, FixedArg<double, 3>>
      cvFun(cv, 0.0, 0.0, fixedUseCholesky, fixedLambda1);

  double expected = cv.Evaluate(transposeData, useCholesky, lambda1, lambda2);
  arma::vec parameters(2);
  parameters(0) = transposeData;
  parameters(1) = lambda1;
  double actual = cvFun.Evaluate(parameters);

  BOOST_REQUIRE_CLOSE(expected, actual, 1e-5);
}

/**
 * This class provides the interface of CV classes, but really implements a
 * simple quadratic function of three variables.
 */
template<typename MLAlgorithm,
         typename Metric = void,
         typename MatType = void,
         typename PredictionsType = void,
         typename WeightsType = void>
class QuadraticFunction
{
 public:
  QuadraticFunction(double a,
                    double b,
                    double c,
                    double d,
                    double xMin = 0.0,
                    double yMin = 0.0,
                    double zMin = 0.0) :
      a(a), b(b), c(c), d(d), xMin(xMin), yMin(yMin), zMin(zMin) {}

  double Evaluate(double x, double y, double z)
  {
    return a * pow(x - xMin, 2)  + b * pow(y - yMin, 2) + c * pow(z - zMin, 2)
        + d;
  }

  // Declaring and defining it just in order to provide the same interface as
  // other CV classes.
  MLAlgorithm Model()
  {
    return MLAlgorithm();
  }

 private:
  double a, b, c, d, xMin, yMin, zMin;
};

/**
 * Test CVFunction approximates gradient in the expected way.
 */
BOOST_AUTO_TEST_CASE(CVFunctionGradientTest)
{
  double a = 1.0;
  double b = -1.5;
  double c = 2.5;
  double d = 3.0;
  QuadraticFunction<LARS> lf(a, b, c, d);

  double relativeDelta = 0.01;
  double minDelta = 0.001;
  CVFunction<decltype(lf), LARS, 3> cvFun(lf, relativeDelta, minDelta);

  double x = 0.0;
  double y = -1.0;
  double z = 2.0;
  arma::mat gradient;
  cvFun.Gradient(arma::vec("0.0 -1.0 2.0"), gradient);

  double xDelta = minDelta;
  double yDelta = relativeDelta * abs(y);
  double zDelta = relativeDelta * abs(z);

  double aproximateXPartialDerivative = a * (2 * x + xDelta);
  double aproximateYPartialDerivative = b * (2 * y + yDelta);
  double aproximateZPartialDerivative = c * (2 * z + zDelta);

  BOOST_REQUIRE_EQUAL(gradient.n_elem, 3);
  BOOST_REQUIRE_CLOSE(gradient(0), aproximateXPartialDerivative, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(1), aproximateYPartialDerivative, 1e-5);
  BOOST_REQUIRE_CLOSE(gradient(2), aproximateZPartialDerivative, 1e-5);
}

void InitProneToOverfittingData(arma::mat& xs,
                                arma::rowvec& ys,
                                double& validationSize)
{
  // Total number of data points.
  size_t N = 10;
  // Total number of features (all except the first one are redundant).
  size_t M = 5;

  arma::rowvec data = arma::linspace<arma::rowvec>(0.0, 10.0, N);
  xs = data;
  for (size_t i = 2; i <= M; ++i)
    xs = arma::join_cols(xs, arma::pow(data, i));

  // Responses that approximately follow the function y = 2 * x. Adding noise to
  // avoid having a polynomial of degree 1 that exactly fits the points.
  ys = 2 * data + 0.05 * arma::randn(1, N);

  validationSize = 0.3;
}

template<typename T1, typename T2>
void FindLARSBestLambdas(arma::mat& xs,
                         arma::rowvec& ys,
                         double& validationSize,
                         bool transposeData,
                         bool useCholesky,
                         const T1& lambda1Set,
                         const T2& lambda2Set,
                         double& bestLambda1,
                         double& bestLambda2,
                         double& bestObjective)
{
  SimpleCV<LARS, MSE> cv(validationSize, xs, ys);

  bestObjective = std::numeric_limits<double>::max();

  for (double lambda1 : lambda1Set)
    for (double lambda2 : lambda2Set)
    {
      double objective =
          cv.Evaluate(transposeData, useCholesky, lambda1, lambda2);
      if (objective < bestObjective)
      {
        bestObjective = objective;
        bestLambda1 = lambda1;
        bestLambda2 = lambda2;
      }
    }
}

 /**
 * Test grid-search optimization leads to the best parameters from the specified
 * ones.
 */
BOOST_AUTO_TEST_CASE(GridSearchTest)
{
  arma::mat xs;
  arma::rowvec ys;
  double validationSize;
  InitProneToOverfittingData(xs, ys, validationSize);

  bool transposeData = true;
  bool useCholesky = false;
  arma::vec lambda1Set("0 0.001 0.01 0.1 1.0 10.0 100.0");
  std::array<double, 4> lambda2Set{{0.0, 0.05, 0.5, 5.0}};

  double expectedLambda1, expectedLambda2, expectedObjective;
  FindLARSBestLambdas(xs, ys, validationSize, transposeData, useCholesky,
      lambda1Set, lambda2Set, expectedLambda1, expectedLambda2,
      expectedObjective);

  SimpleCV<LARS, MSE> cv(validationSize, xs, ys);
  CVFunction<decltype(cv), LARS, 4, FixedArg<bool, 0>, FixedArg<bool, 1>>
      cvFun(cv, 0.0, 0.0, {transposeData}, {useCholesky});

  IncrementPolicy policy(true);
  DatasetMapper<IncrementPolicy, double> datasetInfo(policy, 2);
  for (double lambda1 : lambda1Set)
    datasetInfo.MapString<size_t>(lambda1, 0);
  for (double lambda2 : lambda2Set)
    datasetInfo.MapString<size_t>(lambda2, 1);

  GridSearch optimizer;
  arma::mat actualParameters;
  double actualObjective =
      optimizer.Optimize(cvFun, actualParameters, datasetInfo);

  BOOST_REQUIRE_CLOSE(expectedObjective, actualObjective, 1e-5);
  BOOST_REQUIRE_CLOSE(expectedLambda1, actualParameters(0, 0), 1e-5);
  BOOST_REQUIRE_CLOSE(expectedLambda2, actualParameters(1, 0), 1e-5);
}

/**
 * Test HyperParameterTuner.
 */
BOOST_AUTO_TEST_CASE(HPTTest)
{
  arma::mat xs;
  arma::rowvec ys;
  double validationSize;
  InitProneToOverfittingData(xs, ys, validationSize);

  bool transposeData = true;
  bool useCholesky = false;
  arma::vec lambda1Set("0 0.001 0.01 0.1 1.0 10.0 100.0");
  arma::vec lambda2Set("0.0 0.05 0.5 5.0");

  double expectedLambda1, expectedLambda2, expectedObjective;
  FindLARSBestLambdas(xs, ys, validationSize, transposeData, useCholesky,
      lambda1Set, lambda2Set, expectedLambda1, expectedLambda2,
      expectedObjective);

  double actualLambda1, actualLambda2;
  HyperParameterTuner<LARS, MSE, SimpleCV, GridSearch>
      hpt(validationSize, xs, ys);
  std::tie(actualLambda1, actualLambda2) = hpt.Optimize(Fixed(transposeData),
      Fixed(useCholesky), lambda1Set, lambda2Set);

  BOOST_REQUIRE_CLOSE(expectedObjective, hpt.BestObjective(), 1e-5);
  BOOST_REQUIRE_CLOSE(expectedLambda1, actualLambda1, 1e-5);
  BOOST_REQUIRE_CLOSE(expectedLambda2, actualLambda2, 1e-5);

  /* Checking that the model provided by the hyper-parameter tuner shows the
   * same performance. */
  size_t validationFirstColumn = round(xs.n_cols * (1.0 - validationSize));
  arma::mat validationXs = xs.cols(validationFirstColumn, xs.n_cols - 1);
  arma::rowvec validationYs = ys.cols(validationFirstColumn, ys.n_cols - 1);
  double objective = MSE::Evaluate(hpt.BestModel(), validationXs, validationYs);
  BOOST_REQUIRE_CLOSE(expectedObjective, objective, 1e-5);
}

/**
 * Test HyperParamterTuner maximizes Accuracy rather than minimizes it.
 */
BOOST_AUTO_TEST_CASE(HPTMaximizationTest)
{
  // Initializing a linearly separable dataset.
  arma::mat xs = arma::linspace<arma::rowvec>(0.0, 10.0, 50);
  arma::Row<size_t> ys = arma::join_rows(arma::zeros<arma::Row<size_t>>(25),
      arma::ones<arma::Row<size_t>>(25));

  // We will train and validate on the same dataset.
  double validationSize = 0.5;
  arma::mat doubledXs = arma::join_rows(xs, xs);
  arma::Row<size_t> doubledYs = arma::join_rows(ys, ys);

  // Defining lambdas to choose from. Zero should be preferred since big lambdas
  // are likely to restrict capabilities of logistic regression.
  arma::vec lambdas("0 1e12");

  // Making sure that the assumption above is true.
  SimpleCV<LogisticRegression<>, Accuracy>
      cv(validationSize, doubledXs, doubledYs);
  BOOST_REQUIRE_GT(cv.Evaluate(0.0), cv.Evaluate(1e12));

  HyperParameterTuner<LogisticRegression<>, Accuracy, SimpleCV>
      hpt(validationSize, doubledXs, doubledYs);

  double actualLambda;
  std::tie(actualLambda) = hpt.Optimize(lambdas);

  BOOST_REQUIRE_CLOSE(hpt.BestObjective(), 1.0, 1e-5);
  BOOST_REQUIRE_CLOSE(actualLambda, 0.0, 1e-5);
}

/**
 * Test HyperParameterTuner works with GradientDescent.
 */
BOOST_AUTO_TEST_CASE(HPTGradientDescentTest)
{
  // Constructor arguments for the fake CV function (QuadraticFunction).
  double a = 1.0;
  double b = -1.5;
  double c = 2.5;
  double d = 3.0;

  // Optimal values for three "hyper-parameters".
  double xMin = 1.5;
  double yMin = 0.0;
  double zMin = -2.0;

  // We pass LARS just because some ML algorithm should be passed. We pass MSE
  // to tell HyperParameterTuner that the objective function (QuadraticFunction)
  // should be minimized.
  HyperParameterTuner<LARS, MSE, QuadraticFunction, GradientDescent>
      hpt(a, b, c, d, xMin, yMin, zMin);

  // Setting GradientDescent to find more close solution to the optimal one.
  hpt.Optimizer().StepSize() = 0.1;
  hpt.Optimizer().Tolerance() = 1e-15;

  // Always using the same small increase of arguments in calculation of partial
  // derivatives.
  hpt.RelativeDelta() = 0.0;
  hpt.MinDelta() = 1e-10;

  // We will try to find optimal values only for two "hyper-parameters".
  double x0 = 3.0;
  double y = yMin;
  double z0 = -3.0;

  double xOptimized, zOptimized;
  std::tie(xOptimized, zOptimized) = hpt.Optimize(x0, Fixed(y), z0);
  BOOST_REQUIRE_CLOSE(xOptimized, xMin, 1e-4);
  BOOST_REQUIRE_CLOSE(zOptimized, zMin, 1e-4);
}

BOOST_AUTO_TEST_SUITE_END();


/*
 * Copyright 2013-2015 Raphael Bost
 *
 * This file is part of ciphermed.

 *  ciphermed is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  ciphermed 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
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with ciphermed.  If not, see <http://www.gnu.org/licenses/>. 2
 *
 */

#include <iostream>
#include <math/num_th_alg.hh>
#include <util/util.hh>
#include <NTL/ZZ.h>

using namespace std;

static void test_fact_generation(size_t m_bits)
{
    mpz_class m = 0;
    mpz_setbit(m.get_mpz_t(),m_bits);

    mpz_class p;

    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));

    ScopedTimer *t = new ScopedTimer("Test factorization generation");
    std::vector<mpz_class> fact = gen_rand_prime_with_factorization(m,&p,randstate,25);
    delete t;

    cout << "Prime generated is \n" << p << endl;
    cout << "Factors:\n";

    for (size_t i = 0; i < fact.size(); i++) {
        cout << fact[i] << endl;
    }

    cout << "\n" << m_bits << " bits queried\n";
    cout << "p is " << mpz_sizeinbase(p.get_mpz_t(),2) << " bits" << endl;
}

static void test_simple_safe_prime(size_t n_bits)
{
    gmp_randstate_t randstate;
    gmp_randinit_default(randstate);
    gmp_randseed_ui(randstate,time(NULL));
    ScopedTimer *t;

    /*
    t = new ScopedTimer("Test simple safe prime");
    mpz_class p = simple_safe_prime_gen(n_bits,randstate,25);
    delete t;

    cout << "Prime generated is \n" << 2*p+1 << endl;
    cout << "\n" << n_bits << " bits queried\n";
    cout << "p is " << mpz_sizeinbase(p.get_mpz_t(),2) << " bits" << endl;
*/
    cout << "\n\nWith NTL:" << endl;

    t = new ScopedTimer("NTL safe prime");
    NTL::ZZ q = NTL::GenGermainPrime_ZZ(n_bits+1);
    delete t;
    cout << "Prime generated is \n" << q << endl;
    cout << "q is " << NTL::NumBits(q) << " bits" << endl;

    mpz_class r;
    t = new ScopedTimer("Our safe prime");
    gen_germain_prime(r,n_bits,randstate);
    delete t;
    cout << "Prime generated is \n" << r << endl;
    cout << "q is " <<mpz_sizeinbase(r.get_mpz_t(),2) << " bits" << endl;

}

int main()
{
//    test_fact_generation(512);
    test_simple_safe_prime(512);

    return 0;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>, Randi Cabezas <rcabezas@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>

#include <dpMM/basemeasure.hpp>
#include <dpMM/vmfPriorFull.hpp>

/*
 * vmf base measure; uses monte carlo integration for p(x|hyperparams)
 * http://eprints.pascal-network.org/archive/00007206/01/iMMM.pdf
 */
template<typename T>
class vMFbase : public BaseMeasure<T>
{
public:
  vMFbase(const vMFpriorFull<T>& vmfPrior);
  vMFbase(const vMFbase<T>& vmf);
  ~vMFbase();

  virtual BaseMeasure<T>* copy();
  virtual vMFbase<T>* copyNative();

  T logLikelihood(const Matrix<T,Dynamic,1>& x) const;
  T logLikelihood(const Matrix<T,Dynamic,Dynamic>& x, uint32_t i) const
    {return logLikelihood(x.col(i));};
  void posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k);
  void posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x, const VectorXu& z,
    uint32_t k);
  void sample();

  T logPdfUnderPrior() const;
  virtual T logPdfUnderPriorMarginalized() const;

  virtual T logPdfUnderPriorMarginalized(const Matrix<T,Dynamic,1>& x);

//  virtual NiwSampled<T>* merge(const NiwSampled<T>& other);
//  void fromMerge(const NiwSampled<T>& niwA, const NiwSampled<T>& niwB);

  void print() const;
  virtual uint32_t getDim() const {return(uint32_t(vmf_.D_));};

//  const Matrix<T,Dynamic,Dynamic>& scatter() const {return niw0_.scatter();};
//  const Matrix<T,Dynamic,1>& mean() const {return niw0_.mean();};
//  T count() const {return niw0_.count();};
////  T& count() {return niw0_.count_;};
  const Matrix<T,Dynamic,1>& getMean() const {return vmf_.mu_;};
  const T tau() const {return vmf_.tau();};

  vMFpriorFull<T> vmfPrior_;
  vMF<T> vmf_;
private:

};

// ------------------------- impl -------------------------------------------

template<typename T>
vMFbase<T>::vMFbase(const vMFpriorFull<T>& vmfPrior)
  : vmfPrior_(vmfPrior), vmf_(vmfPrior_.sample())
{};

template<typename T>
vMFbase<T>::vMFbase(const vMFbase<T>& base)
  :  vmfPrior_(base.vmfPrior_), vmf_(base.vmf_)
{};

template<typename T>
vMFbase<T>::~vMFbase()
{};

template<typename T>
BaseMeasure<T>* vMFbase<T>::copy()
{
  return new vMFbase<T>(*this);
};

template<typename T>
vMFbase<T>* vMFbase<T>::copyNative()
{
  return new vMFbase<T>(*this);
};

template<typename T>
T vMFbase<T>::logLikelihood(const Matrix<T,Dynamic,1>& x) const
{
//  cout<<vmf_.logPdf(x)<<endl;
  return vmf_.logPdf(x);
};

template<typename T>
void vMFbase<T>::posterior(const Matrix<T,Dynamic,Dynamic>& x, const VectorXu& z,
    uint32_t k)
{
  vmfPrior_.getSufficientStatistics(x,z,k);
  // needs current vmf since it samples tau|mu_old and then mu|tau
  vmf_ = vmfPrior_.sampleFromPosterior(vmf_);
};

template<typename T>
void vMFbase<T>::posterior(const vector<Matrix<T,Dynamic,Dynamic> >&x, const VectorXu& z,
    uint32_t k)
{
};

template<typename T>
void vMFbase<T>::sample()
{
};

template<typename T>
void vMFbase<T>::print() const
{
  vmf_.print();
};

template<typename T>
T vMFbase<T>::logPdfUnderPrior() const
{
  return 0.;
};

template<typename T>
T vMFbase<T>::logPdfUnderPriorMarginalized() const
{
  return 0.;
};

template<typename T>
T vMFbase<T>::logPdfUnderPriorMarginalized(const Matrix<T,Dynamic,1>& x)
{
  // approximate the log pdf under the prior via monte carlo sampling
  T logPdfMarg = 0;
  uint32_t N = 3;
//#pragma omp parallel for reduction(+:logPdfMarg)
  for(uint32_t t=0; t<N; ++t)
  {
    vMF<T> vmf = vmfPrior_.sample();
    logPdfMarg = logPdfMarg + vmf.logPdf(x);
  }
  return logPdfMarg/T(N);
};


/**
 * @file Model.hpp
 * @copyright Copyright (C) 2016-2021 Flexiv Ltd. All Rights Reserved.
 */

#ifndef FLEXIVRDK_MODEL_HPP_
#define FLEXIVRDK_MODEL_HPP_

#include <Eigen/Eigen>
#include <memory>
#include <vector>

namespace flexiv {

class ModelHandler;

/**
 * @class Model
 * @brief Integrated dynamics engine with robot model and dynamics.
 */
class Model
{
public:
    Model();
    virtual ~Model();

    /**
     * @brief Update robot model using new joint states data
     * @param[in] positions \f$ \mathbb{R}^{Dof \times 1} \f$ new link positions
     * \f$ q~[rad] \f$
     * @param[in] velocities \f$ \mathbb{R}^{Dof \times 1} \f$ new link
     * velocities \f$ \dot{q}~[rad/s] \f$
     * @return True: success, false: failed
     */
    bool updateModel(const std::vector<double>& positions,
        const std::vector<double>& velocities);

    /**
     * @brief Set tool configuration and add to robot model. The tool is
     * installed on the flange
     * @param[in] mass Total mass of the tool \f$ [kg] \f$
     * @param[in] inertiaAtCom \f$ \mathbb{R}^{3 \times 3} \f$ inertia matrix of
     * the tool at COM \f$ [kg \cdot m^2] \f$
     * @param[in] comInTcp \f$ \mathbb{R}^{3 \times 1} \f$ tool COM position in
     * TCP frame \f$ [m] \f$
     * @param[in] tcpInFlange \f$ \mathbb{R}^{3 \times 1} \f$ TCP position in
     * flange frame \f$ [m] \f$
     * @return True: success, false: failed
     */
    bool setTool(double mass, const Eigen::Matrix3d& inertiaAtCom,
        const Eigen::Vector3d& comInTcp, const Eigen::Vector3d& tcpInFlange);

    /**
     * @brief Compute and get the Jacobian matrix at the frame of the specified
     * link \f$ i \f$, expressed in the base frame.
     * @param[in] linkName Name of the link to get Jacobian for
     * @return \f$ \mathbb{R}^{6 \times Dof} \f$ Jacobian matrix, \f$ ^0 J_i \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     * @note Available links can be found in the provided URDF. They are
     * {"base_link", "link1", "link2", "link3", "link4", "link5", "link6",
     * "link7", "flange"}, plus "tool" after setTool() is called
     */
    const Eigen::MatrixXd getJacobian(const std::string& linkName);

    /**
     * @brief Compute and get the time derivative of Jacobian matrix at the
     * frame of the specified link \f$ i \f$, expressed in the base frame.
     * @param[in] linkName Name of the link to get Jacobian derivative for
     * @return \f$ \mathbb{R}^{6 \times Dof} \f$ Time derivative of Jacobian
     * matrix, \f$ ^0 \dot{J_i} \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     * @note Available links can be found in the provided URDF. They are
     * {"base_link", "link1", "link2", "link3", "link4", "link5", "link6",
     * "link7", "flange"}, plus "tool" after setTool() is called
     */
    const Eigen::MatrixXd getJacobianDot(const std::string& linkName);

    /**
     * @brief Compute and get the mass matrix for the generalized coordinates,
     * i.e. joint space
     * @return \f$ \mathbb{S}^{Dof \times Dof}_{++} \f$ Symmetric positive
     * definite mass matrix \f$ M(q)~[kgm^2] \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     */
    const Eigen::MatrixXd getMassMatrix();

    /**
     * @brief Compute and get the Coriolis/centripetal matrix for the
     * generalized coordinates, i.e. joint space
     * @return \f$ \mathbb{R}^{Dof \times Dof} \f$ Coriolis/centripetal matrix
     * \f$ C(q,\dot{q}) \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     * @par Coriolis matrix factorization
     * The factorization of the Coriolis matrix C is not unique, and this API
     * is using the factorization method found in "A new Coriolis matrix
     * factorization", 2012 by M. Bjerkend and K. Pettersen
     */
    const Eigen::MatrixXd getCoriolisMatrix();

    /**
     * @brief Compute and get the gravity force vector for the generalized
     * coordinates, i.e. joint space
     * @return \f$ \mathbb{R}^{Dof \times 1} \f$ gravity force vector \f$
     * g(q)~[Nm] \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     */
    const Eigen::VectorXd getGravityForce();

    /**
     * @brief Compute and get the Coriolis force vector for the generalized
     * coordinates, i.e. joint space
     * @return \f$ \mathbb{R}^{Dof \times 1} \f$ Coriolis force vector \f$
     * c(q,\dot{q})~[Nm] \f$
     * @note Use updateModel() to update robot states first before calling
     * this function
     */
    const Eigen::VectorXd getCoriolisForce();

    friend class RobotClientHandler;

private:
    std::unique_ptr<ModelHandler> m_handler;
};

} /* namespace flexiv */

#endif /* FLEXIVRDK_MODEL_HPP_ */


#ifndef KALMAN_TRACKER_HPP
#define KALMAN_TRACKER_HPP

#include <Eigen/Dense>
#include <boost/any.hpp>

#include <ros/ros.h>

#include <kkl/math/gaussian.hpp>
#include <kkl/alg/kalman_filter.hpp>

namespace hdl_people_tracking {

/**
 * @brief Kalman filter-based tracker with a constant velocity model
 */
class KalmanTracker {
  typedef kkl::alg::KalmanFilter<double, 6, 2, 3> KalmanFilter;
public:
  /**
   * @brief constructor
   * @param id            tracker ID
   * @param time          timestamp
   * @param init_pos      initial position
   * @param associated    associated detection
   */
  KalmanTracker(long id, const ros::Time& time, const Eigen::Vector3d& init_pos, boost::any associated = boost::any())
    : id_(id),
      correction_count(0),
      init_time(time),
      last_prediction_time(time),
      last_correction_time(time),
      last_associated(associated)
  {
    Eigen::Matrix<double, 6, 6> transition = Eigen::Matrix<double, 6, 6>::Identity();
    Eigen::Matrix<double, 6, 2> control = Eigen::Matrix<double, 6, 2>::Zero();
    Eigen::Matrix<double, 3, 6> measurement = Eigen::Matrix<double, 3, 6>::Zero();
    measurement.block<3, 3>(0, 0).setIdentity() * 0.2;

    Eigen::Matrix<double, 6, 6> process_noise = Eigen::Matrix<double, 6, 6>::Zero();
    process_noise.topLeftCorner(3, 3) = Eigen::Matrix3d::Identity() * 0.03;
    process_noise.bottomRightCorner(3, 3) = Eigen::Matrix3d::Identity() * 0.01;
    Eigen::Matrix3d measurement_noise = Eigen::Matrix3d::Identity() * 0.2;

    Eigen::Matrix<double, 6, 1> mean = Eigen::Matrix<double, 6, 1>::Zero();
    mean.head<3>() = init_pos;
    Eigen::Matrix<double, 6, 6> cov = Eigen::Matrix<double, 6, 6>::Identity() * 0.1;

    kalman_filter.reset(new KalmanFilter(transition, control, measurement, process_noise, measurement_noise, mean, cov));
  }
  ~KalmanTracker() {}

  using Ptr = std::shared_ptr<KalmanTracker>;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
  /**
   * @brief predict the current state
   * @param time    current time
   */
  void predict(const ros::Time& time) {
    double difftime = (time - last_prediction_time).toSec();
    difftime = std::max(0.001, difftime);

    kalman_filter->transitionMatrix(0, 3) = difftime;
    kalman_filter->transitionMatrix(1, 4) = difftime;
    kalman_filter->transitionMatrix(2, 5) = difftime;

    kalman_filter->predict(Eigen::Matrix<double, 2, 1>::Zero());
    last_prediction_time = time;

    last_associated = boost::any();
  }

  /**
   * @brief correct the state with an observation
   * @param time    current time
   * @param pos     observed position
   * @param associated   associated detection
   */
  void correct(const ros::Time& time, const Eigen::Vector3d& pos, boost::any associated = boost::any()) {
    kalman_filter->correct(pos);

    correction_count++;
    last_correction_time = time;
    last_associated = associated;
  }

public:
  long id() const {
    return id_;
  }

  ros::Duration age(const ros::Time& time) const {
    return (time - init_time);
  }

  const ros::Time& lastCorrectionTime() const {
    return last_correction_time;
  }

  const boost::any& lastAssociated() const {
    return last_associated;
  }

  Eigen::Vector3d position() const {
    return kalman_filter->mean.head<3>();
  }

  Eigen::Vector3d velocity() const {
    return kalman_filter->mean.tail<3>();
  }

  Eigen::Matrix3d positionCov() const {
    return kalman_filter->cov.block<3, 3>(0, 0);
  }

  Eigen::Matrix3d velocityCov() const {
    return kalman_filter->cov.block<3, 3>(3, 3);
  }

  double squaredMahalanobisDistance(const Eigen::Vector3d& p) const {
    return kkl::math::squaredMahalanobisDistance<double, 3>(
          kalman_filter->mean.head<3>(),
          kalman_filter->cov.block<3, 3>(0, 0),
          p);
  }

  int correctionCount() const {
    return correction_count;
  }

private:
  long id_;

  int correction_count;
  ros::Time init_time;              // time when the tracker was initialized
  ros::Time last_prediction_time;   // tiem when prediction was performed
  ros::Time last_correction_time;   // time when correction was performed

  boost::any last_associated;       // associated detection data

  std::unique_ptr<KalmanFilter> kalman_filter;
};

}

#endif // KALMANTRACKER_HPP


#ifndef SEQUENTIAL_LINE_SEARCH_PREFERENCE_REGRESSOR_HPP
#define SEQUENTIAL_LINE_SEARCH_PREFERENCE_REGRESSOR_HPP

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <sequential-line-search/preference.hpp>
#include <sequential-line-search/regressor.hpp>
#include <string>
#include <utility>
#include <vector>

namespace sequential_line_search
{
    /// \brief Class for performing regression based on preference data.
    ///
    /// \details See [Chu+, ICML 2005; Brochu+, NIPS 2007].
    class PreferenceRegressor : public Regressor
    {
    public:
        PreferenceRegressor(const Eigen::MatrixXd&         X,
                            const std::vector<Preference>& D,
                            const bool                     use_map_hyperparams          = false,
                            const double                   default_kernel_signal_var    = 0.500,
                            const double                   default_kernel_length_scale  = 0.500,
                            const double                   default_noise_level          = 0.005,
                            const double                   kernel_hyperparams_prior_var = 0.250,
                            const double                   btl_scale                    = 0.010,
                            const unsigned                 num_map_estimation_iters     = 100,
                            const KernelType               kernel_type = KernelType::ArdMatern52Kernel);

        double PredictMu(const Eigen::VectorXd& x) const override;
        double PredictSigma(const Eigen::VectorXd& x) const override;

        Eigen::VectorXd PredictMuDerivative(const Eigen::VectorXd& x) const override;
        Eigen::VectorXd PredictSigmaDerivative(const Eigen::VectorXd& x) const override;

        const bool m_use_map_hyperparams;

        /// \brief Find the data point that is likely to have the largest value from the so-far observed data points.
        Eigen::VectorXd FindArgMax() const;

        // Data
        Eigen::MatrixXd         m_X;
        std::vector<Preference> m_D;

        /// \brief Noise level hyperparameter
        ///
        /// \details This value is either derived by the MAP estimation or copied from the default values
        double m_noise_hyperparam;

        /// \brief Kernel hyperparameters
        ///
        /// \details These values are either derived by the MAP estimation or copied from the default values
        Eigen::VectorXd m_kernel_hyperparams;

        /// \brief Kernel matrix calculated in the MAP estimation procedure.
        Eigen::MatrixXd m_K;

        /// \brief Kernel matrix stored as a Cholesky-decomposed form.
        Eigen::LLT<Eigen::MatrixXd> m_K_llt;

        // IO
        void DampData(const std::string& dir_path, const std::string& prefix = "") const;

        // Getter
        const Eigen::MatrixXd& GetLargeX() const override { return m_X; }
        const Eigen::VectorXd& GetSmallY() const override { return m_y; }

        const Eigen::VectorXd& GetKernelHyperparams() const override { return m_kernel_hyperparams; }
        double                 GetNoiseHyperparam() const override { return m_noise_hyperparam; }

        // Default hyperparameters; when MAP is enabled, they are used as initial guesses.
        const double m_default_kernel_signal_var;
        const double m_default_kernel_length_scale;
        const double m_default_noise_level;

        /// \brief Variance of the prior distribution. Used only when MAP is enabled.
        const double m_kernel_hyperparams_prior_var;

        /// \brief Scale parameter in the BTL model
        const double m_btl_scale;

    private:
        /// \brief Goodness values derived by the MAP estimation.
        Eigen::VectorXd m_y;

        void PerformMapEstimation(const unsigned num_iters);
    };
} // namespace sequential_line_search

#endif // SEQUENTIAL_LINE_SEARCH_PREFERENCE_REGRESSOR_HPP


/**
 * @file tests/ann_dist_test.cpp
 * @author Atharva Khandait
 * @author Nishant Kumar
 *
 * Tests the ann distributions.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>

#include <mlpack/methods/ann/dists/bernoulli_distribution.hpp>
#include <mlpack/methods/ann/dists/normal_distribution.hpp>
#include <mlpack/methods/ann/init_rules/random_init.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

#include <mlpack/methods/ann/activation_functions/logistic_function.hpp>

using namespace mlpack;
using namespace mlpack::ann;

BOOST_AUTO_TEST_SUITE(ANNDistTest);

/**
 * Simple bernoulli distribution module test.
 */
BOOST_AUTO_TEST_CASE(SimpleBernoulliDistributionTest)
{
  arma::mat param = arma::mat("1 1 0");
  BernoulliDistribution<> module(param, false);

  arma::mat sample = module.Sample();
  // As the probabilities are [1, 1, 0], the bernoulli samples should be
  // [1, 1, 0] as well.
  CheckMatrices(param, sample);
}

/**
 * Jacobian bernoulli distribution module test when we don't apply logistic.
 */
BOOST_AUTO_TEST_CASE(JacobianBernoulliDistributionTest)
{
  for (size_t i = 0; i < 5; ++i)
  {
    const size_t targetElements = math::RandInt(2, 1000);

    arma::mat param;
    param.randn(targetElements, 1);

    arma::mat target;
    target.randn(targetElements, 1);

    BernoulliDistribution<> module(param, false);

    const double perturbation = 1e-6;
    double outputA, outputB, original;
    arma::mat jacobianA, jacobianB;

    // Initialize the jacobian matrix.
    jacobianA = arma::zeros(targetElements, 1);

    for (size_t j = 0; j < targetElements; ++j)
    {
      original = module.Probability()(j);
      module.Probability()(j) = original - perturbation;
      outputA = module.LogProbability(target);
      module.Probability()(j) = original + perturbation;
      outputB = module.LogProbability(target);
      module.Probability()(j) = original;
      outputB -= outputA;
      outputB /= 2 * perturbation;
      jacobianA(j) = outputB;
    }

    module.LogProbBackward(target, jacobianB);
    BOOST_REQUIRE_LE(arma::max(arma::max(arma::abs(jacobianA - jacobianB))),
        1e-5);
  }
}

/**
 * Jacobian bernoulli distribution module test when we apply logistic.
 */
BOOST_AUTO_TEST_CASE(JacobianBernoulliDistributionLogisticTest)
{
  for (size_t i = 0; i < 5; ++i)
  {
    const size_t targetElements = math::RandInt(2, 1000);

    arma::mat param;
    param.randn(targetElements, 1);

    arma::mat target;
    target.randn(targetElements, 1);

    BernoulliDistribution<> module(param);

    const double perturbation = 1e-6;
    double outputA, outputB, original;
    arma::mat jacobianA, jacobianB;

    // Initialize the jacobian matrix.
    jacobianA = arma::zeros(targetElements, 1);

    for (size_t j = 0; j < targetElements; ++j)
    {
      original = module.Logits()(j);
      module.Logits()(j) = original - perturbation;
      LogisticFunction::Fn(module.Logits(), module.Probability());
      outputA = module.LogProbability(target);
      module.Logits()(j) = original + perturbation;
      LogisticFunction::Fn(module.Logits(), module.Probability());
      outputB = module.LogProbability(target);
      module.Logits()(j) = original;
      LogisticFunction::Fn(module.Logits(), module.Probability());
      outputB -= outputA;
      outputB /= 2 * perturbation;
      jacobianA(j) = outputB;
    }

    module.LogProbBackward(target, jacobianB);
    BOOST_REQUIRE_LE(arma::max(arma::max(arma::abs(jacobianA - jacobianB))),
        3e-5);
  }
}

/**
 * Normal Distribution module test.
 */
BOOST_AUTO_TEST_CASE(NormalDistributionTest)
{
  arma::vec mu = {1.1, 1.2, 1.5, 1.7};
  arma::vec sigma = {0.1, 0.11, 0.5, 0.23};

  ann::NormalDistribution<> normalDist(mu, sigma);

  arma::vec x = {1.05, 1.1, 1.7, 2.5};

  arma::vec prob;
  normalDist.LogProbability(x, prob);

  // Testing output of log probability for some random mu, sigma and x.
  BOOST_REQUIRE_CLOSE(prob[0], 1.2586464, 1e-3);
  BOOST_REQUIRE_CLOSE(prob[1], 0.8751131, 1e-3);
  BOOST_REQUIRE_CLOSE(prob[2], -0.30579138, 1e-3);
  BOOST_REQUIRE_CLOSE(prob[3], -5.498411, 1e-3);

  arma::vec dmu, dsigma;
  normalDist.ProbBackward(x, dmu, dsigma);

  // Testing output of dmu and dsigma for some random mu, sigma and x.
  BOOST_REQUIRE_CLOSE(dmu[0], -17.603287, 1e-3);
  BOOST_REQUIRE_CLOSE(dsigma[0], -26.40487, 1e-3);
  BOOST_REQUIRE_CLOSE(dmu[1], -19.827663, 1e-3);
  BOOST_REQUIRE_CLOSE(dsigma[1], -3.7852707, 1e-3);
  BOOST_REQUIRE_CLOSE(dmu[2], 0.5892323, 1e-3);
  BOOST_REQUIRE_CLOSE(dsigma[2], -1.2373875, 1e-3);
  BOOST_REQUIRE_CLOSE(dmu[3], 0.061901994, 1e-3);
  BOOST_REQUIRE_CLOSE(dsigma[3], 0.19751444, 1e-3);
}

/**
 * Jacobian Normal Distribution module test for mean.
 */
BOOST_AUTO_TEST_CASE(JacobianNormalDistributionMeanTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t targetElements = math::RandInt(2, 1000);

    arma::mat mu;
    mu.randn(targetElements, 1);

    arma::mat sigma;
    sigma.randu(targetElements, 1);

    arma::mat x;
    x.randn(targetElements, 1);

    NormalDistribution<> module(mu, sigma);

    const double perturbation = 1e-6;
    arma::mat output, outputA, outputB, jacobianA, jacobianB;

    // Initialize the jacobian matrix.
    module.Probability(x, output);
    jacobianA = arma::zeros(x.n_elem, output.n_elem);

    for (size_t j = 0; j < x.n_elem; ++j)
    {
      double original = module.Mean()(j);
      module.Mean()(j) = original - perturbation;
      module.Probability(x, outputA);
      module.Mean()(j) = original + perturbation;
      module.Probability(x, outputB);
      module.Mean()(j) = original;

      outputB -= outputA;
      outputB /= 2 * perturbation;
      jacobianA.row(j) = outputB.t();
    }

    // Initialize the derivative parameter.
    arma::mat deriv = arma::zeros(output.n_rows, output.n_cols);

    // Share the derivative parameter.
    arma::mat derivTemp = arma::mat(deriv.memptr(), deriv.n_rows, deriv.n_cols,
        false, false);

    // Initialize the jacobian matrix.
    jacobianB = arma::zeros(mu.n_elem, output.n_elem);

    for (size_t k = 0; k < derivTemp.n_elem; ++k)
    {
      deriv.zeros();
      derivTemp(k) = 1;

      arma::mat deltaMu, deltaSigma;
      module.ProbBackward(x, deltaMu, deltaSigma);

      jacobianB.col(k) = deltaMu % deriv;
    }

    BOOST_REQUIRE_LE(arma::max(arma::max(arma::abs(jacobianA - jacobianB))),
        5e-3);
  }
}

/**
 * Jacobian Normal Distribution module test for standard deviation.
 */
BOOST_AUTO_TEST_CASE(JacobianNormalDistributionStandardDeviationTest)
{
  for (size_t i = 0; i < 5; i++)
  {
    const size_t targetElements = math::RandInt(2, 1000);

    arma::mat mu;
    mu.randn(targetElements, 1);

    arma::mat sigma;
    sigma.randu(targetElements, 1);

    arma::mat x;
    x.randn(targetElements, 1);

    NormalDistribution<> module(mu, sigma);

    const double perturbation = 1e-6;
    arma::mat output, outputA, outputB, jacobianA, jacobianB;

    // Initialize the jacobian matrix.
    module.Probability(x, output);
    jacobianA = arma::zeros(x.n_elem, output.n_elem);

    for (size_t j = 0; j < x.n_elem; ++j)
    {
      double original = module.StandardDeviation()(j);
      module.StandardDeviation()(j) = original - perturbation;
      module.Probability(x, outputA);
      module.StandardDeviation()(j) = original + perturbation;
      module.Probability(x, outputB);
      module.StandardDeviation()(j) = original;

      outputB -= outputA;
      outputB /= 2 * perturbation;
      jacobianA.row(j) = outputB.t();
    }

    // Initialize the derivative parameter.
    arma::mat deriv = arma::zeros(output.n_rows, output.n_cols);

    // Share the derivative parameter.
    arma::mat derivTemp = arma::mat(deriv.memptr(), deriv.n_rows, deriv.n_cols,
        false, false);

    // Initialize the jacobian matrix.
    jacobianB = arma::zeros(sigma.n_elem, output.n_elem);

    for (size_t k = 0; k < derivTemp.n_elem; ++k)
    {
      deriv.zeros();
      derivTemp(k) = 1;

      arma::mat deltaMu, deltaSigma;
      module.ProbBackward(x, deltaMu, deltaSigma);

      jacobianB.col(k) = deltaSigma % deriv;
    }

    BOOST_REQUIRE_LE(arma::max(arma::max(arma::abs(jacobianA - jacobianB))),
        5e-3);
  }
}

BOOST_AUTO_TEST_SUITE_END();


/*
Copyright (c) 2017 Ryoichi Ishikawa. All rights reserved.

This software is released under the MIT License.
http://opensource.org/licenses/mit-license.php
*/

#include "FloorDetector.h"
#include <Eigen\Eigen>
#include <Eigen\Core>
#include <Eigen\Dense>
#include "pch.h"
void FloorDetection(Platform::Array<unsigned char>^ buffer,int rowpitch,int height,double scale,double& HoloHeight, Eigen::Vector3d& floorpt) {
	//Buffer 2 Array

	float * imageData = (float*)buffer->Data;

	//get plane and Height
	//around 1m x 1m
	int dwidth = 1.0 / scale;
	//ransac
	int ransac_max = 100;
	std::vector<Eigen::Vector3d> points;
	std::vector<unsigned int> idces;
	Eigen::Vector3d bestn, bestp;
	bestn << 1, 1, 1;
	unsigned int cnt = 0;
	int maxcnt = -1;
	for (int x = rowpitch / sizeof(float) / 2;x<rowpitch / 2 / sizeof(float) + dwidth;x++) {
		for (int y = height / 2;y<height / 2 + dwidth;y++) {
			Eigen::Vector3d p_temp;
			p_temp << x*scale, y*scale, imageData[x + y*(rowpitch / sizeof(float))] * 3.0;
			points.push_back(p_temp);
			idces.push_back(cnt);
			cnt++;
		}
	}
	std::vector<unsigned int> bestlist;
	for (int ransac_t = 0;ransac_t<ransac_max;ransac_t++) {
		random_shuffle(idces.begin(), idces.end());
		std::vector<unsigned int> candlist;
		Eigen::Vector3d v01, v02, nfloor, cand_p;
		cand_p = points.at(idces.at(0));
		v01 = points.at(idces.at(1)) - cand_p;
		v02 = points.at(idces.at(2)) - cand_p;
		nfloor = v01.cross(v02);
		nfloor = nfloor.normalized();
		int inlcnt = 0;
		for (int idx = 3;idx<idces.size();idx++) {
			Eigen::Vector3d targp = points.at(idces.at(idx)) - cand_p;

			double err = abs(targp.dot(nfloor));
			if (err<0.005) {
				//inlier
				candlist.push_back(idces.at(idx));
				inlcnt++;
			}
		} if (maxcnt<inlcnt) {
			maxcnt = inlcnt;
			bestn = nfloor;
			bestp = cand_p;
			bestlist = std::vector<unsigned int>(candlist);
		}
	}
	//plane fitting
	//solve least square problem
	//ax+by+z+d=0: ax+by+d=-z
	Eigen::MatrixXd A(bestlist.size(), 3);
	Eigen::VectorXd B(bestlist.size());
	for (int idx = 0;idx<bestlist.size();idx++) {
		Eigen::Vector3d targp = points.at(idces.at(idx));
		A(idx, 0) = targp(0);//x
		A(idx, 1) = targp(1);//y
		A(idx, 2) = 1;//1
		B(idx) = -targp(2);//-z
	}
	Eigen::Vector3d ansX = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);

	//rendering: 1.0m upper from hololens - 2.0m lower from hololens (3.0m range)
	//hololens point (scale*(dwidth/2),scale*(dwidth/2),1.0)
	//ax+by+z+d=0: n<<a,b,1
	Eigen::Vector3d phl;
	bestn << ansX(0), ansX(1), 1;
	bestp << scale*(rowpitch / sizeof(float) / 2), scale*(rowpitch / sizeof(float) / 2), -(ansX(0) + ansX(1))*scale*(rowpitch / sizeof(float) / 2) - ansX(2);
	bestn = bestn.normalized();
	if (bestn(2)<0)bestn = -bestn;

	//rendering: 1.0m upper from hololens - 2.0m lower from hololens (3.0m range)
	//hololens point (scale*(dwidth/2),scale*(dwidth/2),1.0)
	phl << scale*(rowpitch / sizeof(float) / 2), scale*(rowpitch / sizeof(float) / 2), 1.0;
	HoloHeight = -(phl - bestp).dot(bestn);
	floorpt = -HoloHeight*bestn;//hololens 2 floor

};


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

**/
//==================================================================================================
#ifndef BOOST_SIMD_ARITHMETIC_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_HPP_INCLUDED

namespace boost { namespace simd
{
  /*!
    @ingroup group-functions
    @defgroup group-arithmetic Arithmetic functions

    These functions provide scalar and SIMD algorithms for classical arithmetic operators and
    functions of the C and C++ standard library. Other functions are also provided, in particular,
    provision for saturated operations through the use of a @ref group-decorator.

    All these functions can be included individually or all of them just by including
    <boost/simd/arithmetic.hpp>

     - **Possibly saturated operations**

       The functors:

         |                  |                 |                 |           |              |
         |:----------------:|:---------------:|:---------------:|:---------:|:------------:|
         | @ref abs         | @ref dec        | @ref dist       | @ref inc  | @ref minus   |
         | @ref multiplies  | @ref plus       | @ref oneminus   | @ref sqr  | @ref toint   |
         | @ref unary_minus | @ref unary_plus | @ref touint     |           |              |

       can be decorated with `saturated_` (see @ref group-decorator). This decorator
       has no effect on floating calls,  but on integer calls replaces the operation by its
       saturated equivalent.

       Typically: overflows will be replaced by the @ref Valmin/@ref Valmax proper value
       instead of providing undefined behaviour (for signed integral types) or wrapping
       modulo @ref Valmax + 1 (for unsigned ones).

       Peculiarly saturated_(@ref abs) and saturated_(@ref dist) ensure that the result will
       never be stricly negative (which is for instance the case of `abs(Valmin<T>())` for
       @c T being any signed integral type).

       @ref toint is a rather common operation as it converts floating number to signed integers
       of the same bit size, nevertheless it probably is its saturated version you have to use
       because it acts properly on large or not finite values, this is why an alias for
       `saturated_(toint)` is provided as @ref ifix.

       @par Example:

          @snippet saturated_abs.cpp saturated_abs

       @par Possible output:

          @snippet saturated_abs.txt saturated_abs_results

     - **Rounding operations**

         |                 |                 |              |                  |
         |:---------------:|:---------------:|:------------:|:----------------:|
         | @ref ceil       |  @ref fix       | @ref floor   |  @ref iceil      |
         | @ref ifix       |  @ref ifloor    | @ref iround  |  @ref itrunc     |
         | @ref inearbyint |  @ref nearbyint | @ref round   |  @ref trunc      |

          - The operations prefixed by 'i' return a value of the integral type iT
          associated to the entry type. (If T is the entry type iT is
          @c as_integer_t<T>)

          - The other ones return the same type as the entry.

        @par Example:

          @snippet roundings.cpp roundings

        @par Possible output:

          @snippet roundings.txt roundings_results

     - **Division operations**

        @ref divides is the function associated to standard division. There is another one
        which provides more flexibility, namely rounded divisions.

        With two parameters @ref div and @ref divides are equivalent, but @ref div can admit
        a first option parameter that modifies its behaviour.

         | option          |          call           |      result similar to           |
         |-----------------|-------------------------|----------------------------------|
         | @ref ceil       |   div(ceil, a, b)       |      T(ceil(fT(a)/fT(b)))        |
         | @ref floor      |   div(floor, a, b)      |      T(floor(fT(a)/fT(b)))       |
         | @ref fix        |   div(fix, a, b)        |      T(fix(fT(a)/fT(b)))         |
         | @ref round      |   div(round, a, b)      |      T(round(fT(a)/fT(b)))       |
         | @ref nearbyint  |   div(nearbyint, a, b)  |      T(nearbyint(fT(a)/fT(b)))   |
         | @ref iceil      |   div(iceil, a, b)      |      iT(iceil(fT(a)/fT(b)))      |
         | @ref ifloor     |   div(ifloor, a, b)     |      iT(ifloor(fT(a)/fT(b)))     |
         | @ref ifix       |   div(ifix, a, b)       |      iT(ifix(fT(a)/fT(b)))       |
         | @ref iround     |   div(iround, a, b)     |      iT(iround(fT(a)/fT(b)))     |
         | @ref inearbyint |   div(inearbyint, a, b) |      iT(inearbyint(fT(a)/fT(b))) |

           - The option parameter is described in the above table where a and b are of type T,
             fT is a supposed floating type associated to T (`as_floating_t<T>` if it
             exists) and iT is the integer type associated to T (`as_integer_t<T>`).
             (fT and iT are here only to support pseudo code description)

           @par Example:

              @snippet divisions.cpp divisions

           @par Possible output:

              @snippet divisions.txt divisions_results

     - **Remainder operations**

       @ref rem is the remainder functor providing same kind of facilities as @ref div

       With two parameters rem(a, b) is equivalent to  @c rem(fix, a, b), but @c rem can admit
       a first optional parameter that modifies its behaviour and moreover can use the
       pedantic_ decorator to assure some limiting case values (see below).

       The option parameter can be chosen between @ref ceil, @ref floor, @ref fix, @ref round,
       @ref nearbyint and if @c opt is the option, the call:

          @c rem(opt, a, b) is equivalent to  @c a-b*div(opt, a, b)

       For floating entries the underlisted corner cases are handled in the following way:
        - if  @c x is \f$\pm\infty\f$ , @ref Nan is returned
        - if  @c x is \f$\pm0\f$ and  @c y is not  @c 0  @c x is returned if  @c pedantic_
          is used (else  @c 0: the sign bit is not preserved)
        - if  @c y is \f$\pm0\f$, @ref Nan is returned
        - if either argument is a nan,  a nan is returned

       @par Example:

          @snippet remainders.cpp remainders

       @par Possible output:

          @snippet remainders.txt remainders_results

     - **complex operations**

       Boost.SIMD  does not provides complex number operations yet, but it will soon. So
       the following functors that have a meaning as a restriction to real number of complex
       functions, can be seen as a prequel:

        |           |             |             |             |                 |
        |:---------:|:-----------:|:-----------:|:-----------:|:---------------:|
        | @ref arg  | @ref conj   | imag        | real        | @ref sqr_abs    |

        For real entries @ref conj and real are identity, imag always 0,
        @ref sqr_abs coincide with @ref sqr and @ref arg results are always in the
        set \f$\{0, \pi,  Nan\}\f$

     - **Fused multiply-add operations**

        |                 |              |               |                  |
        |:---------------:|:------------:|:-------------:|:----------------:|
        | @ref fma        | @ref fnma    |  @ref two_add | @ref two_split   |
        | @ref fms        | @ref fnms    |  @ref two_prod|                  |

      These operations take three parmeters and compute some \f$\pm a * b \pm c \f$
      kind of expression, "n" standing for negate the result, "a" for add,
      "s" for substract and "m" for multiply.

      Correct fused multiply/add implies

      - only one rounding
      - no "intermediate" overflow

      The functions of this family provide this, BUT ONLY each time it is reasonable
      in terms of performance (mainly if the system has the hard
      wired capability).

      If you need "real" fused multiply-add capabilities in all circumstances in your own
      code you can use @c pedantic_(fma) (although it can be expansive) or
      @c std_(fma) (generally still more expansive) by using the decorators.

      @ref two_add, @ref two_prod and @ref two_split are used internally in @c pedantic_(fma)
      and can be useful in searching extra-accuracy in other circumstances as double-double
      computations.

      @c pedantic_(fma) is never used internally by Boost.SIMD

     - **Standard operations**

       The stdlibc++ provides them but only in scalar mode:

         |               |                 |              |             |
         |:-------------:|:---------------:|:------------:|:-----------:|
         | @ref abs      | @ref ceil       | @ref floor   | @ref fma    |
         | @ref hypot    | @ref max        | @ref maxnum  | @ref min    |
         | @ref minnum   | @ref rem (%)    | @ref remquo  | @ref round  |
         | @ref signbit  | @ref sqrt       |              |             |

       Boost.SIMD provides its own scalar and simd versions, but allows
       the use of the @c std_ @ref group-decorator to call the associated system
       library function if the user needs it.

     - **Other operations**

         |              |                 |               |              |              |
         |:------------:|:---------------:|:-------------:|:------------:|:------------:|
         | @ref average | @ref clamp      | @ref meanof   | @ref minmod  | @ref sqr     |
         | @ref sqrt    | @ref sqrt1pm1   | @ref tenpower | @ref tofloat |              |

       @ref clamp is also provided in stdlibc++ for scalar mode, but only since C++17.
       For now, in Boost.SIMD, the pedantic_  decorated version ensures standard
       conformity for a Nan first parameter.
  **/
} }

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/arg.hpp>
#include <boost/simd/function/average.hpp>
#include <boost/simd/function/ceil.hpp>
#include <boost/simd/function/clamp.hpp>
#include <boost/simd/function/conj.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/dist.hpp>
#include <boost/simd/function/div.hpp>
#include <boost/simd/function/extract.hpp>
#include <boost/simd/function/fix.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/fms.hpp>
#include <boost/simd/function/fnma.hpp>
#include <boost/simd/function/fnms.hpp>
#include <boost/simd/function/hypot.hpp>
#include <boost/simd/function/iceil.hpp>
#include <boost/simd/function/ifix.hpp>
#include <boost/simd/function/ifloor.hpp>
#include <boost/simd/function/inc.hpp>
#include <boost/simd/function/inearbyint.hpp>
#include <boost/simd/function/iround.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/meanof.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minmod.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/oneminus.hpp>
#include <boost/simd/function/quadrant.hpp>
#include <boost/simd/function/rec.hpp>
#include <boost/simd/function/rem.hpp>
#include <boost/simd/function/remquo.hpp>
#include <boost/simd/function/nearbyint.hpp>
#include <boost/simd/function/round.hpp>
#include <boost/simd/function/rsqrt.hpp>
#include <boost/simd/function/signbit.hpp>
#include <boost/simd/function/sqr_abs.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/sqrt1pm1.hpp>
#include <boost/simd/function/sqrt.hpp>
#include <boost/simd/function/tenpower.hpp>
#include <boost/simd/function/tofloat.hpp>
#include <boost/simd/function/toint.hpp>
#include <boost/simd/function/touint.hpp>
#include <boost/simd/function/trunc.hpp>
#include <boost/simd/function/two_add.hpp>
#include <boost/simd/function/two_prod.hpp>
#include <boost/simd/function/two_split.hpp>
#include <boost/simd/function/unary_minus.hpp>
#include <boost/simd/function/unary_plus.hpp>

#endif