//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2019 Alexey Moskvin
// Copyright (c) 2020 Ilias Khairullin <ilias@nil.foundation>
//
// 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_MULTIPRECISION_MONTGOMERY_PARAMS_HPP
#define BOOST_MULTIPRECISION_MONTGOMERY_PARAMS_HPP

#include <boost/container/vector.hpp>

#include <boost/type_traits/is_integral.hpp>

#include <nil/crypto3/multiprecision/cpp_int.hpp>
#include <nil/crypto3/multiprecision/cpp_int/cpp_int_config.hpp>
#include <nil/crypto3/multiprecision/modular/base_params.hpp>
#include <nil/crypto3/multiprecision/modular/barrett_params.hpp>

#include <type_traits>
#include <tuple>
#include <array>
#include <cstddef>    // std::size_t
#include <limits>
#include <string>

namespace nil {
    namespace crypto3 {
        namespace multiprecision {
            namespace backends {
                /**
                 * Parameters for Montgomery Reduction
                 */
                template<typename Backend>
                class montgomery_params : public base_params<Backend> {
                    typedef number<Backend> number_type;

                protected:
                    template<typename Number>
                    inline void initialize_montgomery_params(const Number& p) {
                        this->initialize_base_params(p);
                        find_const_variables(p);
                    }

                    inline void initialize_montgomery_params(const montgomery_params<Backend>& p) {
                        this->initialize_base_params(p);
                        find_const_variables(p);
                    }

                    limb_type monty_inverse(limb_type a) {
                        if (a % 2 == 0) {
                            throw std::invalid_argument("Monty_inverse only valid for odd integers");
                        }

                        limb_type b = 1;
                        limb_type r = 0;

                        for (size_t i = 0; i != sizeof(limb_type) * CHAR_BIT; ++i) {
                            const limb_type bi = b % 2;
                            r >>= 1;
                            r += bi << (sizeof(limb_type) * CHAR_BIT - 1);

                            b -= a * bi;
                            b >>= 1;
                        }

                        // Now invert in addition space
                        r = (~static_cast<limb_type>(0) - r) + 1;

                        return r;
                    }

                    template<typename T>
                    void find_const_variables(const T& pp) {
                        number_type p = pp;
                        if (p <= 0 || !(p % 2)) {
                            return;
                        }

                        m_p_words = this->m_mod.backend().size();

                        m_p_dash = monty_inverse(this->m_mod.backend().limbs()[0]);

                        number_type r;

                        default_ops::eval_bit_set(r.backend(), m_p_words * sizeof(limb_type) * CHAR_BIT);

                        m_r2 = r * r;
                        barrett_params<Backend> barrettParams(this->m_mod);
                        barrettParams.barret_reduce(m_r2.backend());
                    }

                public:
                    montgomery_params() : base_params<Backend>() {
                    }

                    template<typename Number>
                    explicit montgomery_params(const Number& p) : base_params<Backend>(p) {
                        initialize_montgomery_params(p);
                    }

                    inline const number_type& r2() const {
                        return m_r2;
                    }

                    inline limb_type p_dash() const {
                        return m_p_dash;
                    }

                    inline size_t p_words() const {
                        return m_p_words;
                    }

                    template<class V>
                    montgomery_params& operator=(const V& v) {
                        initialize_montgomery_params(v);
                        return *this;
                    }

                    inline void montgomery_reduce(Backend& result) const {
                        using default_ops::eval_lt;
                        using default_ops::eval_multiply_add;

                        typedef cpp_int_backend<sizeof(limb_type) * CHAR_BIT * 3, sizeof(limb_type) * CHAR_BIT * 3,
                                                unsigned_magnitude, unchecked, void>
                            cpp_three_int_backend;

                        const size_t p_size = m_p_words;
                        const limb_type p_dash = m_p_dash;
                        const size_t z_size = 2 * (p_words() + 1);

                        boost::container::vector<limb_type> z(
                            result.size(), 0);    // container::vector<limb_type, alloc> z(result.size(), 0);
                        for (size_t i = 0; i < result.size(); ++i) {
                            z[i] = result.limbs()[i];
                        }

                        if (result.size() < z_size) {
                            result.resize(z_size, z_size);
                            z.resize(z_size, 0);
                        }

                        cpp_three_int_backend w(z[0]);

                        result.limbs()[0] = w.limbs()[0] * p_dash;

                        eval_multiply_add(w, result.limbs()[0], this->m_mod.backend().limbs()[0]);
                        eval_right_shift(w, sizeof(limb_type) * CHAR_BIT);

                        for (size_t i = 1; i != p_size; ++i) {
                            for (size_t j = 0; j < i; ++j) {
                                eval_multiply_add(w, result.limbs()[j], this->m_mod.backend().limbs()[i - j]);
                            }

                            eval_add(w, z[i]);

                            result.limbs()[i] = w.limbs()[0] * p_dash;

                            eval_multiply_add(w, result.limbs()[i], this->m_mod.backend().limbs()[0]);

                            eval_right_shift(w, sizeof(limb_type) * CHAR_BIT);
                        }

                        for (size_t i = 0; i != p_size; ++i) {
                            for (size_t j = i + 1; j != p_size; ++j) {
                                eval_multiply_add(w, result.limbs()[j], this->m_mod.backend().limbs()[p_size + i - j]);
                            }

                            eval_add(w, z[p_size + i]);

                            result.limbs()[i] = w.limbs()[0];

                            eval_right_shift(w, sizeof(limb_type) * CHAR_BIT);
                        }

                        eval_add(w, z[z_size - 1]);

                        result.limbs()[p_size] = w.limbs()[0];
                        result.limbs()[p_size + 1] = w.limbs()[1];

                        if (result.size() != p_size + 1) {
                            result.resize(p_size + 1, p_size + 1);
                        }
                        result.normalize();
                    }

                protected:
                    number_type m_r2;
                    limb_type m_p_dash;
                    size_t m_p_words;
                };

                // // fixed precision montgomery params type which supports compile-time execution
                // template<unsigned MinBits, cpp_integer_type SignType, cpp_int_check_type Checked>
                // class montgomery_params<cpp_int_backend<MinBits, MinBits, SignType, Checked, void>>
                //     : public base_params<cpp_int_backend<MinBits, MinBits, SignType, Checked, void>>
                // {
                //  protected:
                //    typedef base_params<cpp_int_backend<MinBits, MinBits, SignType, Checked, void>> base_type;
                //    typedef typename base_type::policy_type policy_type;
                //
                //    typedef typename policy_type::internal_limb_type internal_limb_type;
                //    typedef typename policy_type::internal_double_limb_type internal_double_limb_type;
                //    typedef typename policy_type::Backend Backend;
                //    typedef typename policy_type::Backend_padded_limbs Backend_padded_limbs;
                //    typedef typename policy_type::Backend_doubled_limbs Backend_doubled_limbs;
                //    typedef typename policy_type::Backend_doubled_padded_limbs Backend_doubled_padded_limbs;
                //    typedef typename policy_type::number_type number_type;
                //
                //    constexpr static auto limbs_count = policy_type::limbs_count;
                //    constexpr static auto limb_bits = policy_type::limb_bits;
                //
                //    constexpr void initialize_montgomery_params(const number_type& p)
                //    {
                //       this->initialize_base_params(p);
                //       find_const_variables(p);
                //       find_modulus_mask();
                //    }
                //
                //    constexpr internal_limb_type monty_inverse(internal_limb_type a)
                //    {
                //       if (a % 2 == 0)
                //       {
                //          throw std::invalid_argument("Monty_inverse only valid for odd integers");
                //       }
                //
                //       internal_limb_type b = 1;
                //       internal_limb_type r = 0;
                //
                //       for (size_t i = 0; i != limb_bits; ++i)
                //       {
                //          const internal_limb_type bi = b % 2;
                //          r >>= 1;
                //          r += bi << (limb_bits - 1);
                //
                //          b -= a * bi;
                //          b >>= 1;
                //       }
                //
                //       // Now invert in addition space
                //       r = (~static_cast<internal_limb_type>(0) - r) + 1;
                //
                //       return r;
                //    }
                //
                //    constexpr void find_const_variables(const number_type& pp)
                //    {
                //       using padded_dbl_number_type = number<Backend_doubled_padded_limbs>;
                //
                //       number_type p = pp;
                //       if (p <= 0 || !(p % 2))
                //       {
                //          return;
                //       }
                //
                //       m_p_words = this->m_mod.backend().size();
                //
                //       m_p_dash = monty_inverse(this->m_mod.backend().limbs()[0]);
                //
                //       padded_dbl_number_type r;
                //
                //       default_ops::eval_bit_set(r.backend(), m_p_words * limb_bits);
                //
                //       r = r * r;
                //       barrett_params<Backend> barrettParams(this->m_mod);
                //       barrettParams.barret_reduce(r.backend());
                //       m_r2 = static_cast<Backend>(r.backend());
                //    }
                //
                //    constexpr void find_modulus_mask()
                //    {
                //       m_modulus_mask = static_cast<internal_limb_type>(1u);
                //       eval_left_shift(m_modulus_mask, this->m_mod.backend().size() * limb_bits);
                //       eval_subtract(m_modulus_mask, static_cast<internal_limb_type>(1u));
                //    }
                //
                //  public:
                //    constexpr montgomery_params()
                //        : base_type(), m_p_dash(), m_p_words(), m_modulus_mask() {}
                //
                //    constexpr explicit montgomery_params(const number_type& p)
                //        : base_type(p), m_p_dash(), m_p_words(), m_modulus_mask()
                //    {
                //       initialize_montgomery_params(p);
                //    }
                //
                //    constexpr const auto& r2() const { return m_r2; }
                //
                //    constexpr auto p_dash() const { return m_p_dash; }
                //
                //    constexpr auto p_words() const { return m_p_words; }
                //
                //    constexpr const auto& modulus_mask() const { return m_modulus_mask; }
                //
                //    constexpr montgomery_params& operator=(const number_type& p)
                //    {
                //       initialize_montgomery_params(p);
                //       return *this;
                //    }
                //
                //    template<typename BackendT,
                //        typename = typename boost::enable_if<
                //            /// result should fit in the output parameter
                //            max_precision<BackendT>::value >= max_precision<Backend>::value>::type>
                //    constexpr void montgomery_reduce(BackendT& result) const
                //    {
                //       BackendT input(result);
                //       montgomery_reduce(result, input);
                //    }
                //
                //    template<typename Backend1, typename Backend2,
                //        typename = typename boost::enable_if<
                //            /// result should fit in the output parameter
                //            max_precision<Backend1>::value >= max_precision<Backend>::value &&
                //            /// input number should be represented by backend of appropriate size
                //            max_precision<Backend2>::value <= max_precision<Backend_doubled_limbs>::value>::type>
                //    constexpr void montgomery_reduce(Backend1& result, const Backend2& input) const
                //    {
                //       Backend_doubled_padded_limbs accum(input);
                //       Backend_doubled_padded_limbs prod;
                //
                //       for (auto i = 0; i < this->m_mod.backend().size(); ++i)
                //       {
                //          eval_multiply(prod, this->m_mod.backend(), accum.limbs()[i] * p_dash());
                //          eval_left_shift(prod, i * limb_bits);
                //          eval_add(accum, prod);
                //       }
                //
                //       eval_right_shift(accum, this->m_mod.backend().size() * limb_bits);
                //
                //       if (accum.compare(this->m_mod.backend()) >= 0)
                //       {
                //          eval_subtract(accum, this->m_mod.backend());
                //       }
                //       eval_bitwise_and(accum, m_modulus_mask);
                //       result = accum;
                //    }
                //
                //    template<typename Backend1, typename Backend2,
                //        typename = typename boost::enable_if<
                //            /// result should fit in the output parameter
                //            max_precision<Backend1>::value >= max_precision<Backend>::value &&
                //            /// multiplier should fit in input parameter type
                //            max_precision<Backend2>::value >= max_precision<Backend1>::value>::type>
                //    constexpr void montgomery_mul(Backend1& result, const Backend2& y) const
                //    {
                //       Backend2 x(result);
                //       montgomery_mul(result, x, y);
                //    }
                //
                //    template<typename Backend1, typename Backend2,
                //             typename = typename boost::enable_if<
                //                 /// result should fit in the output parameter
                //                 max_precision<Backend1>::value >= max_precision<Backend>::value &&
                //                 /// multipliers should consist of the same number of limbs as modulus
                //                 max_precision<Backend2>::value >= max_precision<Backend>::value>::type>
                //    constexpr void montgomery_mul(Backend1& result, const Backend2& x, const Backend2& y) const
                //    {
                //       using default_ops::eval_lt;
                //
                //       /// input parameters should be lesser than modulus
                //       BOOST_ASSERT(eval_lt(x, this->m_mod.backend()) && eval_lt(y, this->m_mod.backend()));
                //
                //       Backend_padded_limbs A(internal_limb_type(0u));
                //
                //       for (auto i = 0; i < this->m_mod.backend().size(); i++)
                //       {
                //          internal_limb_type u_i = (A.limbs()[0] + get_limb_value(x, i) * get_limb_value(y, 0)) *
                //          p_dash();
                //
                //          // A += x[i] * y + u_i * m followed by a 1 limb-shift to the right
                //          internal_limb_type k = 0;
                //          internal_limb_type k2 = 0;
                //
                //          internal_double_limb_type z = static_cast<internal_double_limb_type>(get_limb_value(y, 0)) *
                //                                        static_cast<internal_double_limb_type>(get_limb_value(x, i)) +
                //                                        A.limbs()[0] + k;
                //          // TODO: maybe error here in static_cast<internal_limb_type>(z) if internal_double_limb_type
                //          is nil::crypto3::multiprecision::number internal_double_limb_type z2 =
                //          static_cast<internal_double_limb_type>(get_limb_value(this->m_mod.backend(), 0)) *
                //                                         static_cast<internal_double_limb_type>(u_i) +
                //                                         static_cast<internal_limb_type>(z) + k2;
                //          k = z >> std::numeric_limits<internal_limb_type>::digits;
                //          k2 = z2 >> std::numeric_limits<internal_limb_type>::digits;
                //
                //          for (auto j = 1; j < this->m_mod.backend().size(); ++j)
                //          {
                //             internal_double_limb_type t = static_cast<internal_double_limb_type>(get_limb_value(y,
                //             j)) *
                //                                           static_cast<internal_double_limb_type>(get_limb_value(x,
                //                                           i)) + A.limbs()[j] + k;
                //             // TODO: maybe error here in static_cast<internal_limb_type>(t) if
                //             internal_double_limb_type is nil::crypto3::multiprecision::number
                //             internal_double_limb_type t2 =
                //             static_cast<internal_double_limb_type>(get_limb_value(this->m_mod.backend(), j)) *
                //                                            static_cast<internal_double_limb_type>(u_i) +
                //                                            static_cast<internal_limb_type>(t) + k2;
                //             A.limbs()[j-1] = t2;
                //             k = t >> std::numeric_limits<internal_limb_type>::digits;
                //             k2 = t2 >> std::numeric_limits<internal_limb_type>::digits;
                //          }
                //          internal_double_limb_type tmp =
                //          static_cast<internal_double_limb_type>(A.limbs()[this->m_mod.backend().size()]) + k + k2;
                //          A.limbs()[this->m_mod.backend().size()-1] = tmp;
                //          A.limbs()[this->m_mod.backend().size()] = tmp >>
                //          std::numeric_limits<internal_limb_type>::digits;
                //       }
                //       A.resize(this->m_mod.backend().size(), 1);
                //
                //       if (A.compare(this->m_mod.backend()) >= 0)
                //       {
                //          eval_subtract(A, this->m_mod.backend());
                //       }
                //       eval_bitwise_and(A, m_modulus_mask);
                //       result = A;
                //    }
                //
                //    // TODO: replace in modular_params - need to refactor modular_adaptor structure
                //    template<typename Backend1, typename Backend2,
                //        typename = typename boost::enable_if<
                //            /// result should fit in the output parameter
                //            max_precision<Backend1>::value >= max_precision<Backend>::value>::type>
                //    constexpr void mont_exp(Backend1& result, const Backend2& exp) const
                //    {
                //       Backend1 a(result);
                //       mont_exp(result, a, exp);
                //    }
                //
                //    // TODO: replace in modular_params - need to refactor modular_adaptor structure
                //    template<typename Backend1, typename Backend2, typename Backend3,
                //             typename = typename boost::enable_if<
                //                 /// result should fit in the output parameter
                //                 max_precision<Backend1>::value >= max_precision<Backend>::value &&
                //                 /// input number should fit modulus
                //                 max_precision<Backend2>::value >= max_precision<Backend>::value>::type>
                //    constexpr void mont_exp(Backend1& result, const Backend2& a, Backend3 exp) const
                //    {
                //       using default_ops::eval_eq;
                //       using default_ops::eval_right_shift;
                //       using default_ops::eval_left_shift;
                //       using default_ops::eval_modulus;
                //
                //       Backend_doubled_limbs tmp(static_cast<internal_limb_type>(1u));
                //       eval_multiply(tmp, r2().backend());
                //       montgomery_reduce(tmp);
                //       Backend R_mod_m(tmp);
                //
                //       Backend base(a);
                //
                //       Backend3 zero(static_cast<internal_limb_type>(0u));
                //       if (eval_eq(exp, zero))
                //       {
                //          result = static_cast<internal_limb_type>(1u);
                //          return;
                //       }
                //       if (eval_eq(this->m_mod.backend(), static_cast<internal_limb_type>(1u)))
                //       {
                //          result = static_cast<internal_limb_type>(0u);
                //          return;
                //       }
                //
                //       while (true)
                //       {
                //          internal_limb_type lsb = exp.limbs()[0] & 1u;
                //          eval_right_shift(exp, static_cast<internal_limb_type>(1u));
                //          if (lsb)
                //          {
                //             montgomery_mul(R_mod_m, base);
                //             if (eval_eq(exp, zero))
                //             {
                //                break;
                //             }
                //          }
                //          montgomery_mul(base, base);
                //       }
                //       result = R_mod_m;
                //    }
                //
                //  protected:
                //    number_type m_r2;
                //    internal_limb_type m_p_dash;
                //    size_t m_p_words;
                //    Backend_padded_limbs m_modulus_mask;
                // };
            }    // namespace backends
        }        // namespace multiprecision
    }            // namespace crypto3
}    // namespace nil

#endif


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

/*
 Copyright (C)  2017 Cord Harms

 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/termstructures/localcorrtermstructure.hpp>
#include <ql/quotes/simplequote.hpp>
#include <boost/make_shared.hpp>

namespace QuantLib {

    LocalCorrTermStructure::LocalCorrTermStructure(const std::vector<boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>>& processes,
												   const boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>&								   processToCal)
    : CorrelationTermStructureStrike(processes[0]->blackVolatility()->referenceDate(),
		processes[0]->blackVolatility()->calendar(),
		processes[0]->blackVolatility()->businessDayConvention(),
		processes[0]->blackVolatility()->dayCounter()), processToCal_(processToCal){

		processes_ = std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>>(processes.size());

		for (size_t i = 0; i < processes_.size(); i++)
		{
			processes_[i] = boost::shared_ptr<QuantLib::HestonSLVProcess >( new QuantLib::HestonSLVProcess(
				boost::shared_ptr<QuantLib::HestonProcess >(new QuantLib::HestonProcess(
						processes[i]->riskFreeRate(), processes[i]->dividendYield(),
						Handle<Quote>(boost::make_shared<SimpleQuote>(processes[i]->x0())),1.0,
						0.0,0.0,0.0,0.0))
				, boost::shared_ptr<LocalVolTermStructure>(processes[i]->localVolatility().currentLink())));
		}

	}

	LocalCorrTermStructure::LocalCorrTermStructure(const std::vector<boost::shared_ptr<QuantLib::HestonSLVProcess>>& processes,
		const boost::shared_ptr<QuantLib::GeneralizedBlackScholesProcess>&			   processToCal)
		: CorrelationTermStructureStrike(processes[0]->leverageFct()->referenceDate(),
			processes[0]->leverageFct()->calendar(),
			processes[0]->leverageFct()->businessDayConvention(),
			processes[0]->leverageFct()->dayCounter()), processToCal_(processToCal), processes_(processes) {}

    void LocalCorrTermStructure::localCorr(RealStochasticProcess::MatA& corrMatrix,
											   const Date& d,
											   const RealStochasticProcess::VecA& X0,
                                               bool extrapolate) {

		for (size_t i = 0; i < X0.size(); i++)
		{
			checkRange(d, extrapolate);
			checkStrike(X0[i], i, extrapolate);
		}
        Time t = timeFromReference(d);

		localCorrImpl(corrMatrix, t, X0,extrapolate);
    }

    void LocalCorrTermStructure::localCorr(RealStochasticProcess::MatA& corrMatrix,
											   Time t,
											   const RealStochasticProcess::VecA& X0,
                                               bool extrapolate) {
		checkRange(t, extrapolate);
		for (size_t i = 0; i < X0.size(); i++)
		{
			checkStrike(X0[i], i, extrapolate);
		}
        localCorrImpl(corrMatrix, t, X0, extrapolate);

		//cap to 1 and floor to -1:
		for (size_t i = 0; i < corrMatrix.size(); i++)
		{
			for (size_t j = i+1; j < corrMatrix.size(); j++)
			{
				if (corrMatrix[i][j] > 1 || corrMatrix[i][j] < -1) {
					QL_FAIL("Correlation values have to be checked and corrected by checkLambdaValue function.");
				}
			}
		}
    }

    void LocalCorrTermStructure::accept(AcyclicVisitor& v) {
        Visitor<LocalCorrTermStructure>* v1 =
            dynamic_cast<Visitor<LocalCorrTermStructure>*>(&v);
        if (v1 != 0)
            v1->visit(*this);
        else
            QL_FAIL("not a local-Correlation term structure visitor");
    }

	const Date& LocalCorrTermStructure::referenceDate() const {
		return processes_[0]->leverageFct()->referenceDate();
	}

	DayCounter LocalCorrTermStructure::dayCounter() const {
		return processes_[0]->leverageFct()->dayCounter();
	}

	Date LocalCorrTermStructure::maxDate() const {
		Date minMaxDate = processes_[0]->leverageFct()->maxDate();
		for (size_t i = 0; i < processes_.size(); i++)
		{
			if (minMaxDate < processes_[i]->leverageFct()->maxDate()) minMaxDate = processes_[i]->leverageFct()->maxDate();
		}
		return minMaxDate;
	}

	Real LocalCorrTermStructure::minStrike(Natural ulId) const {
		return processes_[ulId]->leverageFct()->minStrike();
	}

	Real LocalCorrTermStructure::maxStrike(Natural ulId) const {
		return processes_[ulId]->leverageFct()->maxStrike();
	}

}


/* Software License Agreement (BSD License)
 *
 * Copyright (c) 2014, Ross Linscott (rossklin@gmail.com)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *     Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *
 *     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.
 *
 *     The names of its contributors may not be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 *
 * 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.
 */

#if defined(NDEBUG)
#undef NDEBUG
#endif

#include <cstdlib>
#include <cassert>
#include <cmath>
#include <cstring>

#include <vector>
#include <utility>
#include <exception>

#include <boost/numeric/odeint.hpp>

#include <Rcpp.h>

using std::vector;

using Rcpp::NumericMatrix;
using Rcpp::NumericVector;
using Rcpp::CharacterVector;
using Rcpp::XPtr;
using Rcpp::wrap;
using Rcpp::as;

using namespace boost::numeric::odeint;
using namespace boost::math;
using namespace std;

/*
  Thanks
  http://www.boost.org/doc/libs/1_55_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html#boost_numeric_odeint.odeint_in_detail.steppers.implicit_solvers
  for example code on an sde
*/

template< class T > class stochastic_euler
{
public:

    typedef T state_type;
    typedef T deriv_type;
    typedef double value_type;
    typedef double time_type;
    typedef unsigned short order_type;
    typedef boost::numeric::odeint::stepper_tag stepper_category;

    static order_type order( void ) { return 1; }

    template< class System >
    void do_step( System system , state_type &x , time_type t , time_type dt ) const
    {
        deriv_type det , stoch ;
        system.first( x , det );
        system.second( x , stoch );
        for( size_t i=0 ; i<x.size() ; ++i )
            x[i] += dt * det[i] + sqrt( dt ) * stoch[i];
    }
};

struct r_compute{
  Rcpp::Function f;

  r_compute(Rcpp::Function g) : f(g){}

  void operator()(vector<double> &q, vector<double> &out){
    out = as<vector<double> >(f(q));
  }

};

//' Simulates an SDE explicitly (derivative free)
//'
//' For an Ito form SDE dx = f(x) dt + g(x) E sqrt(dt)
//'
//' @param d_det R function representing f(x)
//' @param d_stoch R function representing g(x) E
//' @param start numeric vector with initial state
//' @param from scalar with initial time
//' @param to scalar with final time
//' @param steps number of steps to take
//' @export
// [[Rcpp::export]]
NumericMatrix solve_general_sde(Rcpp::Function d_det
			   , Rcpp::Function d_stoch
			   , vector<double> start
			   , double from, double to, int steps ) {
  stochastic_euler< vector<double> > se;
  const double dt = (to - from)/steps;
  r_compute sd = r_compute(d_det);
  r_compute ss = r_compute(d_stoch);

  vector<double> state(start);

  NumericMatrix result(steps+1, start.size());

  for(int j = 0; j < start.size(); ++j)
    result(0, j) = state[j];

  for(int i = 1; i <= steps; ++i) {
    se.do_step(std::make_pair(sd, ss), state, i*dt, dt);
    for(int j = 0; j < start.size(); ++j)
      result(i, j) = state[j];
  }

  return result;
}


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_SYSV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_SYSV_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include "boost/numeric/bindings/traits/ublas_symmetric.hpp"
#include <boost/numeric/bindings/lapack/ilaenv.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    /////////////////////////////////////////////////////////////////////
    //
    // system of linear equations A * X = B with A symmetric matrix
    //
    /////////////////////////////////////////////////////////////////////

    namespace detail {

      inline
      integer_t sytrf_block (float, integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "SSYTRF", ul2, n);
      }
      inline
      integer_t sytrf_block (double, integer_t const ispec, char const ul, integer_t const n) {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "DSYTRF", ul2, n);
      }
      inline
      integer_t sytrf_block (traits::complex_f,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "CSYTRF", ul2, n);
      }
      inline
      integer_t sytrf_block (traits::complex_d,
                       integer_t const ispec, char const ul, integer_t const n)
      {
        char ul2[2] = "x"; ul2[0] = ul;
        return ilaenv (ispec, "ZSYTRF", ul2, n);
      }
    }

    template <typename SymmA>
    integer_t sytrf_block (char const q, char const ul, SymmA const& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::sytrf_block (val_t(), ispec, ul, n);
    }

    template <typename SymmA>
    integer_t sytrf_block (char const q, SymmA const& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t ispec = (q == 'O' ? 1 : 2);
      return detail::sytrf_block (val_t(), ispec, ul, n);
    }

    template <typename SymmA>
    integer_t sytrf_work (char const q, char const ul, SymmA const& a)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');
      assert (ul == 'U' || ul == 'L');

      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::sytrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename SymmA>
    integer_t sytrf_work (char const q, SymmA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif
      assert (q == 'O' || q == 'M');

      char ul = traits::matrix_uplo_tag (a);
      integer_t n = traits::matrix_size1 (a);
      assert (n == traits::matrix_size2 (a));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      integer_t lw = -13;
      if (q == 'M')
        lw = 1;
      if (q == 'O')
        lw = n * detail::sytrf_block (val_t(), 1, ul, n);
      return lw;
    }

    template <typename SymmA>
    inline
    integer_t sysv_work (char const q, char const ul, SymmA const& a) {
      return sytrf_work (q, ul, a);
    }

    template <typename SymmA>
    inline
    integer_t sysv_work (char const q, SymmA const& a) { return sytrf_work (q, a); }

    /*
     * sysv() computes the solution to a system of linear equations
     * A * X = B, where A is an N-by-N symmetric matrix and X and B
     * are N-by-NRHS matrices.
     *
     * The diagonal pivoting method is used to factor A as
     *   A = U * D * U^T,  if UPLO = 'U',
     *   A = L * D * L^T,  if UPLO = 'L',
     * where  U (or L) is a product of permutation and unit upper
     * (lower) triangular matrices, and D is symmetric and block
     * diagonal with 1-by-1 and 2-by-2 diagonal blocks.
     * The factored form of A is then used to solve the system
     * of equations A * X = B.
     */

    namespace detail
    {

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 float* a, integer_t const lda, integer_t* ipiv,
                 float* b, integer_t const ldb,
                 float* w, integer_t const lw, integer_t* info)
      {
        LAPACK_SSYSV (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, w, &lw, info);
      }

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 double* a, integer_t const lda, integer_t* ipiv,
                 double* b, integer_t const ldb,
                 double* w, integer_t const lw, integer_t* info)
      {
        LAPACK_DSYSV (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, w, &lw, info);
      }

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_f* b, integer_t const ldb,
                 traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CSYSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      inline
      void sysv (char const uplo, integer_t const n, integer_t const nrhs,
                 traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                 traits::complex_d* b, integer_t const ldb,
                 traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZSYSV (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb,
                      traits::complex_ptr (w), &lw, info);
      }

      template <typename SymmA, typename MatrB, typename IVec, typename Work>
      int sysv (char const ul, SymmA& a, IVec& i, MatrB& b, Work& w)
      {
        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        sysv (ul, n, traits::matrix_size2 (b),
              traits::matrix_storage (a),
              traits::leading_dimension (a),
              traits::vector_storage (i),
              traits::matrix_storage (b),
              traits::leading_dimension (b),
              traits::vector_storage (w),
              traits::vector_size (w),
              &info);
        return info;
      }

    }

    template <typename SymmA, typename MatrB, typename IVec, typename Work>
    inline
    int sysv (char const ul, SymmA& a, IVec& i, MatrB& b, Work& w)
    {
      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      return detail::sysv (ul, a, i, b, w);
    }

    template <typename SymmA, typename MatrB, typename IVec, typename Work>
    inline
    int sysv (SymmA& a, IVec& i, MatrB& b, Work& w) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::sysv (uplo, a, i, b, w);
    }

    template <typename SymmA, typename MatrB>
    int sysv (char const ul, SymmA& a, MatrB& b)
    {
      // with 'internal' pivot and work vectors

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = sytrf_work ('O', ul, a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
        typedef typename SymmA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        if (w.valid())
          info =  detail::sysv (ul, a, i, b, w);
      }
      return info;
    }

    template <typename SymmA, typename MatrB>
    int sysv (SymmA& a, MatrB& b)
    {
      // with 'internal' pivot and work vectors

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t const n = traits::matrix_size1 (a);
      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      traits::detail::array<integer_t> i (n);

      if (i.valid()) {
        info = -102;
        integer_t lw = sytrf_work ('O', a);
        assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
        typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
        typedef typename SymmA::value_type val_t;
#endif
        traits::detail::array<val_t> w (lw);
        if (w.valid())
          info =  detail::sysv (uplo, a, i, b, w);
      }
      return info;
    }

    /*
     * sytrf() computes the factorization of a symmetric matrix A using
     * the  Bunch-Kaufman diagonal pivoting method. The form of the
     * factorization is
     *    A = U * D * U^T  or  A = L * D * L^T
     * where U (or L) is a product of permutation and unit upper (lower)
     * triangular matrices, and D is symmetric and block diagonal with
     * 1-by-1 and 2-by-2 diagonal blocks.
     */

    namespace detail
    {
      inline
      void sytrf (char const uplo, integer_t const n,
                  float* a, integer_t const lda, integer_t* ipiv,
                  float* w, integer_t const lw, integer_t* info)
      {
        LAPACK_SSYTRF (&uplo, &n, a, &lda, ipiv, w, &lw, info);
      }

      inline
      void sytrf (char const uplo, integer_t const n,
                  double* a, integer_t const lda, integer_t* ipiv,
                  double* w, integer_t const lw, integer_t* info)
      {
        LAPACK_DSYTRF (&uplo, &n, a, &lda, ipiv, w, &lw, info);
      }

      inline
      void sytrf (char const uplo, integer_t const n,
                  traits::complex_f* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_f* w, integer_t const lw, integer_t* info)
      {
        LAPACK_CSYTRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      inline
      void sytrf (char const uplo, integer_t const n,
                  traits::complex_d* a, integer_t const lda, integer_t* ipiv,
                  traits::complex_d* w, integer_t const lw, integer_t* info)
      {
        LAPACK_ZSYTRF (&uplo, &n,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (w), &lw, info);
      }

      template <typename SymmA, typename IVec, typename Work>
      int sytrf (char const ul, SymmA& a, IVec& i, Work& w) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::vector_size (i));

        integer_t info;
        sytrf (ul, n, traits::matrix_storage (a),
               traits::leading_dimension (a),
               traits::vector_storage (i),
               traits::vector_storage (w),
               traits::vector_size (w),
               &info);
        return info;
      }

    }

    template <typename SymmA, typename IVec, typename Work>
    inline
    int sytrf (char const ul, SymmA& a, IVec& i, Work& w)
    {
      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      return detail::sytrf (ul, a, i, w);
    }

    template <typename SymmA, typename IVec, typename Work>
    inline
    int sytrf (SymmA& a, IVec& i, Work& w)
    {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      assert (traits::vector_size (w) >= 1);
      char uplo = traits::matrix_uplo_tag (a);
      return detail::sytrf (uplo, a, i, w);
    }

    template <typename SymmA, typename Ivec>
    int sytrf (char const ul, SymmA& a, Ivec& i)
    {
      // with 'internal' work vector

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      integer_t info = -101;
      integer_t lw = sytrf_work ('O', ul, a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info =  detail::sytrf (ul, a, i, w);
      return info;
    }

    template <typename SymmA, typename Ivec>
    int sytrf (SymmA& a, Ivec& i)
    {
      // with 'internal' work vector

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      integer_t info = -101;
      integer_t lw = sytrf_work ('O', a);
      assert (lw >= 1); // paranoia ?
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<SymmA>::value_type val_t;
#else
      typedef typename SymmA::value_type val_t;
#endif
      traits::detail::array<val_t> w (lw);
      if (w.valid())
        info =  detail::sytrf (uplo, a, i, w);
      return info;
    }

    /*
     * sytrs() solves a system of linear equations A*X = B with
     * a symmetric matrix A using the factorization
     *    A = U * D * U^T   or  A = L * D * L^T
     * computed by sytrf().
     */

    namespace detail {

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  float const* a, integer_t const lda, integer_t const* ipiv,
                  float* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_SSYTRS (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  double const* a, integer_t const lda, integer_t const* ipiv,
                  double* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_DSYTRS (&uplo, &n, &nrhs, a, &lda, ipiv, b, &ldb, info);
      }

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_f const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_f* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_CSYTRS (&uplo, &n, &nrhs,
                      traits::complex_ptr (a), &lda, ipiv,
                      traits::complex_ptr (b), &ldb, info);
      }

      inline
      void sytrs (char const uplo, integer_t const n, integer_t const nrhs,
                  traits::complex_d const* a, integer_t const lda,
                  integer_t const* ipiv,
                  traits::complex_d* b, integer_t const ldb, integer_t* info)
      {
        LAPACK_ZSYTRS (&uplo, &n, &nrhs,
                       traits::complex_ptr (a), &lda, ipiv,
                       traits::complex_ptr (b), &ldb, info);
      }

      template <typename SymmA, typename MatrB, typename IVec>
      int sytrs (char const ul, SymmA const& a, IVec const& i, MatrB& b) {

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::matrix_size1 (b));
        assert (n == traits::vector_size (i));

        integer_t info;
        sytrs (ul, n, traits::matrix_size2 (b),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::matrix_storage (a),
#else
               traits::matrix_storage_const (a),
#endif
               traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
               traits::vector_storage (i),
#else
               traits::vector_storage_const (i),
#endif
               traits::matrix_storage (b),
               traits::leading_dimension (b), &info);
        return info;
      }

    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int sytrs (char const ul, SymmA const& a, IVec const& i, MatrB& b) {

      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::sytrs (ul, a, i, b);
    }

    template <typename SymmA, typename MatrB, typename IVec>
    inline
    int sytrs (SymmA const& a, IVec const& i, MatrB& b) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrB>::matrix_structure,
        traits::general_t
      >::value));
#endif

      char uplo = traits::matrix_uplo_tag (a);
      return detail::sytrs (uplo, a, i, b);
    }

    namespace detail
    {
      inline
      void sytri (char const uplo, integer_t const n, float* a, integer_t const lda,
          integer_t const* ipiv, float* work, integer_t* info)
      {
        LAPACK_SSYTRI (&uplo, &n, a, &lda, ipiv, work, info);
      }

      inline
      void sytri (char const uplo, integer_t const n, double* a, integer_t const lda,
          integer_t const* ipiv, double* work, integer_t* info)
      {
        LAPACK_DSYTRI (&uplo, &n, a, &lda, ipiv, work, info);
      }

      inline
      void sytri (char const uplo, integer_t const n, traits::complex_f* a,
          integer_t const lda, integer_t const* ipiv, traits::complex_f* work, integer_t* info)
      {
        LAPACK_CSYTRI (&uplo, &n, traits::complex_ptr (a), &lda, ipiv,
            traits::complex_ptr (work), info);
      }

      inline
      void sytri (char const uplo, integer_t const n, traits::complex_d* a,
          integer_t const lda, integer_t const* ipiv, traits::complex_d* work, integer_t* info)
      {
        LAPACK_ZSYTRI (&uplo, &n, traits::complex_ptr (a), &lda, ipiv,
            traits::complex_ptr (work), info);
      }

      template <typename SymmA, typename IVec, typename Work>
      int sytri (char const ul, SymmA& a, IVec const& ipiv, Work& work)
      {
        assert (ul == 'U' || ul == 'L');

        integer_t const n = traits::matrix_size1 (a);
        assert (n == traits::matrix_size2 (a));
        assert (n == traits::vector_size (ipiv));
        assert (n == traits::vector_size (work));

        integer_t info;
        //const double* dummy = traits::matrix_storage (a);
        detail::sytri (ul, n, traits::matrix_storage (a),
            traits::leading_dimension (a),
#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
            traits::vector_storage (ipiv),
#else
            traits::vector_storage_const (ipiv),
#endif
            traits::vector_storage (work),
            &info);
        return info;
      }

    } // namespace detail

    //Internal allocation of workspace, general matrix with up/low tag
    template <typename SymmA, typename IVec>
    int sytri (char const ul, SymmA& a, IVec const& ipiv)
    {
      assert (ul == 'U' || ul == 'L');

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
            typename traits::matrix_traits<SymmA>::matrix_structure,
            traits::general_t
            >::value));
#endif

      typedef typename SymmA::value_type value_type;
      std::ptrdiff_t n = traits::matrix_size1(a);
      traits::detail::array<value_type> work(std::max<std::ptrdiff_t>(1,n));

      return detail::sytri (ul, a, ipiv, work);
    }

    //Internal allocation of workspace, symmetric matrix

    /*Warning: the function will work only if SymmA is a
      symmetric_adaptor. With SymmA = symmetric_matrix a
      boost::STATIC_ASSERTION_FAILURE will be thrown at compile
      time, because symmetric_matrix has a symmetric_packed_t
      structure instead of symmetric_t. Use sptri() for
      symmetric packed matrices.
      */
    template <typename SymmA, typename IVec>
    int sytri (SymmA& a, IVec const& ipiv)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<SymmA>::matrix_structure,
        traits::symmetric_t
      >::value));
#endif

      typedef typename SymmA::value_type value_type;
      std::ptrdiff_t n = traits::matrix_size1(a);
      traits::detail::array<value_type> work(std::max<std::ptrdiff_t>(1,n));

      char uplo = traits::matrix_uplo_tag (a);
      return detail::sytri (uplo, a, ipiv, work);
    }

  } // namespace lapack

}}} // namespace boost::numeric::bindings

#endif