// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_DENSIFY_SPHERICAL_HPP
#define BOOST_GEOMETRY_STRATEGIES_DENSIFY_SPHERICAL_HPP

#include <boost/geometry/strategies/detail.hpp>
#include <boost/geometry/strategies/densify/services.hpp>

#include <boost/geometry/strategies/spherical/densify.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace densify
{

template
<
    typename RadiusTypeOrSphere = double,
    typename CalculationType = void
>
class spherical
    : public strategies::detail::spherical_base<RadiusTypeOrSphere>
{
    using base_t = strategies::detail::spherical_base<RadiusTypeOrSphere>;

public:
    spherical() = default;

    template <typename RadiusOrSphere>
    explicit spherical(RadiusOrSphere const& radius_or_sphere)
        : base_t(radius_or_sphere)
    {}

    template <typename Geometry>
    auto densify(Geometry const&) const
    {
        return strategy::densify::spherical
                <
                    typename base_t::radius_type, CalculationType
                >(base_t::radius());
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, spherical_equatorial_tag>
{
    using type = strategies::densify::spherical<>;
};

template <typename R, typename CT>
struct strategy_converter<strategy::densify::spherical<R, CT> >
{
    static auto get(strategy::densify::spherical<R, CT> const& s)
    {
        return strategies::densify::spherical<R, CT>(s.radius());
    }
};

} // namespace services

}} // namespace strategies::densify

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_DENSIFY_SPHERICAL_HPP


#include "stealthtext.h"

// cryptopp headers

#include "cryptopp/hex.h"
using CryptoPP::HexEncoder;

#include "cryptopp/base64.h"
using CryptoPP::Base64Decoder;

#include "cryptopp/filters.h"
using CryptoPP::Redirector;
using CryptoPP::StringSink;
using CryptoPP::StringSource;
using CryptoPP::AuthenticatedDecryptionFilter;

#include "cryptopp/sha.h"
using CryptoPP::SHA256;
using CryptoPP::HashFilter;

#include "cryptopp/gcm.h"
using CryptoPP::GCM;

#include <boost/algorithm/string.hpp>

// SHA256 Key Derivation
void SHAKD(std::string twofa, std::string iv_str, byte key[KEY_SIZE]) {

        std::string salt;
        StringSource bar(iv_str, true,
               new HexEncoder(new StringSink(salt)));

        SHA256 hash;
        std::string string_key;
        StringSource foo(twofa + salt, true,
            new HashFilter(hash, new StringSink(string_key)));

        // convert string string_key into byte mykey
        for (int idx=0; idx<KEY_SIZE; idx++) {
           key[idx] = string_key[idx];
        }
}

// fill txdescr with address, amount, timestamp, as strings
bool decryptstealthtxt(std::string msg64,
                       std::string twofa,
                       std::vector<std::string> &txdescr) {

        std::string decoded, recovered;
        StringSource(msg64, true,
                new Base64Decoder(
                           new StringSink(decoded)));

        // AES
        byte myiv[IV_SIZE];
        for (int idx=0; idx<IV_SIZE; idx++) {
            myiv[idx] = decoded[idx];
        }

        // needed to salt key derivation
        std::string iv_str;
        iv_str = decoded.substr(0, IV_SIZE);

	byte mykey[KEY_SIZE];
        SHAKD(twofa, iv_str, mykey);

        std::string ctxt = decoded.substr(IV_SIZE, decoded.size() - IV_SIZE);

        try
        {

            GCM< AES >::Decryption decryptor;
            decryptor.SetKeyWithIV(mykey, sizeof(mykey),
                                       myiv, sizeof(myiv));

            /* CGM ***************************************************/
            AuthenticatedDecryptionFilter df(decryptor,
                new StringSink(recovered),
                AuthenticatedDecryptionFilter::DEFAULT_FLAGS,
                TAG_SIZE
            ); // AuthenticatedDecryptionFilter

            StringSource(ctxt, true,
                new Redirector(df)
            ); // StringSource

            // If the object does not throw, here's the only
            //  opportunity to check the data's integrity
            bool b = df.GetLastResult();
            if (!b) {
                 return false;
             }
        }

        catch(const CryptoPP::Exception& e) {
            return false;
        }

        boost::split(txdescr, recovered, boost::is_any_of(","));

        // might as well check
        if (txdescr.size() != 3) {
          return false;
        }

	return true;
}


#pragma once
#ifndef CHEETAHSTATE_H
#define CHEETAHSTATE_H

//C Libraries
#include <stdint.h>

#include <iostream>
#include <memory>

//External Libraries
#include <Eigen/Dense>
#include "ros/ros.h"

//Cheetah Libraries
#include "utils/cheetah_data_t.hpp"
#include "utils/utils.hpp"
// #include "RosPublisher.h"
// #include "PassiveTimeSync.h"

class CheetahState {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        CheetahState();
//          CheetahState(const Eigen::Matrix<double,18,1>& q, const Eigen::Matrix<double,18,1>& dq, bool computeContacts = true);
        CheetahState(const cheetah_lcm_packet_t& cheetah_data);

//         void set(const Eigen::Matrix<double,18,1>& q, const Eigen::Matrix<double,18,1>& dq, bool computeContacts = true);
        void set(const cheetah_lcm_packet_t& cheetah_data);
        void setBaseRotation(const Eigen::Matrix3d& R);
        void setBasePosition(const Eigen::Vector3d& p);
        void setBaseVelocity(const Eigen::Vector3d& v);
        void setCovarianceMatrix(const Eigen::MatrixXd& P);
//         void setMotorPositions(const Eigen::Matrix<double,10,1>& qM);
//         void setMotorVelocities(const Eigen::Matrix<double,10,1>& dqM);
        void clear();

        Eigen::Matrix<double,18,1> q() const;
        Eigen::Matrix<double,18,1> dq() const;
        Eigen::Vector3d getPosition() const;
        Eigen::Quaternion<double> getQuaternion() const;
        Eigen::Matrix3d getRotation() const;
        Eigen::Vector3d getEulerAngles() const;
        Eigen::Vector3d getEulerRates() const;
        Eigen::Matrix<double, 12, 1> getEncoderPositions() const;
        Eigen::Matrix<double,12,1> getEncoderVelocities() const;
//         Eigen::Matrix<double,10,1> getMotorPositions() const;
//         Eigen::Matrix<double,10,1> getMotorVelocities() const;
//         Eigen::Matrix<double,4,1> getGRF() const;
        bool getLeftFrontContact() const;
        bool getLeftHindContact() const;
        bool getRightFrontContact() const;
        bool getRightHindContact() const;
        Eigen::Vector3d getAngularVelocity() const;
        Eigen::Vector3d getKinematicVelocity() const;
        Eigen::Vector3d getBodyVelocity() const;

        // Extract robot pose:
        double x() const;
        double y() const;
        double z() const;
        double yaw() const;
        double pitch() const;
        double roll() const;
        // Extract robot joint values:
        double rightFrontMotor1() const;
        double rightFrontMotor2() const;
        double rightFrontMotor3() const;
        double leftFrontMotor1() const;
        double leftFrontMotor2() const;
        double leftFrontMotor3() const;
        double rightHindMotor1() const;
        double rightHindMotor2() const;
        double rightHindMotor3() const;
        double leftHindMotor1() const;
        double leftHindMotor2() const;
        double leftHindMotor3() const;

        // Extract robot velocity:
        double dx() const;
        double dy() const;
        double dz() const;
        double dyaw() const;
        double dpitch() const;
        double droll() const;
        // Extract robot joint velocities:
        double drightFrontMotor1() const;
        double drightFrontMotor2() const;
        double drightFrontMotor3() const;
        double dleftFrontMotor1() const;
        double dleftFrontMotor2() const;
        double dleftFrontMotor3() const;
        double drightHindMotor1() const;
        double drightHindMotor2() const;
        double drightHindMotor3() const;
        double dleftHindMotor1() const;
        double dleftHindMotor2() const;
        double dleftHindMotor3() const;

        //extract robot covariance
        Eigen::MatrixXd P() const;
        friend std::ostream& operator<<(std::ostream& os, const  CheetahState& obj);

        //get and set time
        void setTime(double time) { time_ = time; }
        double getTime() const;
        ros::Time getRosTime() const;
        //set basetime_
        void setBasetime(ros::Time base) {basetime_ = base;}
    private:
        Eigen::Matrix<double, 18,1> q_;
        Eigen::Matrix<double, 18,1> dq_;
        Eigen::Matrix<double,4,1> GRF_; //!< ground reaction force
        Eigen::MatrixXd P_; //covariance
        bool left_front_contact_;
        bool left_hind_contact_;
        bool right_front_contact_;
        bool right_hind_contact_;
        double time_;
        ros::Time basetime_;

};

#endif


// MIT License
//
// Copyright (c) 2018 Lennart Braun
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#include <algorithm>
#include <array>
#include <cassert>
#include <cstdint>
#include <boost/asio.hpp>
#include <botan/blake2b.h>
#include <botan/hex.h>
#include "ot_co15.hpp"
#include "util/threading.hpp"

OT_CO15::OT_CO15(Connection& connection) : connection_(connection)
{
}

void OT_CO15::send_0(Sender_SharedState& state,
                     std::array<uint8_t, curve25519_ge_byte_size>& message_out)
{
    // sample y <- Zp
    curve25519::sc_random(state.y);

    // S = y*G
    curve25519::x25519_ge_scalarmult_base(&state.S, state.y);

    curve25519::ge_p3_tobytes(message_out.data(), &state.S);
}

std::pair<bytes_t, bytes_t> OT_CO15::send_1(const Sender_SharedState& state,
                                            const std::array<uint8_t, curve25519_ge_byte_size>& message_in)
{
    curve25519::ge_p3 R;
    // assert R in GG
    if (!x25519_ge_frombytes_vartime(&R, message_in.data()))
        std::terminate();

    auto hash(Botan::Blake2b(128));

    auto output = std::make_pair<>(bytes_t(16), bytes_t(16));
    assert(output.first.size() == hash.output_length());
    assert(output.second.size() == hash.output_length());

    std::array<uint8_t, 3*curve25519_ge_byte_size> hash_input;
    curve25519::ge_p3_tobytes(hash_input.data(), &state.S);
    curve25519::ge_p3_tobytes(hash_input.data() + 32, &R);

    // j = 0:
    // y*R
    curve25519::ge_p2 a_times_R_p2;
    curve25519::x25519_ge_scalarmult(&a_times_R_p2, state.y, &R);
    curve25519::x25519_ge_tobytes(hash_input.data() + 64, &a_times_R_p2);

    // H(S, R, y*R)
    hash.update(hash_input.data(), hash_input.size());
    hash.final(output.first.data());

    // j = 1:
    // y*(R - S)
    {
        curve25519::ge_cached S_cached;
        curve25519::x25519_ge_p3_to_cached(&S_cached, &state.S);

        curve25519::ge_p1p1 R_minus_S_p1p1;
        curve25519::x25519_ge_sub(&R_minus_S_p1p1, &R, &S_cached);

        curve25519::ge_p3 R_minus_S_p3;
        curve25519::x25519_ge_p1p1_to_p3(&R_minus_S_p3, &R_minus_S_p1p1);

        curve25519::ge_p2 y_times_R_minus_S_p2;
        curve25519::x25519_ge_scalarmult(&y_times_R_minus_S_p2, state.y, &R_minus_S_p3);
        curve25519::x25519_ge_tobytes(hash_input.data() + 64, &y_times_R_minus_S_p2);

    }

    // H(S, R, y*(R - S))
    hash.update(hash_input.data(), hash_input.size());
    hash.final(output.second.data());

    return output;
}

void OT_CO15::recv_0(Receiver_State& state, bool choice)
{
    state.choice = choice;
    // sample b <- Zp
    curve25519::sc_random(state.x);
}

void OT_CO15::recv_1(Receiver_SharedState& sstate,
                     const std::array<uint8_t, curve25519_ge_byte_size>& message_in)
{
    // recv S
    auto res = curve25519::x25519_ge_frombytes_vartime(&sstate.S, message_in.data());
    // assert S in GG
    if (res == 0)
        std::terminate();
}

void OT_CO15::recv_2(Receiver_State& state, const Receiver_SharedState& sstate,
                     std::array<uint8_t, curve25519_ge_byte_size>& message_out)
{
    curve25519::x25519_ge_scalarmult_base(&state.R, state.x);
    // FIXME: not constant time
    if (state.choice == 1)
    {
        curve25519::ge_p1p1 R_p1p1;
        curve25519::ge_cached S_cached;
        curve25519::x25519_ge_p3_to_cached(&S_cached, &sstate.S);
        curve25519::x25519_ge_add(&R_p1p1, &state.R, &S_cached);
        curve25519::x25519_ge_p1p1_to_p3(&state.R, &R_p1p1);
    }

    curve25519::ge_p3_tobytes(message_out.data(), &state.R);
}

bytes_t OT_CO15::recv_3(const Receiver_State& state, const Receiver_SharedState& sstate)
{
    // k_R = H_(S, R, y*S)

    bytes_t hash_output(16);

    std::array<uint8_t, 3*curve25519_ge_byte_size> hash_input;
    curve25519::ge_p3_tobytes(hash_input.data(), &sstate.S);
    curve25519::ge_p3_tobytes(hash_input.data() + 32, &state.R);

    curve25519::ge_p2 y_times_S;
    curve25519::x25519_ge_scalarmult(&y_times_S, state.x, &sstate.S);
    curve25519::x25519_ge_tobytes(hash_input.data() + 64, &y_times_S);

    auto hash(Botan::Blake2b(128));
    assert(hash_output.size() == hash.output_length());
    hash.update(hash_input.data(), hash_input.size());
    hash.final(hash_output.data());

    return hash_output;
}

std::vector<std::pair<bytes_t, bytes_t>> OT_CO15::send(size_t number_ots)
{
    Sender_SharedState state;
    std::array<uint8_t, curve25519_ge_byte_size> msg_s0;
    std::vector<std::array<uint8_t, curve25519_ge_byte_size>> msgs_r1(number_ots);
    std::vector<std::pair<bytes_t, bytes_t>> output(number_ots);

    send_0(state, msg_s0);

    auto fut_send_msg_s0 = connection_.async_send(msg_s0.data(), msg_s0.size());
    auto fut_recv_msg_r1 = connection_.async_recv(reinterpret_cast<uint8_t*>(msgs_r1.data()), msgs_r1.size() * curve25519_ge_byte_size);

    auto msg_r1_size = fut_recv_msg_r1.get();
    assert(msg_r1_size == msgs_r1.size() * curve25519_ge_byte_size);

    for (size_t i = 0; i < number_ots; ++i)
    {
        output[i] = send_1(state, msgs_r1[i]);
    }

    auto msg_s0_size = fut_send_msg_s0.get();
    assert(msg_s0_size == msg_s0.size());

    return output;
}

std::vector<bytes_t> OT_CO15::recv(const std::vector<bool>& choices)
{
    auto number_ots = choices.size();
    std::vector<Receiver_State> states(number_ots);
    Receiver_SharedState sstate;
    std::array<uint8_t, curve25519_ge_byte_size> msg_s0;
    std::vector<std::array<uint8_t, curve25519_ge_byte_size>> msgs_r1(number_ots);
    std::vector<bytes_t> output(number_ots);

    auto fut_recv_msg_s0 = connection_.async_recv(msg_s0.data(), msg_s0.size());

    for (size_t i = 0; i < number_ots; ++i)
    {
        recv_0(states[i], choices[i]);
    }

    auto msg_s0_size = fut_recv_msg_s0.get();
    assert(msg_s0_size == msg_s0.size());

    recv_1(sstate, msg_s0);

    for (size_t i = 0; i < number_ots; ++i)
    {
        recv_2(states[i], sstate, msgs_r1[i]);
    }

    auto fut_send_msg_r1 = connection_.async_send(reinterpret_cast<uint8_t*>(msgs_r1.data()), msgs_r1.size() * curve25519_ge_byte_size);

    for (size_t i = 0; i < number_ots; ++i)
    {
        output[i] = recv_3(states[i], sstate);
    }

    auto msg_r1_size = fut_send_msg_r1.get();
    assert(msg_r1_size == msgs_r1.size() * curve25519_ge_byte_size);

    return output;
}

std::pair<bytes_t, bytes_t> OT_CO15::send()
{
    Sender_SharedState state;
    std::array<uint8_t, curve25519_ge_byte_size> msg_s0;
    std::array<uint8_t, curve25519_ge_byte_size> msg_r1;

    send_0(state, msg_s0);
    connection_.send(msg_s0.data(), msg_s0.size());
    connection_.recv(msg_r1.data(), msg_r1.size());
    return send_1(state, msg_r1);
}

bytes_t OT_CO15::recv(bool choice)
{
    Receiver_State state;
    Receiver_SharedState sstate;
    std::array<uint8_t, curve25519_ge_byte_size> msg_s0;
    std::array<uint8_t, curve25519_ge_byte_size> msg_r1;

    recv_0(state, choice);
    connection_.recv(msg_s0.data(), msg_s0.size());
    recv_1(sstate, msg_s0);
    recv_2(state, sstate, msg_r1);
    connection_.send(msg_r1.data(), msg_r1.size());
    return recv_3(state, sstate);
}

std::vector<std::pair<bytes_t, bytes_t>> OT_CO15::parallel_send(size_t number_ots, size_t number_threads, boost::asio::thread_pool& thread_pool)
{
    Sender_SharedState sstate;
    std::array<uint8_t, curve25519_ge_byte_size> msg_s0;
    std::vector<std::array<uint8_t, curve25519_ge_byte_size>> msgs_r1(number_ots);
    std::vector<std::pair<bytes_t, bytes_t>> output(number_ots);

    send_0(sstate, msg_s0);

    auto fut_send_msg_s0 = connection_.async_send(msg_s0.data(), msg_s0.size());
    auto fut_recv_msg_r1 = connection_.async_recv(reinterpret_cast<uint8_t*>(msgs_r1.data()), msgs_r1.size() * curve25519_ge_byte_size);

    auto msg_r1_size = fut_recv_msg_r1.get();
    assert(msg_r1_size == msgs_r1.size() * curve25519_ge_byte_size);

    compute(thread_pool, number_ots, number_threads, [this, &sstate, &msgs_r1, &output](size_t index){ output[index] = send_1(sstate, msgs_r1[index]); });

    auto msg_s0_size = fut_send_msg_s0.get();
    assert(msg_s0_size == msg_s0.size());

    return output;
}

std::vector<bytes_t> OT_CO15::parallel_recv(const std::vector<bool>& choices, size_t number_threads, boost::asio::thread_pool& thread_pool)
{
    auto number_ots = choices.size();
    std::vector<Receiver_State> states(number_ots);
    Receiver_SharedState sstate;
    std::array<uint8_t, curve25519_ge_byte_size> msg_s0;
    std::vector<std::array<uint8_t, curve25519_ge_byte_size>> msgs_r1(number_ots);
    std::vector<bytes_t> output(number_ots);

    auto fut_recv_msg_s0 = connection_.async_recv(msg_s0.data(), msg_s0.size());

    compute(thread_pool, number_ots, number_threads, [this, &states, &choices](size_t index){ recv_0(states[index], choices[index]); });

    auto msg_s0_size = fut_recv_msg_s0.get();
    assert(msg_s0_size == msg_s0.size());

    recv_1(sstate, msg_s0);

    compute(thread_pool, number_ots, number_threads, [this, &states, &sstate, &msgs_r1](size_t index){ recv_2(states[index], sstate, msgs_r1[index]); });

    auto fut_send_msg_r1 = connection_.async_send(reinterpret_cast<uint8_t*>(msgs_r1.data()), msgs_r1.size() * curve25519_ge_byte_size);

    compute(thread_pool, number_ots, number_threads, [this, &states, &sstate, &output](size_t index){ output[index] = recv_3(states[index], sstate); });

    auto msg_r1_size = fut_send_msg_r1.get();
    assert(msg_r1_size == msgs_r1.size() * curve25519_ge_byte_size);

    return output;
}


#ifndef QSIM_DECODE_HPP_
#define QSIM_DECODE_HPP_
#include <armadillo>

double coeff_a1(unsigned int q_idx, arma::cx_dvec &psi);
double coeff_a2(unsigned int q_idx, unsigned int x, arma::cx_dvec &psi);
double coeff_a3(unsigned int q_idx, unsigned int x, unsigned int z, arma::cx_dvec &psi);
double optimized_theta(unsigned int q_idx, unsigned int x, unsigned int z, arma::cx_dvec &psi);
double optimal_overlap(unsigned int q_idx, unsigned int x, unsigned int z, arma::cx_dvec &psi);
void optimal_update(unsigned int setbits, arma::cx_dvec &psi);
void decode(arma::cx_dvec &psi, unsigned int setbits, double eps);
double decode_cost(arma::cx_dvec &psi, unsigned int setbits, double eps);

#endif // QSIM_DECODE_HPP_


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

/*
 Copyright (C) 2014 Master IMAFA - Polytech'Nice Sophia - Université de Nice Sophia Antipolis

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

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

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

#include <ql/experimental/exoticoptions/analyticholderextensibleoptionengine.hpp>
#include <ql/math/distributions/bivariatenormaldistribution.hpp>
#include <ql/exercise.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

    AnalyticHolderExtensibleOptionEngine::AnalyticHolderExtensibleOptionEngine(
             const boost::shared_ptr<GeneralizedBlackScholesProcess>& process)
    : process_(process) {
        registerWith(process_);
    }

    void AnalyticHolderExtensibleOptionEngine::calculate() const {
        //Spot
        Real S = process_->x0();
        Real r = riskFreeRate();
        Real b = r - dividendYield();
        Real X1 = strike();
        Real X2 = arguments_.secondStrike;
        Time T2 = secondExpiryTime();
        Time t1 = firstExpiryTime();
        Real A = arguments_.premium;

        Real z1 = this->z1();

        Real z2 = this->z2();

        Real rho = sqrt(t1 / T2);

        boost::shared_ptr<PlainVanillaPayoff> payoff =
            boost::dynamic_pointer_cast<PlainVanillaPayoff>(arguments_.payoff);

        //QuantLib requires sigma * sqrt(T) rather than just sigma/volatility
        Real vol = volatility();

        //calculate dividend discount factor assuming continuous compounding (e^-rt)
        DiscountFactor growth = dividendDiscount(t1);
        //calculate payoff discount factor assuming continuous compounding
        DiscountFactor discount = riskFreeDiscount(t1);
        Real result = 0;
        Real minusInf=-std::numeric_limits<Real>::infinity();

        Real y1 = this->y1(payoff->optionType()),
             y2 = this->y2(payoff->optionType());
        if (payoff->optionType() == Option::Call) {
            //instantiate payoff function for a call
            boost::shared_ptr<PlainVanillaPayoff> vanillaCallPayoff =
                boost::make_shared<PlainVanillaPayoff>(Option::Call, X1);
            Real BSM = BlackScholesCalculator(vanillaCallPayoff, S, growth, vol*sqrt(t1), discount).value();
            result = BSM
                + S*exp((b - r)*T2)*M2(y1, y2, minusInf, z1, rho)
                - X2*exp(-r*T2)*M2(y1 - vol*sqrt(t1), y2 - vol*sqrt(t1), minusInf, z1 - vol*sqrt(T2), rho)
                - S*exp((b - r)*t1)*N2(y1, z2) + X1*exp(-r*t1)*N2(y1 - vol*sqrt(t1), z2 - vol*sqrt(t1))
                - A*exp(-r*t1)*N2(y1 - vol*sqrt(t1), y2 - vol*sqrt(t1));
        } else {
            //instantiate payoff function for a call
            boost::shared_ptr<PlainVanillaPayoff> vanillaPutPayoff =
                boost::make_shared<PlainVanillaPayoff>(Option::Put, X1);
            result = BlackScholesCalculator(vanillaPutPayoff, S, growth, vol*sqrt(t1), discount).value()
                - S*exp((b - r)*T2)*M2(y1, y2, minusInf, -z1, rho)
                + X2*exp(-r*T2)*M2(y1 - vol*sqrt(t1), y2 - vol*sqrt(t1), minusInf, -z1 + vol*sqrt(T2), rho)
                + S*exp((b - r)*t1)*N2(z2, y2) - X1*exp(-r*t1)*N2(z2 - vol*sqrt(t1), y2 - vol*sqrt(t1))
                - A*exp(-r*t1)*N2(y1 - vol*sqrt(t1), y2 - vol*sqrt(t1));
        }
        this->results_.value = result;
    }

    Real AnalyticHolderExtensibleOptionEngine::I1Call() const {
        Real Sv = process_->x0();
        Real A = arguments_.premium;

        if(A==0)
        {
            return 0;
        }
        else
        {
            BlackScholesCalculator bs = bsCalculator(Sv, Option::Call);
            Real ci = bs.value();
            Real dc = bs.delta();

            Real yi = ci - A;
            //da/ds = 0
            Real di = dc - 0;
            Real epsilon = 0.001;

            //Newton-Raphson process
            while (std::fabs(yi) > epsilon){
                Sv = Sv - yi / di;

                bs = bsCalculator(Sv, Option::Call);
                ci = bs.value();
                dc = bs.delta();

                yi = ci - A;
                di = dc - 0;
            }
            return Sv;
        }
    }

    Real AnalyticHolderExtensibleOptionEngine::I2Call() const {
        Real Sv = process_->x0();
        Real X1 = strike();
        Real X2 = arguments_.secondStrike;
        Real A = arguments_.premium;
        Time T2 = secondExpiryTime();
        Time t1 = firstExpiryTime();
        Real r=riskFreeRate();

        Real val=X1-X2*std::exp(-r*(T2-t1));
        if(A< val){
            return std::numeric_limits<Real>::infinity();
        } else {
            BlackScholesCalculator bs = bsCalculator(Sv, Option::Call);
            Real ci = bs.value();
            Real dc = bs.delta();

            Real yi = ci - A - Sv + X1;
            //da/ds = 1
            Real di = dc - 1;
            Real epsilon = 0.001;

            //Newton-Raphson process
            while (std::fabs(yi) > epsilon){
                Sv = Sv - yi / di;

                bs = bsCalculator(Sv, Option::Call);
                ci = bs.value();
                dc = bs.delta();

                yi = ci - A - Sv + X1;
                di = dc - 1;
            }
            return Sv;
        }
    }

    Real AnalyticHolderExtensibleOptionEngine::I1Put() const {
        Real Sv = process_->x0();
        //Srtike
        Real X1 = strike();
        //Premium
        Real A = arguments_.premium;

        BlackScholesCalculator bs = bsCalculator(Sv, Option::Put);
        Real pi = bs.value();
        Real dc = bs.delta();

        Real yi = pi - A + Sv - X1;
        //da/ds = 1
        Real di = dc - 1;
        Real epsilon = 0.001;

        //Newton-Raphson prosess
        while (std::fabs(yi) > epsilon){
            Sv = Sv - yi / di;

            bs = bsCalculator(Sv, Option::Put);
            pi = bs.value();
            dc = bs.delta();

            yi = pi - A + Sv - X1;
            di = dc - 1;
        }
        return Sv;
    }

    Real AnalyticHolderExtensibleOptionEngine::I2Put() const {
        Real Sv = process_->x0();
        Real A = arguments_.premium;
        if(A==0){
            return std::numeric_limits<Real>::infinity();
        }
        else{
            BlackScholesCalculator bs = bsCalculator(Sv, Option::Put);
            Real pi = bs.value();
            Real dc = bs.delta();

            Real yi = pi - A;
            //da/ds = 0
            Real di = dc - 0;
            Real epsilon = 0.001;

            //Newton-Raphson prosess
            while (std::fabs(yi) > epsilon){
                Sv = Sv - yi / di;

                bs = bsCalculator(Sv, Option::Put);
                pi = bs.value();
                dc = bs.delta();

                yi = pi - A;
                di = dc - 0;
            }
            return Sv;
        }
    }

    BlackScholesCalculator AnalyticHolderExtensibleOptionEngine::bsCalculator(
                                    Real spot, Option::Type optionType) const {
        //Real spot = process_->x0();
        Real vol;
        DiscountFactor growth;
        DiscountFactor discount;
        Real X2 = arguments_.secondStrike;
        Time T2 = secondExpiryTime();
        Time t1 = firstExpiryTime();
        Time t = T2 - t1;

        //payoff
        boost::shared_ptr<PlainVanillaPayoff > vanillaPayoff =
            boost::make_shared<PlainVanillaPayoff>(optionType, X2);

        //QuantLib requires sigma * sqrt(T) rather than just sigma/volatility
        vol = volatility() * std::sqrt(t);
        //calculate dividend discount factor assuming continuous compounding (e^-rt)
        growth = dividendDiscount(t);
        //calculate payoff discount factor assuming continuous compounding
        discount = riskFreeDiscount(t);

        BlackScholesCalculator bs(vanillaPayoff, spot, growth, vol, discount);
        return bs;
    }

    Real AnalyticHolderExtensibleOptionEngine::M2(Real a, Real b, Real c, Real d, Real rho) const {
        BivariateCumulativeNormalDistributionDr78 CmlNormDist(rho);
        return CmlNormDist(b, d) - CmlNormDist(a, d) - CmlNormDist(b, c) + CmlNormDist(a,c);
    }

    Real AnalyticHolderExtensibleOptionEngine::N2(Real a, Real b) const {
        CumulativeNormalDistribution  NormDist;
        return NormDist(b) - NormDist(a);
    }

    Real AnalyticHolderExtensibleOptionEngine::strike() const {
        boost::shared_ptr<PlainVanillaPayoff> payoff =
            boost::dynamic_pointer_cast<PlainVanillaPayoff>(arguments_.payoff);
        QL_REQUIRE(payoff, "non-plain payoff given");
        return payoff->strike();
    }

    Time AnalyticHolderExtensibleOptionEngine::firstExpiryTime() const {
        return process_->time(arguments_.exercise->lastDate());
    }

    Time AnalyticHolderExtensibleOptionEngine::secondExpiryTime() const {
        return process_->time(arguments_.secondExpiryDate);
    }

    Volatility AnalyticHolderExtensibleOptionEngine::volatility() const {
        return process_->blackVolatility()->blackVol(firstExpiryTime(), strike());
    }
    Rate AnalyticHolderExtensibleOptionEngine::riskFreeRate() const {
        return process_->riskFreeRate()->zeroRate(firstExpiryTime(), Continuous,
            NoFrequency);
    }
    Rate AnalyticHolderExtensibleOptionEngine::dividendYield() const {
        return process_->dividendYield()->zeroRate(firstExpiryTime(),
            Continuous, NoFrequency);
    }

    DiscountFactor AnalyticHolderExtensibleOptionEngine::dividendDiscount(Time t) const {
        return process_->dividendYield()->discount(t);
    }

    DiscountFactor AnalyticHolderExtensibleOptionEngine::riskFreeDiscount(Time t) const {
        return process_->riskFreeRate()->discount(t);
    }

    Real AnalyticHolderExtensibleOptionEngine::y1(Option::Type type) const {
        Real S = process_->x0();
        Real I2 = (type == Option::Call) ? I2Call() : I2Put();

        Real b = riskFreeRate() - dividendYield();
        Real vol = volatility();
        Time t1 = firstExpiryTime();

        return (log(S / I2) + (b + pow(vol, 2) / 2)*t1) / (vol*sqrt(t1));
    }

    Real AnalyticHolderExtensibleOptionEngine::y2(Option::Type type) const {
        Real S = process_->x0();
        Real I1 = (type == Option::Call) ? I1Call() : I1Put();

        Real b = riskFreeRate() - dividendYield();
        Real vol = volatility();
        Time t1 = firstExpiryTime();

        return (log(S / I1) + (b + pow(vol, 2) / 2)*t1) / (vol*sqrt(t1));
    }

    Real AnalyticHolderExtensibleOptionEngine::z1() const {
        Real S = process_->x0();
        Real X2 = arguments_.secondStrike;
        Real b = riskFreeRate() - dividendYield();
        Real vol = volatility();
        Time T2 = secondExpiryTime();

        return (log(S / X2) + (b + pow(vol, 2) / 2)*T2) / (vol*sqrt(T2));
    }

    Real AnalyticHolderExtensibleOptionEngine::z2() const {
        Real S = process_->x0();
        Real X1 = strike();

        Real b = riskFreeRate() - dividendYield();
        Real vol = volatility();
        Time t1 = firstExpiryTime();

        return (log(S / X1) + (b + pow(vol, 2) / 2)*t1) / (vol*sqrt(t1));
    }

}


// Boost.Geometry

// Copyright (c) 2015-2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_EXPAND_BY_EPSILON_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_EXPAND_BY_EPSILON_HPP

#include <algorithm>
#include <cstddef>
#include <type_traits>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/coordinate_type.hpp>

#include <boost/geometry/util/math.hpp>

#include <boost/geometry/views/detail/indexed_point_view.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace expand
{

template
<
    typename Point,
    template <typename> class PlusOrMinus,
    std::size_t I = 0,
    std::size_t D = dimension<Point>::value
>
struct corner_by_epsilon
{
    static inline void apply(Point & point)
    {
        typedef typename coordinate_type<Point>::type coord_type;
        coord_type const coord = get<I>(point);
        coord_type const seps = math::scaled_epsilon(coord);

        set<I>(point, PlusOrMinus<coord_type>()(coord, seps));

        corner_by_epsilon<Point, PlusOrMinus, I+1>::apply(point);
    }

    static inline void apply(Point & point,
                             typename coordinate_type<Point>::type const& eps)
    {
        typedef typename coordinate_type<Point>::type coord_type;
        coord_type const coord = get<I>(point);
        coord_type const seps = math::scaled_epsilon(coord, eps);

        set<I>(point, PlusOrMinus<coord_type>()(coord, seps));

        corner_by_epsilon<Point, PlusOrMinus, I + 1>::apply(point);
    }
};

template
<
    typename Point,
    template <typename> class PlusOrMinus,
    std::size_t D
>
struct corner_by_epsilon<Point, PlusOrMinus, D, D>
{
    static inline void apply(Point const&) {}
    static inline void apply(Point const&, typename coordinate_type<Point>::type const&) {}
};

template
<
    typename Box,
    bool Enable = ! std::is_integral<typename coordinate_type<Box>::type>::value
>
struct expand_by_epsilon
{
    static inline void apply(Box & box)
    {
        typedef detail::indexed_point_view<Box, min_corner> min_type;
        min_type min_point(box);
        corner_by_epsilon<min_type, std::minus>::apply(min_point);

        typedef detail::indexed_point_view<Box, max_corner> max_type;
        max_type max_point(box);
        corner_by_epsilon<max_type, std::plus>::apply(max_point);
    }

    static inline void apply(Box & box,
                             typename coordinate_type<Box>::type const& eps)
    {
        typedef detail::indexed_point_view<Box, min_corner> min_type;
        min_type min_point(box);
        corner_by_epsilon<min_type, std::minus>::apply(min_point, eps);

        typedef detail::indexed_point_view<Box, max_corner> max_type;
        max_type max_point(box);
        corner_by_epsilon<max_type, std::plus>::apply(max_point, eps);
    }
};

template <typename Box>
struct expand_by_epsilon<Box, false>
{
    static inline void apply(Box &) {}
    static inline void apply(Box &, typename coordinate_type<Box>::type const&) {}
};

} // namespace expand

template <typename Box>
inline void expand_by_epsilon(Box & box)
{
    expand::expand_by_epsilon<Box>::apply(box);
}

template <typename Box>
inline void expand_by_epsilon(Box & box,
                              typename coordinate_type<Box>::type const& eps)
{
    expand::expand_by_epsilon<Box>::apply(box, eps);
}

} // namespace detail
#endif // DOXYGEN_NO_DETAIL

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_EXPAND_BY_EPSILON_HPP


#include <cstdint>
#include <cmath>
#include <array>
#include <iostream>
#include <fstream>
#include <algorithm>
#include <iterator>
#include <charconv>
#include <random>
#include <boost/container/flat_map.hpp>
#include <boost/program_options.hpp>
#include <omp.h>
#include <nlohmann/json.hpp>
#include <OpenImageIO/imageio.h>

#define BRDF_SAMPLING_RES_THETA_H       90
#define BRDF_SAMPLING_RES_THETA_D       90
#define BRDF_SAMPLING_RES_PHI_D         360

#define RED_SCALE (1.0/1500.0)
#define GREEN_SCALE (1.15/1500.0)
#define BLUE_SCALE (1.66/1500.0)

// Read BRDF data
bool read_brdf(const char *filename, double* &brdf)
{
	FILE *f = fopen(filename, "rb");
	if (!f)
		return false;

	int dims[3];
	fread(dims, sizeof(int), 3, f);
	int n = dims[0] * dims[1] * dims[2];
	if (n != BRDF_SAMPLING_RES_THETA_H *
		 BRDF_SAMPLING_RES_THETA_D *
		 BRDF_SAMPLING_RES_PHI_D / 2)
	{
		fprintf(stderr, "Dimensions don't match\n");
		fclose(f);
		return false;
	}

	brdf = (double*) malloc (sizeof(double)*3*n);
	fread(brdf, sizeof(double), 3*n, f);

	fclose(f);
	return true;
}

// Lookup theta_half index
// This is a non-linear mapping!
// In:  [0 .. pi/2]
// Out: [0 .. 89]
inline int theta_half_index(double theta_half)
{
	if (theta_half <= 0.0)
		return 0;
	double theta_half_deg = ((theta_half / (M_PI/2.0))*BRDF_SAMPLING_RES_THETA_H);
	double temp = theta_half_deg*BRDF_SAMPLING_RES_THETA_H;
	temp = sqrt(temp);
	int ret_val = (int)temp;
	if (ret_val < 0) ret_val = 0;
	if (ret_val >= BRDF_SAMPLING_RES_THETA_H)
		ret_val = BRDF_SAMPLING_RES_THETA_H-1;
	return ret_val;
}

inline int theta_diff_index(double theta_diff)
{
	int tmp = int(theta_diff / (M_PI * 0.5) * BRDF_SAMPLING_RES_THETA_D);
	if (tmp < 0)
		return 0;
	else if (tmp < BRDF_SAMPLING_RES_THETA_D - 1)
		return tmp;
	else
		return BRDF_SAMPLING_RES_THETA_D - 1;
}

inline int phi_diff_index(double phi_diff)
{
	// Because of reciprocity, the BRDF is unchanged under
	// phi_diff -> phi_diff + M_PI
	if (phi_diff < 0.0)
		phi_diff += M_PI;

	// In: phi_diff in [0 .. pi]
	// Out: tmp in [0 .. 179]
	int tmp = int(phi_diff / M_PI * BRDF_SAMPLING_RES_PHI_D / 2);
	if (tmp < 0)
		return 0;
	else if (tmp < BRDF_SAMPLING_RES_PHI_D / 2 - 1)
		return tmp;
	else
		return BRDF_SAMPLING_RES_PHI_D / 2 - 1;
}

double gamma( double v ) {
  return v / (v + 0.155 ) * 1.019;
}

int main( int argc, char *argv[] ) {
  boost::program_options::options_description options("オプション");
  options.add_options()
    ("help,h",    "ヘルプを表示")
    ("input,i", boost::program_options::value<std::string>(), "入力ファイル")
    ("output,o", boost::program_options::value<std::string>(), "出力ファイル");
  boost::program_options::variables_map params;
  boost::program_options::store( boost::program_options::parse_command_line( argc, argv, options ), params );
  boost::program_options::notify( params );
  if( params.count("help") || !params.count("input") || !params.count("output") ) {
    std::cout << options << std::endl;
    return !params.count( "help" );
  }
  double* brdf;
  if ( !read_brdf( params[ "input" ].as< std::string >().c_str(), brdf ) ) {
    std::cerr <<  "Unable to read " << params[ "input" ].as< std::string >() << std::endl;
    return 1;
  }
  double max = 0.0;
  std::vector< float > v;
  v.reserve( BRDF_SAMPLING_RES_THETA_D * BRDF_SAMPLING_RES_THETA_H * 3 );
  double sum = 0.0;
  for( unsigned int i = 0; i != BRDF_SAMPLING_RES_THETA_D; ++i ) {
    double theta_diff = double( BRDF_SAMPLING_RES_THETA_D - i - 1 ) / double( BRDF_SAMPLING_RES_THETA_D ) * M_PI / 2.0;
    for( unsigned int j = 0; j != BRDF_SAMPLING_RES_THETA_H; ++j ) {
      double theta_half = double( j ) / double( BRDF_SAMPLING_RES_THETA_H ) * M_PI / 2.0;
      int ind = phi_diff_index(M_PI/2) +
        theta_diff_index(theta_diff) * BRDF_SAMPLING_RES_PHI_D / 2 +
        theta_half_index(theta_half) * BRDF_SAMPLING_RES_PHI_D / 2 *
        BRDF_SAMPLING_RES_THETA_D;
      auto red_val = gamma( std::max( brdf[ind] * RED_SCALE, 0.0 ) );
      auto green_val = gamma( std::max( brdf[ind + BRDF_SAMPLING_RES_THETA_H*BRDF_SAMPLING_RES_THETA_D*BRDF_SAMPLING_RES_PHI_D/2] * GREEN_SCALE, 0.0 ) );
      auto blue_val = gamma( std::max( brdf[ind + BRDF_SAMPLING_RES_THETA_H*BRDF_SAMPLING_RES_THETA_D*BRDF_SAMPLING_RES_PHI_D] * BLUE_SCALE, 0.0 ) );
      max = std::max( std::max( std::max( max, red_val ), green_val ), blue_val );
      sum += red_val + green_val + blue_val;
      v.push_back( red_val );
      v.push_back( green_val );
      v.push_back( blue_val );
    }
  }
  double average = sum / ( BRDF_SAMPLING_RES_THETA_D * BRDF_SAMPLING_RES_THETA_H * 3 );
  std::vector< uint8_t > image;
  image.reserve( BRDF_SAMPLING_RES_THETA_D * BRDF_SAMPLING_RES_THETA_H * 3 );
  for( unsigned int i = 0; i != BRDF_SAMPLING_RES_THETA_D; ++i ) {
    for( unsigned int j = 0; j != BRDF_SAMPLING_RES_THETA_H; ++j ) {
      int index = i * BRDF_SAMPLING_RES_THETA_H * 3 + j * 3;
      std::cout << v[ index ]/(average*2) << " " << v[ index + 1 ]/(average*2) << " " << v[ index + 2 ]/(average*2) << std::endl;
      image.push_back( uint8_t( std::min( std::max( v[ index ]/(average*2), 0.0  ), 1.0 ) * 255.0 ) );
      image.push_back( uint8_t( std::min( std::max( v[ index + 1 ]/(average*2), 0.0  ), 1.0 ) * 255.0 ) );
      image.push_back( uint8_t( std::min( std::max( v[ index + 2 ]/(average*2), 0.0  ), 1.0 ) * 255.0 ) );
    }
  }
  using namespace OIIO_NAMESPACE;
  ImageOutput *out = ImageOutput::create( params[ "output" ].as< std::string >() );
  if ( !out ) {
    std::cerr << "Unable to open output file" << std::endl;
    return -1;
  }
  ImageSpec spec ( BRDF_SAMPLING_RES_THETA_H, BRDF_SAMPLING_RES_THETA_D, 3, TypeDesc::UINT8);
  out->open( params[ "output" ].as< std::string >(), spec );
  out->write_image( TypeDesc::UINT8, image.data() );
  out->close();
  return 0;
}


// Copyright Alexander Nasonov & Paul A. Bristow 2006.

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

#ifndef BOOST_DETAIL_LCAST_PRECISION_HPP_INCLUDED
#define BOOST_DETAIL_LCAST_PRECISION_HPP_INCLUDED

#include <climits>
#include <ios>
#include <limits>

#include <boost/config.hpp>
#include <boost/integer_traits.hpp>

#ifndef BOOST_NO_IS_ABSTRACT
// Fix for SF:1358600 - lexical_cast & pure virtual functions & VC 8 STL
#include <boost/type_traits/conditional.hpp>
#include <boost/type_traits/is_abstract.hpp>
#endif

#if defined(BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS) || \
  (defined(BOOST_MSVC) && (BOOST_MSVC<1310))

#define BOOST_LCAST_NO_COMPILE_TIME_PRECISION
#endif

#ifdef BOOST_LCAST_NO_COMPILE_TIME_PRECISION
#include <boost/assert.hpp>
#else
#include <boost/static_assert.hpp>
#endif

namespace boost { namespace detail {

class lcast_abstract_stub {};

#ifndef BOOST_LCAST_NO_COMPILE_TIME_PRECISION
// Calculate an argument to pass to std::ios_base::precision from
// lexical_cast. See alternative implementation for broken standard
// libraries in lcast_get_precision below. Keep them in sync, please.
template<class T>
struct lcast_precision
{
#ifdef BOOST_NO_IS_ABSTRACT
    typedef std::numeric_limits<T> limits; // No fix for SF:1358600.
#else
    typedef BOOST_DEDUCED_TYPENAME boost::conditional<
        boost::is_abstract<T>::value
      , std::numeric_limits<lcast_abstract_stub>
      , std::numeric_limits<T>
      >::type limits;
#endif

    BOOST_STATIC_CONSTANT(bool, use_default_precision =
            !limits::is_specialized || limits::is_exact
        );

    BOOST_STATIC_CONSTANT(bool, is_specialized_bin =
            !use_default_precision &&
            limits::radix == 2 && limits::digits > 0
        );

    BOOST_STATIC_CONSTANT(bool, is_specialized_dec =
            !use_default_precision &&
            limits::radix == 10 && limits::digits10 > 0
        );

    BOOST_STATIC_CONSTANT(std::streamsize, streamsize_max =
            boost::integer_traits<std::streamsize>::const_max
        );

    BOOST_STATIC_CONSTANT(unsigned int, precision_dec = limits::digits10 + 1U);

    BOOST_STATIC_ASSERT(!is_specialized_dec ||
            precision_dec <= streamsize_max + 0UL
        );

    BOOST_STATIC_CONSTANT(unsigned long, precision_bin =
            2UL + limits::digits * 30103UL / 100000UL
        );

    BOOST_STATIC_ASSERT(!is_specialized_bin ||
            (limits::digits + 0UL < ULONG_MAX / 30103UL &&
            precision_bin > limits::digits10 + 0UL &&
            precision_bin <= streamsize_max + 0UL)
        );

    BOOST_STATIC_CONSTANT(std::streamsize, value =
            is_specialized_bin ? precision_bin
                               : is_specialized_dec ? precision_dec : 6
        );
};
#endif

template<class T>
inline std::streamsize lcast_get_precision(T* = 0)
{
#ifndef BOOST_LCAST_NO_COMPILE_TIME_PRECISION
    return lcast_precision<T>::value;
#else // Follow lcast_precision algorithm at run-time:

#ifdef BOOST_NO_IS_ABSTRACT
    typedef std::numeric_limits<T> limits; // No fix for SF:1358600.
#else
    typedef BOOST_DEDUCED_TYPENAME boost::conditional<
        boost::is_abstract<T>::value
      , std::numeric_limits<lcast_abstract_stub>
      , std::numeric_limits<T>
      >::type limits;
#endif

    bool const use_default_precision =
        !limits::is_specialized || limits::is_exact;

    if(!use_default_precision)
    { // Includes all built-in floating-point types, float, double ...
      // and UDT types for which digits (significand bits) is defined (not zero)

        bool const is_specialized_bin =
            limits::radix == 2 && limits::digits > 0;
        bool const is_specialized_dec =
            limits::radix == 10 && limits::digits10 > 0;
        std::streamsize const streamsize_max =
            (boost::integer_traits<std::streamsize>::max)();
        (void)streamsize_max;

        if(is_specialized_bin)
        { // Floating-point types with
          // limits::digits defined by the specialization.

            unsigned long const digits = limits::digits;
            unsigned long const precision = 2UL + digits * 30103UL / 100000UL;
            // unsigned long is selected because it is at least 32-bits
            // and thus ULONG_MAX / 30103UL is big enough for all types.
            BOOST_ASSERT(
                    digits < ULONG_MAX / 30103UL &&
                    precision > limits::digits10 + 0UL &&
                    precision <= streamsize_max + 0UL
                );
            return precision;
        }
        else if(is_specialized_dec)
        {   // Decimal Floating-point type, most likely a User Defined Type
            // rather than a real floating-point hardware type.
            unsigned int const precision = limits::digits10 + 1U;
            BOOST_ASSERT(precision <= streamsize_max + 0UL);
            return precision;
        }
    }

    // Integral type (for which precision has no effect)
    // or type T for which limits is NOT specialized,
    // so assume stream precision remains the default 6 decimal digits.
    // Warning: if your User-defined Floating-point type T is NOT specialized,
    // then you may lose accuracy by only using 6 decimal digits.
    // To avoid this, you need to specialize T with either
    // radix == 2 and digits == the number of significand bits,
    // OR
    // radix = 10 and digits10 == the number of decimal digits.

    return 6;
#endif
}

template<class T>
inline void lcast_set_precision(std::ios_base& stream, T*)
{
    stream.precision(lcast_get_precision<T>());
}

template<class Source, class Target>
inline void lcast_set_precision(std::ios_base& stream, Source*, Target*)
{
    std::streamsize const s = lcast_get_precision(static_cast<Source*>(0));
    std::streamsize const t = lcast_get_precision(static_cast<Target*>(0));
    stream.precision(s > t ? s : t);
}

}}

#endif //  BOOST_DETAIL_LCAST_PRECISION_HPP_INCLUDED


//
//		HSP3.0 plugin sample
//		onion software/onitama 2004/9
//

#define WIN32_LEAN_AND_MEAN		// Exclude rarely-used stuff from Windows headers
#define NOMINMAX
#include <Windows.h>
#include <cstdio>
#include <cstdlib>
#include <unordered_map>
#include "../HSP_Includes/hsp3plugin.h"
#include "Boost-FixedPoint-Core.h"
#include "Boost-FixedPoint.h"

#include <boost/multiprecision/cpp_int.hpp>
/*------------------------------------------------------------*/
/*
controller
*/
/*------------------------------------------------------------*/
static boost_fixedpoint ref_fval;						// 返値のための変数
static double dp1;
extern int N;

static void *reffunc(int *type_res, int cmd)
{
	//		関数・システム変数の実行処理 (値の参照時に呼ばれます)
	//
	//			'('で始まるかを調べる
	//
	if (*type != TYPE_MARK) puterror(HSPERR_INVALID_FUNCPARAM);
	if (*val != '(') puterror(HSPERR_INVALID_FUNCPARAM);
	code_next();
	if (cmd >= 0) {
		N = cmd;
							// 整数値を取得(デフォルトなし)
		int cp = code_getprm();
		switch (cp) {
		case PARAM_DEFAULT:
		case PARAM_END:
		case PARAM_ENDSPLIT:
			ref_fval = boost_fixedpoint();
			break;
		default:
			switch (mpval->flag) {
			case 2:
				ref_fval = boost_fixedpoint((char*)mpval->pt);
				break;
			case 3:
				ref_fval = boost_fixedpoint(*(double*)mpval->pt);
				break;
			case 4:
				ref_fval = boost_fixedpoint(*(int*)mpval->pt);
				break;
			default:
				ref_fval = boost_fixedpoint(0);
			}
		}
						// 返値を設定
	}else{
		puterror(HSPERR_UNSUPPORTED_FUNCTION);
	}

	//			'('で終わるかを調べる
	//
	if (*type != TYPE_MARK) puterror(HSPERR_INVALID_FUNCPARAM);
	if (*val != ')') puterror(HSPERR_INVALID_FUNCPARAM);
	code_next();

	*type_res = HspVarBoost_Fixedpoint_typeid();		// 返値のタイプを指定する
	return (void *)&ref_fval;
}

/*------------------------------------------------------------*/

static int termfunc(int option)
{
	//		終了処理 (アプリケーション終了時に呼ばれます)
	//
	return 0;
}

/*------------------------------------------------------------*/
/*
interface
*/
/*------------------------------------------------------------*/

int WINAPI DllMain(HINSTANCE hInstance, DWORD fdwReason, PVOID pvReserved)
{
	//		DLLエントリー (何もする必要はありません)
	//
	return TRUE;
}

EXPORT void WINAPI hsp3cmdinit(HSP3TYPEINFO *info)
{
	//		プラグイン初期化 (実行・終了処理を登録します)
	//
	hsp3sdk_init(info);		// SDKの初期化(最初に行なって下さい)

	info->reffunc = reffunc;		// 参照関数(reffunc)の登録
	info->termfunc = termfunc;		// 終了関数(termfunc)の登録

	registvar(-1, HspVarBoost_Fixedpoint_Init);		// 新しい型の追加
}

/*------------------------------------------------------------*/


/* vim: set sw=4 sts=4 et foldmethod=syntax : */

#include <gcs/constraints/linear_equality.hh>
#include <gcs/problem.hh>
#include <gcs/solve.hh>

#include <cstdlib>
#include <iostream>
#include <string>
#include <vector>

#include <boost/program_options.hpp>

using namespace gcs;

using std::cerr;
using std::cout;
using std::endl;
using std::string;
using std::to_string;
using std::pair;
using std::vector;

using namespace std::literals::string_literals;

namespace po = boost::program_options;

auto main(int argc, char * argv[]) -> int
{
    po::options_description display_options{ "Program options" };
    display_options.add_options()
        ("help", "Display help information")
        ("prove", "Create a proof");

    po::options_description all_options{ "All options" };

    all_options.add(display_options);

    po::variables_map options_vars;

    try {
        po::store(po::command_line_parser(argc, argv)
                .options(all_options)
                .run(), options_vars);
        po::notify(options_vars);
    }
    catch (const po::error & e) {
        cerr << "Error: " << e.what() << endl;
        cerr << "Try " << argv[0] << " --help" << endl;
        return EXIT_FAILURE;
    }

    if (options_vars.count("help")) {
        cout << "Usage: " << argv[0] << " [options]" << endl;
        cout << endl;
        cout << display_options << endl;
        return EXIT_SUCCESS;
    }

    Problem p = options_vars.count("prove") ? Problem{ Proof{ "odd_even_sum.opb", "odd_even_sum.veripb" } } : Problem{ };

    auto a = p.create_integer_variable(0_i, 5_i, "a");
    auto b = p.create_integer_variable(0_i, 5_i, "b");
    auto c = p.create_integer_variable(0_i, 5_i, "c");
    auto d = p.create_integer_variable(0_i, 10_i, "d");
    auto e = p.create_integer_variable(0_i, 2_i, "e");

    p.post(LinearEquality{ Linear{ { 2_i, a }, { 2_i, b }, { 2_i, c }, { -2_i, d }, { 1_i, e } }, 1_i, true });

    auto stats = solve(p, [&] (const State & s) -> bool {
            cout << s(a) << " " << s(b) << " " << s(c) << " " << s(d) << " " << s(e) << endl;
            return true;
            });

    cout << stats;

    return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_MULS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_MULS_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/muls.hpp>
#include <boost/simd/include/functions/scalar/genmask.hpp>
#include <boost/simd/include/functions/scalar/abs.hpp>
#include <boost/simd/include/functions/scalar/bitofsign.hpp>
#include <boost/simd/include/functions/scalar/bitwise_xor.hpp>
#include <boost/simd/include/functions/scalar/saturate.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/dispatch/meta/upgrade.hpp>
#include <boost/dispatch/meta/as_unsigned.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( muls_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)(scalar_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return a0*a1;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( muls_, tag::cpu_
                                    , (A0)
                                    , (scalar_< unsigned_<A0> >)(scalar_< unsigned_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type utype;
      utype res = utype(a0)*utype(a1);
      return static_cast<A0>(res) | genmask(static_cast<A0>(res >> sizeof(A0)*CHAR_BIT));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( muls_, tag::cpu_
                                    , (A0)
                                    , (scalar_< integer_<A0> >)(scalar_< integer_<A0> >)
                                    )
  {

    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::upgrade<A0>::type uptype;
      return A0(saturate<A0>(static_cast<uptype>(a0)*static_cast<uptype>(a1)));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( muls_, tag::cpu_
                                    , (A0)
                                    , (scalar_< int32_<A0> >)(scalar_< int32_<A0> >)
                                    )
  {

    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_unsigned<A0>::type untype;
      typedef typename dispatch::meta::upgrade<A0>::type utype;

      utype res = utype(a0)*utype(a1);
      untype res2 = (untype(a0 ^ a1) >> (sizeof(A0)*CHAR_BIT-1)) + Valmax<A0>();

      A0 hi = (res >> sizeof(A0)*CHAR_BIT);
      A0 lo = res;

      if(hi != (lo >> (sizeof(A0)*CHAR_BIT-1)))
        res = res2;

      return res;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( muls_, tag::cpu_
                                    , (A0)
                                    , (scalar_< uint64_<A0> >)(scalar_< uint64_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (a1 == 0 || a0 == 0) return Zero<A0>();
      if (a1 >= a0)
        return aux(a0, a1);
      else
        return aux(a1, a0);
    }
  private :
    static BOOST_FORCEINLINE A0 aux(const A0& mini,  const A0& maxi)
    {
      A0 z = Valmax<A0>()/maxi;
      return (z < mini) ? Valmax<A0>() : mini*maxi;
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( muls_, tag::cpu_
                                    , (A0)
                                    , (scalar_< int64_<A0> >)(scalar_< int64_<A0> >)
                                    )
  {
    typedef A0 result_type;
    typedef typename dispatch::meta::as_unsigned<result_type>::type untype;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      if (a1 == 0 || a0 == 0) return Zero<result_type>();
      result_type sign =  b_xor(bitofsign(a0), bitofsign(a1));
      untype aa0 = boost::simd::abs(a0);
      untype aa1 = boost::simd::abs(a1);
      if (aa1 >= aa0)
        return aux(a0, a1, sign, aa0, aa1);
      else
        return aux(a1, a0, sign, aa1, aa0);
    }
  private :
    static BOOST_FORCEINLINE A0 aux(const A0& mini,  const A0& maxi,
                         const A0& sign,
                         const untype& amini, const untype& amaxi)
    {
        untype z = Valmax<result_type>()/amaxi;
        return (z < amini)
          ? ( sign ? Valmin<result_type>():Valmax<result_type>())
          : mini*maxi;
    }
  };
} } }

#endif


/*  Copyright (c) 2013, Abdullah A. Hassan <voodooattack@hotmail.com>
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
 *
 *  1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
 *
 *  2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided with the distribution.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *  INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 *  IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
 *  OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 *  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef NOISEMODULES_H
#define NOISEMODULES_H

#include "config.hpp"

#ifdef NOISE_DIR_IS_LIBNOISE
#include <libnoise/noise.h>
#else
#include <noise/noise.h>
#endif

#include <boost/multi_array.hpp>

namespace ADWIF
{
  class HeightMapModule: public noise::module::Module
  {
  public:
    inline static double cubicInterpolate (const double p[4], double x) {
      return p[1] + 0.5 * x*(p[2] - p[0] + x*(2.0*p[0] - 5.0*p[1] +
      4.0*p[2] - p[3] + x*(3.0*(p[1] - p[2]) + p[3] - p[0])));
    }

    inline static double bicubicInterpolate (const double p[4][4], double x, double y) {
      double arr[4];
      arr[0] = cubicInterpolate(p[0], y);
      arr[1] = cubicInterpolate(p[1], y);
      arr[2] = cubicInterpolate(p[2], y);
      arr[3] = cubicInterpolate(p[3], y);
      return cubicInterpolate(arr, x);
    }

    HeightMapModule(const boost::multi_array<double, 2> & heightMap,
                    int cellSizeX, int cellSizeY): Module(0), myHeightmap(heightMap),
                    myCellSizeX(cellSizeX), myCellSizeY(cellSizeY) { }

    virtual int GetSourceModuleCount() const { return 0; }

    virtual double GetValue(double x, double y, double z) const;

    int myCellSizeX, myCellSizeY;
    const boost::multi_array<double, 2> myHeightmap;
  };

}

#endif // NOISEMODULES_H


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

/*
 Copyright (C) 2015 Peter Caspers
 Copyright (C) 2015 Roland Lichters

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

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

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

#include <ql/experimental/models/betaetacore.hpp>

#include <ql/errors.hpp>
#include <ql/math/modifiedbessel.hpp>
#include <ql/math/integrals/gausslobattointegral.hpp>
#include <ql/math/integrals/segmentintegral.hpp>
#include <ql/math/interpolations/bilinearinterpolation.hpp>
#include <ql/math/interpolations/bicubicsplineinterpolation.hpp>
#include <ql/methods/finitedifferences/meshers/concentrating1dmesher.hpp>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/make_shared.hpp>

#include <iostream>

namespace QuantLib {

BetaEtaCore::BetaEtaCore(const Array &times, const Array &alpha,
                         const Array &kappa, const Real &beta, const Real &eta)
    : times_(times), alpha_(alpha), kappa_(kappa), beta_(beta), eta_(eta),
      ghPoints_(8), prob_y_0_cutoff(1E-6), kappa_cutoff(1E-6) {

    QL_REQUIRE(beta > 0.0, "beta (" << beta << ") must be positive");
    QL_REQUIRE(eta >= 0.0 && eta <= 1.0, " eta (" << eta
                                                  << ") must be in [0,1]");
    QL_REQUIRE(alpha.size() == times.size() + 1,
               "alpha size (" << alpha.size()
                              << ") must be equal to times size ("
                              << times.size() << ") plus one");
    QL_REQUIRE(kappa.size() == 1 || kappa.size() == times.size() + 1,
               "kappa size (" << kappa.size()
                              << ") must be equal to times size ("
                              << times.size() << ") plus one or equal to one");
    for (Size i = 0; i < times.size(); ++i) {
        QL_REQUIRE(times[i] > 0.0, "time #" << i << " (" << times[i]
                                            << ") must be positive");
        if (i < times.size() - 1) {
            QL_REQUIRE(times[i] < times[i + 1],
                       "times must be strictly increasing, #"
                           << i << " and #" << (i + 1) << " are " << times[i]
                           << " and " << times[i + 1] << " respectively");
        }
    }

    // integrator and fallback to compute M directly in terms of x
    integrator_ = boost::make_shared<GaussLobattoIntegral>(10000, 1E-8, 1E-8);
    integrator2_ = boost::make_shared<SegmentIntegral>(250);

    // integrator to compute M for case eta = 1
    ghIntegrator_ = boost::make_shared<GaussHermiteIntegration>(ghPoints_);

    // integrator and fallback to tabulate M
    preIntegrator_ =
        boost::make_shared<GaussLobattoIntegral>(10000, 1E-8, 1E-8);
    preIntegrator2_ = boost::make_shared<SegmentIntegral>(250);

    // tabulation data
    etaSize_ = detail::eta_pre_size;
    uSize_ = detail::u_pre_size;
    SuSize_ = detail::Su_pre_size;
    vSize_ = detail::v_pre_size;
    y0Size_ = detail::y0_pre_size;

    eta_pre_ = std::vector<Real>(detail::eta_pre, detail::eta_pre + etaSize_);
    u_pre_ = std::vector<Real>(detail::u_pre, detail::u_pre + uSize_);
    Su_pre_ = std::vector<Real>(detail::Su_pre, detail::Su_pre + SuSize_);
    v_pre_ = std::vector<Real>(detail::v_pre, detail::v_pre + vSize_);
    y0_pre_ = std::vector<Real>(detail::y0_pre, detail::y0_pre + y0Size_);

    // interpolation of tabulated data
    for (Size i = 0; i < etaSize_; ++i) {
        boost::shared_ptr<Matrix> zTmp =
            boost::make_shared<Matrix>(Su_pre_.size(), u_pre_.size());
        boost::shared_ptr<Matrix> z2Tmp =
            boost::make_shared<Matrix>(v_pre_.size(), y0_pre_.size());
        for (Size uu = 0; uu < uSize_; ++uu)
            for (Size vv = 0; vv < SuSize_; ++vv)
                (*zTmp)[vv][uu] = detail::M_pre[i][uu][vv];
        for (Size vv = 0; vv < vSize_; ++vv)
            for (Size yy = 0; yy < y0Size_; ++yy)
                (*z2Tmp)[yy][vv] = detail::p_pre[i][yy][vv];
        M_datasets_.push_back(zTmp);
        p_datasets_.push_back(z2Tmp);
        boost::shared_ptr<BilinearInterpolation> tmp =
            boost::make_shared<BilinearInterpolation>(
                u_pre_.begin(), u_pre_.end(), Su_pre_.begin(), Su_pre_.end(),
                *(M_datasets_[i]));
        boost::shared_ptr<BilinearInterpolation> tmp2 =
            boost::make_shared<BilinearInterpolation>(
                v_pre_.begin(), v_pre_.end(), y0_pre_.begin(), y0_pre_.end(),
                *(p_datasets_[i]));
        tmp->enableExtrapolation();
        tmp2->enableExtrapolation();
        M_surfaces_.push_back(tmp);
        p_surfaces_.push_back(tmp2);
    }
};

// integrand to compute M directly in terms of x
class BetaEtaCore::mIntegrand1 {
    const BetaEtaCore *model_;
    const Real t0_, x0_, t_;

  public:
    mIntegrand1(const BetaEtaCore *model, const Real t0, const Real x0,
                const Real t)
        : model_(model), t0_(t0), x0_(x0), t_(t) {}
    Real operator()(Real x) const {
        return model_->p(t0_, x0_, t_, x) *
               exp(-model_->lambda(t_) * (x - x0_));
    }
};

// integrand to compute prob_y_0 (directly in terms of x, eta < 0.5)
class BetaEtaCore::pIntegrand1 {
    const BetaEtaCore *model_;
    const Real t0_, x0_, t_;

  public:
    pIntegrand1(const BetaEtaCore *model, const Real t0, const Real x0,
                const Real t)
        : model_(model), t0_(t0), x0_(x0), t_(t) {}
    Real operator()(Real x) const { return model_->p(t0_, x0_, t_, x); }
};

// integrand to precompute M, 0 < eta < 1, eta != 0.5
class BetaEtaCore::mIntegrand2 {
    const BetaEtaCore *model_;
    const Real S_, u0_;

  public:
    mIntegrand2(const BetaEtaCore *model, const Real S, const Real u0)
        : model_(model), S_(S), u0_(u0) {}
    Real operator()(Real u) const {
        if (close(u, 0))
            return 0.0;
        Real eta = model_->eta();
        return model_->p_y_core1(
                   S_ * std::pow(1.0 - eta, 2.0 * eta) *
                       std::pow(u0_, 2.0 - 2.0 * eta),
                   std::pow(u0_, 1.0 - eta) * std::pow(1.0 - eta, eta - 1.0),
                   std::pow(u, 1.0 - eta) * std::pow(1.0 - eta, eta - 1.0),
                   eta) *
               exp(-(u - u0_));
    }
};

// integrand to precompute prob_y_0 (eta < 0.5)
class BetaEtaCore::pIntegrand2 {
    const BetaEtaCore *model_;
    const Real v_, y0_;

  public:
    pIntegrand2(const BetaEtaCore *model, const Real v, const Real y0)
        : model_(model), v_(v), y0_(y0) {}
    Real operator()(Real y) const {
        return model_->p_y_core0(v_, y0_, y, model_->eta());
    }
};

// integrand to compute M, eta = 1
class BetaEtaCore::mIntegrand3 {
    const BetaEtaCore *model_;
    const Real v_, y0_, lambda_;

  public:
    mIntegrand3(const BetaEtaCore *model, const Real v, const Real y0,
                const Real lambda)
        : model_(model), v_(v), y0_(y0), lambda_(lambda) {}
    Real operator()(Real z) const {
        Real beta = model_->beta();
        Real y = M_SQRT2 * std::sqrt(v_) * z + y0_ - beta * v_ / 2.0;
        return exp(-lambda_ * (exp(beta * y) - exp(beta * y0_)) / beta) *
               exp(-z * z);
    }
};

const Real BetaEtaCore::M(const Time t0, const Real x0, const Real t,
                          const bool useTabulation) const {

    // since we are assuming a reflecting barrier we can
    // return M = 0 for all eta when y is negative or zero
    if (x0 <= -1.0 / beta_)
        return 0.0;

    Real lambda = this->lambda(t);
    Real v = this->tau(t0, t);

    // for zero volatility we obviously have M = 0
    if (close(v, 0.0))
        return 0.0;

    // without the reflecting barrier at y=0 we could write
    // M = 0.5 *lambda * lambda * v for eta = 0
    // with the reflecting barrier assumed here, we do not
    // have a closed form solution

    if (close(eta_, 0.5)) {
        return M_eta_05(t0, x0, t);
    }
    if (close(eta_, 1.0)) {
        return M_eta_1(t0, x0, t);
    }

    Real result;
    if (useTabulation) {
        result = M_tabulated(t0, x0, t);
    } else {
        Real s = std::sqrt(tau(t0, t));
        if (close(s, 0.0))
            return 0.0;
        mIntegrand1 in(this, t0, x0, t);
        std::pair<Real, Real> d = detail::domain(
            in, x0, 1E-10, 1E-12, 1E-6, 1.1, -1.0 / beta_, QL_MAX_REAL);
        Real a = d.first;
        Real b = d.second;
        try {
            result = std::log(integrator_->operator()(in, a, b));
        } catch (...) {
            try {
                result = std::log(integrator2_->operator()(in, a, b));
            } catch (...) {
                QL_FAIL("could not compute M(" << t0 << "," << x0 << "," << t
                                               << "), tried integration over "
                                               << a << "..." << b);
            }
        }
    }

    Real singularProb = prob_y_0(t0, x0, t, useTabulation);
    Real singularTerm = 0.0;
    singularTerm = singularProb * exp(-lambda * (-1.0 / beta_ - x0));

    // only take the singular term into account if numerically significant
    if (singularTerm > std::exp(result) * QL_EPSILON) {
        result = std::log(std::exp(result) + singularTerm);
    }

    return result;
};

const Real BetaEtaCore::M_eta_1(const Real t0, const Real x0,
                                const Real t) const {
    if (x0 < -1.0 / beta_)
        return 0.0;
    Real lambda = this->lambda(t);

    Real y0 = y(x0, 1.0);
    Real v = this->tau(t0, t);
    Real result = M_1_SQRTPI *
                  ghIntegrator_->operator()(mIntegrand3(this, v, y0, lambda));
    return std::log(result);
}

const Real BetaEtaCore::M_eta_05(const Real t0, const Real x0,
                                 const Real t) const {
    if (x0 < -1.0 / beta_)
        return 0.0;
    Real lambda = this->lambda(t);
    Real v = this->tau(t0, t);
    return (1.0 + beta_ * x0) * lambda * lambda * v /
           (2.0 + beta_ * lambda * v);
}

const Real BetaEtaCore::M_tabulated(const Real t0, const Real x0,
                                    const Real t) const {

    Real vraw = this->tau(t0, t);
    Real lambda = this->lambda(t);

    if (close(eta_, 0.5) || close(eta_, 1.0)) {
        return M(t0, x0, t);
    }

    Real u0 = lambda / beta_ * std::fabs(1.0 + beta_ * x0);
    Real Su = vraw * beta_ * beta_ /
              std::pow(1.0 + beta_ * x0, 2.0 - 2.0 * eta_) *
              std::pow(u0, 2.0 - 0.5 * eta_);

    int etaIdx = std::upper_bound(eta_pre_.begin(), eta_pre_.end(), eta_) -
                 eta_pre_.begin();

    Real eta_weight_1 =
        (etaIdx < static_cast<int>(eta_pre_.size()) ? eta_pre_[etaIdx] - eta_
                                                    : 1.0 - eta_) /
        (etaIdx < static_cast<int>(eta_pre_.size())
             ? (eta_pre_[etaIdx] - eta_pre_[etaIdx - 1])
             : (1.0 - eta_pre_[etaIdx - 1]));

    Real eta_weight_2 = (eta_ - eta_pre_[etaIdx - 1]) /
                        (etaIdx < static_cast<int>(eta_pre_.size())
                             ? (eta_pre_[etaIdx] - eta_pre_[etaIdx - 1])
                             : (1.0 - eta_pre_[etaIdx - 1]));

    Real result_eta_lower = M_surfaces_[etaIdx - 1]->operator()(u0, Su);

    Real result_eta_higher;
    if (etaIdx < static_cast<int>(eta_pre_.size())) {
        result_eta_higher = M_surfaces_[etaIdx]->operator()(u0, Su);
    } else {
        result_eta_higher = M_eta_1(t0, x0, t);
    }

    Real result =
        (result_eta_lower * eta_weight_1 + result_eta_higher * eta_weight_2);

    if (u0 > u_pre_.back() || Su > Su_pre_.back())
        QL_FAIL("tabulated value lookup ("
                << u0 << "," << Su
                << ") would require extrapolation, bounds are u0_max="
                << u_pre_.back() << " and Su_max=" << Su_pre_.back());

    return result;
}

const Real BetaEtaCore::M(const Real u0, const Real Su) const {
    Real res;
    if (close(Su, 0.0)) {
        res = 1.0;
    } else {
        QL_REQUIRE(!close(eta_, 1.0), "M(u0,Su) is only defined for eta < 1");
        mIntegrand2 ig(this, Su / std::pow(u0, 2.0 - 0.5 * eta_), u0);
        std::pair<Real, Real> d =
            detail::domain(ig, u0, 1E-10, 1E-12, 1E-6, 1.1, 1E-10, QL_MAX_REAL);
        try {
            res = preIntegrator_->operator()(ig, d.first, d.second);
        } catch (...) {
            try {
                res = preIntegrator2_->operator()(ig, d.first, d.second);
            } catch (...) {
                QL_FAIL("could not compute M(" << u0 << "," << Su
                                               << "), tried integration over "
                                               << d.first << "..." << d.second);
            }
        }
    }
    Real a = close(res, 0.0) ? -50.0 : std::log(res);
    return a;
}

const Real BetaEtaCore::p_y_core0(const Real v, const Real y0, const Real y,
                                  const Real eta) const {
    QL_REQUIRE(!close(eta, 1.0), "eta must not be one in p_y_core0");
    if (close(y, 0.0) || close(y0, 0.0)) // i.e. x, x0 = -1/beta
        return 0.0;
    Real nu = 1.0 / (2.0 - 2.0 * eta);
    // 0.0 <= eta < 0.5
    if (eta < 0.5) {
        return std::pow(y0 / y, nu) * y / v *
               modifiedBesselFunction_i_exponentiallyWeighted(-nu, y0 * y / v) *
               exp(-(y - y0) * (y - y0) / (2.0 * v));
    }
    // 0.5 <= eta < 1.0
    return std::pow(y0 / y, nu) * y / v *
           modifiedBesselFunction_i_exponentiallyWeighted(nu, y0 * y / v) *
           exp(-(y - y0) * (y - y0) / (2.0 * v));
}

const Real BetaEtaCore::p_y_core1(const Real v, const Real y0, const Real y,
                                  const Real eta) const {
    QL_REQUIRE(!close(eta, 1.0), "eta must not be one in p_y_core1");
    return p_y_core0(v, y0, y, eta) * std::pow(y, eta / (eta - 1.0));
}

const Real BetaEtaCore::p_y(const Real v, const Real y0, const Real y,
                            const Real eta) const {
    // eta = 1.0
    if (close(eta, 1.0)) {
        return exp(-beta_ * y) / std::sqrt(2.0 * M_PI * v) *
               exp(-0.5 * (y - y0 + 0.5 * beta_ * v) *
                   (y - y0 + 0.5 * beta_ * v) / v);
    }
    // eta < 1.0
    return p_y_core1(v, y0, y, eta) * std::pow(1.0 - eta, eta / (eta - 1.0)) *
           std::pow(beta_, eta / (eta - 1.0));
}

const Real BetaEtaCore::p(const Time t0, const Real x0, const Real t,
                          const Real x) const {
    if (x <= -1.0 / beta_)
        return 0.0;
    // to avoid numerical instabilities when eta is close to
    // but not equal to one we interpolate the density between the largest
    // stable value for eta and one. Since the tabulation should be
    // stable w.r.t. the spanned eta grid, we use the largest grid value
    // from there as the cutoff value (which would typically be something
    // close to 0.99).
    Real v = this->tau(t0, t);
    if (eta_ <= eta_pre_.back()) {
        Real y0 = this->y(x0, eta_);
        Real y = this->y(x, eta_);
        return p_y(v, y0, y, eta_);
    } else {
        Real y0a = this->y(x0, eta_pre_.back());
        Real ya = this->y(x, eta_pre_.back());
        Real y0b = this->y(x0, 1.0);
        Real yb = this->y(x, 1.0);
        return 0.5 * (p_y(v, y0a, ya, eta_pre_.back()) + p_y(v, y0b, yb, 1.0));
    }
};

const Real BetaEtaCore::prob_y_0(const Real v, const Real y0) const {
    if (close(v, 0.0) || eta_ > eta_pre_.back())
        return 0.0;
    if (eta_ >= 0.5) {
        Real nu = 1.0 / (2.0 - 2.0 * eta_);
        Real res = boost::math::gamma_q(nu, y0 * y0 / (2.0 * v));
        return res;
    }
    // eta < 0.5
    pIntegrand2 inC(this, v, y0);
    std::pair<Real, Real> d =
        detail::domain(inC, y0, 1E-10, 1E-12, 1E-6, 1.1, 0.0, QL_MAX_REAL);
    Real a = d.first;
    Real b = d.second;
    Real result;
    try {
        result = 1.0 - integrator_->operator()(inC, a, b);
    } catch (...) {
        try {
            result = 1.0 - integrator2_->operator()(inC, a, b);
        } catch (...) {
            QL_FAIL("could not compute prob_y_0("
                    << v << "," << y0 << "), tried integration over " << a
                    << "..." << b);
        }
    }
    return result;
}

const Real BetaEtaCore::prob_y_0_tabulated(const Real v, const Real y0) const {
    if (close(v, 0.0) || eta_ > eta_pre_.back())
        return 0.0;
    int etaIdx = std::upper_bound(eta_pre_.begin(), eta_pre_.end(), eta_) -
                 eta_pre_.begin();

    // weight formulas are more general than needed because
    // etaIdx < eta_pre_.size() by the condition above
    Real eta_weight_1 =
        (etaIdx < static_cast<int>(eta_pre_.size()) ? eta_pre_[etaIdx] - eta_
                                                    : 1.0 - eta_) /
        (etaIdx < static_cast<int>(eta_pre_.size())
             ? (eta_pre_[etaIdx] - eta_pre_[etaIdx - 1])
             : (1.0 - eta_pre_[etaIdx - 1]));

    Real eta_weight_2 = (eta_ - eta_pre_[etaIdx - 1]) /
                        (etaIdx < static_cast<int>(eta_pre_.size())
                             ? (eta_pre_[etaIdx] - eta_pre_[etaIdx - 1])
                             : (1.0 - eta_pre_[etaIdx - 1]));

    Real result_eta_lower = p_surfaces_[etaIdx - 1]->operator()(v, y0);
    Real result_eta_higher = p_surfaces_[etaIdx]->operator()(v, y0);
    Real result =
        (result_eta_lower * eta_weight_1 + result_eta_higher * eta_weight_2);

    // if (v > v_pre_.back() || y0 > y0_pre_.back())
    //     QL_FAIL("tabulated value lookup ("
    //             << v << "," << y0
    //             << ") would require extrapolation, bounds are v_max="
    //             << v_pre_.back() << " and y0_max=" << y0_pre_.back());

    return result;
}

const Real BetaEtaCore::prob_y_0(const Time t0, const Real x0, const Time t,
                                 bool useTabulation) const {
    Real v = tau(t0, t);
    Real y0 = y(x0, eta_);
    Real result;
    if (useTabulation) {
        result = prob_y_0_tabulated(v, y0);
    } else {
        result = prob_y_0(v, y0);
    }
    // TODO ...
    if(result < prob_y_0_cutoff)
        result = 0.0;
    return result;
};

namespace detail {

const void
betaeta_tabulate(betaeta_tabulation_type type, std::ostream &out,
                 const Real eta_min, const Real eta_max, const Real u0_min,
                 const Real u0_max, const Real Su_min, const Real Su_max,
                 const Size u_size, const Size Su_size, const Size eta_size,
                 const Real c_u, const Real density_u, const Real c_Su,
                 const Real density_Su, const Real c_e, const Real density_e) {

    Concentrating1dMesher um(u0_min, u0_max, u_size,
                             std::make_pair(c_u, density_u), true);
    Concentrating1dMesher sum(Su_min, Su_max, Su_size,
                              std::make_pair(c_Su, density_Su), true);
    Concentrating1dMesher em(eta_min, eta_max, eta_size,
                             std::make_pair(c_e, density_e), true);

    out.precision(8);
    if (type == Cpp_M || type == Cpp_p) {

        out << "/* -*- mode: c++; tab-width: 4; indent-tabs-mode:"
            << "nil; c-basic-offset: 4 -*- */\n"
            << "\n "
            << "/*\n"
            << " Copyright (C) 2015 Peter Caspers\n"
            << " Copyright (C) 2015 Roland Lichters\n"
            << "\n"
            << " This file is part of QuantLib, a "
               "free-software/open-source "
               "library\n"
            << " for financial quantitative analysts and developers - "
               "http://quantlib.org/\n"
            << "\n"
            << " QuantLib is free software: you can redistribute it and/or "
               "modify it\n"
            << " under the terms of the QuantLib license.  You should have "
               "received a\n"
            << " copy of the license along with this program; if not, "
               "please "
               "email\n"
            << " <quantlib-dev@lists.sf.net>. The license is also "
               "available "
               "online at\n"
            << " <http://quantlib.org/license.shtml>.\n"
            << "\n"
            << " This program is distributed in the hope that it will be "
               "useful, but WITHOUT\n"
            << " ANY WARRANTY; without even the implied warranty of "
               "MERCHANTABILITY or FITNESS\n"
            << " FOR A PARTICULAR PURPOSE.  See the license for more "
               "details.\n"
            << "*/\n\n";

        out << "// this file was generated by "
               "QuantLib::detail::betaeta_tabulate\n"
               "// using the following parameters:\n";
        if (type == Cpp_M) {
            out << "// u0_min = " << u0_min << " u0_max = " << u0_max << "\n";
            out << "// Su_min = " << Su_min << " Su_max = " << Su_max << "\n";
            out << "// u_size = " << u_size << " Su_size = " << Su_size
                << " eta_size = " << eta_size << "\n";
            out << "// c_u = " << c_u << " density_u = " << density_u << "\n";
            out << "// c_Su = " << c_Su << " density_Su = " << density_Su
                << "\n";
            out << "// c_e = " << c_e << " density_e = " << density_e << "\n\n";
            out << "namespace QuantLib {\n"
                << "namespace detail {\n\n";
            out << "extern \"C\" const double eta_pre[] = {";
            for (Size i = 0; i < em.size() - 1; ++i)
                out << em.location(i) << (i < em.size() - 2 ? "," : "};\n\n");
            out << "extern \"C\" const double u_pre[] = {";
            for (Size i = 0; i < um.size(); ++i)
                out << um.location(i) << (i < um.size() - 1 ? "," : "};\n\n");
            out << "extern \"C\" const double Su_pre[] = {";
            for (int i = 0; i < static_cast<int>(sum.size()); ++i)
                out << (i == -1 ? 0.0 : sum.location(i))
                    << (i < static_cast<int>(sum.size()) - 1 ? "," : "};\n\n");
            out << "extern \"C\" const double M_pre[][" << um.size() << "]["
                << (sum.size()) << "] = {\n";
        } else {
            out << "// y0_min = " << u0_min << " y0_max = " << u0_max << "\n";
            out << "// v_min = " << Su_min << " v_max = " << Su_max << "\n";
            out << "// y0_size = " << u_size << " v_size = " << Su_size
                << " eta_size = " << eta_size << "\n";
            out << "// c_y0 = " << c_u << " density_y0 = " << density_u << "\n";
            out << "// c_v = " << c_Su << " density_v = " << density_Su
                << "\n";
            out << "// c_e = " << c_e << " density_e = " << density_e << "\n\n";
            out << "// note that the eta grid is taken from "
                   "betaetatabulation.cpp\n\n";
            out << "namespace QuantLib {\n"
                << "namespace detail {\n\n";
            out << "extern \"C\" const double y0_pre[] = {";
            for (Size i = 0; i < um.size(); ++i)
                out << um.location(i) << (i < um.size() - 1 ? "," : "};\n\n");
            out << "extern \"C\" const double v_pre[] = {";
            for (int i = 0; i < static_cast<int>(sum.size()); ++i)
                out << (i == -1 ? 0.0 : sum.location(i))
                    << (i < static_cast<int>(sum.size()) - 1 ? "," : "};\n\n");
            out << "extern \"C\" const double p_pre[][" << um.size() << "]["
                << (sum.size()) << "] = {\n";
        }
    }

    Array times;
    Array alpha(1, 0.01);
    Array kappa(1, 0.01);

    if (type == Cpp_M || type == Cpp_p) {
        for (Size e = 0; e < eta_size - 1; ++e) {
            Real eta = em.location(e);
            BetaEtaCore core(times, alpha, kappa, 1.0, eta);
            out << "// ========================  eta=" << eta << "\n";
            out << "{ ";
            for (Size i = 0; i < um.size(); ++i) {
                Real u0 = um.location(i);
                if (type == Cpp_M) {
                    out << "// eta=" << eta << " u0=" << u0 << "\n";
                } else {
                    out << "// eta=" << eta << " y0=" << u0 << "\n";
                }
                out << "{";
                for (int j = 0; j < static_cast<int>(sum.size()); ++j) {
                    Real v = j == -1 ? 0.0 : sum.location(j);
                    Real lres;
                    if (type == Cpp_M) {
                        lres = core.M(u0, v);
                    } else {
                        lres = core.prob_y_0(v, u0);
                    }
                    out << lres
                        << (j < static_cast<int>(sum.size()) - 1 ? "," : "}");
                }
                out << (i < um.size() - 1 ? ",\n" : "}");
            }
            out << (e < eta_size - 2 ? ",\n" : "};\n");
        }
        out << "} // namespace detail\n"
            << "} // namespace QuantLib\n";
    }

    if (type == GnuplotEUV) {
        for (Size e = 0; e < eta_size - 1; ++e) {
            Real eta = em.location(e);
            BetaEtaCore core(times, alpha, kappa, 1.0, eta);
            for (Size i = 0; i < um.size(); ++i) {
                Real u0 = um.location(i);
                for (int j = 0; j < static_cast<int>(sum.size()); ++j) {
                    Real v = j == -1 ? 0.0 : sum.location(j);
                    Real lres = core.M(u0, v);
                    out << eta << " " << u0 << " " << v << " " << lres << "\n";
                }
                out << "\n";
            }
        }
    }

    if (type == GnuplotUEV) {
        for (Size i = 0; i < um.size(); ++i) {
            Real u0 = um.location(i);
            for (Size e = 0; e < eta_size - 1; ++e) {
                Real eta = em.location(e);
                BetaEtaCore core(times, alpha, kappa, 1.0, eta);
                for (int j = 0; j < static_cast<int>(sum.size()); ++j) {
                    Real v = j == -1 ? 0.0 : sum.location(j);
                    Real lres = core.M(u0, v);
                    out << u0 << " " << eta << " " << v << " " << lres << "\n";
                }
                out << "\n";
            }
        }
    }

    if (type == GnuplotVEU) {
        for (int j = 0; j < static_cast<int>(sum.size()); ++j) {
            Real v = j == -1 ? 0.0 : sum.location(j);
            for (Size e = 0; e < eta_size - 1; ++e) {
                Real eta = em.location(e);
                BetaEtaCore core(times, alpha, kappa, 1.0, eta);
                for (Size i = 0; i < um.size(); ++i) {
                    Real u0 = um.location(i);
                    Real lres = core.M(u0, v);
                    out << v << " " << eta << " " << u0 << " " << lres << "\n";
                }
                out << "\n";
            }
        }
    }

    if (type == GnuplotP) {
        for (Size e = 0; e < eta_size - 1; ++e) {
            Real eta = em.location(e);
            for (Size i = 0; i < um.size(); ++i) {
                Real u0 = um.location(i);
                for (int j = 0; j < static_cast<int>(sum.size()); ++j) {
                    Real v = j == -1 ? 0.0 : sum.location(j);
                    BetaEtaCore core(times, alpha, kappa, 1.0, eta);
                    Real lres = core.prob_y_0(v, u0);
                    out << eta << " " << v << " " << u0 << " " << lres << "\n";
                }
                out << "\n";
            }
        }
    }
}

} // namespace detail

} // namespace QuantLib


//==================================================================================================
/*!
  @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_ARCH_COMMON_SCALAR_FUNCTION_GAMMA_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_GAMMA_HPP_INCLUDED

#include <boost/simd/function/std.hpp>
#include <boost/config.hpp>
#include <boost/simd/arch/common/detail/generic/gamma_kernel.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/three.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/copysign.hpp>
#include <boost/simd/function/floor.hpp>
#include <boost/simd/function/is_eqz.hpp>
#include <boost/simd/function/is_even.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/sinpi.hpp>
#include <boost/simd/function/stirling.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <cmath>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  using bs::std_tag;
  BOOST_DISPATCH_OVERLOAD ( gamma_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      if (is_eqz(a0)) return copysign(Inf<A0>(), a0);
      #ifndef BOOST_SIMD_NO_INVALIDS
      if( is_nan(a0) || (a0 == Minf<A0>()) ) return Nan<A0>();
      if (a0 == Inf<A0>()) return a0;
      #endif

      A0 x = a0;
      if (inftest(a0)) return Inf<A0>();
      A0 q = bs::abs(x);
      if(x < A0(-33.0))
      {
//        return std::tgamma(a0);
        A0 st = stirling(q);
        A0 p =  floor(q);
        auto iseven =  is_even((int32_t)p);
        if (p == q) return Nan<A0>();
        A0 z = q - p;
        if( z > Half<A0>() )
        {
          p += One<A0>();
          z = q - p;
        }
        z = q*sinpi(z);
        if( is_eqz(z) ) return Nan<A0>();
        st = Pi<A0>()/(bs::abs(z)*st);
        return iseven  ? -st : st;
      }
      A0 z = One<A0>();
      while( x >= Three<A0>() )
      {
        x -= One<A0>();
        z *= x;
      }
      while( is_ltz(x) )
      {
        z /= x;
        x += One<A0>();
      }
      while( x < Two<A0>() )
      {
        if( is_eqz(x)) return Nan<A0>();
        z /= x;
        x +=  One<A0>();
      }
      if( x == Two<A0>() ) return(z);
      x -= Two<A0>();
      return z*detail::gamma_kernel<A0>::gamma1(x);
    }
  private:
    static BOOST_FORCEINLINE bool inftest(const float a0)
    {
      return a0 > 35.4f;
    }
    static BOOST_FORCEINLINE bool inftest(const double a0)
    {
      return a0 > 171.624;
    }

  };
  BOOST_DISPATCH_OVERLOAD ( gamma_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0) const BOOST_NOEXCEPT
    {
      return std::tgamma(a0);
    }
  };
} } }

#endif


#include "aslam/calibration/error-terms/ErrorTermPose.h"

#include <Eigen/Dense>

#include <sm/kinematics/rotations.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>
#include <sm/kinematics/RotationVector.hpp>

namespace aslam {
namespace calibration {

ErrorTermPose::ErrorTermPose(const aslam::backend::TransformationExpression& T,
    const Eigen::Vector3d & t,
    const Eigen::Vector4d & q,
    const Covariance& sigma2,
    ErrorTermGroupReference etgr) :
    ErrorTermGroupMember(etgr),
    _q(q), _C(sm::kinematics::quat2r(q)), _t(t),_T(T) {
  setInvR(sigma2.inverse());
  aslam::backend::DesignVariable::set_t dv;
  _T.getDesignVariables(dv);
  setDesignVariablesIterator(dv.begin(), dv.end());
}

ErrorTermPose::ErrorTermPose(const aslam::backend::TransformationExpression& T, const Eigen::Vector3d & t,
    const Eigen::Vector4d & q, const Eigen::Matrix3d & cov_t, const Eigen::Matrix3d & cov_r, ErrorTermGroupReference etgr):
    ErrorTermPose(T, t, q, Covariance::Identity(), etgr) {
  Covariance Q = Covariance::Zero();
  Q.topLeftCorner<3, 3>() = cov_t;
  Q.bottomRightCorner<3, 3>() = cov_r;
  setInvR(Q.inverse());
}

ErrorTermPose::ErrorTermPose(const aslam::backend::TransformationExpression& T, const PoseMeasurement& pm, const Eigen::Matrix3d & cov_t, const Eigen::Matrix3d & cov_r, ErrorTermGroupReference etgr) :
  ErrorTermPose(T, pm.t, pm.q, cov_t, cov_r, etgr) {
}

sm::kinematics::RotationVector rotVec;

double ErrorTermPose::evaluateErrorImplementation() {
  const Eigen::Matrix4d T = _T.toTransformationMatrix();
  error_t error;
  error.head<3>() = T.topRightCorner<3, 1>() - _t;
  error.tail<3>() = rotVec.rotationMatrixToParameters(T.topLeftCorner<3, 3>() * _C.transpose());
  setError(error);
  return evaluateChiSquaredError();
}

void ErrorTermPose::evaluateJacobiansImplementation(aslam::backend::JacobianContainer& jacobians) {
  const Eigen::Matrix4d T = _T.toTransformationMatrix();
  Eigen::Matrix<double, 6, 6> J = Eigen::Matrix<double, 6, 6>::Identity();
  J.topRightCorner<3, 3>() = sm::kinematics::crossMx(T.topRightCorner<3, 1>());
  J.bottomRightCorner<3, 3>() = (rotVec.parametersToSMatrix(rotVec.rotationMatrixToParameters(T.topLeftCorner<3, 3>() * _C.transpose()))).inverse();
  _T.evaluateJacobians(jacobians, J);
}

Eigen::VectorXd ErrorTermPose::getPrediction() const {
  const Eigen::Matrix4d T = _T.toTransformationMatrix();
  Eigen::VectorXd pred(7);
  pred.head<3>() = T.topRightCorner<3, 1>();
  pred.tail<4>() = sm::kinematics::r2quat(T.topLeftCorner<3, 3>());
  return pred;
}

Eigen::VectorXd ErrorTermPose::getMeasurement() const {
  Eigen::VectorXd m(7);
  m.head<3>() = _t;
  m.tail<4>() = _q;
  return m;
}

}
}