// This file is part of OpenCV project.
// It is subject to the license terms in the LICENSE file found in the top-level directory
// of this distribution and at http://opencv.org/license.html

#include <iostream>
#include <unordered_map>

#include "opencv2/core/base.hpp"
#include "opencv2/core/types.hpp"
#include "opencv2/core/utils/logger.hpp"

#if defined(HAVE_EIGEN)
#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>

#include "opencv2/core/eigen.hpp"
#endif

namespace cv
{
namespace kinfu
{
/*!
 * \class BlockSparseMat
 * Naive implementation of Sparse Block Matrix
 */
template<typename _Tp, size_t blockM, size_t blockN>
struct BlockSparseMat
{
    struct Point2iHash
    {
        size_t operator()(const cv::Point2i& point) const noexcept
        {
            size_t seed                     = 0;
            constexpr uint32_t GOLDEN_RATIO = 0x9e3779b9;
            seed ^= std::hash<int>()(point.x) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
            seed ^= std::hash<int>()(point.y) + GOLDEN_RATIO + (seed << 6) + (seed >> 2);
            return seed;
        }
    };
    typedef Matx<_Tp, blockM, blockN> MatType;
    typedef std::unordered_map<Point2i, MatType, Point2iHash> IDtoBlockValueMap;

    BlockSparseMat(size_t _nBlocks) : nBlocks(_nBlocks), ijValue() {}

    void clear()
    {
        ijValue.clear();
    }

    inline MatType& refBlock(size_t i, size_t j)
    {
        Point2i p((int)i, (int)j);
        auto it = ijValue.find(p);
        if (it == ijValue.end())
        {
            it = ijValue.insert({ p, MatType::zeros() }).first;
        }
        return it->second;
    }

    inline _Tp& refElem(size_t i, size_t j)
    {
        Point2i ib((int)(i / blockM), (int)(j / blockN));
        Point2i iv((int)(i % blockM), (int)(j % blockN));
        return refBlock(ib.x, ib.y)(iv.x, iv.y);
    }

    inline MatType valBlock(size_t i, size_t j) const
    {
        Point2i p((int)i, (int)j);
        auto it = ijValue.find(p);
        if (it == ijValue.end())
            return MatType::zeros();
        else
            return it->second;
    }

    inline _Tp valElem(size_t i, size_t j) const
    {
        Point2i ib((int)(i / blockM), (int)(j / blockN));
        Point2i iv((int)(i % blockM), (int)(j % blockN));
        return valBlock(ib.x, ib.y)(iv.x, iv.y);
    }

    Mat diagonal() const
    {
        // Diagonal max length is the number of columns in the sparse matrix
        int diagLength = blockN * nBlocks;
        cv::Mat diag   = cv::Mat::zeros(diagLength, 1, cv::DataType<_Tp>::type);

        for (int i = 0; i < diagLength; i++)
        {
            diag.at<_Tp>(i, 0) = valElem(i, i);
        }
        return diag;
    }

#if defined(HAVE_EIGEN)
    Eigen::SparseMatrix<_Tp> toEigen() const
    {
        std::vector<Eigen::Triplet<_Tp>> tripletList;
        tripletList.reserve(ijValue.size() * blockM * blockN);
        for (const auto& ijv : ijValue)
        {
            int xb = ijv.first.x, yb = ijv.first.y;
            MatType vblock = ijv.second;
            for (size_t i = 0; i < blockM; i++)
            {
                for (size_t j = 0; j < blockN; j++)
                {
                    _Tp val = vblock((int)i, (int)j);
                    if (abs(val) >= NON_ZERO_VAL_THRESHOLD)
                    {
                        tripletList.push_back(Eigen::Triplet<_Tp>((int)(blockM * xb + i), (int)(blockN * yb + j), val));
                    }
                }
            }
        }
        Eigen::SparseMatrix<_Tp> EigenMat(blockM * nBlocks, blockN * nBlocks);
        EigenMat.setFromTriplets(tripletList.begin(), tripletList.end());
        EigenMat.makeCompressed();

        return EigenMat;
    }
#endif
    inline size_t nonZeroBlocks() const { return ijValue.size(); }

    BlockSparseMat<_Tp, blockM, blockN>& operator+=(const BlockSparseMat<_Tp, blockM, blockN>& other)
    {
        for (const auto& oijv : other.ijValue)
        {
            Point2i p = oijv.first;
            MatType vblock = oijv.second;
            this->refBlock(p.x, p.y) += vblock;
        }

        return *this;
    }

#if defined(HAVE_EIGEN)
    //! Function to solve a sparse linear system of equations HX = B
    //! Requires Eigen
    bool sparseSolve(InputArray B, OutputArray X, bool checkSymmetry = true, OutputArray predB = cv::noArray()) const
    {
        Eigen::SparseMatrix<_Tp> bigA = toEigen();
        Mat mb = B.getMat().t();
        Eigen::Matrix<_Tp, -1, 1> bigB;
        cv2eigen(mb, bigB);

        Eigen::SparseMatrix<_Tp> bigAtranspose = bigA.transpose();
        if(checkSymmetry && !bigA.isApprox(bigAtranspose))
        {
            CV_Error(Error::StsBadArg, "H matrix is not symmetrical");
            return false;
        }

        Eigen::SimplicialLDLT<Eigen::SparseMatrix<_Tp>> solver;

        solver.compute(bigA);
        if (solver.info() != Eigen::Success)
        {
            CV_LOG_INFO(NULL, "Failed to eigen-decompose");
            return false;
        }
        else
        {
            Eigen::Matrix<_Tp, -1, 1> solutionX = solver.solve(bigB);
            if (solver.info() != Eigen::Success)
            {
                CV_LOG_INFO(NULL, "Failed to eigen-solve");
                return false;
            }
            else
            {
                eigen2cv(solutionX, X);
                if (predB.needed())
                {
                    Eigen::Matrix<_Tp, -1, 1> predBEigen = bigA * solutionX;
                    eigen2cv(predBEigen, predB);
                }
                return true;
            }
        }
    }
#else
    bool sparseSolve(InputArray /*B*/, OutputArray /*X*/, bool /*checkSymmetry*/ = true, OutputArray /*predB*/ = cv::noArray()) const
    {
        CV_Error(Error::StsNotImplemented, "Eigen library required for matrix solve, dense solver is not implemented");
    }
#endif

    static constexpr _Tp NON_ZERO_VAL_THRESHOLD = _Tp(0.0001);
    size_t nBlocks;
    IDtoBlockValueMap ijValue;
};

}  // namespace kinfu
}  // namespace cv


#ifndef __postfix_t_hpp__
#define __postfix_t_hpp__

#include <deque>
#include <iosfwd>
#include <unordered_map>
#include <sstream>
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/math/constants/constants.hpp>
#include "debug.hpp"
#include "num_t.hpp"
#include "stream_saver_t.hpp"
#include "display_types_t.hpp"
#include "make_property.hpp"
#include "hex_printer_t.hpp"
#include "bin_printer_t.hpp"
#include "oct_printer_t.hpp"
#include "commaifier_t.hpp"
#include "constants.hpp"

/**
 * A postfix calculation engine.
 */
struct postfix_t
{
   postfix_t()
      :enabled_(true)
      ,display_type_(display_types_t::dec)
   {
   }

   /**
    * Remove everything from the results stack.
    */
   void clear()
   {
      numbers_.clear();
   }

   /**
    * Give the engine something to do.
    */
   void push(const std::string& op)
   {
      try
      {
         std::string op_copy = op;
         boost::algorithm::to_lower(op_copy);
         auto handler = handlers_.find(op_copy);
         if (handler != handlers_.end())
         {
            handler->second(*this);
            return;
         }

         op_copy = remove_commas(op_copy);
         if (check_matches_and_strip_hexadecimal(op_copy))
         {
            push_hexadecimal(op_copy);
         }
         else if (check_matches_and_strip_octal(op_copy))
         {
            push_octal(op_copy);
         }
         else if (check_matches_and_strip_binary(op_copy))
         {
            push_binary(op_copy);
         }
         else
         {
            push_decimal(op_copy);
         }
      }
      catch (std::exception& e)
      {
         ooo(eee)
            << "failed to push: "
            << e.what();
      }
   }

   // Overloads for type safety.
   void push(const num_t& number) { numbers_.push_back(number); }
   void push(double number) { numbers_.push_back(number); }
   void push(int number) { numbers_.push_back(number); }
   void push(unsigned int number) { numbers_.push_back(number); }
   void push(short number) { numbers_.push_back(number); }
   void push(unsigned short number) { numbers_.push_back(number); }

   void push(char op)
   {
      std::string converted(1, op);
      this->push(converted);
   }

   void push(unsigned char op)
   {
      std::string converted(1, op);
      this->push(converted);
   }

   /**
    * @return the top of the stack without removing it.
    */
   num_t top() const
   {
      ensure(numbers_.size() > 0);
      return numbers_.back();
   }

   /**
    * @return the top of the stack and remove it.
    */
   num_t pop()
   {
      num_t result = top();
      numbers_.pop_back();
      return result;
   }

   /**
    * @return true when we can apply a binary operation.
    */
   bool can_bop() const
   {
      return numbers_.size() >= 2;
   }

   /**
    * @return true when we can apply a unary operation.
    */
   bool can_uop() const
   {
      return numbers_.size() >= 1;
   }

   /**
    * @return the stack rendered into a readable form.
    */
   std::string str() const
   {
      std::stringstream ss;
      ss << *this;
      std::string result(ss.str());
      return result;
   }

private:
   friend std::ostream& operator<<(std::ostream& o, const postfix_t& p);

   make_property(bool, enabled);
   make_property(display_types_t, display_type);

   // This is not an std::stack because stacks can't iterate
   // and I want to be able to iterate in order to print.
   std::deque<num_t> numbers_;

   /**
    * Let num_t handle the parsing.
    */
   void push_decimal(const std::string& d)
   {
      try
      {
         num_t bignum(d);
         numbers_.push_back(bignum);
      }
      catch (std::runtime_error&)
      {
         throw "failed push";
      }
   }

   std::string remove_commas(const std::string& s)
   {
      std::string result = s;
      result.erase
      (
         std::remove(result.begin(), result.end(), ','), result.end()
      );
      return result;
   }

   /**
    * Attempt to interpret a nondecimal number.
    * @param[in] the nondecimal number with prefix removed.
    * @param[in] digits the mapping of digit to value for
    * all values in the base. Alphabetical characters should
    * be lowercase.
    */
   void push_nondecimal
   (
      const std::string& n,
      const std::unordered_map<char, num_t>& digits
   )
   {
      std::vector<std::string> components;
      boost::split(components, n, boost::is_any_of("."), boost::token_compress_on);
      num_t result = 0;
      num_t multiplier = 1;
      num_t base_multiplier = digits.size();
      num_t divider = base_multiplier;
      switch (components.size())
      {
         case 2:
            {
               std::string::const_iterator i =
                  components[1].cbegin();
               std::string::const_iterator end =
                  components[1].cend();
               for (;i < end; ++i)
               {
                  auto p = digits.find(*i);
                  if (p == digits.end())
                  {
                     throw "push_nondecimal invalid digit";
                  }
                  result += p->second / divider;
                  divider *= base_multiplier;
               }
            }
            // intentional fallthrough
         case 1:
            {
               std::string::const_iterator i =
                  components[0].cend() - 1;
               std::string::const_iterator begin =
                  components[0].cbegin();
               for (; begin <= i; --i)
               {
                  auto p = digits.find(*i);
                  if (p == digits.end())
                  {
                     throw "push_nondecimal invalid digit";
                  }
                  result += multiplier * p->second;
                  multiplier *= base_multiplier;
               }
            }
            break;
         case 0:
            throw "push_nondecimal no components";
         default:
            throw "push_nondecimal too many components";
      }
      numbers_.push_back(result);
   }

   std::unordered_map<char, num_t> hex_digits =
   {
      {'0', 0}, {'1', 1}, {'2', 2}, {'3', 3},
      {'4', 4}, {'5', 5}, {'6', 6}, {'7', 7},
      {'8', 8}, {'9', 9}, {'a', 10}, {'b', 11},
      {'c', 12}, {'d', 13}, {'e', 14}, {'f', 15},
   };
   /**
    * @param[in] a hexidecimal number string
    */
   void push_hexadecimal(const std::string& x)
   {
      push_nondecimal(x, hex_digits);
   }

   std::unordered_map<char, num_t> oct_digits =
   {
      {'0', 0}, {'1', 1}, {'2', 2}, {'3', 3},
      {'4', 4}, {'5', 5}, {'6', 6}, {'7', 7},
   };
   /**
    * @param[in] an octal number string
    */
   void push_octal(const std::string& o)
   {
      push_nondecimal(o, oct_digits);
   }

   std::unordered_map<char, num_t> bin_digits =
   {
      {'0', 0}, {'1', 1},
   };
   /**
    * @param[in] a binary number string
    */
   void push_binary(const std::string& b)
   {
      push_nondecimal(b, bin_digits);
   }

   /**
    * Check a string to see if it begins with any of the
    * candidate prefixes and remove it. The first matched
    * prefix ends the operation.
    * @param[in] candidate_prefixes the set of prefixes
    * @param[in] s the string to check
    * @return true if one of the prefixes matched and was
    * removed.
    */
   bool check_matches_and_strip_prefix
   (
      const std::vector<std::string>& candidate_prefixes,
      std::string& s
   )
   {
      bool result = false;
      for (auto prefix : candidate_prefixes)
      {
         result = result || boost::starts_with(s, prefix);
         if (result)
         {
            s.erase(0, prefix.size());
            break;
         }
      }
      return result;
   }

   std::vector<std::string> hex_prefixes =
   {
      "x", "h", "0x", "0h"
   };
   /**
    * Checks to see if a number is marked with a hexadecimal
    * prefix and removes it.
    * @param[in] x the potential hexadecimal number
    * @return true if x was hexadecimal
    */
   bool check_matches_and_strip_hexadecimal(std::string& x)
   {
      bool result = check_matches_and_strip_prefix(hex_prefixes, x);
      return result;
   }

   std::vector<std::string> octal_prefixes =
   {
      "o", "0o"
   };
   /**
    * Checks to see if a number is marked with a octal
    * prefix and removes it.
    * @param[in] x the potential octal number
    * @return true if x was octal
    */
   bool check_matches_and_strip_octal(std::string& o)
   {
      bool result = check_matches_and_strip_prefix(octal_prefixes, o);
      return result;
   }

   std::vector<std::string> binary_prefixes =
   {
      "b", "0b"
   };
   /**
    * Checks to see if a number is marked with a binary
    * prefix and removes it.
    * @param[in] x the potential binary number
    * @return true if x was binary
    */
   bool check_matches_and_strip_binary(std::string& b)
   {
      bool result = check_matches_and_strip_prefix(binary_prefixes, b);
      return result;
   }
   /**
    * The operators.
    */
   std::unordered_map
   <
      std::string,
      std::function<void(postfix_t&)>
   > handlers_ =
   {
      {
         "+",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            num_t result = lhs + rhs;
            p.push(result);
         }
      },
      {
         "-",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            num_t result = lhs - rhs;
            p.push(result);
         }
      },
      {
         "*",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            num_t result = lhs * rhs;
            p.push(result);
         }
      },
      {
         "/",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            num_t result = lhs / rhs;
            p.push(result);
         }
      },
      {
         "%",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            num_t result = fmod(lhs, rhs);
            p.push(result);
         }
      },
      {
         "<<",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t orig_rhs = rhs;
            num_t lhs = p.pop();
            num_t orig_lhs = lhs;
            num_t result = lhs;
            while (p.enabled() && rhs > 0)
            {
               result *= 2;
               --rhs;
            }
            if (p.enabled())
            {
               p.push(result);
            }
            else
            {
               p.push(orig_lhs);
               p.push(orig_rhs);
            }
         }
      },
      {
         ">>",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t orig_rhs = rhs;
            num_t lhs = p.pop();
            num_t orig_lhs = lhs;
            num_t result = lhs;
            while (p.enabled() && rhs > 0)
            {
               result /= 2;
               --rhs;
            }
            if (p.enabled())
            {
               p.push(result);
            }
            else
            {
               p.push(orig_lhs);
               p.push(orig_rhs);
            }
         }
      },
      {
         "^",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            num_t result = pow(lhs, rhs);
            p.push(result);
         }
      },
      {
         "!",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t orig_rhs = rhs;
            num_t result = 1;
            if (rhs == 0)
            {
               p.push(result);
               return;
            }
            while
            (
               p.enabled()
               &&
               rhs > 1
               && boost::math::isfinite(result)
            )
            {
               result *= rhs;
               --rhs;
            }
            if (p.enabled())
            {
               p.push(result);
            }
            else
            {
               p.push(orig_rhs);
            }
         }
      },
      {
         "++",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs + 1;
            p.push(result);
         }
      },
      {
         "--",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs - 1;
            p.push(result);
         }
      },
      {
         "abs",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = fabs(rhs);
            p.push(result);
         }
      },
      {
         "acos",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = acos(rhs);
            p.push(result);
         }
      },
      {
         "asin",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = asin(rhs);
            p.push(result);
         }
      },
      {
         "atan",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = atan(rhs);
            p.push(result);
         }
      },
      {
         "atan2",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            num_t result = atan2(lhs, rhs);
            p.push(result);
         }
      },
      {
         "avg",
         [](postfix_t& p)
         {
            num_t result = 0;
            num_t count = 0;
            while (p.can_uop())
            {
               ++count;
               result += p.pop();
            }
            result /= count;
            p.push(result);
         }
      },
      {
         "bin",
         [](postfix_t& p)
         {
            p.display_type(display_types_t::bin);
         }
      },
      {
         "c",
         [](postfix_t& p)
         {
           p.push(299792458);
         }
      },
      {
         "ceil",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = ceil(rhs);
            p.push(result);
         }
      },
      {
         "cf",
         [](postfix_t& p)
         {
           ensure(p.can_uop());
           num_t rhs = p.pop();
           num_t result = (rhs * 9 / 5) + 32;
           p.push(result);
         }
      },
      {
         "clear",
         [](postfix_t& p)
         {
            p.clear();
         }
      },
      {
         "clr",
         [](postfix_t& p)
         {
            p.clear();
         }
      },
      {
         "cos",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = cos(rhs);
            p.push(result);
         }
      },
      {
         "cosh",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = cosh(rhs);
            p.push(result);
         }
      },
      {
         "dec",
         [](postfix_t& p)
         {
            p.display_type(display_types_t::dec);
         }
      },
      {
         "com",
         [](postfix_t& p)
         {
            p.display_type(display_types_t::com);
         }
      },
      {
         "e",
         [](postfix_t& p)
         {
            p.push(exp(num_t(1)));
         }
      },
      {
         "exp",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = exp(rhs);
            p.push(result);
         }
      },
      {
         "eng",
         [](postfix_t& p)
         {
            p.display_type(display_types_t::eng);
         }
      },
      {
         "fc",
         [](postfix_t& p)
         {
           ensure(p.can_uop());
           num_t rhs = p.pop();
           num_t result = (rhs - 32) * 5 / 9;
           p.push(result);
         }
      },
      {
         "floor",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = floor(rhs);
            p.push(result);
         }
      },
      {
         "fpj",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs * joules_in_one_foot_pound;
            p.push(result);
         }
      },
      {
         "frac",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs - trunc(rhs);
            p.push(result);
         }
      },
      {
         "frexp",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            int exp = 0;
            num_t rhs = p.pop();
            num_t result = frexp(rhs, &exp);
            p.push(result);
            p.push(exp);
         }
      },
      {
         "ftm",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs / feet_in_one_meter;
            p.push(result);
         }
      },
      {
         "gl",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs * liters_in_one_gallon;
            p.push(result);
         }
      },
      {
         "hex",
         [](postfix_t& p)
         {
            p.display_type(display_types_t::hex);
         }
      },
      {
         "hpw",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs * watts_in_one_horsepower;
            p.push(result);
         }
      },
      {
         "jfp",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs / joules_in_one_foot_pound;
            p.push(result);
         }
      },
      {
         "kgp",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs / pounds_in_one_kilogram;
            p.push(result);
         }
      },
      {
         "lg",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.top();
            num_t result = rhs / liters_in_one_gallon;
            p.push(result);
         }
      },
      {
         "log",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = log10(rhs);
            p.push(result);
         }
      },
      {
         "ln",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = log(rhs);
            p.push(result);
         }
      },
      {
         "mft",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs * feet_in_one_meter;
            p.push(result);
         }
      },
      {
         "neg",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = -rhs;
            p.push(result);
         }
      },
      {
         "oct",
         [](postfix_t& p)
         {
            p.display_type(display_types_t::oct);
         }
      },
      {
         "pkg",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs * pounds_in_one_kilogram;
            p.push(result);
         }
      },
      {
         "pi",
         [](postfix_t& p)
         {
            p.push(boost::math::constants::pi<num_t>());
         }
      },
      {
         "pop",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            p.pop();
         }
      },
      {
         "prod",
         [](postfix_t& p)
         {
            num_t result = 1;
            while (p.can_uop())
            {
               result *= p.pop();
            }
            p.push(result);
         }
      },
      {
         "round",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = round(rhs);
            p.push(result);
         }
      },
      {
         "sin",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = sin(rhs);
            p.push(result);
         }
      },
      {
         "sinh",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = sinh(rhs);
            p.push(result);
         }
      },
      {
         "sqrt",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = sqrt(rhs);
            p.push(result);
         }
      },
      {
         "stddev",
         [](postfix_t& p)
         {
            num_t sum = 0;
            num_t count = 0;
            std::vector<num_t> copies;
            while (p.can_uop())
            {
               num_t x = p.pop();
               sum += x;
               ++count;
               copies.push_back(x);
            }
            num_t avg = sum / count;
            num_t sum_of_square_diffs = 0;
            for (auto n : copies)
            {
               num_t diff = n - avg;
               diff *= diff;
               sum_of_square_diffs += diff;
            }
            num_t result = sum_of_square_diffs / (count - 1);
            p.push(result);
         }
      },
      {
         "sum",
         [](postfix_t& p)
         {
            num_t result = 0;
            while (p.can_uop())
            {
               result += p.pop();
            }
            p.push(result);
         }
      },
      {
         "swap",
         [](postfix_t& p)
         {
            ensure(p.can_bop());
            num_t rhs = p.pop();
            num_t lhs = p.pop();
            p.push(rhs);
            p.push(lhs);
         }
      },
      {
         "tan",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = tan(rhs);
            p.push(result);
         }
      },
      {
         "tanh",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = tanh(rhs);
            p.push(result);
         }
      },
      {
         "trunc",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = trunc(rhs);
            p.push(result);
         }
      },
      {
         "whp",
         [](postfix_t& p)
         {
            ensure(p.can_uop());
            num_t rhs = p.pop();
            num_t result = rhs / watts_in_one_horsepower;
            p.push(result);
         }
      },
   };
};

/**
 * Ostream support for postfix_t.
 */
inline std::ostream& operator<<(std::ostream& o, const postfix_t& p)
{
   static hex_printer_t hex_printer;
   static oct_printer_t oct_printer;
   static bin_printer_t bin_printer;
   stream_saver_t saver(o);

   const std::size_t count = p.numbers_.size();
   std::size_t i = 0;
   for (num_t num : p.numbers_)
   {
      switch (p.display_type())
      {
         case display_types_t::hex:
            o << hex_printer.convert(num);
            break;
         case display_types_t::oct:
            o << oct_printer.convert(num);
            break;
         case display_types_t::bin:
            o << bin_printer.convert(num);
            break;
         case display_types_t::eng:
            o << num;
            break;
         case display_types_t::com:
            o << commaifier_t(num.str()).str();
            break;
         case display_types_t::dec:
         default:
            o << num.str();
            break;
      }
      if (i < count - 1) o << "   ";
      ++i;
   }

   return o;
}

#endif


/****************************************************************************
 * Copyright (c) 2017-2021 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include "ArborX_EnableDeviceTypes.hpp" // ARBORX_DEVICE_TYPES
#include <ArborX_KDOP.hpp>
#include <ArborX_LinearBVH.hpp>

#include <Kokkos_CopyViews.hpp>

#include <boost/test/unit_test.hpp>

#include <vector>

#include "Search_UnitTestHelpers.hpp"

BOOST_AUTO_TEST_CASE_TEMPLATE(intersects_kdop, DeviceType, ARBORX_DEVICE_TYPES)
{
  using ExecutionSpace = typename DeviceType::execution_space;
  using MemorySpace = typename DeviceType::memory_space;
  using Tree = ArborX::BVH<MemorySpace>;

  using ArborX::Point;

  std::vector<ArborX::Point> primitives = {
      {{0, 0, 0}}, // 0
      {{1, 1, 1}}, // 1
      {{2, 2, 2}}, // 2
      {{3, 3, 3}}, // 3
      {{1, 0, 0}}, // 4
      {{2, 0, 0}}, // 5
      {{3, 0, 0}}, // 6
      {{0, 1, 0}}, // 7
      {{0, 2, 0}}, // 8
      {{0, 3, 0}}, // 9
      {{0, 0, 1}}, // 10
      {{0, 0, 2}}, // 11
      {{0, 0, 3}}, // 12
  };
  Tree const tree(
      ExecutionSpace{},
      Kokkos::create_mirror_view_and_copy(
          MemorySpace{},
          Kokkos::View<Point *, Kokkos::HostSpace, Kokkos::MemoryUnmanaged>(
              primitives.data(), primitives.size())));

  // (0,0,0)->(1,2,3) box with (0,0,0)--(0,0,3) edge cut off
  ArborX::Experimental::KDOP<18> x;
  // bottom
  x += ArborX::Point{0.25, 0, 0};
  x += ArborX::Point{1, 0, 0};
  x += ArborX::Point{1, 2, 0};
  x += ArborX::Point{0, 2, 0};
  x += ArborX::Point{0, 0.25, 0};
  // top
  x += ArborX::Point{0.25, 0, 3};
  x += ArborX::Point{1, 0, 3};
  x += ArborX::Point{1, 2, 3};
  x += ArborX::Point{0, 2, 3};
  x += ArborX::Point{0, 0.25, 3};

  using IntersectsKDop = decltype(ArborX::intersects(x));
  std::vector<IntersectsKDop> predicates = {ArborX::intersects(x)};

  ARBORX_TEST_QUERY_TREE(ExecutionSpace{}, tree,
                         makeIntersectsBoxQueries<DeviceType>({}),
                         make_reference_solution<int>({}, {0}));
  ARBORX_TEST_QUERY_TREE(
      ExecutionSpace{}, tree,
      Kokkos::create_mirror_view_and_copy(
          MemorySpace{}, Kokkos::View<IntersectsKDop *, Kokkos::HostSpace,
                                      Kokkos::MemoryUnmanaged>(
                             predicates.data(), predicates.size())),
      make_reference_solution<int>({1, 4, 7, 8}, {0, 4}));
}


#include "AdapterBase.h"

#include "config.h"
#include "form/BC.h"
#include "form/FacetInfo.h"
#include "form/RefElement.h"
#include "geometry/Vector.h"
#include "tensor/EigenMap.h"
#include "tensor/Utility.h"

#include <Eigen/Core>
#include <Eigen/LU>

#include <memory>
#include <stdexcept>
#include <utility>

namespace tndm {

AdapterBase::AdapterBase(std::shared_ptr<Curvilinear<DomainDimension>> cl,
                         RefElement<DomainDimension - 1u> const& space,
                         SimplexQuadratureRule<DomainDimension - 1u> const& quad_rule,
                         std::array<double, DomainDimension> const& up,
                         std::array<double, DomainDimension> const& ref_normal)
    : cl_(std::move(cl)), quad_rule_(quad_rule), up_(up), ref_normal_(ref_normal),
      nbf_(space.numBasisFunctions()) {

    e_q = space.evaluateBasisAt(quad_rule_.points());
    e_q_T = space.evaluateBasisAt(quad_rule_.points(), {1, 0});

    for (std::size_t f = 0; f < DomainDimension + 1u; ++f) {
        auto facetParam = cl_->facetParam(f, quad_rule_.points());
        geoDxi_q.emplace_back(cl_->evaluateGradientAt(facetParam));
    }
}

void AdapterBase::begin_preparation(std::size_t numFaultFaces) {
    auto nq = quad_rule_.size();
    fault_.setStorage(std::make_shared<fault_t>(numFaultFaces * nq), 0u, numFaultFaces, nq);
    auto nbf2 = nbf_ * nbf_;
    mass_.setStorage(std::make_shared<mass_t>(numFaultFaces * nbf2), 0u, numFaultFaces, nbf2);
}

void AdapterBase::prepare(std::size_t faultNo, FacetInfo const& info,
                          LinearAllocator<double>& scratch) {
    auto nq = quad_rule_.size();

    auto J = make_scratch_tensor(scratch, cl_->jacobianResultInfo(nq));
    auto JInv = make_scratch_tensor(scratch, cl_->jacobianResultInfo(nq));
    auto detJ = make_scratch_tensor(scratch, cl_->detJResultInfo(nq));
    auto normal =
        Tensor(fault_[faultNo].template get<Normal>().data()->data(), cl_->normalResultInfo(nq));
    auto& nl = fault_[faultNo].template get<NormalLength>();
    auto fault_basis_q = Tensor(fault_[faultNo].template get<FaultBasis>().data()->data(),
                                cl_->facetBasisResultInfo(nq));
    auto m = make_scratch_tensor<Matrix<double>>(scratch, nbf_, nbf_);
    auto mInv = Matrix<double>(mass_[faultNo].template get<MInv>().data(), nbf_, nbf_);
    cl_->jacobian(info.up[0], geoDxi_q[info.localNo[0]], J);
    cl_->detJ(info.up[0], J, detJ);
    cl_->jacobianInv(J, JInv);
    cl_->normal(info.localNo[0], detJ, JInv, normal);
    for (std::size_t i = 0; i < nq; ++i) {
        auto& sign_flipped = fault_[faultNo].template get<SignFlipped>()[i];
        auto& normal_i = fault_[faultNo].template get<Normal>()[i];
        nl[i] = norm(normal_i);

        auto n_ref_dot_n = dot(ref_normal_, normal_i);
        if (std::fabs(n_ref_dot_n) < 10000.0 * std::numeric_limits<double>::epsilon()) {
            throw std::logic_error("Normal and reference normal are almost perpendicular.");
        }
        sign_flipped = n_ref_dot_n < 0;
        if (sign_flipped) {
            normal_i = -1.0 * normal_i;
        }
    }
    cl_->facetBasis(up_, normal, fault_basis_q);
    for (std::size_t q = 0; q < nq; ++q) {
        if (fault_[faultNo].template get<SignFlipped>()[q]) {
            for (std::size_t i = 0; i < fault_basis_q.shape(1); ++i) {
                for (std::size_t j = 0; j < fault_basis_q.shape(0); ++j) {
                    fault_basis_q(i, j, q) *= -1.0;
                }
            }
        }
    }

    auto const& w = quad_rule_.weights();
    for (std::size_t i = 0; i < nbf_; ++i) {
        for (std::size_t j = 0; j < nbf_; ++j) {
            m(i, j) = 0.0;
            for (std::size_t q = 0; q < nq; ++q) {
                m(i, j) += w[q] * nl[q] * e_q(i, q) * e_q(j, q);
            }
        }
    }
    EigenMap(mInv) = EigenMap(m).inverse();

    scratch.reset();
}

} // namespace tndm


#include <kdop_mesh_reorder.h>
#include <mesh_array.h>
#include <tiny.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

#include <vector>

BOOST_AUTO_TEST_SUITE(kdop);

BOOST_AUTO_TEST_CASE(kdop_mesh_reorder_sphere)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::real_type          T;

  mesh_array::T3Mesh surf;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  size_t const slices = 12u;
  size_t const segments = 12u;
  T const radius = 1.0f;
  mesh_array::make_sphere<MT>(radius, slices, segments, surf, sX, sY, sZ);

  mesh_array::T4Mesh mesh_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zin;
  mesh_array::tetgen(surf, sX, sY, sZ, mesh_in, Xin, Yin, Zin);

  mesh_array::T4Mesh mesh_out;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> dists;

  kdop::mesh_reorder( mesh_in, Xin, Yin, Zin, mesh_out, Xout, Yout, Zout );

}
BOOST_AUTO_TEST_CASE(kdop_mesh_reorder_box)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::real_type          T;

  mesh_array::T3Mesh surf;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  T      const width   = 1.0f;
  T      const height  = 1.0f;
  T      const depth   = 1.0f;
  mesh_array::make_box<MT>( width, height, depth, surf, sX, sY, sZ);

  mesh_array::T4Mesh mesh_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zin;
  mesh_array::tetgen(surf, sX, sY, sZ, mesh_in, Xin, Yin, Zin);

  mesh_array::T4Mesh mesh_out;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> dists;

  kdop::mesh_reorder( mesh_in, Xin, Yin, Zin, mesh_out, Xout, Yout, Zout );

}
BOOST_AUTO_TEST_CASE(kdop_mesh_reorder_cone)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::real_type          T;

  mesh_array::T3Mesh surf;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  T      const height   = 1.0f;
  T      const radius   = 1.0f;
  size_t const slices   = 12u;

  mesh_array::make_cone<MT>(radius, height, slices, surf, sX, sY, sZ);

  mesh_array::T4Mesh mesh_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zin;
  mesh_array::tetgen(surf, sX, sY, sZ, mesh_in, Xin, Yin, Zin);

  mesh_array::T4Mesh mesh_out;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> dists;

  kdop::mesh_reorder( mesh_in, Xin, Yin, Zin, mesh_out, Xout, Yout, Zout );

}
BOOST_AUTO_TEST_CASE(kdop_mesh_reorder_capsule)
{
  typedef tiny::MathTypes<float> MT;
  typedef MT::real_type          T;

  mesh_array::T3Mesh surf;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sX;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sY;
  mesh_array::VertexAttribute<T,mesh_array::T3Mesh> sZ;

  T      const height   = 1.0f;
  T      const radius   = 1.0f;
  size_t const slices   = 12u;
  size_t const segments = 12u;

  mesh_array::make_capsule<MT>(radius, height, slices, segments, surf, sX, sY, sZ);

  mesh_array::T4Mesh mesh_in;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yin;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zin;
  mesh_array::tetgen(surf, sX, sY, sZ, mesh_in, Xin, Yin, Zin);

  mesh_array::T4Mesh mesh_out;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Xout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Yout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> Zout;
  mesh_array::VertexAttribute<T,mesh_array::T4Mesh> dists;

  kdop::mesh_reorder( mesh_in, Xin, Yin, Zin, mesh_out, Xout, Yout, Zout );

}

BOOST_AUTO_TEST_SUITE_END();


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

/*
 Copyright (C) 2007 Ferdinando Ametrano
 Copyright (C) 2007 Katiuscia Manzoni
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005 StatPro Italia srl

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

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

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

/*! \file capfloortermvolcurve.hpp
    \brief Cap/floor at-the-money term-volatility curve
*/

#ifndef quantlib_cap_volatility_vector_hpp
#define quantlib_cap_volatility_vector_hpp

#include <ql/termstructures/volatility/capfloor/capfloortermvolatilitystructure.hpp>
#include <ql/math/interpolation.hpp>
#include <ql/quote.hpp>
#include <ql/patterns/lazyobject.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>
#include <boost/noncopyable.hpp>
#include <vector>

namespace QuantLib {

    //! Cap/floor at-the-money term-volatility vector
    /*! This class provides the at-the-money volatility for a given cap/floor
        interpolating a volatility vector whose elements are the market
        volatilities of a set of caps/floors with given length.
    */
    class CapFloorTermVolCurve : public LazyObject,
                                 public CapFloorTermVolatilityStructure,
                                 private boost::noncopyable {
      public:
        //! floating reference date, floating market data
        CapFloorTermVolCurve(Natural settlementDays,
                             const Calendar& calendar,
                             BusinessDayConvention bdc,
                             const std::vector<Period>& optionTenors,
                             const std::vector<Handle<Quote> >& vols,
                             const DayCounter& dc = Actual365Fixed());
        //! fixed reference date, floating market data
        CapFloorTermVolCurve(const Date& settlementDate,
                             const Calendar& calendar,
                             BusinessDayConvention bdc,
                             const std::vector<Period>& optionTenors,
                             const std::vector<Handle<Quote> >& vols,
                             const DayCounter& dc = Actual365Fixed());
        //! fixed reference date, fixed market data
        CapFloorTermVolCurve(const Date& settlementDate,
                             const Calendar& calendar,
                             BusinessDayConvention bdc,
                             const std::vector<Period>& optionTenors,
                             const std::vector<Volatility>& vols,
                             const DayCounter& dc = Actual365Fixed());
        //! floating reference date, fixed market data
        CapFloorTermVolCurve(Natural settlementDays,
                             const Calendar& calendar,
                             BusinessDayConvention bdc,
                             const std::vector<Period>& optionTenors,
                             const std::vector<Volatility>& vols,
                             const DayCounter& dc = Actual365Fixed());
        //! \name TermStructure interface
        //@{
        Date maxDate() const;
        //@}
        //! \name VolatilityTermStructure interface
        //@{
        Real minStrike() const;
        Real maxStrike() const;
        //@}
        //! \name LazyObject interface
        //@{
        void update();
        void performCalculations() const;
        //@}
        //! \name some inspectors
        //@{
        const std::vector<Period>& optionTenors() const;
        const std::vector<Date>& optionDates() const;
        const std::vector<Time>& optionTimes() const;
        //@}
      protected:
        Volatility volatilityImpl(Time length,
                                  Rate) const;
      private:
        void checkInputs() const;
        void initializeOptionDatesAndTimes() const;
        void registerWithMarketData();
        void interpolate();

        Size nOptionTenors_;
        std::vector<Period> optionTenors_;
        mutable std::vector<Date> optionDates_;
        mutable std::vector<Time> optionTimes_;
        Date evaluationDate_;

        std::vector<Handle<Quote> > volHandles_;
        mutable std::vector<Volatility> vols_;

        // make it not mutable if possible
        mutable Interpolation interpolation_;
    };

    // inline definitions

    inline Date CapFloorTermVolCurve::maxDate() const {
        calculate();
        return optionDateFromTenor(optionTenors_.back());
    }

    inline Real CapFloorTermVolCurve::minStrike() const {
        return QL_MIN_REAL;
    }

    inline Real CapFloorTermVolCurve::maxStrike() const {
        return QL_MAX_REAL;
    }

    inline
    Volatility CapFloorTermVolCurve::volatilityImpl(Time t,
                                                    Rate) const {
        calculate();
        return interpolation_(t, true);
    }

    inline
    const std::vector<Period>& CapFloorTermVolCurve::optionTenors() const {
        return optionTenors_;
    }

    inline
    const std::vector<Date>& CapFloorTermVolCurve::optionDates() const {
        // what if quotes are not available?
        calculate();
        return optionDates_;
    }

    inline
    const std::vector<Time>& CapFloorTermVolCurve::optionTimes() const {
        // what if quotes are not available?
        calculate();
        return optionTimes_;
    }

}

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

/*
 Copyright (C) 2007 Ferdinando Ametrano
 Copyright (C) 2007 Katiuscia Manzoni
 Copyright (C) 2000, 2001, 2002, 2003 RiskMap srl
 Copyright (C) 2003, 2004, 2005 StatPro Italia srl

 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/math/interpolations/cubicinterpolation.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/utilities/dataformatters.hpp>

namespace QuantLib {

    // floating reference date, floating market data
    inline CapFloorTermVolCurve::CapFloorTermVolCurve(
                        Natural settlementDays,
                        const Calendar& calendar,
                        BusinessDayConvention bdc,
                        const std::vector<Period>& optionTenors,
                        const std::vector<Handle<Quote> >& vols,
                        const DayCounter& dc)
    : CapFloorTermVolatilityStructure(settlementDays, calendar, bdc, dc),
      nOptionTenors_(optionTenors.size()),
      optionTenors_(optionTenors),
      optionDates_(nOptionTenors_),
      optionTimes_(nOptionTenors_),
      volHandles_(vols),
      vols_(vols.size()) // do not initialize with nOptionTenors_
    {
        checkInputs();
        initializeOptionDatesAndTimes();
        registerWithMarketData();
        interpolate();
    }

    // fixed reference date, floating market data
    inline CapFloorTermVolCurve::CapFloorTermVolCurve(
                            const Date& settlementDate,
                            const Calendar& calendar,
                            BusinessDayConvention bdc,
                            const std::vector<Period>& optionTenors,
                            const std::vector<Handle<Quote> >& vols,
                            const DayCounter& dayCounter)
    : CapFloorTermVolatilityStructure(settlementDate, calendar, bdc, dayCounter),
      nOptionTenors_(optionTenors.size()),
      optionTenors_(optionTenors),
      optionDates_(nOptionTenors_),
      optionTimes_(nOptionTenors_),
      volHandles_(vols),
      vols_(vols.size()) // do not initialize with nOptionTenors_
    {
        checkInputs();
        initializeOptionDatesAndTimes();
        registerWithMarketData();
        interpolate();
    }

    // fixed reference date, fixed market data
    inline CapFloorTermVolCurve::CapFloorTermVolCurve(
                                const Date& settlementDate,
                                const Calendar& calendar,
                                BusinessDayConvention bdc,
                                const std::vector<Period>& optionTenors,
                                const std::vector<Volatility>& vols,
                                const DayCounter& dayCounter)
    : CapFloorTermVolatilityStructure(settlementDate, calendar, bdc, dayCounter),
      nOptionTenors_(optionTenors.size()),
      optionTenors_(optionTenors),
      optionDates_(nOptionTenors_),
      optionTimes_(nOptionTenors_),
      volHandles_(vols.size()), // do not initialize with nOptionTenors_
      vols_(vols)
    {
        checkInputs();
        initializeOptionDatesAndTimes();
        // fill dummy handles to allow generic handle-based computations later
        for (Size i=0; i<nOptionTenors_; ++i)
            volHandles_[i] = Handle<Quote>(boost::shared_ptr<Quote>(new
                SimpleQuote(vols_[i])));
        interpolate();
    }

    // floating reference date, fixed market data
    inline CapFloorTermVolCurve::CapFloorTermVolCurve(
                                Natural settlementDays,
                                const Calendar& calendar,
                                BusinessDayConvention bdc,
                                const std::vector<Period>& optionTenors,
                                const std::vector<Volatility>& vols,
                                const DayCounter& dayCounter)
    : CapFloorTermVolatilityStructure(settlementDays, calendar, bdc, dayCounter),
      nOptionTenors_(optionTenors.size()),
      optionTenors_(optionTenors),
      optionDates_(nOptionTenors_),
      optionTimes_(nOptionTenors_),
      volHandles_(vols.size()), // do not initialize with nOptionTenors_
      vols_(vols)
    {
        checkInputs();
        initializeOptionDatesAndTimes();
        // fill dummy handles to allow generic handle-based computations later
        for (Size i=0; i<nOptionTenors_; ++i)
            volHandles_[i] = Handle<Quote>(boost::shared_ptr<Quote>(new
                SimpleQuote(vols_[i])));
        interpolate();
    }

    inline void CapFloorTermVolCurve::checkInputs() const
    {
        QL_REQUIRE(!optionTenors_.empty(), "empty option tenor vector");
        QL_REQUIRE(nOptionTenors_==vols_.size(),
                   "mismatch between number of option tenors (" <<
                   nOptionTenors_ << ") and number of volatilities (" <<
                   vols_.size() << ")");
        QL_REQUIRE(optionTenors_[0]>0*Days,
                   "negative first option tenor: " << optionTenors_[0]);
        for (Size i=1; i<nOptionTenors_; ++i)
            QL_REQUIRE(optionTenors_[i]>optionTenors_[i-1],
                       "non increasing option tenor: " << io::ordinal(i) <<
                       " is " << optionTenors_[i-1] << ", " <<
                       io::ordinal(i+1) << " is " << optionTenors_[i]);
    }

    inline void CapFloorTermVolCurve::registerWithMarketData()
    {
        for (Size i=0; i<volHandles_.size(); ++i)
            registerWith(volHandles_[i]);
    }

    inline void CapFloorTermVolCurve::interpolate()
    {
        interpolation_ = CubicInterpolation(
                                    optionTimes_.begin(), optionTimes_.end(),
                                    vols_.begin(),
                                    CubicInterpolation::Spline, false,
                                    CubicInterpolation::SecondDerivative, 0.0,
                                    CubicInterpolation::SecondDerivative, 0.0);
    }

    inline void CapFloorTermVolCurve::update()
    {
        // recalculate dates if necessary...
        if (moving_) {
            Date d = Settings::instance().evaluationDate();
            if (evaluationDate_ != d) {
                evaluationDate_ = d;
                initializeOptionDatesAndTimes();
            }
        }
        CapFloorTermVolatilityStructure::update();
        LazyObject::update();
    }

    inline void CapFloorTermVolCurve::initializeOptionDatesAndTimes() const
    {
        for (Size i=0; i<nOptionTenors_; ++i) {
            optionDates_[i] = optionDateFromTenor(optionTenors_[i]);
            optionTimes_[i] = timeFromReference(optionDates_[i]);
        }
    }

    inline void CapFloorTermVolCurve::performCalculations() const
    {
        // check if date recalculation must be called here

        for (Size i=0; i<vols_.size(); ++i)
            vols_[i] = volHandles_[i]->value();

        interpolation_.update();
    }

}

#endif


// ======================================================================
/*!
 * \file
 * \brief Regression tests for FastMath.StringConversion.h
 */
// ======================================================================

#include "FastMath.h"
#include <boost/test/included/unit_test.hpp>
#include <cmath>

using namespace boost::unit_test;

test_suite* init_unit_test_suite(int argc, char* argv[])
{
  const char* name = "Fast math tester";
  unit_test_log.set_threshold_level(log_messages);
  framework::master_test_suite().p_name.value = name;
  BOOST_TEST_MESSAGE("");
  BOOST_TEST_MESSAGE(name);
  BOOST_TEST_MESSAGE(std::string(std::strlen(name), '='));
  return NULL;
}

BOOST_AUTO_TEST_CASE(fmifloor)
{
  BOOST_TEST_MESSAGE(" + Fmi::floor()");
  BOOST_CHECK_EQUAL(std::floor(0.1), Fmi::floor(0.1));
  BOOST_CHECK_EQUAL(std::floor(0.5), Fmi::floor(0.5));
  BOOST_CHECK_EQUAL(std::floor(1.5), Fmi::floor(1.5));
  BOOST_CHECK_EQUAL(std::floor(-0.1), Fmi::floor(-0.1));
  BOOST_CHECK_EQUAL(std::floor(-0.5), Fmi::floor(-0.5));
  BOOST_CHECK_EQUAL(std::floor(-1.5), Fmi::floor(-1.5));
}

BOOST_AUTO_TEST_CASE(fmiceil)
{
  BOOST_TEST_MESSAGE(" + Fmi::ceil()");
  BOOST_CHECK_EQUAL(std::ceil(0.1), Fmi::ceil(0.1));
  BOOST_CHECK_EQUAL(std::ceil(0.5), Fmi::ceil(0.5));
  BOOST_CHECK_EQUAL(std::ceil(1.5), Fmi::ceil(1.5));
  BOOST_CHECK_EQUAL(std::ceil(-0.1), Fmi::ceil(-0.1));
  BOOST_CHECK_EQUAL(std::ceil(-0.5), Fmi::ceil(-0.5));
  BOOST_CHECK_EQUAL(std::ceil(-1.5), Fmi::ceil(-1.5));
}


#include <boost/test/unit_test.hpp>
#include <rushhour_schedule_sweep.h>

namespace rushhour {
namespace schedule {

BOOST_AUTO_TEST_CASE(Sweep_IncreasingSweep) {
  Sweep schedule(2.0, 5.0, 100);
  BOOST_REQUIRE_EQUAL(100, schedule.size());

  BOOST_CHECK_CLOSE(1.0e9 / 2.0, static_cast<double>(schedule[0].count()), 1.0);
  auto last = std::chrono::nanoseconds::max();
  for (auto ns : schedule) {
    BOOST_CHECK_LE(ns.count(), last.count());
    last = ns;
  }
  BOOST_CHECK_CLOSE(1.0e9 / 5.0, static_cast<double>(schedule[schedule.size()-1].count()), 1.0);
}

BOOST_AUTO_TEST_CASE(Sweep_DecreasingSweep) {
  Sweep schedule(5.0, 2.0, 100);
  BOOST_REQUIRE_EQUAL(100, schedule.size());

  BOOST_CHECK_CLOSE(1.0e9 / 5.0, static_cast<double>(schedule[0].count()), 1.0);
  auto last = std::chrono::nanoseconds::min();
  for (auto ns : schedule) {
    BOOST_CHECK_GE(ns.count(), last.count());
    last = ns;
  }
  BOOST_CHECK_CLOSE(1.0e9 / 2.0, static_cast<double>(schedule[schedule.size()-1].count()), 1.0);
}

BOOST_AUTO_TEST_CASE(Sweep_FlatSweep) {
  Sweep schedule(75.0, 75.0, 10);
  BOOST_REQUIRE_EQUAL(10, schedule.size());
  for (auto ns : schedule) {
    BOOST_CHECK_CLOSE(1.0e9 / 75.0, static_cast<double>(ns.count()), 1.0);
  }
}

BOOST_AUTO_TEST_CASE(Sweep_SizeOfOne) {
  Sweep schedule(75.0, 105.0, 1);
  BOOST_REQUIRE_EQUAL(1, schedule.size());
  BOOST_CHECK_CLOSE(1.0e9 / 75.0, static_cast<double>(schedule[0].count()), 1.0);
}

BOOST_AUTO_TEST_CASE(Sweep_SizeZero) {
  Sweep schedule(75.0, 105.0, 0);
  BOOST_REQUIRE_EQUAL(0, schedule.size());
  BOOST_CHECK_THROW(schedule[0], std::out_of_range);
}

BOOST_AUTO_TEST_CASE(Sweep_OutOfBounds) {
  Sweep schedule(75.0, 105.0, 5);
  BOOST_REQUIRE_EQUAL(5, schedule.size());
  BOOST_CHECK_THROW(schedule[5], std::out_of_range);
}

}  // namespace schedule
}  // namespace rushhour


#define BOOST_TEST_MODULE lue core time clock
#include <boost/test/unit_test.hpp>
#include "lue/core/time.hpp"

BOOST_AUTO_TEST_CASE(seconds)
{
    namespace ldm = lue::data_model;

    // Clock with a Unix time epoch and a tick period of 10 seconds
    std::size_t const nr_seconds_per_tick = 10;

    using TickPeriod = ldm::time::TickPeriod<ldm::time::Second>;
    using Clock = ldm::time::Clock<TickPeriod>;

    TickPeriod tick_period(nr_seconds_per_tick);
    Clock clock;

    BOOST_CHECK(clock.epoch().kind() == ldm::time::Epoch::Kind::common_era);
    BOOST_REQUIRE(clock.epoch().origin());
    BOOST_CHECK_EQUAL(*clock.epoch().origin(), "1970-01-01T00:00:00+00:00");
    BOOST_REQUIRE(clock.epoch().calendar());
    BOOST_CHECK(*clock.epoch().calendar() == ldm::time::Calendar::gregorian);
}


#include <stan/mcmc/hmc/nuts/softabs_nuts.hpp>
#include <stan/mcmc/hmc/nuts/unit_e_nuts.hpp>
#include <stan/mcmc/hmc/nuts/diag_e_nuts.hpp>
#include <stan/mcmc/hmc/nuts/dense_e_nuts.hpp>
#include <stan/mcmc/hmc/hamiltonians/unit_e_point.hpp>
#include <stan/mcmc/hmc/hamiltonians/diag_e_point.hpp>
#include <stan/mcmc/hmc/hamiltonians/dense_e_point.hpp>

#include <test/unit/mcmc/hmc/mock_hmc.hpp>

#include <boost/random/additive_combine.hpp>

#include <gtest/gtest.h>

typedef boost::ecuyer1988 rng_t;

TEST(McmcNutsDerivedNuts, compute_criterion_unit_e) {

  rng_t base_rng(0);

  int model_size = 1;

  stan::mcmc::unit_e_point start(model_size);
  stan::mcmc::unit_e_point finish(model_size);
  Eigen::VectorXd p_sharp_start(model_size);
  Eigen::VectorXd p_sharp_finish(model_size);
  Eigen::VectorXd rho(model_size);

  stan::mcmc::mock_model model(model_size);
  stan::mcmc::unit_e_nuts<stan::mcmc::mock_model, rng_t> sampler(model, base_rng);

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = 1;

  p_sharp_start = start.p;
  p_sharp_finish = finish.p;
  rho = start.p + finish.p;

  EXPECT_TRUE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = -1;

  p_sharp_start = start.p;
  p_sharp_finish = finish.p;
  rho = start.p + finish.p;

  EXPECT_FALSE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));
}

TEST(McmcNutsDerivedNuts, compute_criterion_diag_e) {

  rng_t base_rng(0);

  int model_size = 1;

  stan::mcmc::diag_e_point start(model_size);
  stan::mcmc::diag_e_point finish(model_size);
  Eigen::VectorXd p_sharp_start(model_size);
  Eigen::VectorXd p_sharp_finish(model_size);
  Eigen::VectorXd rho(model_size);

  stan::mcmc::mock_model model(model_size);
  stan::mcmc::diag_e_nuts<stan::mcmc::mock_model, rng_t> sampler(model, base_rng);

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = 1;

  p_sharp_start = start.inv_e_metric_.cwiseProduct(start.p);
  p_sharp_finish = finish.inv_e_metric_.cwiseProduct(finish.p);
  rho = start.p + finish.p;

  EXPECT_TRUE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = -1;

  p_sharp_start = start.inv_e_metric_.cwiseProduct(start.p);
  p_sharp_finish = finish.inv_e_metric_.cwiseProduct(finish.p);
  rho = start.p + finish.p;

  EXPECT_FALSE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));
}

TEST(McmcNutsDerivedNuts, compute_criterion_dense_e) {

  rng_t base_rng(0);

  int model_size = 1;

  stan::mcmc::dense_e_point start(model_size);
  stan::mcmc::dense_e_point finish(model_size);
  Eigen::VectorXd p_sharp_start(model_size);
  Eigen::VectorXd p_sharp_finish(model_size);
  Eigen::VectorXd rho(model_size);

  stan::mcmc::mock_model model(model_size);
  stan::mcmc::dense_e_nuts<stan::mcmc::mock_model, rng_t> sampler(model, base_rng);

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = 1;

  p_sharp_start = start.inv_e_metric_ * start.p;
  p_sharp_finish = finish.inv_e_metric_ * finish.p;
  rho = start.p + finish.p;

  EXPECT_TRUE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = -1;

  p_sharp_start = start.inv_e_metric_ * start.p;
  p_sharp_finish = finish.inv_e_metric_ * finish.p;
  rho = start.p + finish.p;

  EXPECT_FALSE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));
}

TEST(McmcNutsDerivedNuts, compute_criterion_softabs) {

  rng_t base_rng(0);

  int model_size = 1;

  stan::mcmc::softabs_point start(model_size);
  stan::mcmc::softabs_point finish(model_size);
  Eigen::VectorXd p_sharp_start(model_size);
  Eigen::VectorXd p_sharp_finish(model_size);
  Eigen::VectorXd rho(model_size);

  stan::mcmc::mock_model model(model_size);
  stan::mcmc::softabs_nuts<stan::mcmc::mock_model, rng_t>
    sampler(model, base_rng);

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = 1;

  p_sharp_start = start.p;
  p_sharp_finish = finish.p;
  rho = start.p + finish.p;

  EXPECT_TRUE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));

  start.q(0) = 1;
  start.p(0) = 1;

  finish.q(0) = 2;
  finish.p(0) = -1;

  p_sharp_start = start.p;
  p_sharp_finish = finish.p;
  rho = start.p + finish.p;

  EXPECT_FALSE(sampler.compute_criterion(p_sharp_start, p_sharp_finish, rho));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is 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)

#include <geometry_test_common.hpp>

#include <boost/geometry/extensions/nsphere/nsphere.hpp>

#include <boost/geometry/algorithms/detail/assign_values.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/adapted/c_array.hpp>
#include <test_common/test_point.hpp>

BOOST_GEOMETRY_REGISTER_C_ARRAY_CS(cs::cartesian)

namespace bg = boost::geometry;

// define a custom circle
struct custom_circle
{
    float x,y;
    int r;
};

// adapt custom circle using traits
namespace boost { namespace geometry { namespace traits {

template<>
struct tag<custom_circle>
{
    typedef nsphere_tag type;
};

template<>
struct point_type<custom_circle>
{
    typedef model::point<double, 2, cs::cartesian> type;
};

template<>
struct radius_type<custom_circle>
{
    typedef int type;
};

template<>
struct access<custom_circle, 0>
{
    static inline const float& get(custom_circle const& c)
    {
        return c.x;
    }

    static inline void set(custom_circle& c, float const& value)
    {
        c.y = value;
    }
};

template<>
struct access<custom_circle, 1>
{
    static inline const float& get(const custom_circle& c)
    {
        return c.y;
    }

    static inline void set(custom_circle& c, float const& value)
    {
        c.y = value;
    }
};

template<>
struct radius_access<custom_circle, int, 0>
{
    static inline int get(custom_circle const& c)
    {
        return c.r;
    }

    static inline void set(custom_circle& c, int const& radius)
    {
        c.r = radius;
    }
};

}}} // namespace bg::traits

template <typename S, typename RT, typename CT>
void check_nsphere(S& to_check, RT radius, CT center_x, CT center_y, CT center_z)
{
    BOOST_CONCEPT_ASSERT( (bg::concept::ConstNsphere<S>) );
    BOOST_CONCEPT_ASSERT( (bg::concept::Nsphere<S>) );

    BOOST_CHECK_EQUAL(bg::get_radius<0>(to_check), radius);

    BOOST_CHECK_EQUAL(bg::get<0>(to_check), center_x);
    BOOST_CHECK_EQUAL(bg::get<1>(to_check), center_y);
    if (bg::dimension<S>::value >= 3)
    {
        BOOST_CHECK_EQUAL(bg::get<2>(to_check), center_z);
    }
}

template <typename P, typename T>
void test_construction()
{
    typedef typename bg::coordinate_type<P>::type ctype;

    bg::model::nsphere<P, T> c1;
    check_nsphere(c1, 0, 0,0,0);

    P center;
    bg::assign_values(center, 1, 2, 3);
    bg::model::nsphere<P, T> c2(center, 4);
    check_nsphere(c2, 4, 1,2,3);
}

template <typename C>
void test_assignment_3d()
{
    C c;

    // by hand
    bg::set<0>(c, 5);
    bg::set<1>(c, 50);
    bg::set<2>(c, 500);

    bg::set_radius<0>(c, 5000);
    check_nsphere(c, 5000, 5,50,500);

    bg::assign_values(c, 6, 60, 600);
    check_nsphere(c, 5000, 6,60,600);
}

template <typename C>
void test_assignment_2d()
{
    C c;

    // by hand
    bg::set<0>(c, 5);
    bg::set<1>(c, 50);

    bg::set_radius<0>(c, 5000);
}

template <typename P, typename T>
void test_all()
{
    test_construction<P, T>();
    test_assignment_3d<bg::model::nsphere<P, T> >();
}

template <typename P>
void test_all()
{
    test_all<P, int>();
    test_all<P, float>();
    test_all<P, double>();
}

int test_main(int, char* [])
{
    test_all<int[3]>();
    test_all<float[3]>();
    test_all<double[3]>();
    test_all<test::test_point>();
    test_all<bg::model::point<int, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<float, 3, bg::cs::cartesian> >();
    test_all<bg::model::point<double, 3, bg::cs::cartesian> >();

    test_assignment_2d<custom_circle>();

    return 0;
}


// SIRFilter.hpp
// Copyright 2019 Mikko Lauri
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SIRFILTER_HPP
#define SIRFILTER_HPP
#include "History.h"
#include "PRNG.h"
#include "ParticleUtilities.h"
#include <boost/random/uniform_int_distribution.hpp>
#include <boost/random/discrete_distribution.hpp>

namespace pgi {
template <typename State, typename StateTransitionModel,
          typename ObservationModel>
void SIR(std::vector<State>& states, std::vector<double>& weights,
         const History& h, const StateTransitionModel& t,
         const ObservationModel& o, PRNG& rng) {
  assert(states.size() == weights.size());
  for (const auto& ao : h) {
    SIR_step(states, weights, ao.action_index_, ao.observation_index_, t, o,
             0.2 * static_cast<double>(weights.size()), rng);
  }
}

template <typename State, typename StateTransitionModel,
          typename ObservationModel>
double SIR_step(std::vector<State>& states, std::vector<double>& weights,
              std::size_t j_act, std::size_t j_obs,
              const StateTransitionModel& t, const ObservationModel& o,
              double effective_sample_size_threshold, PRNG& rng) {
  double po = 0.0; // p(o | a)
  for (std::size_t i = 0; i < states.size(); ++i) {
    states[i] = t.sample_next_state(states[i], j_act, rng);
    const double p = o.get(j_obs, states[i], j_act);
    po += weights[i] * p;
    weights[i] *= p;
  }
  normalize(weights);
  if (effective_sample_size_threshold > effective_size(weights)) {
    resample_particles(states, weights, rng);
    for (auto& s : states) {
      s.reinvigorate(rng);
    }
  }
  return po;
}

template <typename State>
void sample_to_fixed_size(std::vector<State>& states,
  std::vector<double>& weights, int N, PRNG& rng)
{
  std::size_t sz = weights.size();
  const int ns = N - static_cast<int>(sz);
  if (ns == 0)
    return;

  if (ns < 0)
  {
    // need to discard samples
    std::vector<State> keep_states(N);
    std::vector<double> keep_weights(N);
    for (int i = 0; i < N; ++i)
    {
      boost::random::discrete_distribution<> dist(weights.begin(), weights.end());
      const auto k = rng(dist);
      keep_states[i] = states[k];
      keep_weights[i] = weights[k];
      weights[k] = 0.0;
    }
    states = keep_states;
    weights = keep_weights;
    normalize(weights);
  }
  else
  {
    // need more samples
    resample_particles(states, weights, rng);

    weights.resize(N);
    std::fill(weights.begin(), weights.end(), 1.0 / static_cast<double>(N));

    states.resize(N);

    boost::random::uniform_int_distribution<std::size_t> dist(0, weights.size()-1);
    for (int i = 0; i < ns; ++i)
    {
      const std::size_t r = rng(dist);
      states[sz+i] = states[r];
      states[sz+i].reinvigorate(rng);
    }
  }
}

template <typename State>
void resample_particles(std::vector<State>& states,
                        std::vector<double>& weights, PRNG& rng) {
  std::vector<std::size_t> resample_indices(0, weights.size());
  resample(weights, resample_indices, rng);

  std::vector<State> resampled_states(states);
  for (std::size_t i = 0; i < states.size(); ++i)
    resampled_states[i] = states[resample_indices[i]];
  states = resampled_states;
  std::fill(weights.begin(), weights.end(),
            1.0 / static_cast<double>(weights.size()));
}

}  // namespace pgi

#endif  // SIRFILTER_HPP


//
// Copyright (c) 2002--2010
// Toon Knapen, Karl Meerbergen, Kresimir Fresl,
// Thomas Klimpel and Rutger ter Borg
//
// 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)
//
// THIS FILE IS AUTOMATICALLY GENERATED
// PLEASE DO NOT EDIT!
//

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_HSEQR_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_COMPUTATIONAL_HSEQR_HPP

#include <boost/assert.hpp>
#include <boost/numeric/bindings/begin.hpp>
#include <boost/numeric/bindings/detail/array.hpp>
#include <boost/numeric/bindings/detail/complex_utils.hpp>
#include <boost/numeric/bindings/is_column_major.hpp>
#include <boost/numeric/bindings/is_complex.hpp>
#include <boost/numeric/bindings/is_mutable.hpp>
#include <boost/numeric/bindings/is_real.hpp>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/remove_imaginary.hpp>
#include <boost/numeric/bindings/size.hpp>
#include <boost/numeric/bindings/stride.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>
#include <boost/numeric/bindings/value_type.hpp>
#include <boost/static_assert.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/remove_const.hpp>
#include <boost/utility/enable_if.hpp>

//
// The LAPACK-backend for hseqr is the netlib-compatible backend.
//
#include <boost/numeric/bindings/lapack/detail/lapack.h>
#include <boost/numeric/bindings/lapack/detail/lapack_option.hpp>

namespace boost {
namespace numeric {
namespace bindings {
namespace lapack {

//
// The detail namespace contains value-type-overloaded functions that
// dispatch to the appropriate back-end LAPACK-routine.
//
namespace detail {

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * float value-type.
//
inline std::ptrdiff_t hseqr( const char job, const char compz,
        const fortran_int_t n, const fortran_int_t ilo,
        const fortran_int_t ihi, float* h, const fortran_int_t ldh, float* wr,
        float* wi, float* z, const fortran_int_t ldz, float* work,
        const fortran_int_t lwork ) {
    fortran_int_t info(0);
    LAPACK_SHSEQR( &job, &compz, &n, &ilo, &ihi, h, &ldh, wr, wi, z, &ldz,
            work, &lwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * double value-type.
//
inline std::ptrdiff_t hseqr( const char job, const char compz,
        const fortran_int_t n, const fortran_int_t ilo,
        const fortran_int_t ihi, double* h, const fortran_int_t ldh,
        double* wr, double* wi, double* z, const fortran_int_t ldz,
        double* work, const fortran_int_t lwork ) {
    fortran_int_t info(0);
    LAPACK_DHSEQR( &job, &compz, &n, &ilo, &ihi, h, &ldh, wr, wi, z, &ldz,
            work, &lwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<float> value-type.
//
inline std::ptrdiff_t hseqr( const char job, const char compz,
        const fortran_int_t n, const fortran_int_t ilo,
        const fortran_int_t ihi, std::complex<float>* h,
        const fortran_int_t ldh, std::complex<float>* w,
        std::complex<float>* z, const fortran_int_t ldz,
        std::complex<float>* work, const fortran_int_t lwork ) {
    fortran_int_t info(0);
    LAPACK_CHSEQR( &job, &compz, &n, &ilo, &ihi, h, &ldh, w, z, &ldz, work,
            &lwork, &info );
    return info;
}

//
// Overloaded function for dispatching to
// * netlib-compatible LAPACK backend (the default), and
// * complex<double> value-type.
//
inline std::ptrdiff_t hseqr( const char job, const char compz,
        const fortran_int_t n, const fortran_int_t ilo,
        const fortran_int_t ihi, std::complex<double>* h,
        const fortran_int_t ldh, std::complex<double>* w,
        std::complex<double>* z, const fortran_int_t ldz,
        std::complex<double>* work, const fortran_int_t lwork ) {
    fortran_int_t info(0);
    LAPACK_ZHSEQR( &job, &compz, &n, &ilo, &ihi, h, &ldh, w, z, &ldz, work,
            &lwork, &info );
    return info;
}

} // namespace detail

//
// Value-type based template class. Use this class if you need a type
// for dispatching to hseqr.
//
template< typename Value, typename Enable = void >
struct hseqr_impl {};

//
// This implementation is enabled if Value is a real type.
//
template< typename Value >
struct hseqr_impl< Value, typename boost::enable_if< is_real< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixH, typename VectorWR, typename VectorWI,
            typename MatrixZ, typename WORK >
    static std::ptrdiff_t invoke( const char job, const char compz,
            const fortran_int_t ilo, const fortran_int_t ihi,
            MatrixH& h, VectorWR& wr, VectorWI& wi, MatrixZ& z,
            detail::workspace1< WORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixH >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixZ >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixH >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorWR >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixH >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorWI >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixH >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixZ >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixH >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorWR >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorWI >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixZ >::value) );
        BOOST_ASSERT( bindings::size(work.select(real_type())) >=
                min_size_work( bindings::size_column(h) ));
        BOOST_ASSERT( bindings::size(wr) >= bindings::size_column(h) );
        BOOST_ASSERT( bindings::size_column(h) >= 0 );
        BOOST_ASSERT( bindings::size_minor(h) == 1 ||
                bindings::stride_minor(h) == 1 );
        BOOST_ASSERT( bindings::size_minor(z) == 1 ||
                bindings::stride_minor(z) == 1 );
        BOOST_ASSERT( bindings::stride_major(h) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(h)) );
        BOOST_ASSERT( compz == 'N' || compz == 'I' || compz == 'V' );
        BOOST_ASSERT( job == 'E' || job == 'S' );
        return detail::hseqr( job, compz, bindings::size_column(h), ilo, ihi,
                bindings::begin_value(h), bindings::stride_major(h),
                bindings::begin_value(wr), bindings::begin_value(wi),
                bindings::begin_value(z), bindings::stride_major(z),
                bindings::begin_value(work.select(real_type())),
                bindings::size(work.select(real_type())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixH, typename VectorWR, typename VectorWI,
            typename MatrixZ >
    static std::ptrdiff_t invoke( const char job, const char compz,
            const fortran_int_t ilo, const fortran_int_t ihi,
            MatrixH& h, VectorWR& wr, VectorWI& wi, MatrixZ& z,
            minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< real_type > tmp_work( min_size_work(
                bindings::size_column(h) ) );
        return invoke( job, compz, ilo, ihi, h, wr, wi, z,
                workspace( tmp_work ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixH, typename VectorWR, typename VectorWI,
            typename MatrixZ >
    static std::ptrdiff_t invoke( const char job, const char compz,
            const fortran_int_t ilo, const fortran_int_t ihi,
            MatrixH& h, VectorWR& wr, VectorWI& wi, MatrixZ& z,
            optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        real_type opt_size_work;
        detail::hseqr( job, compz, bindings::size_column(h), ilo, ihi,
                bindings::begin_value(h), bindings::stride_major(h),
                bindings::begin_value(wr), bindings::begin_value(wi),
                bindings::begin_value(z), bindings::stride_major(z),
                &opt_size_work, -1 );
        bindings::detail::array< real_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( job, compz, ilo, ihi, h, wr, wi, z,
                workspace( tmp_work ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return std::max< std::ptrdiff_t >(1,n);
    }
};

//
// This implementation is enabled if Value is a complex type.
//
template< typename Value >
struct hseqr_impl< Value, typename boost::enable_if< is_complex< Value > >::type > {

    typedef Value value_type;
    typedef typename remove_imaginary< Value >::type real_type;

    //
    // Static member function for user-defined workspaces, that
    // * Deduces the required arguments for dispatching to LAPACK, and
    // * Asserts that most arguments make sense.
    //
    template< typename MatrixH, typename VectorW, typename MatrixZ,
            typename WORK >
    static std::ptrdiff_t invoke( const char job, const char compz,
            const fortran_int_t ilo, const fortran_int_t ihi,
            MatrixH& h, VectorW& w, MatrixZ& z, detail::workspace1<
            WORK > work ) {
        namespace bindings = ::boost::numeric::bindings;
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixH >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_column_major< MatrixZ >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixH >::type >::type,
                typename remove_const< typename bindings::value_type<
                VectorW >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (boost::is_same< typename remove_const<
                typename bindings::value_type< MatrixH >::type >::type,
                typename remove_const< typename bindings::value_type<
                MatrixZ >::type >::type >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixH >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< VectorW >::value) );
        BOOST_STATIC_ASSERT( (bindings::is_mutable< MatrixZ >::value) );
        BOOST_ASSERT( bindings::size(work.select(value_type())) >=
                min_size_work( bindings::size_column(h) ));
        BOOST_ASSERT( bindings::size_column(h) >= 0 );
        BOOST_ASSERT( bindings::size_minor(h) == 1 ||
                bindings::stride_minor(h) == 1 );
        BOOST_ASSERT( bindings::size_minor(z) == 1 ||
                bindings::stride_minor(z) == 1 );
        BOOST_ASSERT( bindings::stride_major(h) >= std::max< std::ptrdiff_t >(1,
                bindings::size_column(h)) );
        BOOST_ASSERT( compz == 'N' || compz == 'I' || compz == 'V' );
        BOOST_ASSERT( job == 'E' || job == 'S' );
        return detail::hseqr( job, compz, bindings::size_column(h), ilo, ihi,
                bindings::begin_value(h), bindings::stride_major(h),
                bindings::begin_value(w), bindings::begin_value(z),
                bindings::stride_major(z),
                bindings::begin_value(work.select(value_type())),
                bindings::size(work.select(value_type())) );
    }

    //
    // Static member function that
    // * Figures out the minimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member function
    // * Enables the unblocked algorithm (BLAS level 2)
    //
    template< typename MatrixH, typename VectorW, typename MatrixZ >
    static std::ptrdiff_t invoke( const char job, const char compz,
            const fortran_int_t ilo, const fortran_int_t ihi,
            MatrixH& h, VectorW& w, MatrixZ& z, minimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        bindings::detail::array< value_type > tmp_work( min_size_work(
                bindings::size_column(h) ) );
        return invoke( job, compz, ilo, ihi, h, w, z, workspace( tmp_work ) );
    }

    //
    // Static member function that
    // * Figures out the optimal workspace requirements, and passes
    //   the results to the user-defined workspace overload of the
    //   invoke static member
    // * Enables the blocked algorithm (BLAS level 3)
    //
    template< typename MatrixH, typename VectorW, typename MatrixZ >
    static std::ptrdiff_t invoke( const char job, const char compz,
            const fortran_int_t ilo, const fortran_int_t ihi,
            MatrixH& h, VectorW& w, MatrixZ& z, optimal_workspace ) {
        namespace bindings = ::boost::numeric::bindings;
        value_type opt_size_work;
        detail::hseqr( job, compz, bindings::size_column(h), ilo, ihi,
                bindings::begin_value(h), bindings::stride_major(h),
                bindings::begin_value(w), bindings::begin_value(z),
                bindings::stride_major(z), &opt_size_work, -1 );
        bindings::detail::array< value_type > tmp_work(
                traits::detail::to_int( opt_size_work ) );
        return invoke( job, compz, ilo, ihi, h, w, z, workspace( tmp_work ) );
    }

    //
    // Static member function that returns the minimum size of
    // workspace-array work.
    //
    static std::ptrdiff_t min_size_work( const std::ptrdiff_t n ) {
        return std::max< std::ptrdiff_t >(1,n);
    }
};

//
// Functions for direct use. These functions are overloaded for temporaries,
// so that wrapped types can still be passed and used for write-access. In
// addition, if applicable, they are overloaded for user-defined workspaces.
// Calls to these functions are passed to the hseqr_impl classes. In the
// documentation, most overloads are collapsed to avoid a large number of
// prototypes which are very similar.
//

//
// Overloaded function for hseqr. Its overload differs for
// * User-defined workspace
//
template< typename MatrixH, typename VectorWR, typename VectorWI,
        typename MatrixZ, typename Workspace >
inline typename boost::enable_if< detail::is_workspace< Workspace >,
        std::ptrdiff_t >::type
hseqr( const char job, const char compz, const fortran_int_t ilo,
        const fortran_int_t ihi, MatrixH& h, VectorWR& wr, VectorWI& wi,
        MatrixZ& z, Workspace work ) {
    return hseqr_impl< typename bindings::value_type<
            MatrixH >::type >::invoke( job, compz, ilo, ihi, h, wr, wi, z,
            work );
}

//
// Overloaded function for hseqr. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixH, typename VectorWR, typename VectorWI,
        typename MatrixZ >
inline typename boost::disable_if< detail::is_workspace< MatrixZ >,
        std::ptrdiff_t >::type
hseqr( const char job, const char compz, const fortran_int_t ilo,
        const fortran_int_t ihi, MatrixH& h, VectorWR& wr, VectorWI& wi,
        MatrixZ& z ) {
    return hseqr_impl< typename bindings::value_type<
            MatrixH >::type >::invoke( job, compz, ilo, ihi, h, wr, wi, z,
            optimal_workspace() );
}

//
// Overloaded function for hseqr. Its overload differs for
// * User-defined workspace
//
template< typename MatrixH, typename VectorW, typename MatrixZ,
        typename Workspace >
inline typename boost::enable_if< mpl::and_< is_complex<
        typename bindings::value_type< MatrixH >::type >,
        detail::is_workspace< Workspace > >,
        std::ptrdiff_t >::type
hseqr( const char job, const char compz, const fortran_int_t ilo,
        const fortran_int_t ihi, MatrixH& h, VectorW& w, MatrixZ& z,
        Workspace work ) {
    return hseqr_impl< typename bindings::value_type<
            MatrixH >::type >::invoke( job, compz, ilo, ihi, h, w, z, work );
}

//
// Overloaded function for hseqr. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixH, typename VectorW, typename MatrixZ >
inline typename boost::disable_if< mpl::or_< is_real<
        typename bindings::value_type< MatrixH >::type >,
        detail::is_workspace< MatrixZ > >,
        std::ptrdiff_t >::type
hseqr( const char job, const char compz, const fortran_int_t ilo,
        const fortran_int_t ihi, MatrixH& h, VectorW& w, MatrixZ& z ) {
    return hseqr_impl< typename bindings::value_type<
            MatrixH >::type >::invoke( job, compz, ilo, ihi, h, w, z,
            optimal_workspace() );
}

//
// Overloaded function for hseqr. Its overload differs for
// * User-defined workspace
//
template< typename MatrixH, typename VectorW, typename MatrixZ,
        typename Workspace >
inline typename boost::enable_if< mpl::and_< is_real<
        typename bindings::value_type< MatrixH >::type >,
        detail::is_workspace< Workspace > >,
        std::ptrdiff_t >::type
hseqr( const char job, const char compz, const fortran_int_t ilo,
        const fortran_int_t ihi, MatrixH& h, VectorW& w, MatrixZ& z,
        Workspace work ) {
    std::ptrdiff_t info = hseqr_impl< typename bindings::value_type<
            MatrixH >::type >::invoke( job, compz, ilo, ihi, h,
            bindings::detail::real_part_view(w), bindings::detail::imag_part_view(w),
            z, work );
    bindings::detail::interlace(w);
    return info;
}

//
// Overloaded function for hseqr. Its overload differs for
// * Default workspace-type (optimal)
//
template< typename MatrixH, typename VectorW, typename MatrixZ >
inline typename boost::disable_if< mpl::or_< is_complex<
        typename bindings::value_type< MatrixH >::type >,
        detail::is_workspace< MatrixZ > >,
        std::ptrdiff_t >::type
hseqr( const char job, const char compz, const fortran_int_t ilo,
        const fortran_int_t ihi, MatrixH& h, VectorW& w, MatrixZ& z ) {
    std::ptrdiff_t info = hseqr_impl< typename bindings::value_type<
            MatrixH >::type >::invoke( job, compz, ilo, ihi, h,
            bindings::detail::real_part_view(w), bindings::detail::imag_part_view(w),
            z, optimal_workspace() );
    bindings::detail::interlace(w);
    return info;
}

} // namespace lapack
} // namespace bindings
} // namespace numeric
} // namespace boost

#endif