// Author: Francesco Regazzoni - MOX, Politecnico di Milano
// Email:  francesco.regazzoni@polimi.it
// Date:   2020

#ifndef MODEL_RDQ20_SE_HPP
#define MODEL_RDQ20_SE_HPP

#include <vector>

#include <Eigen/Dense>

#include "sarcomere.hpp"

class model_RDQ20_SE : public sarcomere {

  //    Class implementing the spatially-explicit ODE model (SE-ODE) for
  //    cardiomyocytes force generation presented in [1, 2].
  //
  //    References:
  //
  //    [1] F. Regazzoni "Mathematical modeling and Machine Learning for the
  //        numerical simulation of cardiac electromechanics", PhD Thesis -
  //        Politecnico di Milano (2020)
  //        http://hdl.handle.net/10589/152617
  //    [2] F. Regazzoni, L. Dede', A. Quarteroni "Biophysically detailed
  //        mathematical models of multiscale cardiac active mechanics",
  //        PLOS Computational Biology (2020)
  //        https://doi.org/10.1371/journal.pcbi.1008294

public:
  // Constructor.
  model_RDQ20_SE(
      std::string parameters_file =
          "../../params/params_RDQ20-SE_human_body-temperature.json");

  // Solve one time step, updating the state passed as reference.
  void solve_time_step(std::vector<double> &state, const double &calcium,
                       const double &sarcomere_length, const double &dSL_dt,
                       const double &dt);

  // Compute the active tension.
  double get_active_tension(const std::vector<double> &state,
                            const double &sarcomere_length);

  // Compute the permissivity.
  double get_permissivity(const std::vector<double> &state);

  // Compute the active stiffness.
  double get_active_stiffness(const std::vector<double> &state,
                              const double &sarcomere_length);

private:
  // Allocate memory to store variables
  void allocate_variables();

  // Initialize transition rates.
  void initialize_rates();

  // Deserialize the model state into state_RU and state_XB.
  void deserialize_state(const std::vector<double> &state);

  // Serialize the model state from state_RU and state_XB.
  void serialize_state(std::vector<double> &state);

  // Update the RU transition rates.
  void RU_update_rates(const double &calcium, const double &sarcomere_length);

  // Update the RU state variables.
  void RU_update_state(const double &dt);

  // Update the XB state variables.
  void XB_update_state(const double &sarcomere_length, const double &dSL_dt,
                       const double &dt);

  // Coordinate of the i-th RU.
  inline double y_j(const unsigned int &i_RU) {
    return prm_LA * (i_RU + 0.5) / prm_n_RU;
  }

  // Coordinate of the left end of the AF.
  inline double yLA(const double &sarcomere_length) {
    return 2 * prm_LA - sarcomere_length;
  }

  // First coordinate of the MF.
  inline double yM0(const double &sarcomere_length) {
    return (2 * prm_LA - sarcomere_length + prm_LB) * 0.5;
  }

  // Second coordinate of the MF.
  inline double yM1(const double &sarcomere_length) {
    return (2 * prm_LA - sarcomere_length + prm_LM) * 0.5;
  }

  // Smoothed indicator function of the MF.
  inline double ChiMF(const double &sarcomere_length,
                      const unsigned int &i_RU) {
    return 0.5 * std::tanh((y_j(i_RU) - yM0(sarcomere_length)) / prm_Lsmooth) +
           0.5 * std::tanh(-(y_j(i_RU) - yM1(sarcomere_length)) / prm_Lsmooth);
  }

  // Smoothed indicator function of the single-overlap region.
  inline double ChiSF(const double &sarcomere_length,
                      const unsigned int &i_RU) {
    return 0.5 +
           0.5 * std::tanh((y_j(i_RU) - yLA(sarcomere_length)) / prm_Lsmooth);
  }

  // Model Parameters.
  unsigned int prm_n_RU; // [-]
  double prm_LA;         // [micro m]
  double prm_LM;         // [micro m]
  double prm_LB;         // [micro m]
  double prm_SL0;        // [micro m]
  double prm_Lsmooth;    // [micro m]
  double prm_Q;          // [-]
  double prm_Kd0;        // [micro M]
  double prm_alphaKd;    // [micro M / micro m]
  double prm_mu;         // [-]
  double prm_gamma;      // [-]
  double prm_Koff;       // [s^-1]
  double prm_Kbasic;     // [s^-1]
  double prm_r0;         // [s^-1]
  double prm_alpha;      // [-]
  double prm_mu0_fP;     // [s^-1]
  double prm_mu1_fP;     // [s^-1]
  double prm_a_XB;       // [kPa]

  // Numerical Parameters.
  double prm_time_step_update_RU_state =
      2.5e-5; // Time step used to update the RU state. [s]

  // Additional variables.
  std::vector<std::array<std::array<std::array<double, 4>, 4>, 4>>
      state_RU;                                // State variables of RU.
  std::vector<std::array<double, 4>> state_XB; // State variables of XB.

  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      rates_RU; // RU rates.

  int permissivity_of_state[4] = {0, 0, 1,
                                  1}; // Permissivity state (0 = N, 1 = P)
                                      // associated with the basic RU states.

  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      flux_RU_L; // Probability flux of left-ward RUs of the triplets.
  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      flux_RU_C; // Probability flux of central RUs of the triplets.
  std::vector<
      std::array<std::array<std::array<std::array<double, 4>, 4>, 4>, 4>>
      flux_RU_R; // Probability flux of right-ward RUs of the triplets.

  Eigen::Matrix<double, 4, 4> XB_A;   // Matrix defining the local XB system.
  Eigen::Matrix<double, 4, 1> XB_rhs; // Right-hand side of the local XB system.
  Eigen::Matrix<double, 4, 1>
      XB_sol; // Vector used to store the solution of the local XB system.

  unsigned int n_states_RU; // Number of RU states.
  unsigned int n_states_XB; // Number of XB states.
  unsigned int i_RU;        // Index of regulatory_unit.
  unsigned int RU_L;        // Index of left regulatory unit.
  unsigned int RU_C;        // Index of central regulatory unit.
  unsigned int RU_R;        // Index of right regulatory unit.
  unsigned int RU_new;      // Index of regulatory unit (new state).
  unsigned int RU_dummy;    // Index of regulatory unit external to the
                            // considered triplet.
  unsigned int i_XB;        // Index of XB state.
};

#endif /* MODEL_RDQ20_SE_HPP */


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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 NT2_ELLIPTIC_FUNCTIONS_SCALAR_ELLIE_HPP_INCLUDED
#define NT2_ELLIPTIC_FUNCTIONS_SCALAR_ELLIE_HPP_INCLUDED
#include <nt2/elliptic/functions/ellie.hpp>
#include <boost/math/special_functions.hpp>
#include <nt2/include/functions/scalar/is_ltz.hpp>
#include <nt2/include/functions/scalar/sin.hpp>
#include <nt2/include/functions/scalar/is_eqz.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/sin.hpp>
#include <nt2/include/functions/scalar/sqr.hpp>
#include <nt2/include/functions/scalar/sqrt.hpp>
#include <nt2/include/functions/scalar/oneminus.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/tan.hpp>
#include <nt2/include/functions/scalar/atan.hpp>
#include <nt2/include/functions/scalar/average.hpp>
#include <nt2/include/functions/scalar/ellpe.hpp>
#include <nt2/include/functions/scalar/ellpk.hpp>
#include <nt2/include/functions/scalar/ceil.hpp>
#include <nt2/sdk/error/policies.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/eps.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellie_, tag::cpu_
                            , (A0)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      return nt2::ellie(result_type(a0), result_type(a1));
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellie_, tag::cpu_
                            , (A0)
                            , (scalar_< double_<A0> >)
                              (scalar_< double_<A0> >)
                            )
  {

    typedef typename boost::dispatch::meta::as_floating<A0>::type result_type;

    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      typedef result_type type;
      if (a1>nt2::One<A0>()||(nt2::is_ltz(a1))) return Nan<type>();
      if (nt2::is_eqz(a1))  return type(a0);
      return boost::math::ellint_2(nt2::sqrt(type(a1)), type(a0), nt2_policy());
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ellie_, tag::cpu_
                            , (A0)
                            , (scalar_< single_<A0> >)
                              (scalar_< single_<A0> >)
                            )
  {
    typedef A0 result_type;
    NT2_FUNCTOR_CALL_REPEAT(2)
    {
      typedef result_type type;
      if (a1>nt2::One<A0>()||(nt2::is_ltz(a1))) return nt2::Nan<type>();
      else if (nt2::is_eqz(a1))
        return a0;
      else if (a1 == nt2::One<A0>())
        return nt2::sin(a0);
      else
      {
        type lphi = nt2::abs(a0);
        type m   =  a1;
        type a = nt2::One<type>();
        type b = nt2::sqrt(nt2::oneminus(m));
        type c = nt2::sqrt(m);
        type d = nt2::One<type>();
        type e = nt2::Zero<type>();
        type t = nt2::tan( lphi );
        int mod = toint((lphi+nt2::Pio_2<type>())/nt2::Pi<type>());
        while( nt2::abs(c) > nt2::Eps<type>()*nt2::abs(a) )
        {
          type temp = b/a;
          lphi = lphi + nt2::atan(t*temp) + mod * nt2::Pi<type>();
          mod = nt2::toint((lphi+nt2::Pio_2<type>())/Pi<type>());
          t *= nt2::oneplus(temp)/( nt2::oneminus(temp * nt2::sqr(t)));
          c = nt2::average(a,-b);
          temp = nt2::sqrt(a*b);
          a = nt2::average(a,b);
          b = temp;
          d += d;
          e += c*nt2::sin(lphi);
        }

        b = nt2::oneminus(m);
        type temp = nt2::ellpe(b)/nt2::ellpk(b);
        temp *= (nt2::atan(t) + mod * nt2::Pi<type>())/(d * a);
        temp += e;
        if(nt2::is_ltz(a0))  temp = -temp;
        return temp ;
      }
      }
  };
} }

#endif


// boost\math\distributions\non_central_chi_squared.hpp

// Copyright John Maddock 2008.

// 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_MATH_SPECIAL_NON_CENTRAL_CHI_SQUARE_HPP
#define BOOST_MATH_SPECIAL_NON_CENTRAL_CHI_SQUARE_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp> // for incomplete gamma. gamma_q
#include <boost/math/special_functions/bessel.hpp> // for cyl_bessel_i
#include <boost/math/special_functions/round.hpp> // for iround
#include <boost/math/distributions/complement.hpp> // complements
#include <boost/math/distributions/chi_squared.hpp> // central distribution
#include <boost/math/distributions/detail/common_error_handling.hpp> // error checks
#include <boost/math/special_functions/fpclassify.hpp> // isnan.
#include <boost/math/tools/roots.hpp> // for root finding.
#include <boost/math/distributions/detail/generic_mode.hpp>
#include <boost/math/distributions/detail/generic_quantile.hpp>

namespace boost
{
   namespace math
   {

      template <class RealType, class Policy>
      class non_central_chi_squared_distribution;

      namespace detail{

         template <class T, class Policy>
         T non_central_chi_square_q(T x, T f, T theta, const Policy& pol, T init_sum = 0)
         {
            //
            // Computes the complement of the Non-Central Chi-Square
            // Distribution CDF by summing a weighted sum of complements
            // of the central-distributions.  The weighting factor is
            // a Poisson Distribution.
            //
            // This is an application of the technique described in:
            //
            // Computing discrete mixtures of continuous
            // distributions: noncentral chisquare, noncentral t
            // and the distribution of the square of the sample
            // multiple correlation coeficient.
            // D. Benton, K. Krishnamoorthy.
            // Computational Statistics & Data Analysis 43 (2003) 249 - 267
            //
            BOOST_MATH_STD_USING

            // Special case:
            if(x == 0)
               return 1;

            //
            // Initialize the variables we'll be using:
            //
            T lambda = theta / 2;
            T del = f / 2;
            T y = x / 2;
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T sum = init_sum;
            //
            // k is the starting location for iteration, we'll
            // move both forwards and backwards from this point.
            // k is chosen as the peek of the Poisson weights, which
            // will occur *before* the largest term.
            //
            int k = iround(lambda, pol);
            // Forwards and backwards Poisson weights:
            T poisf = boost::math::gamma_p_derivative(1 + k, lambda, pol);
            T poisb = poisf * k / lambda;
            // Initial forwards central chi squared term:
            T gamf = boost::math::gamma_q(del + k, y, pol);
            // Forwards and backwards recursion terms on the central chi squared:
            T xtermf = boost::math::gamma_p_derivative(del + 1 + k, y, pol);
            T xtermb = xtermf * (del + k) / y;
            // Initial backwards central chi squared term:
            T gamb = gamf - xtermb;

            //
            // Forwards iteration first, this is the
            // stable direction for the gamma function
            // recurrences:
            //
            int i;
            for(i = k; static_cast<boost::uintmax_t>(i-k) < max_iter; ++i)
            {
               T term = poisf * gamf;
               sum += term;
               poisf *= lambda / (i + 1);
               gamf += xtermf;
               xtermf *= y / (del + i + 1);
               if(((sum == 0) || (fabs(term / sum) < errtol)) && (term >= poisf * gamf))
                  break;
            }
            //Error check:
            if(static_cast<boost::uintmax_t>(i-k) >= max_iter)
               policies::raise_evaluation_error(
                  "cdf(non_central_chi_squared_distribution<%1%>, %1%)",
                  "Series did not converge, closest value was %1%", sum, pol);
            //
            // Now backwards iteration: the gamma
            // function recurrences are unstable in this
            // direction, we rely on the terms deminishing in size
            // faster than we introduce cancellation errors.
            // For this reason it's very important that we start
            // *before* the largest term so that backwards iteration
            // is strictly converging.
            //
            for(i = k - 1; i >= 0; --i)
            {
               T term = poisb * gamb;
               sum += term;
               poisb *= i / lambda;
               xtermb *= (del + i) / y;
               gamb -= xtermb;
               if((sum == 0) || (fabs(term / sum) < errtol))
                  break;
            }

            return sum;
         }

         template <class T, class Policy>
         T non_central_chi_square_p_ding(T x, T f, T theta, const Policy& pol, T init_sum = 0)
         {
            //
            // This is an implementation of:
            //
            // Algorithm AS 275:
            // Computing the Non-Central #2 Distribution Function
            // Cherng G. Ding
            // Applied Statistics, Vol. 41, No. 2. (1992), pp. 478-482.
            //
            // This uses a stable forward iteration to sum the
            // CDF, unfortunately this can not be used for large
            // values of the non-centrality parameter because:
            // * The first term may underfow to zero.
            // * We may need an extra-ordinary number of terms
            //   before we reach the first *significant* term.
            //
            BOOST_MATH_STD_USING
            // Special case:
            if(x == 0)
               return 0;
            T tk = boost::math::gamma_p_derivative(f/2 + 1, x/2, pol);
            T lambda = theta / 2;
            T vk = exp(-lambda);
            T uk = vk;
            T sum = init_sum + tk * vk;
            if(sum == 0)
               return sum;

            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();

            int i;
            T lterm(0), term(0);
            for(i = 1; static_cast<boost::uintmax_t>(i) < max_iter; ++i)
            {
               tk = tk * x / (f + 2 * i);
               uk = uk * lambda / i;
               vk = vk + uk;
               lterm = term;
               term = vk * tk;
               sum += term;
               if((fabs(term / sum) < errtol) && (term <= lterm))
                  break;
            }
            //Error check:
            if(static_cast<boost::uintmax_t>(i) >= max_iter)
               policies::raise_evaluation_error(
                  "cdf(non_central_chi_squared_distribution<%1%>, %1%)",
                  "Series did not converge, closest value was %1%", sum, pol);
            return sum;
         }

         template <class T, class Policy>
         T non_central_chi_square_p(T y, T n, T lambda, const Policy& pol, T init_sum)
         {
            //
            // This is taken more or less directly from:
            //
            // Computing discrete mixtures of continuous
            // distributions: noncentral chisquare, noncentral t
            // and the distribution of the square of the sample
            // multiple correlation coeficient.
            // D. Benton, K. Krishnamoorthy.
            // Computational Statistics & Data Analysis 43 (2003) 249 - 267
            //
            // We're summing a Poisson weighting term multiplied by
            // a central chi squared distribution.
            //
            BOOST_MATH_STD_USING
            // Special case:
            if(y == 0)
               return 0;
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T errorf(0), errorb(0);

            T x = y / 2;
            T del = lambda / 2;
            //
            // Starting location for the iteration, we'll iterate
            // both forwards and backwards from this point.  The
            // location chosen is the maximum of the Poisson weight
            // function, which ocurrs *after* the largest term in the
            // sum.
            //
            int k = iround(del, pol);
            T a = n / 2 + k;
            // Central chi squared term for forward iteration:
            T gamkf = boost::math::gamma_p(a, x, pol);

            if(lambda == 0)
               return gamkf;
            // Central chi squared term for backward iteration:
            T gamkb = gamkf;
            // Forwards Poisson weight:
            T poiskf = gamma_p_derivative(k+1, del, pol);
            // Backwards Poisson weight:
            T poiskb = poiskf;
            // Forwards gamma function recursion term:
            T xtermf = boost::math::gamma_p_derivative(a, x, pol);
            // Backwards gamma function recursion term:
            T xtermb = xtermf * x / a;
            T sum = init_sum + poiskf * gamkf;
            if(sum == 0)
               return sum;
            int i = 1;
            //
            // Backwards recursion first, this is the stable
            // direction for gamma function recurrences:
            //
            while(i <= k)
            {
               xtermb *= (a - i + 1) / x;
               gamkb += xtermb;
               poiskb = poiskb * (k - i + 1) / del;
               errorf = errorb;
               errorb = gamkb * poiskb;
               sum += errorb;
               if((fabs(errorb / sum) < errtol) && (errorb <= errorf))
                  break;
               ++i;
            }
            i = 1;
            //
            // Now forwards recursion, the gamma function
            // recurrence relation is unstable in this direction,
            // so we rely on the magnitude of successive terms
            // decreasing faster than we introduce cancellation error.
            // For this reason it's vital that k is chosen to be *after*
            // the largest term, so that successive forward iterations
            // are strictly (and rapidly) converging.
            //
            do
            {
               xtermf = xtermf * x / (a + i - 1);
               gamkf = gamkf - xtermf;
               poiskf = poiskf * del / (k + i);
               errorf = poiskf * gamkf;
               sum += errorf;
               ++i;
            }while((fabs(errorf / sum) > errtol) && (static_cast<boost::uintmax_t>(i) < max_iter));

            //Error check:
            if(static_cast<boost::uintmax_t>(i) >= max_iter)
               policies::raise_evaluation_error(
                  "cdf(non_central_chi_squared_distribution<%1%>, %1%)",
                  "Series did not converge, closest value was %1%", sum, pol);

            return sum;
         }

         template <class T, class Policy>
         T non_central_chi_square_pdf(T x, T n, T lambda, const Policy& pol)
         {
            //
            // As above but for the PDF:
            //
            BOOST_MATH_STD_USING
            boost::uintmax_t max_iter = policies::get_max_series_iterations<Policy>();
            T errtol = boost::math::policies::get_epsilon<T, Policy>();
            T x2 = x / 2;
            T n2 = n / 2;
            T l2 = lambda / 2;
            T sum = 0;
            int k = itrunc(l2);
            T pois = gamma_p_derivative(k + 1, l2, pol) * gamma_p_derivative(n2 + k, x2);
            if(pois == 0)
               return 0;
            T poisb = pois;
            for(int i = k; ; ++i)
            {
               sum += pois;
               if(pois / sum < errtol)
                  break;
               if(static_cast<boost::uintmax_t>(i - k) >= max_iter)
                  return policies::raise_evaluation_error(
                     "pdf(non_central_chi_squared_distribution<%1%>, %1%)",
                     "Series did not converge, closest value was %1%", sum, pol);
               pois *= l2 * x2 / ((i + 1) * (n2 + i));
            }
            for(int i = k - 1; i >= 0; --i)
            {
               poisb *= (i + 1) * (n2 + i) / (l2 * x2);
               sum += poisb;
               if(poisb / sum < errtol)
                  break;
            }
            return sum / 2;
         }

         template <class RealType, class Policy>
         inline RealType non_central_chi_squared_cdf(RealType x, RealType k, RealType l, bool invert, const Policy&)
         {
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            BOOST_MATH_STD_USING
            value_type result;
            if(l == 0)
               result = cdf(boost::math::chi_squared_distribution<RealType, Policy>(k), x);
            else if(x > k + l)
            {
               // Complement is the smaller of the two:
               result = detail::non_central_chi_square_q(
                  static_cast<value_type>(x),
                  static_cast<value_type>(k),
                  static_cast<value_type>(l),
                  forwarding_policy(),
                  static_cast<value_type>(invert ? 0 : -1));
               invert = !invert;
            }
            else if(l < 200)
            {
               // For small values of the non-centrality parameter
               // we can use Ding's method:
               result = detail::non_central_chi_square_p_ding(
                  static_cast<value_type>(x),
                  static_cast<value_type>(k),
                  static_cast<value_type>(l),
                  forwarding_policy(),
                  static_cast<value_type>(invert ? -1 : 0));
            }
            else
            {
               // For largers values of the non-centrality
               // parameter Ding's method will consume an
               // extra-ordinary number of terms, and worse
               // may return zero when the result is in fact
               // finite, use Krishnamoorthy's method instead:
               result = detail::non_central_chi_square_p(
                  static_cast<value_type>(x),
                  static_cast<value_type>(k),
                  static_cast<value_type>(l),
                  forwarding_policy(),
                  static_cast<value_type>(invert ? -1 : 0));
            }
            if(invert)
               result = -result;
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               "boost::math::non_central_chi_squared_cdf<%1%>(%1%, %1%, %1%)");
         }

         template <class T, class Policy>
         struct nccs_quantile_functor
         {
            nccs_quantile_functor(const non_central_chi_squared_distribution<T,Policy>& d, T t, bool c)
               : dist(d), target(t), comp(c) {}

            T operator()(const T& x)
            {
               return comp ?
                  target - cdf(complement(dist, x))
                  : cdf(dist, x) - target;
            }

         private:
            non_central_chi_squared_distribution<T,Policy> dist;
            T target;
            bool comp;
         };

         template <class RealType, class Policy>
         RealType nccs_quantile(const non_central_chi_squared_distribution<RealType, Policy>& dist, const RealType& p, bool comp)
         {
            static const char* function = "quantile(non_central_chi_squared_distribution<%1%>, %1%)";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            value_type k = dist.degrees_of_freedom();
            value_type l = dist.non_centrality();
            value_type r;
            if(!detail::check_df(
               function,
               k, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !detail::check_probability(
               function,
               static_cast<value_type>(p),
               &r,
               Policy()))
                  return (RealType)r;

            value_type b = (l * l) / (k + 3 * l);
            value_type c = (k + 3 * l) / (k + 2 * l);
            value_type ff = (k + 2 * l) / (c * c);
            value_type guess;
            if(comp)
               guess = b + c * quantile(complement(chi_squared_distribution<value_type, forwarding_policy>(ff), p));
            else
               guess = b + c * quantile(chi_squared_distribution<value_type, forwarding_policy>(ff), p);

            if(guess < 0)
               guess = tools::min_value<value_type>();

            value_type result = detail::generic_quantile(
               non_central_chi_squared_distribution<value_type, forwarding_policy>(k, l),
               p,
               guess,
               comp,
               function);
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }

         template <class RealType, class Policy>
         RealType nccs_pdf(const non_central_chi_squared_distribution<RealType, Policy>& dist, const RealType& x)
         {
            BOOST_MATH_STD_USING
            static const char* function = "pdf(non_central_chi_squared_distribution<%1%>, %1%)";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;

            value_type k = dist.degrees_of_freedom();
            value_type l = dist.non_centrality();
            value_type r;
            if(!detail::check_df(
               function,
               k, &r, Policy())
               ||
            !detail::check_non_centrality(
               function,
               l,
               &r,
               Policy())
               ||
            !detail::check_positive_x(
               function,
               (value_type)x,
               &r,
               Policy()))
                  return (RealType)r;

         if(l == 0)
            return pdf(boost::math::chi_squared_distribution<RealType, forwarding_policy>(dist.degrees_of_freedom()), x);

         // Special case:
         if(x == 0)
            return 0;
         if(l > 50)
         {
            r = non_central_chi_square_pdf(static_cast<value_type>(x), k, l, forwarding_policy());
         }
         else
         {
            r = log(x / l) * (k / 4 - 0.5f) - (x + l) / 2;
            if(fabs(r) >= tools::log_max_value<RealType>() / 4)
            {
               r = non_central_chi_square_pdf(static_cast<value_type>(x), k, l, forwarding_policy());
            }
            else
            {
               r = exp(r);
               r = 0.5f * r
                  * boost::math::cyl_bessel_i(k/2 - 1, sqrt(l * x), forwarding_policy());
            }
         }
         return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               r,
               function);
         }

         template <class RealType, class Policy>
         struct degrees_of_freedom_finder
         {
            degrees_of_freedom_finder(
               RealType lam_, RealType x_, RealType p_, bool c)
               : lam(lam_), x(x_), p(p_), comp(c) {}

            RealType operator()(const RealType& v)
            {
               non_central_chi_squared_distribution<RealType, Policy> d(v, lam);
               return comp ?
                  RealType(p - cdf(complement(d, x)))
                  : RealType(cdf(d, x) - p);
            }
         private:
            RealType lam;
            RealType x;
            RealType p;
            bool comp;
         };

         template <class RealType, class Policy>
         inline RealType find_degrees_of_freedom(
            RealType lam, RealType x, RealType p, RealType q, const Policy& pol)
         {
            const char* function = "non_central_chi_squared<%1%>::find_degrees_of_freedom";
            if((p == 0) || (q == 0))
            {
               //
               // Can't a thing if one of p and q is zero:
               //
               return policies::raise_evaluation_error<RealType>(function,
                  "Can't find degrees of freedom when the probability is 0 or 1, only possible answer is %1%",
                  RealType(std::numeric_limits<RealType>::quiet_NaN()), Policy());
            }
            degrees_of_freedom_finder<RealType, Policy> f(lam, x, p < q ? p : q, p < q ? false : true);
            tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
            boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
            //
            // Pick an initial guess that we know will give us a probability
            // right around 0.5.
            //
            RealType guess = x - lam;
            if(guess < 1)
               guess = 1;
            std::pair<RealType, RealType> ir = tools::bracket_and_solve_root(
               f, guess, RealType(2), false, tol, max_iter, pol);
            RealType result = ir.first + (ir.second - ir.first) / 2;
            if(max_iter >= policies::get_max_root_iterations<Policy>())
            {
               policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
                  " or there is no answer to problem.  Current best guess is %1%", result, Policy());
            }
            return result;
         }

         template <class RealType, class Policy>
         struct non_centrality_finder
         {
            non_centrality_finder(
               RealType v_, RealType x_, RealType p_, bool c)
               : v(v_), x(x_), p(p_), comp(c) {}

            RealType operator()(const RealType& lam)
            {
               non_central_chi_squared_distribution<RealType, Policy> d(v, lam);
               return comp ?
                  RealType(p - cdf(complement(d, x)))
                  : RealType(cdf(d, x) - p);
            }
         private:
            RealType v;
            RealType x;
            RealType p;
            bool comp;
         };

         template <class RealType, class Policy>
         inline RealType find_non_centrality(
            RealType v, RealType x, RealType p, RealType q, const Policy& pol)
         {
            const char* function = "non_central_chi_squared<%1%>::find_non_centrality";
            if((p == 0) || (q == 0))
            {
               //
               // Can't do a thing if one of p and q is zero:
               //
               return policies::raise_evaluation_error<RealType>(function,
                  "Can't find non centrality parameter when the probability is 0 or 1, only possible answer is %1%",
                  RealType(std::numeric_limits<RealType>::quiet_NaN()), Policy());
            }
            non_centrality_finder<RealType, Policy> f(v, x, p < q ? p : q, p < q ? false : true);
            tools::eps_tolerance<RealType> tol(policies::digits<RealType, Policy>());
            boost::uintmax_t max_iter = policies::get_max_root_iterations<Policy>();
            //
            // Pick an initial guess that we know will give us a probability
            // right around 0.5.
            //
            RealType guess = x - v;
            if(guess < 1)
               guess = 1;
            std::pair<RealType, RealType> ir = tools::bracket_and_solve_root(
               f, guess, RealType(2), false, tol, max_iter, pol);
            RealType result = ir.first + (ir.second - ir.first) / 2;
            if(max_iter >= policies::get_max_root_iterations<Policy>())
            {
               policies::raise_evaluation_error<RealType>(function, "Unable to locate solution in a reasonable time:"
                  " or there is no answer to problem.  Current best guess is %1%", result, Policy());
            }
            return result;
         }

      }

      template <class RealType = double, class Policy = policies::policy<> >
      class non_central_chi_squared_distribution
      {
      public:
         typedef RealType value_type;
         typedef Policy policy_type;

         non_central_chi_squared_distribution(RealType df_, RealType lambda) : df(df_), ncp(lambda)
         {
            const char* function = "boost::math::non_central_chi_squared_distribution<%1%>::non_central_chi_squared_distribution(%1%,%1%)";
            RealType r;
            detail::check_df(
               function,
               df, &r, Policy());
            detail::check_non_centrality(
               function,
               ncp,
               &r,
               Policy());
         } // non_central_chi_squared_distribution constructor.

         RealType degrees_of_freedom() const
         { // Private data getter function.
            return df;
         }
         RealType non_centrality() const
         { // Private data getter function.
            return ncp;
         }
         static RealType find_degrees_of_freedom(RealType lam, RealType x, RealType p)
         {
            const char* function = "non_central_chi_squared<%1%>::find_degrees_of_freedom";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;
            value_type result = detail::find_degrees_of_freedom(
               static_cast<value_type>(lam),
               static_cast<value_type>(x),
               static_cast<value_type>(p),
               static_cast<value_type>(1-p),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
         template <class A, class B, class C>
         static RealType find_degrees_of_freedom(const complemented3_type<A,B,C>& c)
         {
            const char* function = "non_central_chi_squared<%1%>::find_degrees_of_freedom";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;
            value_type result = detail::find_degrees_of_freedom(
               static_cast<value_type>(c.dist),
               static_cast<value_type>(c.param1),
               static_cast<value_type>(1-c.param2),
               static_cast<value_type>(c.param2),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
         static RealType find_non_centrality(RealType v, RealType x, RealType p)
         {
            const char* function = "non_central_chi_squared<%1%>::find_non_centrality";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;
            value_type result = detail::find_non_centrality(
               static_cast<value_type>(v),
               static_cast<value_type>(x),
               static_cast<value_type>(p),
               static_cast<value_type>(1-p),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
         template <class A, class B, class C>
         static RealType find_non_centrality(const complemented3_type<A,B,C>& c)
         {
            const char* function = "non_central_chi_squared<%1%>::find_non_centrality";
            typedef typename policies::evaluation<RealType, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;
            value_type result = detail::find_non_centrality(
               static_cast<value_type>(c.dist),
               static_cast<value_type>(c.param1),
               static_cast<value_type>(1-c.param2),
               static_cast<value_type>(c.param2),
               forwarding_policy());
            return policies::checked_narrowing_cast<RealType, forwarding_policy>(
               result,
               function);
         }
      private:
         // Data member, initialized by constructor.
         RealType df; // degrees of freedom.
         RealType ncp; // non-centrality parameter
      }; // template <class RealType, class Policy> class non_central_chi_squared_distribution

      typedef non_central_chi_squared_distribution<double> non_central_chi_squared; // Reserved name of type double.

      // Non-member functions to give properties of the distribution.

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> range(const non_central_chi_squared_distribution<RealType, Policy>& /* dist */)
      { // Range of permissible values for random variable k.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>()); // Max integer?
      }

      template <class RealType, class Policy>
      inline const std::pair<RealType, RealType> support(const non_central_chi_squared_distribution<RealType, Policy>& /* dist */)
      { // Range of supported values for random variable k.
         // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
         using boost::math::tools::max_value;
         return std::pair<RealType, RealType>(static_cast<RealType>(0),  max_value<RealType>());
      }

      template <class RealType, class Policy>
      inline RealType mean(const non_central_chi_squared_distribution<RealType, Policy>& dist)
      { // Mean of poisson distribution = lambda.
         const char* function = "boost::math::non_central_chi_squared_distribution<%1%>::mean()";
         RealType k = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            k, &r, Policy())
            ||
         !detail::check_non_centrality(
            function,
            l,
            &r,
            Policy()))
               return r;
         return k + l;
      } // mean

      template <class RealType, class Policy>
      inline RealType mode(const non_central_chi_squared_distribution<RealType, Policy>& dist)
      { // mode.
         static const char* function = "mode(non_central_chi_squared_distribution<%1%> const&)";

         RealType k = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            k, &r, Policy())
            ||
         !detail::check_non_centrality(
            function,
            l,
            &r,
            Policy()))
               return (RealType)r;
         return detail::generic_find_mode(dist, 1 + k, function);
      }

      template <class RealType, class Policy>
      inline RealType variance(const non_central_chi_squared_distribution<RealType, Policy>& dist)
      { // variance.
         const char* function = "boost::math::non_central_chi_squared_distribution<%1%>::variance()";
         RealType k = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            k, &r, Policy())
            ||
         !detail::check_non_centrality(
            function,
            l,
            &r,
            Policy()))
               return r;
         return 2 * (2 * l + k);
      }

      // RealType standard_deviation(const non_central_chi_squared_distribution<RealType, Policy>& dist)
      // standard_deviation provided by derived accessors.

      template <class RealType, class Policy>
      inline RealType skewness(const non_central_chi_squared_distribution<RealType, Policy>& dist)
      { // skewness = sqrt(l).
         const char* function = "boost::math::non_central_chi_squared_distribution<%1%>::skewness()";
         RealType k = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            k, &r, Policy())
            ||
         !detail::check_non_centrality(
            function,
            l,
            &r,
            Policy()))
               return r;
         BOOST_MATH_STD_USING
            return pow(2 / (k + 2 * l), RealType(3)/2) * (k + 3 * l);
      }

      template <class RealType, class Policy>
      inline RealType kurtosis_excess(const non_central_chi_squared_distribution<RealType, Policy>& dist)
      {
         const char* function = "boost::math::non_central_chi_squared_distribution<%1%>::kurtosis_excess()";
         RealType k = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            k, &r, Policy())
            ||
         !detail::check_non_centrality(
            function,
            l,
            &r,
            Policy()))
               return r;
         return 12 * (k + 4 * l) / ((k + 2 * l) * (k + 2 * l));
      } // kurtosis_excess

      template <class RealType, class Policy>
      inline RealType kurtosis(const non_central_chi_squared_distribution<RealType, Policy>& dist)
      {
         return kurtosis_excess(dist) + 3;
      }

      template <class RealType, class Policy>
      inline RealType pdf(const non_central_chi_squared_distribution<RealType, Policy>& dist, const RealType& x)
      { // Probability Density/Mass Function.
         return detail::nccs_pdf(dist, x);
      } // pdf

      template <class RealType, class Policy>
      RealType cdf(const non_central_chi_squared_distribution<RealType, Policy>& dist, const RealType& x)
      {
         const char* function = "boost::math::non_central_chi_squared_distribution<%1%>::cdf(%1%)";
         RealType k = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            k, &r, Policy())
            ||
         !detail::check_non_centrality(
            function,
            l,
            &r,
            Policy())
            ||
         !detail::check_positive_x(
            function,
            x,
            &r,
            Policy()))
               return r;

         return detail::non_central_chi_squared_cdf(x, k, l, false, Policy());
      } // cdf

      template <class RealType, class Policy>
      RealType cdf(const complemented2_type<non_central_chi_squared_distribution<RealType, Policy>, RealType>& c)
      { // Complemented Cumulative Distribution Function
         const char* function = "boost::math::non_central_chi_squared_distribution<%1%>::cdf(%1%)";
         non_central_chi_squared_distribution<RealType, Policy> const& dist = c.dist;
         RealType x = c.param;
         RealType k = dist.degrees_of_freedom();
         RealType l = dist.non_centrality();
         RealType r;
         if(!detail::check_df(
            function,
            k, &r, Policy())
            ||
         !detail::check_non_centrality(
            function,
            l,
            &r,
            Policy())
            ||
         !detail::check_positive_x(
            function,
            x,
            &r,
            Policy()))
               return r;

         return detail::non_central_chi_squared_cdf(x, k, l, true, Policy());
      } // ccdf

      template <class RealType, class Policy>
      inline RealType quantile(const non_central_chi_squared_distribution<RealType, Policy>& dist, const RealType& p)
      { // Quantile (or Percent Point) function.
         return detail::nccs_quantile(dist, p, false);
      } // quantile

      template <class RealType, class Policy>
      inline RealType quantile(const complemented2_type<non_central_chi_squared_distribution<RealType, Policy>, RealType>& c)
      { // Quantile (or Percent Point) function.
         return detail::nccs_quantile(c.dist, c.param, true);
      } // quantile complement.

   } // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_SPECIAL_NON_CENTRAL_CHI_SQUARE_HPP


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson, John B. Mains
//
// 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.

#ifndef SMOOTH__FEEDBACK__QP_HPP_
#define SMOOTH__FEEDBACK__QP_HPP_

/**
 * @file
 * @brief Quadratic Program definition.
 */

#include <Eigen/Dense>
#include <Eigen/Sparse>

namespace smooth::feedback {

/**
 * @brief Quadratic program definition.
 *
 * @tparam M number of constraints
 * @tparam N number of variables
 *
 * The quadratic program is on the form
 * \f[
 * \begin{cases}
 *  \min_{x} & \frac{1}{2} x^T P x + q^T x, \\
 *  \text{s.t.} & l \leq A x \leq u,
 * \end{cases}
 * \f]
 * where \f$ P \in \mathbb{R}^{n \times n}, q \in \mathbb{R}^n, l, u \in \mathbb{R}^m, A \in
 * \mathbb{R}^{m \times n} \f$.
 */
template<Eigen::Index M, Eigen::Index N, typename Scalar = double>
struct QuadraticProgram
{
  /// Positive semi-definite square cost (only upper triangular part is used)
  Eigen::Matrix<Scalar, N, N> P;
  /// Linear cost
  Eigen::Matrix<Scalar, N, 1> q;

  /// Inequality matrix
  Eigen::Matrix<Scalar, M, N> A;
  /// Inequality lower bound
  Eigen::Matrix<Scalar, M, 1> l;
  /// Inequality upper bound
  Eigen::Matrix<Scalar, M, 1> u;
};

/**
 * @brief Sparse quadratic program definition.
 *
 * The quadratic program is on the form
 * \f[
 * \begin{cases}
 *  \min_{x} & \frac{1}{2} x^T P x + q^T x, \\
 *  \text{s.t.} & l \leq A x \leq u,
 * \end{cases}
 * \f]
 * where \f$ P \in \mathbb{R}^{n \times n}, q \in \mathbb{R}^n, l, u \in \mathbb{R}^m, A \in
 * \mathbb{R}^{m \times n} \f$.
 */
template<typename Scalar = double>
struct QuadraticProgramSparse
{
  /// Positive semi-definite square cost (only upper trianglular part is used)
  Eigen::SparseMatrix<Scalar> P;
  /// Linear cost
  Eigen::Matrix<Scalar, -1, 1> q;

  /**
   * @brief Inequality matrix
   *
   * @note The constraint matrix is stored in row-major format,
   * i.e. coefficients for each constraint are contiguous in memory
   */
  Eigen::SparseMatrix<Scalar, Eigen::RowMajor> A;
  /// Inequality lower bound
  Eigen::Matrix<Scalar, -1, 1> l;
  /// Inequality upper bound
  Eigen::Matrix<Scalar, -1, 1> u;
};

/// @brief Solver exit codes
enum class QPSolutionStatus {
  Optimal,           /// @brief Solution satisifes optimality condition. Solution is polished if
                     /// `QPSolverParams::polish = true`.
  PolishFailed,      /// @brief Solution satisfies optimality condition but is not polished
  PrimalInfeasible,  /// @brief A certificate of primal infeasibility was found, no solution
                     /// returned
  DualInfeasible,    /// @brief A certificate of dual infeasibility was found, no solution returned
  MaxIterations,  /// @brief Max number of iterations was reached, returned solution is not optimal
  MaxTime,        /// @brief Max time was reached, returned solution is not optimal
  Unknown         /// @brief Solution is useless because of other reasons, no solution returned
};

/// Solver solution
template<Eigen::Index M, Eigen::Index N, typename Scalar = double>
struct QPSolution
{
  /// Exit code
  QPSolutionStatus code = QPSolutionStatus::Unknown;
  /// Number of iterations
  uint32_t iter;
  /// Primal vector
  Eigen::Matrix<Scalar, N, 1> primal;
  /// Dual vector
  Eigen::Matrix<Scalar, M, 1> dual;
  /// Solution objective value
  Scalar objective{0.};
};

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__QP_HPP_


/*
 * Copyright (c) 2013-2015 Masahide Kashiwagi (kashi@waseda.jp)
 */

#ifndef INTERVAL_VECTOR_HPP
#define INTERVAL_VECTOR_HPP

// utilities for interval vector/matrix

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <kv/interval.hpp>

namespace kv {

namespace ub = boost::numeric::ublas;

template <class T> inline ub::vector<T> mid (const ub::vector< interval<T> >& I) {
	int i;
	int s = I.size();
	ub::vector<T> r(s);

	for (i=0; i<s; i++) {
		r(i) = mid(I(i));
	}

	return r;
}

template <class T> inline ub::matrix<T> mid (const ub::matrix< interval<T> >& I) {
	int i, j;
	int s1 = I.size1();
	int s2 = I.size2();
	ub::matrix<T> r(s1, s2);

	for (i=0; i<s1; i++) {
		for (j=0; j<s2; j++) {
			r(i,j) = mid(I(i,j));
		}
	}

	return r;
}

template <class T> inline ub::vector<T> rad (const ub::vector< interval<T> >& I) {
	int i;
	int s = I.size();
	ub::vector<T> r(s);

	for (i=0; i<s; i++) {
		r(i) = rad(I(i));
	}

	return r;
}

template <class T> inline ub::matrix<T> rad (const ub::matrix< interval<T> >& I) {
	int i, j;
	int s1 = I.size1();
	int s2 = I.size2();
	ub::matrix<T> r(s1, s2);

	for (i=0; i<s1; i++) {
		for (j=0; j<s2; j++) {
			r(i,j) = rad(I(i,j));
		}
	}

	return r;
}

template <class T> inline ub::vector<T> mag (const ub::vector< interval<T> >& I) {
	int i;
	int s = I.size();
	ub::vector<T> r(s);

	for (i=0; i<s; i++) {
		r(i) = mag(I(i));
	}

	return r;
}

template <class T> inline ub::matrix<T> mag (const ub::matrix< interval<T> >& I) {
	int i, j;
	int s1 = I.size1();
	int s2 = I.size2();
	ub::matrix<T> r(s1, s2);

	for (i=0; i<s1; i++) {
		for (j=0; j<s2; j++) {
			r(i,j) = mag(I(i,j));
		}
	}

	return r;
}

template <class T> inline ub::vector<T> mag (const ub::vector< T >& v) {
	int i;
	int s = v.size();
	ub::vector<T> r(s);

	for (i=0; i<s; i++) {
		r(i) = mag(v(i));
	}

	return r;
}

template <class T> inline ub::matrix<T> mag (const ub::matrix< T >& m) {
	int i, j;
	int s1 = m.size1();
	int s2 = m.size2();
	ub::matrix<T> r(s1, s2);

	for (i=0; i<s1; i++) {
		for (j=0; j<s2; j++) {
			r(i,j) = mag(m(i,j));
		}
	}

	return r;
}

template <class T> inline ub::vector<T> mig (const ub::vector< interval<T> >& I) {
	int i;
	int s = I.size();
	ub::vector<T> r(s);

	for (i=0; i<s; i++) {
		r(i) = mig(I(i));
	}

	return r;
}

template <class T> inline ub::matrix<T> mig (const ub::matrix< interval<T> >& I) {
	int i, j;
	int s1 = I.size1();
	int s2 = I.size2();
	ub::matrix<T> r(s1, s2);

	for (i=0; i<s1; i++) {
		for (j=0; j<s2; j++) {
			r(i,j) = mig(I(i,j));
		}
	}

	return r;
}

template <class T> inline bool zero_in (const ub::vector< interval<T> >& I) {
	int i;
	int s = I.size();

	for (i=0; i<s; i++) {
		if (!zero_in(I(i))) return false;
	}

	return true;
}

template <class T> inline bool subset (const ub::vector< interval<T> >& I, const ub::vector< interval<T> > & J) {
	int i;
	int s = I.size();

	for (i=0; i<s; i++) {
		if (!subset(I(i), J(i))) return false;
	}

	return true;
}

template <class T> inline bool proper_subset (const ub::vector< interval<T> >& I, const ub::vector< interval<T> > & J) {
	int i;
	int s = I.size();

	for (i=0; i<s; i++) {
		if (!proper_subset(I(i), J(i))) return false;
	}

	return true;
}

template <class T> inline bool overlap (const ub::vector< interval<T> >& I, const ub::vector< interval<T> > & J) {
	int i;
	int s = I.size();

	for (i=0; i<s; i++) {
		if (!overlap(I(i), J(i))) return false;
	}

	return true;
}

template <class T> inline ub::vector< interval<T> > intersect (const ub::vector< interval<T> >& I, const ub::vector< interval<T> > & J) {
	int i;
	int s = I.size();
	ub::vector< interval<T> > r(s);

	for (i=0; i<s; i++) {
		r(i) = intersect(I(i), J(i));
	}

	return r;
}

template <class T> inline ub::matrix< interval<T> > intersect (const ub::matrix< interval<T> >& I, const ub::matrix< interval<T> > & J) {
	int i, j;
	int s1 = I.size1();
	int s2 = I.size2();
	ub::matrix< interval<T> > r(s1, s2);

	for (i=0; i<s1; i++) {
		for (j=0; j<s2; j++) {
			r(i, j) = intersect(I(i, j), J(i, j));
		}
	}

	return r;
}

template <class T> inline ub::vector< interval<T> > hull (const ub::vector< interval<T> >& I, const ub::vector< interval<T> > & J) {
	int i;
	int s = I.size();
	ub::vector< interval<T> > r(s);

	for (i=0; i<s; i++) {
		r(i) = interval<T>::hull(I(i), J(i));
	}

	return r;
}

template <class T> inline ub::matrix< interval<T> > hull (const ub::matrix< interval<T> >& I, const ub::matrix< interval<T> > & J) {
	int i, j;
	int s1 = I.size1();
	int s2 = I.size2();
	ub::matrix< interval<T> > r(s1, s2);

	for (i=0; i<s1; i++) {
		for (j=0; j<s2; j++) {
			r(i, j) = interval<T>::hull(I(i, j), J(i, j));
		}
	}

	return r;
}

template <class T> inline T max_norm (const ub::vector<T>& x) {
	int i;
	int s = x.size();
	T r, tmp;

	r = 0.;
	for (i=0; i<s; i++) {
		// tmp = abs(x(i));
		tmp = (x(i) >= 0.) ? x(i) : -x(i);
		if (tmp > r) r = tmp;
	}

	return r;
}

template <class T> inline T max_norm (const ub::matrix<T>& x) {
	int i, j;
	int s1 = x.size1();
	int s2 = x.size2();
	T r, tmp, tmp2;

	r = 0.;
	for (i=0; i<s1; i++) {
		tmp = 0.;
		rop<T>::begin();
		for (j=0; j<s2; j++) {
			tmp2 = (x(i, j) >= 0.) ? x(i, j) : -x(i, j);
			tmp = rop<T>::add_up(tmp, tmp2);
		}
		rop<T>::end();
		if (tmp > r) r = tmp;
	}

	return r;
}

template <class T> inline T max_norm (const ub::vector< interval<T> >& x) {
	int i;
	int s = x.size();
	T r, tmp;

	r = 0.;
	for (i=0; i<s; i++) {
		tmp = norm(x(i));
		if (tmp > r) r = tmp;
	}

	return r;
}

template <class T> inline T max_norm (const ub::matrix< interval<T> >& x) {
	int i, j;
	int s1 = x.size1();
	int s2 = x.size2();
	T r, tmp;

	r = 0.;
	for (i=0; i<s1; i++) {
		tmp = 0.;
		rop<T>::begin();
		for (j=0; j<s2; j++) {
			tmp = rop<T>::add_up(tmp, norm(x(i, j)));
		}
		rop<T>::end();
		if (tmp > r) r = tmp;
	}

	return r;
}

} // namespace kv

#endif // INTERVAL_VECTOR_HPP