/* -*-c++-*--------------------------------------------------------------------
 * 2018 Bernd Pfrommer bernd.pfrommer@gmail.com
 */

#include "tagslam/init_pose.h"
#include "tagslam/camera_intrinsics.h"
#include "tagslam/logging.h"
#include "tagslam/rpp.h"

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>

#include <boost/range/irange.hpp>
#include <iostream>
#include <sstream>

//#define DEBUG_POSE

namespace tagslam {
  namespace init_pose {
    using boost::irange;
    std::string cv_type_to_str(int type) {
      std::string r;
      uchar depth = type & CV_MAT_DEPTH_MASK;
      uchar chans = 1 + (type >> CV_CN_SHIFT);
      switch ( depth ) {
      case CV_8U:  r = "8U"; break;
      case CV_8S:  r = "8S"; break;
      case CV_16U: r = "16U"; break;
      case CV_16S: r = "16S"; break;
      case CV_32S: r = "32S"; break;
      case CV_32F: r = "32F"; break;
      case CV_64F: r = "64F"; break;
      default:     r = "User"; break;
      }
      r += "C";
      r += (chans+'0');
      return r;
    }

    std::string cv_info(const cv::Mat &m) {
      std::stringstream ss;
      std::vector<cv::Mat> c(m.channels());
      cv::split(m, c);
      ss << cv_type_to_str(m.type()) << " (" << m.cols << "x" << m.rows << ")";
      for (int i = 0; i < (int)c.size(); i++) {
        cv::Point minLoc, maxLoc;
        double minVal, maxVal;
        cv::minMaxLoc(c[i], &minVal, &maxVal, &minLoc, &maxLoc);
        ss << "[" << minVal << " - " << maxVal << "]";
      }
      return (ss.str());
    }

    static std::string print_elem(const cv::Mat &m,
                                  const int row, const int col) {
      std::stringstream ss;
      switch (m.channels()) {
      case 1:
        ss << m.at<double>(row, col);
        break;
      case 2:
        ss << "[" << m.at<cv::Vec2d>(row, col)[0] << ", "
           << m.at<cv::Vec2d>(row, col)[1] << "]";
        break;
      case 3:
        ss << "[" << m.at<cv::Vec3d>(row, col)[0] << ", "
           << m.at<cv::Vec3d>(row, col)[1] << ", "
           << m.at<cv::Vec3d>(row, col)[2] <<  "]";
        break;
      default:
        BOMB_OUT("too many channels to print!");
      }
      return (ss.str());
    }

    static std::string print_row(const cv::Mat &m, const int row) {
      std::stringstream ss;
      ss << "[";
      for (const auto j: irange(0, m.cols - 1)) {
        ss << print_elem(m, row, j) << ", ";
      }
      if (m.cols > 0) {
        ss << print_elem(m, row, m.cols - 1);
      }
      ss << "]";
      return (ss.str());
    }

    std::string print_as_python_mat(const cv::Mat &m) {
      std::stringstream ss;
      ss << "[";
      for (const auto i: irange(0, m.rows - 1)) {
        ss << print_row(m, i);
        ss << "," << std::endl;
      }
      if (m.rows > 0) {
        ss << print_row(m, m.rows - 1);
      }
      ss << "]";
      return (ss.str());
    }

    std::string print_row(
      const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> &m, int row) {
      std::stringstream ss;
      ss << "[";
      for (int j = 0; j < m.cols() - 1; j++) {
        ss << m(row, j) << ", ";
      }
      ss << m(row, m.cols() - 1) << "]";
      return (ss.str());
    }

    std::string print_as_python_mat(
      const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> &m) {
      std::stringstream ss;
      if (m.rows() == 0 || m.cols() == 0) {
        return ("[[]]");
      }
      ss << "[";
      for (int i = 0; i < m.rows() - 1; i++) {
        ss << print_row(m, i);
        ss << "," << std::endl;
      }
      ss << print_row(m, m.rows() - 1);
      ss << "]" << std::endl;
      return (ss.str());
    }

#ifdef DEBUG_POSE
    static void test_reprojection(const cv::Mat &ip,
                                  const cv::Mat &wp,
                                  const cv::Mat &R,
                                  const cv::Mat &tvec) {
      cv::Mat rwp = (R * wp.t()).t();
      for (int i = 0; i < rwp.rows; i++) {
        rwp.row(i) = rwp.row(i) + tvec.t();
        for (int j = 0; j < 2; j++) {
          std::cout << rwp.row(i).at<double>(j)/rwp.row(i).at<double>(2)
            - ip.at<cv::Vec2d>(i,0)[j] << " ";
        }
        std::cout << std::endl;
      }
    }
#endif

#ifdef ALTERNATIVE_DECOMPOSITIONS
    static void decompose_homography_simple(const cv::Mat &H,
                                            cv::Mat *R,
                                            cv::Mat *rvec, cv::Mat *tvec) {
      cv::Mat RR(3,3,CV_64F);
      // now normalize and orthogonalize the first two columns,
      // which are the first two columns of the rotation matrix R
      cv::Mat r1 = H.col(0)/cv::norm(H.col(0));
      cv::Mat r2 = H.col(1) - H.col(1).dot(r1)*r1;
      r2 = r2/cv::norm(r2);
      r1.copyTo(RR.col(0));
      r2.copyTo(RR.col(1));
      // r3 = r1 x r2
      RR.at<double>(0,2) = r1.at<double>(1,0)*r2.at<double>(2,0) -
        r1.at<double>(2,0)*r2.at<double>(1,0);
      RR.at<double>(1,2) = r1.at<double>(2,0)*r2.at<double>(0,0) -
        r1.at<double>(0,0)*r2.at<double>(2,0);
      RR.at<double>(2,2) = r1.at<double>(0,0)*r2.at<double>(1,0) -
        r1.at<double>(1,0)*r2.at<double>(0,0);
      *tvec = cv::Mat(3,1,CV_64F);
      H.col(2).copyTo(*tvec);
      *R = RR;
      std::cout << "R from simple: " << std::endl << RR << std::endl;
      std::cout << "T from simple: " << std::endl << *tvec << std::endl;
      *rvec = cv::Mat(3,1,CV_64F);
      cv::Rodrigues(RR, *rvec);
    }

    static bool decompose_homography(
      const cv::Mat &H, cv::Mat *R, cv::Mat *rvec, cv::Mat *tvec) {
      //
      // This follows Ma "An invitation to 3D vision
      //
      cv::Mat HTH, V, SIG, tmp;
      cv::SVD::compute(H.t() * H, SIG, V, tmp);
      std::cout << "H: " << std::endl << H << std::endl;
      std::cout << "V: " << std::endl << V << std::endl;
      std::cout << "SIG^2: " << std::endl << SIG << std::endl;
      double sig1(SIG.at<double>(0)),  sig2(SIG.at<double>(1)),
        sig3(SIG.at<double>(2));
      if (fabs(sig2 - 1.0) > 1e-7) {
        BOMB_OUT("2nd singular value must be 1.0, but is " << sig2);
      }
      if (sig1 < sig2 || sig2 < sig3) {
        BOMB_OUT("singular values not sorted!");
      }
      double c1(std::sqrt(sig1  - 1.0)), c3(std::sqrt(1.0 - sig3));
      double dsig(std::sqrt(sig1 - sig3));

      cv::Mat v1 = V.col(0);
      cv::Mat v2 = V.col(1);
      cv::Mat v3 = V.col(2);
      cv::Mat u1 = (c3 * v1 + c1 * v3) / dsig;
      cv::Mat u2 = (c3 * v1 - c1 * v3) / dsig;
      cv::Mat U1(3,3, CV_64F), U2(3,3,CV_64F),
                 W1(3,3, CV_64F), W2(3,3, CV_64F);
      v2.copyTo(U1(cv::Rect(0, 0, 1, 3)));
      u1.copyTo(U1(cv::Rect(1, 0, 1, 3)));
      v2.cross(u1).copyTo(U1(cv::Rect(2, 0, 1, 3)));

      v2.copyTo(U2(cv::Rect(0, 0, 1, 3)));
      u2.copyTo(U2(cv::Rect(1, 0, 1, 3)));
      v2.cross(u2).copyTo(U2(cv::Rect(2, 0, 1, 3)));

      cv::Mat Hv2(H * v2), Hu1(H * u1), Hu2(H * u2);
      Hv2.copyTo(W1(cv::Rect(0, 0, 1, 3)));
      Hu1.copyTo(W1(cv::Rect(1, 0, 1, 3)));
      Hv2.cross(Hu1).copyTo(W1(cv::Rect(2, 0, 1, 3)));

      Hv2.copyTo(W2(cv::Rect(0, 0, 1, 3)));
      Hu2.copyTo(W2(cv::Rect(1, 0, 1, 3)));
      Hv2.cross(Hu2).copyTo(W2(cv::Rect(2, 0, 1, 3)));

      cv::Mat R1 = W1 * U1.t();
      cv::Mat N1 = v2.cross(u1);
      cv::Mat T1 = (H - R1) * N1;
      std::cout << "R1: " << std::endl << R1 << std::endl;
      std::cout << "N1: " << std::endl << N1 << std::endl;
      std::cout << "T1: " << std::endl << T1 << std::endl;

      cv::Mat R2 = W2 * U2.t();
      cv::Mat N2 = v2.cross(u2);
      // must add term: R * e3 for shift to zero coordinates
      cv::Mat T2 = (H - R2) * N2;
      std::cout << "R2: " << std::endl << R2 << std::endl;
      std::cout << "N2: " << std::endl << N2 << std::endl;
      std::cout << "T2: " << std::endl << T2 << std::endl;

      // which ever solution has larger N along z component wins
      bool hasGoodR1 = R1.at<double>(2,2) < 0;
      bool hasGoodR2 = R2.at<double>(2,2) < 0;
      bool use1 = hasGoodR1 && (fabs(N1.at<double>(2))
                                > fabs(N2.at<double>(2)) || !hasGoodR2);
      *R = use1 ? R1 : R2;
      cv::Mat N    = use1 ? N1 : N2;
      cv::Mat Traw = use1 ? T1 : T2;
      cv::Mat T  = ((N.at<double>(2) < 0) ? -Traw : Traw) + R->col(2);
      *tvec = T;
      std::cout << "R: " << std::endl << *R << std::endl;
      std::cout << "T: " << std::endl << *tvec << std::endl;
      *rvec = cv::Mat(3,1,CV_64F);
      cv::Rodrigues(*R, *rvec);
      return ((hasGoodR1 || hasGoodR2) && T.at<double>(2) > 0);
    }

#endif // end of ALTERNATIVE_DECOMPOSITIONS

    static void decompose_homography_svd(const cv::Mat &H,
                                         cv::Mat *R,
                                         cv::Mat *rvec, cv::Mat *tvec) {
      //
      // Taken from Kostas Daniliidis' MEAM 620 lecture notes
      //
      cv::Mat RR(3,3,CV_64F);
      cv::Mat H12 = cv::Mat_<double>(3,3);
      H.col(0).copyTo(H12.col(0));
      H.col(1).copyTo(H12.col(1));
      H.col(0).cross(H.col(1)).copyTo(H12.col(2));
      cv::Mat W, U, VT;
      cv::SVD::compute(H12, W, U, VT);
      cv::Mat diag = (cv::Mat_<double>(3,3) << 1, 0, 0,
                      0, 1, 0,   0, 0, cv::determinant(U*VT));
      cv::Mat RSVD = U * diag * VT;

      RR = RSVD;
      *tvec = cv::Mat(3,1,CV_64F);
      cv::Mat hl(3, 1, CV_64F);
      H.col(2).copyTo(hl);
      hl = hl / cv::norm(H.col(0));
      hl.copyTo(*tvec);
      *R = RR;
#ifdef DEBUG_POSE
      std::cout << "R from SVD: " << std::endl <<
        print_as_python_mat(RR) << std::endl;
      std::cout << "T from SVD: " << std::endl <<
        print_as_python_mat(*tvec) << std::endl;
      cv::Mat T = cv::Mat::eye(4,4,CV_64F);
      cv::Mat tmp = T(cv::Rect(0,0,3,3));
      RR.copyTo(tmp);
      tmp = T(cv::Rect(3,0,1,3));
      tvec->copyTo(tmp);
      std::cout << "Transform: " << std::endl <<
        print_as_python_mat(T) << std::endl;
#endif
      *rvec = cv::Mat(3,1,CV_64F);
      cv::Rodrigues(RR, *rvec);
    }

    static cv::Mat
    undistort_points(const cv::Mat &im,
                     const cv::Mat &K,  DistortionModel distModel,
                     const cv::Mat &D) {
      const cv::Mat im2chan(im.rows, 1, CV_64FC2, im.data);
      switch (distModel) {
      case RADTAN: {
        cv::Mat im_undist;
        cv::undistortPoints(im2chan, im_undist, K, D);
        return (im_undist);
        break;  }
      case EQUIDISTANT: {
        cv::Mat im_undist;
        cv::fisheye::undistortPoints(im2chan, im_undist, K, D);
        return (im_undist);
        break; }
      default:
        BOMB_OUT("ERROR: invalid distortion model: " << distModel);
        break;
      }
      return (im);
    }

    static bool find_pose_by_homography(const cv::Mat &wp, const cv::Mat &ip,
                                        cv::Mat *rvec,  cv::Mat *tvec) {
      cv::Mat wc = wp.clone();
      cv::Mat wo = wc(cv::Rect(0,0,2,wc.rows));
#ifdef DEBUG_POSE
      std::cout << "world points: " << cv_info(wp) << std::endl
                << print_as_python_mat(wp) << std::endl;
      std::cout << "image points: " << cv_info(ip) << std::endl
                << print_as_python_mat(ip) << std::endl;
#endif
      // use opencv to get the homography
      cv::Mat HL;
      HL = cv::findHomography(wo, ip);
#ifdef DEBUG_POSE
      std::cout << "HL from getHomography" << std::endl << HL << std::endl;
#endif
      // first normalize the homography matrix (Ma, pg 136 top) so it has form
      //     H = R + 1/d*T*N^T
      cv::Mat SL;
      cv::SVD::compute(HL, SL);
      cv::Mat H  = HL * 1.0/SL.at<double>(0, 1);
      // H has scale invariance and scale can be negative! We can ensure that
      // x_j^T * H * x_i > 0  (Ma, top of page 136) by testing
      // that the trace is positive, and flipping sign
      // if it is not.
      cv::Mat iph = cv::Mat::ones(ip.rows, 3, CV_64F);
      cv::Mat wph = cv::Mat::ones(ip.rows, 4, CV_64F);
      for (int i = 0; i < ip.rows; i++) {
        iph.at<double>(i, 0) = ip.at<cv::Vec2d>(i,0)[0];
        iph.at<double>(i, 1) = ip.at<cv::Vec2d>(i,0)[1];
      }
      cv::Mat wh = wp.clone();
      wh.col(2) = 1.0;
      cv::Mat d = iph * H * wh.t();
      double sumDiag = cv::trace(d)[0];
      cv::Mat Hpos = (sumDiag >= 0)? H : -H;
      //std::cout << "raw homography: " << std::endl << Hpos << std::endl;
      cv::Mat R;
#ifdef DEBUG_POSE
#if 0
      bool foo = decompose_homography(Hpos, &R, rvec, tvec);
      std::cout << "fancy homography reprojection test: " << std::endl;
      test_reprojection(ip, wp, R, *tvec);
      decompose_homography_simple(Hpos, &R, rvec, tvec);
      std::cout << "simple homography reprojection test: " << std::endl;
      test_reprojection(ip, wp, R, *tvec);
#endif
#endif
      // use standard svd decomposition
      decompose_homography_svd(Hpos, &R, rvec, tvec);
#ifdef DEBUG_POSE
      std::cout << "svd homography reprojection test: " << std::endl;
      test_reprojection(ip, wp, R, *tvec);
#endif
      bool rc = true;
      return (rc);
    }

    std::pair<Transform, bool>
    pose_from_4(const Eigen::Matrix<double, 4, 2> &imgPoints,
                const Eigen::Matrix<double, 4, 3> &objPoints,
                const cv::Mat &K, DistortionModel distModel,
                const cv::Mat &D, const Params &params) {
      std::pair<Transform, bool> tf;
      cv::Mat ip(4, 2, CV_64F);
      cv::Mat wp(4, 3, CV_64F);
      cv::eigen2cv(imgPoints, ip);
      cv::eigen2cv(objPoints, wp);
#ifdef DEBUG_POSE
      std::cout << "uv points: " << std::endl << imgPoints << std::endl;
#endif
      const cv::Mat imu = undistort_points(ip, K, distModel, D);
      cv::Mat R, rvec, tvec;
      bool rc =  find_pose_by_homography(wp, imu, &rvec, &tvec);
      const cv::Mat D0  = cv::Mat::zeros(0, 4, CV_64F);
      const cv::Mat eye = cv::Mat::eye(3, 3, CV_64F);
      rc = cv::solvePnP(wp, imu, eye, D0, rvec,  tvec, rc);

      Point3d rvecT, tvecT;
      cv2eigen(rvec, rvecT);
      cv2eigen(tvec, tvecT);
      Eigen::Map<const Eigen::Matrix<double, 4, 2, Eigen::RowMajor>>
        imuE(imu.ptr<double>(), imu.rows, 2);
      tf.first = make_transform(rvecT, tvecT);
      tf.second = rc;
#ifdef DEBUG_POSE
      std::cout << "imu: " << cv_info(imu) << std::endl << imu << std::endl;
      std::cout << "ip=np.matrix(" << print_as_python_mat(imuE) << ")"
                << std::endl;
      std::cout << "wp=np.matrix(" << print_as_python_mat(objPoints) << ")"
                << std::endl;
      std::cout << "T=np.matrix(" << print_as_python_mat(tf.first.matrix())
                << ")" << std::endl;
#endif
      double beta_min(0), beta_max(0), beta_orig(0);
      const double qr = rpp::check_quality(imuE, objPoints, tf.first,
                                           &beta_orig, &beta_min, &beta_max);
      constexpr double RAD_2_DEG = 180.0 / M_PI;
      const double view_angle =
        std::abs(beta_orig + ((beta_orig >= 0) ? (-M_PI_2) : M_PI_2));
      if (std::abs(beta_orig - beta_min) > 0.1 &&
          qr < params.maxAmbiguityRatio) {
        // warn if the original homography angle is not the actual
        // minimum, and the new minimum has significantly lower error
        ROS_WARN_STREAM("original rot: " << beta_orig << " is off from min: "
                        << beta_min << " view angle: " <<
                        view_angle * RAD_2_DEG << ", qr: " << qr );
      }
      if (view_angle * RAD_2_DEG < params.minViewingAngle) {
        ROS_INFO_STREAM("drop tag with low viewing angle: "
                        << view_angle * RAD_2_DEG);
        tf.second = false;
      } else if (view_angle < params.ambiguityAngleThreshold
                 && qr > params.maxAmbiguityRatio) {
        // if viewed more shallow than 60 degrees, the pose better
        // be robust to flipping
        ROS_INFO_STREAM("drop tag at view angle " << view_angle * RAD_2_DEG <<
                        " with risk of flip: " << qr);
        tf.second = false;
      } else {
        ROS_DEBUG_STREAM("homography view angle: " << view_angle
                         << " qr: " << qr);
      }
      return (tf);
    }
  } // end of namespace pnp
}


#include <boost/test/unit_test.hpp>
#include <eosio/testing/tester.hpp>
#include <eosio/chain/merkle.hpp>
#include <fc/io/json.hpp>

using namespace eosio;
using namespace eosio::chain;
using namespace eosio::testing;

struct action_proof_data {
   account_name              receiver;
   scope_name                scope;
   action_name               name;
   bytes                     data;
   uint32_t                  region_id;
   uint32_t                  cycle_index;
   vector<data_access_info>  data_access;
};
FC_REFLECT(action_proof_data, (receiver)(scope)(name)(data)(region_id)(cycle_index)(data_access));

struct merkle_node {
   digest_type                   digest;
   optional<size_t>              left;
   optional<size_t>              right;
   optional<size_t>              parent;
};

digest_type process_merkle(vector<merkle_node>& nodes, vector<size_t> leaf_indices) {
   size_t num_nodes = leaf_indices.size();
   size_t base = 0;

   if (num_nodes == 0) {
      return digest_type();
   } else if (num_nodes == 1) {
      return nodes[leaf_indices.front()].digest;
   }

   while (num_nodes > 1) {
      size_t new_nodes = (num_nodes / 2) + (num_nodes % 2);
      for(size_t idx = 0; idx < new_nodes; idx++) {
         size_t l_idx = idx * 2;
         size_t r_idx = l_idx + 1;
         size_t left = leaf_indices.at(base + l_idx);
         if (r_idx >= num_nodes) {
            nodes.emplace_back(
               merkle_node{
                  digest_type::hash(make_canonical_pair(nodes[left].digest, nodes[left].digest)),
                  left
               });
            nodes[left].parent.emplace(nodes.size() - 1);
         } else {
            size_t right = leaf_indices.at(base + r_idx);
            nodes.emplace_back(
               merkle_node{
                  digest_type::hash(make_canonical_pair(nodes[left].digest, nodes[right].digest)),
                  left,
                  right
               });
            nodes[left].parent.emplace(nodes.size() - 1);
            nodes[right].parent.emplace(nodes.size() - 1);
         }

         leaf_indices.emplace_back(nodes.size() - 1);
      }

      base += num_nodes;
      num_nodes = new_nodes;
   }

   return nodes.back().digest;
}

auto get_proof_path(const vector<merkle_node>& nodes, size_t leaf) {
   vector<digest_type> path;
   digest_type current = nodes[leaf].digest;
   while (nodes[leaf].parent.valid()) {
      size_t parent = *nodes[leaf].parent;
      if (leaf == *nodes[parent].left) {
         if (nodes[parent].right.valid()) {
            size_t right = *nodes[parent].right;
            path.emplace_back(make_canonical_right(nodes[right].digest));
            current = digest_type::hash(make_canonical_pair(nodes[leaf].digest, nodes[right].digest));
         } else {
            path.emplace_back(make_canonical_right(nodes[leaf].digest));
            current = digest_type::hash(make_canonical_pair(nodes[leaf].digest, nodes[leaf].digest));
         }
      } else {
         size_t left = *nodes[parent].left;
         path.emplace_back(make_canonical_left(nodes[left].digest));
         current = digest_type::hash(make_canonical_pair(nodes[left].digest, nodes[leaf].digest));
      }

      leaf = parent;
   }

   return path;
}

digest_type apply_path(const digest_type& digest, const vector<digest_type>& path) {
   digest_type current = digest;

   for (const auto& p: path ) {
      if (is_canonical_left(p)) {
         current = digest_type::hash(make_canonical_pair(p, current));
      } else {
         current = digest_type::hash(make_canonical_pair(current, p));
      }
   }

   return current;
}

bool proof_is_valid(const digest_type& digest, const vector<digest_type>& path, const digest_type& expected_root) {
   return apply_path(digest, path) == expected_root;
}

BOOST_AUTO_TEST_SUITE(proof_tests)

BOOST_FIXTURE_TEST_CASE( prove_block_in_chain, tester ) { try {
   vector<block_id_type> known_blocks;
   known_blocks.reserve(100);
   block_header last_block_header;

    // register a callback on new blocks to record block information
   control->applied_block.connect([&](const block_trace& bt){
      known_blocks.emplace_back(bt.block.id());
      last_block_header = bt.block;
   });

   produce_blocks(100);

   vector<merkle_node> nodes;
   vector<digest_type> ids;
   vector<size_t> block_leaves;
   nodes.reserve(100);
   ids.reserve(100);
   for (const auto& blk_id: known_blocks) {
      nodes.emplace_back(merkle_node{blk_id});
      ids.push_back(blk_id);
      block_leaves.push_back(nodes.size() - 1);
   }

   digest_type block_mroot = process_merkle(nodes, move(block_leaves));

   BOOST_REQUIRE_EQUAL((std::string)block_mroot, (std::string)merkle(ids));

   produce_block();

   BOOST_REQUIRE_EQUAL((std::string)block_mroot, (std::string)last_block_header.block_mroot);

   for (int idx = 0; idx < 100; idx++) {
      vector<digest_type> path = get_proof_path(nodes, idx);

      BOOST_REQUIRE_EQUAL(true, proof_is_valid(nodes[idx].digest, path, last_block_header.block_mroot));

      /** UNCOMMENT TO PRODUCE PROOFS TO STD OUT
      std::cout << idx <<  ":" <<  std::string(known_blocks.at(idx)) << " = ";
      for (const auto& e: path) {
         std::cout << std::string(e) << " ";
      }

      std::cout << "-> " << std::string(last_block_header.block_mroot) << std::endl;
       */
   }

} FC_LOG_AND_RETHROW() } /// transfer_test

struct action_proof_info {
   action_trace trace;

   size_t action_leaf;
   size_t block_leaf;

   uint32_t cycle_index;
   uint32_t region_id;
};

/**
 *  This test case will attempt to allow one account to transfer on behalf
 *  of another account by updating the active authority.
 */
BOOST_FIXTURE_TEST_CASE( prove_action_in_block, tester ) { try {
   vector<merkle_node> nodes;
   vector<size_t> block_leaves;
   vector<action_proof_info> known_actions;
   map<digest_type, digest_type> block_action_mroots;
   block_header last_block_header;
   block_id_type last_block_id;

   // register a callback on new blocks to record block information
   control->applied_block.connect([&](const block_trace& bt){
      nodes.emplace_back(merkle_node{bt.block.id()});
      size_t block_leaf = nodes.size() - 1;
      block_leaves.push_back(block_leaf);
      vector<size_t> shard_leaves;

      for (uint32_t r_idx = 0; r_idx < bt.region_traces.size(); r_idx++) {
         const auto& rt = bt.region_traces.at(r_idx);
         for (uint32_t c_idx = 0; c_idx < rt.cycle_traces.size(); c_idx++) {
            const auto& ct = rt.cycle_traces.at(c_idx);

            for (const auto& st: ct.shard_traces) {
               vector<size_t> action_leaves;

               for (const auto& tt: st.transaction_traces) {
                  for (const auto& at: tt.action_traces) {
                     digest_type::encoder enc;

                     auto a_data = action_proof_data {
                        at.receiver,
                        at.act.account,
                        at.act.name,
                        at.act.data,
                        at.region_id,
                        at.cycle_index,
                        at.data_access
                     };
                     fc::raw::pack(enc, a_data);
                     nodes.emplace_back(merkle_node{enc.result()});
                     size_t action_leaf = nodes.size() - 1;
                     known_actions.emplace_back(action_proof_info{at, action_leaf, block_leaf, c_idx, r_idx });
                     action_leaves.emplace_back(action_leaf);
                  }
               }

               if (action_leaves.size() > 0) {
                  process_merkle(nodes, move(action_leaves));
                  shard_leaves.emplace_back(nodes.size() - 1);
               }
            }
         }
      }

      digest_type action_mroot = process_merkle(nodes, move(shard_leaves));
      BOOST_REQUIRE_EQUAL((std::string)bt.block.action_mroot, (std::string)action_mroot);

      last_block_header = bt.block;
      last_block_id = bt.block.id();
      block_action_mroots[bt.block.id()] = bt.block.action_mroot;
   });

   create_accounts( { N(alice), N(bob), N(carol), N(david), N(elvis) });

   produce_blocks(50);

   push_nonce( N(alice), "AB" );
   push_nonce( N(bob),   "BC" );
   push_nonce( N(carol), "CD" );
   push_nonce( N(david), "DE" );
   push_nonce( N(elvis), "EF" );

   produce_blocks(50);
   digest_type block_mroot = process_merkle(nodes, move(block_leaves));

   produce_block();
   BOOST_REQUIRE_EQUAL((std::string)block_mroot, (std::string)last_block_header.block_mroot);

   /* UNCOMMENT TO PRODUCE PROOFS TO STDOUT
   std::cout << "Best Block ID: " << (std::string)last_block_id << std::endl;
   std::cout << "  Merkle Root: " << (std::string)last_block_header.block_mroot << std::endl;

   for(const auto& ai : known_actions) {
      auto block_path = get_proof_path(nodes, ai.action_leaf);
      auto chain_path = get_proof_path(nodes, ai.block_leaf);

      // prove action in block
      auto shard_root = apply_path(nodes[ai.action_leaf].digest, block_path);
      BOOST_REQUIRE_EQUAL((std::string)shard_root, (std::string)block_action_mroots[nodes[ai.block_leaf].digest]);

      // prove that block is part of the chain
      auto expected_block_mroot = apply_path(nodes[ai.block_leaf].digest, chain_path);
      BOOST_REQUIRE_EQUAL((std::string)expected_block_mroot, (std::string)last_block_header.block_mroot);

      std::cout << "Proof for Action:" << std::endl;
      std::cout << std::setw(14) << "reciever" << ":" << (std::string) ai.trace.receiver << std::endl;
      std::cout << std::setw(14) << "scope" << ":" << (std::string) ai.trace.act.scope << std::endl;
      std::cout << std::setw(14) << "name" << ":" << (std::string) ai.trace.act.name << std::endl;
      std::cout << std::setw(14) << "data" << ":" << fc::json::to_string(ai.trace.act.as<contracts::transfer>()) << std::endl;
      std::cout << std::setw(14) << "data_access" << ":" << fc::json::to_string(ai.trace.data_access) << std::endl;
      std::cout << std::setw(14) << "region" << ":" << ai.region_id << std::endl;
      std::cout << std::setw(14) << "cycle" << ":" << ai.cycle_index << std::endl;
      std::cout << std::setw(14) << "block" << ":" << (std::string)nodes[ai.block_leaf].digest << std::endl;
      std::cout << std::setw(14) << "am_root" << ":" << (std::string)block_action_mroots[nodes[ai.block_leaf].digest] << std::endl;
      std::cout << std::endl;

      auto a_data = action_proof_data {
         ai.trace.receiver,
         ai.trace.act.scope,
         ai.trace.act.name,
         ai.trace.act.data,
         ai.trace.region_id,
         ai.trace.cycle_index,
         ai.trace.data_access
      };
      auto action_data = fc::raw::pack(a_data);

      std::cout << "Action Hex: " << fc::to_hex(action_data) << std::endl;
      std::cout << "Action Hash: " << (std::string)nodes[ai.action_leaf].digest << std::endl;

      std::cout << "Action Path: ";
      for (const auto& p: block_path) {
         std::cout << (std::string)p << " ";
      }
      std::cout << std::endl;

      std::cout << "Block Path: ";
      for (const auto& p: chain_path) {
         std::cout << (std::string)p << " ";
      }
      std::cout << std::endl;
   }*/

} FC_LOG_AND_RETHROW() }

BOOST_AUTO_TEST_SUITE_END()


#include "prefilter.hh"
#include "event.hh"
#include "subscription.hh"

#include <assert.h>
#include <stdlib.h>
#include <string.h>

#include <algorithm>

#include <boost/dynamic_bitset.hpp>

namespace viper {

static unsigned int MurmurHash2(const void *key, int len, unsigned int seed)
{
	const unsigned int m = 0x5bd1e995;
	const int r = 24;
	unsigned int h = seed ^ len;
	const unsigned char *data = (const unsigned char *)key;

	while(len >= 4) {
		unsigned int k = *(unsigned int *)data;

		k *= m;
		k ^= k >> r;
		k *= m;

		h *= m;
		h ^= k;

		data += 4;
		len -= 4;
	}

	switch(len)	{
		case 3: h ^= data[2] << 16;
		case 2: h ^= data[1] << 8;
		case 1: h ^= data[0];
	        	h *= m;
	};

	h ^= h >> 13;
	h *= m;
	h ^= h >> 15;

	return h;
}

static void hash_n(value val, int max, vector<int>& hashes, int n) {
	unsigned int h = 0;
	while (n-- > 0) {
		h = MurmurHash2(&val, sizeof(value), h);
		hashes.push_back(h % max);
	}
}

class BloomFilter {
public:
	BloomFilter(int size, int nb_hashes) : nb_hashes_(nb_hashes), bits_(size) {	}

	void insert(value val) {
		vector<int> hashes;
		hash_n(val, bits_.size(), hashes, nb_hashes_);
		for (vector<int>::const_iterator i = hashes.begin(); i != hashes.end(); ++i)
			bits_.set(*i);
	}

	void insert_1(value val) {
		vector<int> hashes;
		hash_n(val, bits_.size(), hashes, nb_hashes_);
		random_shuffle(hashes.begin(), hashes.end());
		bits_.set(*hashes.begin());
	}

	size_t count() const {
		return bits_.count();
	}

	bool test(int i) const {
		return bits_.test(i);
	}

	bool superset(const BloomFilter& bf) {
		return ((bits_ & bf.bits_) == bf.bits_);
	}

	friend ostream& operator<<(ostream& stream, const BloomFilter& bf);

private:
	boost::dynamic_bitset<> bits_;
	int nb_hashes_;
};

ostream& operator<<(ostream& stream, const BloomFilter& bf) {
	stream << bf.bits_ << " #" << bf.bits_.count();
	return stream;
}

struct BloomSubscription {
	BloomSubscription(const Subscription& s, int size, int nb_hashes) : sub_(s), bf_(size, nb_hashes), matches_(0), tests_(0), duplicates_(NULL) { }
	~BloomSubscription() {
		delete duplicates_;
	}

	const Subscription& sub_;
	unsigned int id_;
	BloomFilter bf_;
	int matches_;
	int tests_;
	vector<BloomSubscription*> *duplicates_;
};

PreFilter::PreFilter(int nb_bits, int nb_hashes, int variant, Graph *graph) : nb_bits_(nb_bits), nb_hashes_(nb_hashes), variant_(variant), graph_(graph), pubs_(0) {
	subs_bf_ = new vector<BloomSubscription*>[nb_bits + 1];
	if (nb_hashes < 1 || nb_hashes > nb_bits) {
		cerr << "Unvalid number of hash functions: " << nb_hashes << endl;
		exit(1);
	}
	if ((variant == ALL_CONTAINMENT_N || variant == ALL_CONTAINMENT_NP) && graph == NULL) {
		cerr << "Containment graph required with variant: " << variant << endl;
		exit(1);
	}
}

PreFilter::~PreFilter() {
	for (vector<BloomSubscription*>::iterator i = subs_.begin(); i != subs_.end(); ++i)
		delete *i;
	delete[] subs_bf_;
}

void PreFilter::add(Subscription& s) {
	BloomSubscription *bs = new BloomSubscription(s, nb_bits_, nb_hashes_);
	s.attributes(PREFILTER_ATTR, bs);
	bs->id_ = subs_.size();
	subs_.push_back(bs);
	// Check for duplicate subscription
	if (graph_ != NULL) {
		const Subscription *ps = graph_->primary(s);
		if (ps != &s) {
			BloomSubscription *pbs = (BloomSubscription *) ps->attributes(PREFILTER_ATTR);
			if (pbs->duplicates_ == NULL)
				pbs->duplicates_ = new vector<BloomSubscription*>;
			pbs->duplicates_->push_back(bs);
		}
	}
	// Create Bloom filter for subscription
	for (subscription::const_iterator i = s.data().begin(); i != s.data().end(); ++i) {
		const predicate& p = i->second;
		if (p.first == EQ) {
			if (variant_ == ONE_NO_CONTAINMENT)
				bs->bf_.insert_1(p.second);
			else
				bs->bf_.insert(p.second);
		}
	}
	if (bs->bf_.count() != 0) {
		for (int i = 0; i < nb_bits_; ++i) {
			if (bs->bf_.test(i)) {
				if (variant_ == ALL_CONTAINMENT_N || variant_ == ALL_CONTAINMENT_NP) {
					// Insert containers first
					vector<BloomSubscription*>::iterator j = subs_bf_[i].begin();
					while (j != subs_bf_[i].end() && !(s >= (*j)->sub_))
						++j;
					subs_bf_[i].insert(j, bs);
				} else {
					// Insert randomly
					subs_bf_[i].push_back(bs);
				}
			}
		}
	} else {
		if (variant_ == ALL_CONTAINMENT_N || variant_ == ALL_CONTAINMENT_NP) {
			// Insert containers first
			vector<BloomSubscription*>::iterator j = subs_bf_[nb_bits_].begin();
			while (j != subs_bf_[nb_bits_].end() && !(s >= (*j)->sub_))
				++j;
			subs_bf_[nb_bits_].insert(j, bs);
		} else {
			// Insert randomly
			subs_bf_[nb_bits_].push_back(bs);
		}
	}
}

bool PreFilter::remove(Subscription& s) {
	// Add only nodes that are not earlier in list
	BloomSubscription *bs = (BloomSubscription *) s.attributes(PREFILTER_ATTR);
	if (bs == NULL)
		return false;
	int count = bs->bf_.count();
	if (count == 0) {
		subs_bf_[nb_bits_].erase(std::remove(subs_bf_[nb_bits_].begin(), subs_bf_[nb_bits_].end(), bs), subs_bf_[nb_bits_].end());
	} else {
		for (int bit = 0; bit < nb_bits_ && count > 0; ++bit) {
			if (bs->bf_.test(bit)) {
				subs_bf_[bit].erase(std::remove(subs_bf_[bit].begin(), subs_bf_[bit].end(), bs), subs_bf_[bit].end());
				count--;
			}
		}
	}
	subs_.erase(std::remove(subs_.begin(), subs_.end(), bs), subs_.end());
	delete bs;
	return true;
}

void PreFilter::match(const Event& e, vector<const Subscription*> *matches) {
	pubs_++;
	// Create Bloom filter for event
	BloomFilter bf(nb_bits_, nb_hashes_);
	for (event::const_iterator i = e.data().begin(); i != e.data().end(); ++i)
		bf.insert(i->second);
	size_t c = bf.count();
	if (VERBOSE >= 3)
		cout << "  " << bf << endl;
	// Only keep subscriptions that have the same bits set
	bool *matched = new bool[subs_.size()];
	memset(matched, 0, subs_.size() * sizeof(bool));
	int match = 0;
	int test = 0;
	vector<const Subscription*> cands;
	// Traverse all lists (except default)
	for (int bit = 0; bit < nb_bits_ && c > 0; ++bit) {
		if (bf.test(bit)) {
			for (vector<BloomSubscription*>::const_iterator i = subs_bf_[bit].begin(); i != subs_bf_[bit].end(); ++i) {
				if (graph_ != NULL) {
					// Do not test duplicate subscriptions
					if (graph_->primary((*i)->sub_) != &(*i)->sub_)
						matched[(*i)->id_] = true;
				}
				if (!matched[(*i)->id_]) {
					matched[(*i)->id_] = true;
					bool m = false;
					if (bf.superset((*i)->bf_)) {
						test++;
						(*i)->tests_++;
						if ((*i)->sub_.matches(e)) {
							int count = (graph_ == NULL ? 1 : graph_->duplicates((*i)->sub_));
							if (matches != NULL) {
								for (int j = 0; j < count; j++)
									matches->push_back(&(*i)->sub_);
							}
							match += count;
							(*i)->matches_++;
							if ((*i)->duplicates_ != NULL) {
								for (vector<BloomSubscription*>::const_iterator j = (*i)->duplicates_->begin(); j != (*i)->duplicates_->end(); ++j) {
									(*j)->matches_++;
									matched[(*j)->id_] = true;
								}
							}
							m = true;
						}
					}
					if (m && variant_ == ALL_CONTAINMENT_NP) {
						// Propagate positive match to containers
						vector<const Subscription*> c;
						graph_->containers((*i)->sub_, c);
						while (!c.empty()) {
							const Subscription *s = c.back();
							c.pop_back();
							BloomSubscription *bs = (BloomSubscription *) s->attributes(PREFILTER_ATTR);
							if (!matched[bs->id_]) {
								int count = graph_->duplicates(bs->sub_);
								if (matches != NULL) {
									for (int j = 0; j < count; j++)
										matches->push_back(s);
								}
								match += count;
								bs->matches_++;
								if (bs->duplicates_ != NULL) {
									for (vector<BloomSubscription*>::const_iterator j = bs->duplicates_->begin(); j != bs->duplicates_->end(); ++j) {
										(*j)->matches_++;
										matched[(*j)->id_] = true;
									}
								}
								matched[bs->id_] = true;
								graph_->containers(*s, c);
							}
						}
					}
					if (!m && (variant_ == ALL_CONTAINMENT_N || variant_ == ALL_CONTAINMENT_NP)) {
						// Propagate negative match to containees
						vector<const Subscription*> c;
						graph_->containees((*i)->sub_, c);
						while (!c.empty()) {
							const Subscription *s = c.back();
							c.pop_back();
							BloomSubscription *bs = (BloomSubscription *) s->attributes(PREFILTER_ATTR);
							if (!matched[bs->id_]) {
								matched[bs->id_] = true;
								graph_->containees(*s, c);
							}
						}
					}
				}
			}
		}
	}
	// Traverse default list
	for (vector<BloomSubscription*>::const_iterator i = subs_bf_[nb_bits_].begin(); i != subs_bf_[nb_bits_].end(); ++i) {
		if (!matched[(*i)->id_]) {
			matched[(*i)->id_] = true;
			test++;
			(*i)->tests_++;
			if ((*i)->sub_.matches(e)) {
				if (matches != NULL)
					matches->push_back(&(*i)->sub_);
				match++;
				(*i)->matches_++;
				if (variant_ == ALL_CONTAINMENT_NP) {
					// Propagate positive match to containers
					vector<const Subscription*> c;
					graph_->containers((*i)->sub_, c);
					while (!c.empty()) {
						const Subscription *s = c.back();
						c.pop_back();
						BloomSubscription *bs = (BloomSubscription *) s->attributes(PREFILTER_ATTR);
						if (!matched[bs->id_]) {
							int count = graph_->duplicates(bs->sub_);
							if (matches != NULL) {
								for (int j = 0; j < count; j++)
									matches->push_back(s);
							}
							match += count;
							bs->matches_++;
							if (bs->duplicates_ != NULL) {
								for (vector<BloomSubscription*>::const_iterator j = bs->duplicates_->begin(); j != bs->duplicates_->end(); ++j) {
									(*j)->matches_++;
									matched[(*j)->id_] = true;
								}
							}
							matched[bs->id_] = true;
							graph_->containers(*s, c);
						}
					}
				}
			} else if (variant_ == ALL_CONTAINMENT_N || variant_ == ALL_CONTAINMENT_NP) {
				// Propagate negative match to containees
				vector<const Subscription*> c;
				graph_->containees((*i)->sub_, c);
				while (!c.empty()) {
					const Subscription *s = c.back();
					c.pop_back();
					BloomSubscription *bs = (BloomSubscription *) s->attributes(PREFILTER_ATTR);
					if (!matched[bs->id_]) {
						matched[bs->id_] = true;
						graph_->containees(*s, c);
					}
				}
			}
		}
	}
	delete[] matched;
	test_stats_.add((double) test / (double) subs_.size());
	match_stats_.add((double) match / (double) subs_.size());
}

void PreFilter::stats(PreFilter::Stats& stats) {
	stats.empty_lists = 0;
	for (int i = 0; i < nb_bits_; i++) {
		if (subs_bf_[i].empty())
			stats.empty_lists++;
		stats.list_size.add(subs_bf_[i].size());
	}
	stats.nobf_size = subs_bf_[nb_bits_].size();
	for (vector<BloomSubscription*>::const_iterator i = subs_.begin(); i != subs_.end(); ++i) {
		stats.matches.add(pubs_ == 0 ? 0.0 : (double) (*i)->matches_ / (double) pubs_);
		stats.tests.add(pubs_ == 0 ? 0.0 : (double) (*i)->tests_ / (double) pubs_);
	}
}

void PreFilter::dump_stats(ostream& stream) {
	int n = 0;
	for (vector<BloomSubscription*>::const_iterator i = subs_.begin(); i != subs_.end(); ++i)
		stream << "  [" << (n++) << "] matches=" << (*i)->matches_ << " tests=" << (*i)->tests_ << endl;
}

ostream& operator<<(ostream& stream, const PreFilter& p) {
	stream << "Pre-filter: #" << p.subs_.size() << " (" << p.nb_bits_ << "," << p.nb_hashes_ << ")" << endl;
	stream << "[";
	for (int i = 0; i < p.nb_bits_; i++)
		stream << p.subs_bf_[i].size() << (i == p.nb_bits_ - 1 ? "" : ",");
	stream << "]," << p.subs_bf_[p.nb_bits_].size() << endl;
	for (vector<BloomSubscription*>::const_iterator i = p.subs_.begin(); i != p.subs_.end(); ++i)
		stream << "  " << (*i)->sub_.name() << " (" << (*i)->sub_ << ")" << endl << "  " << (*i)->bf_ << endl;

	return stream;
}

}


#include "image_segmentation.h"

#include "frames.h"

#include <string>
#include <stdint.h>
#include <tuple>
#include <iostream>
#include <cmath>
#include <vector>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui.hpp>
#include "opencv2/ximgproc/segmentation.hpp"
#include <opencv2/video/background_segm.hpp>
#include <armadillo>

using namespace cv::ximgproc::segmentation;
using namespace std;

ImagSegm::ImagSegm() {
	// empty
}

void ImagSegm::SetBodySegmentationParam(int BodyThresh, int BodyBlurSigma, int BodyBlurWindow, double BodySigma, int BodyK, int MinBodySize, double BodyLength, vector<tuple<double,double>> Origin, bool Tethered) {

	body_thresh = BodyThresh;
	body_blur_sigma = BodyBlurSigma;
	body_blur_window = BodyBlurWindow;
	body_sigma = BodySigma;
	body_K = BodyK;
	min_body_size = MinBodySize;
	body_length = BodyLength;
	origin = Origin;
	tethered = Tethered;

}

void ImagSegm::SetWingSegmentationParam(int WingThresh, int BodyDilation, double WingSigma, int WingK, int MinWingSize, double WingLength) {

	wing_thresh = WingThresh;
	body_dilation = BodyDilation;
	wing_sigma = WingSigma;
	wing_K = WingK;
	min_wing_size = MinWingSize;
	wing_length = WingLength;

}

void ImagSegm::SegmentSingleFrame(frames &frame_in) {

	int N_cam = frame_in.bckg_images.size();

	frame_in.single_seg_frame.clear();

	for (int n=0; n<N_cam; n++) {

		arma::Col<int> img_vec = frame_in.single_raw_frame[n];
		int N_row = get<0>(frame_in.image_size[n]);
		int N_col = get<1>(frame_in.image_size[n]);

		// Get body segment:
		arma::Col<int> body_thresh_img = ImagSegm::BodyThresh(img_vec, N_row, N_col, body_thresh, body_blur_sigma, body_blur_window);
		arma::Col<int> body_seg_img = ImagSegm::GraphSeg(body_thresh_img, N_row, N_col, body_sigma, body_K, min_body_size);
		arma::Mat<double> body_seg_prop = ImagSegm::GetSegmentProperties(body_seg_img, N_row, N_col);
		tuple<arma::Col<double>,arma::Col<int>> body_seg = ImagSegm::SelectBody(body_seg_img, N_row, N_col, body_seg_prop, origin[n]);

		// Get wing segments:
		arma::Col<int> wing_thresh_img = ImagSegm::WingThresh(img_vec, get<1>(body_seg), N_row, N_col, wing_thresh, body_dilation, get<0>(body_seg));
		arma::Col<int> wing_seg_img = ImagSegm::GraphSeg(wing_thresh_img, N_row, N_col, wing_sigma, wing_K, min_wing_size);
		arma::Mat<double> wing_seg_prop = ImagSegm::GetSegmentProperties(wing_seg_img, N_row, N_col);
		tuple<arma::Mat<double>,arma::Mat<int>> wing_seg = ImagSegm::SelectWing(wing_seg_img, N_row, N_col, wing_seg_prop, get<0>(body_seg));

		// Construct segmented frame:
		arma::Col<int> body_frame = get<1>(body_seg);
		arma::Col<double> body_frame_d = arma::conv_to<arma::Mat<double>>::from(body_frame);
		arma::Mat<int> wing_frame = get<1>(wing_seg);
		arma::Mat<double> wing_frame_d = arma::conv_to<arma::Mat<double>>::from(wing_frame);

		arma::Col<double> temp_frame(N_row*N_col);
		temp_frame.zeros();

		for (int j=0; j<(wing_frame.n_cols+1); j++) {
			if (j==0) {
				temp_frame += body_frame_d*(1.0/255.0);
			}
			else {
				temp_frame += wing_frame_d.col(j-1)*((j+1)/255.0);
			}
		}

		arma::Col<int> seg_frame = arma::conv_to<arma::Mat<int>>::from(temp_frame);

		// Load segmented frame into the frames struct:
		frame_in.single_seg_frame.push_back(seg_frame);

	}

}

void ImagSegm::SegmentFrameBatch(frames &frame_in) {

	int N_cam = frame_in.bckg_images.size();

	int N_batch = frame_in.raw_frames[0].n_cols;

	frame_in.seg_frames.clear();

	for (int n=0; n<N_cam; n++) {

		int N_row = get<0>(frame_in.image_size[n]);
		int N_col = get<1>(frame_in.image_size[n]);

		arma::Mat<int> seg_frame_mat(N_row*N_col,N_batch);

		for (int i=0; i<N_batch; i++) {

			arma::Col<int> img_vec = frame_in.raw_frames[n].col(i);

			// Get body segment:
			arma::Col<int> body_thresh_img = ImagSegm::BodyThresh(img_vec, N_row, N_col, body_thresh, body_blur_sigma, body_blur_window);
			arma::Col<int> body_seg_img = ImagSegm::GraphSeg(body_thresh_img, N_row, N_col, body_sigma, body_K, min_body_size);
			arma::Mat<double> body_seg_prop = ImagSegm::GetSegmentProperties(body_seg_img, N_row, N_col);
			tuple<arma::Col<double>,arma::Col<int>> body_seg = ImagSegm::SelectBody(body_seg_img, N_row, N_col, body_seg_prop, origin[n]);

			// Get wing segments:
			arma::Col<int> wing_thresh_img = ImagSegm::WingThresh(img_vec, get<1>(body_seg), N_row, N_col, wing_thresh, body_dilation, get<0>(body_seg));
			arma::Col<int> wing_seg_img = ImagSegm::GraphSeg(wing_thresh_img, N_row, N_col, wing_sigma, wing_K, min_wing_size);
			arma::Mat<double> wing_seg_prop = ImagSegm::GetSegmentProperties(wing_seg_img, N_row, N_col);
			tuple<arma::Mat<double>,arma::Mat<int>> wing_seg = ImagSegm::SelectWing(wing_seg_img, N_row, N_col, wing_seg_prop, get<0>(body_seg));

			// Construct segmented frame:
			arma::Col<int> body_frame = get<1>(body_seg);
			arma::Col<double> body_frame_d = arma::conv_to<arma::Mat<double>>::from(body_frame);
			arma::Mat<int> wing_frame = get<1>(wing_seg);
			arma::Mat<double> wing_frame_d = arma::conv_to<arma::Mat<double>>::from(wing_frame);

			arma::Col<double> temp_frame(N_row*N_col);
			temp_frame.zeros();

			for (int j=0; j<(wing_frame.n_cols+1); j++) {
				if (j==0) {
					temp_frame += body_frame_d*(1.0/255.0);
				}
				else {
					temp_frame += wing_frame_d.col(j-1)*((j+1)/255.0);
				}
			}

			seg_frame_mat.col(i) = arma::conv_to<arma::Mat<int>>::from(temp_frame);

		}

		// Load segmented frame into the frames struct:
		frame_in.raw_frames.push_back(seg_frame_mat);

	}

}

tuple<arma::Col<double>,arma::Col<int>> ImagSegm::SelectBody(arma::Col<int> &frame_in, int N_row, int N_col, arma::Mat<double> seg_prop, tuple<double,double> origin_loc) {

	int N_seg = frame_in.max()+1;
	bool is_body = true;
	tuple<arma::Col<double>, arma::Col<int>> body_img;

	vector<tuple<int,double>> pos_body_segs;

	if (N_seg == 1) {
		cout << "no body segment found" << endl;
		arma::Col<int> body_img_vec(N_row*N_col);
		body_img_vec.zeros();
		arma::Col<double> body_prop_vec(4);
		body_prop_vec.ones();
		body_img = make_tuple((body_prop_vec*-1.0),body_img_vec);
	}
	else {
		for (int i=0; i<N_seg; i++) {
			is_body = true;

			if (tethered==false) {
				// Body origin location is unknown (free flight):
				if (seg_prop(2,i)<(0.1*pow(body_length,2))) {
					is_body = false;
				}
				if (seg_prop(2,i)>(0.75*pow(body_length,2))) {
					is_body = false;
				}
				if (seg_prop(3,i)>0) {
					is_body = false;
				}
			}
			else {
				// Body origin location is known (tethered flight):
				double dist_origin = sqrt(pow(seg_prop(0,i)-get<0>(origin_loc),2)+pow(seg_prop(1,i)-get<1>(origin_loc),2));
				if (dist_origin>(0.75*body_length)) {
					is_body = false;
				}
				if (seg_prop(2,i)<(0.1*pow(body_length,2))) {
					is_body = false;
				}
				if (seg_prop(2,i)>(0.75*pow(body_length,2))) {
					is_body = false;
				}
				if (seg_prop(3,i)>0) {
					is_body = false;
				}
			}

			if (is_body == true) {
				pos_body_segs.push_back(make_tuple(i,seg_prop(2,i)));
			}
		}

		if (pos_body_segs.size()==1) {
			// Only one candidate for the body segment.
			cv::Mat seg_img, bin_img;
			seg_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);
			cv::inRange(seg_img,get<0>(pos_body_segs[0]),get<0>(pos_body_segs[0]),bin_img);
			arma::Col<int> body_img_vec = ImagSegm::CVMat2Vector(bin_img);
			body_img = make_tuple(seg_prop.col(get<0>(pos_body_segs[0])),body_img_vec);
		}
		else if (pos_body_segs.size()>1) {
			// More than one candidate for the body segment, select the segment that is closest to 0.25*pow(body_length,2)
			arma::Col<double> area_diff(pos_body_segs.size());
			for (int j=0; j<pos_body_segs.size(); j++) {
				area_diff(j) = abs(get<1>(pos_body_segs[j])-0.25*pow(body_length,2));
			}
			int min_ind = area_diff.index_min();
			cv::Mat seg_img, bin_img;
			seg_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);
			cv::inRange(seg_img,get<0>(pos_body_segs[min_ind]),get<0>(pos_body_segs[min_ind]),bin_img);
			arma::Col<int> body_img_vec = ImagSegm::CVMat2Vector(bin_img);
			body_img = make_tuple(seg_prop.col(min_ind),body_img_vec);
		}
		else {
			// No candidate for the body segment
			arma::Col<int> body_img_vec(N_row*N_col);
			body_img_vec.zeros();
			arma::Col<double> body_prop_vec(4);
			body_prop_vec.ones();
			body_img = make_tuple((body_prop_vec*-1.0),body_img_vec);
		}
	}

	return body_img;
}

tuple<arma::Mat<double>,arma::Mat<int>> ImagSegm::SelectWing(arma::Col<int> &frame_in, int N_row, int N_col, arma::Mat<double> seg_prop, arma::Col<double> body_prop) {

	int N_seg = frame_in.max()+1;
	bool is_wing = true;
	tuple<arma::Mat<double>,arma::Mat<int>> wing_img;

	vector<int> wing_segs;

	if (N_seg == 1) {
		cout << "no wing segment found" << endl;
		arma::Mat<int> wing_img_vec(N_row*N_col,1);
		wing_img_vec.zeros();
		arma::Mat<double> wing_prop_vec(4,1);
		wing_prop_vec.ones();
		wing_img = make_tuple((wing_prop_vec*-1.0),wing_img_vec);
	}
	else {
		for (int i=0; i<N_seg; i++) {
			is_wing = true;

			double dist_cg = sqrt(pow(seg_prop(0,i)-body_prop(0),2)+pow(seg_prop(1,i)-body_prop(1),2));

			if (dist_cg>(1.5*wing_length)) {
				is_wing = false;
			}
			if (seg_prop(2,i)>(2.0*pow(wing_length,2))) {
				is_wing = false;
			}
			if (seg_prop(3,i)>0) {
				is_wing = false;
			}

			if (is_wing == true) {
				wing_segs.push_back(i);
			}
		}

		if (wing_segs.size()>0) {
			arma::Mat<int> wing_img_vec(N_row*N_col,wing_segs.size());
			arma::Mat<double> wing_prop_vec(4,wing_segs.size());
			for (int j=0; j<wing_segs.size(); j++) {
				cv::Mat seg_img, bin_img;
				seg_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);
				cv::inRange(seg_img,wing_segs[j],wing_segs[j],bin_img);
				wing_img_vec.col(j) = ImagSegm::CVMat2Vector(bin_img);
				wing_prop_vec.col(j) = seg_prop.col(wing_segs[j]);
			}
			wing_img = make_tuple(wing_prop_vec,wing_img_vec);
		}
		else {
			cout << "no wing segment found" << endl;
			arma::Mat<int> wing_img_vec(N_row*N_col,1);
			wing_img_vec.zeros();
			arma::Mat<double> wing_prop_vec(4,1);
			wing_prop_vec.ones();
			wing_img = make_tuple((wing_prop_vec*-1.0),wing_img_vec);
		}
	}

	return wing_img;
}

arma::Col<int> ImagSegm::BodyThresh(arma::Col<int> &frame_in, int N_row, int N_col, int body_thresh, int sigma_blur, int blur_window) {

	cv::Mat img, ThreshImg;

	img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);

	cv::GaussianBlur(img, img, cv::Size(blur_window,blur_window),sigma_blur,sigma_blur);

	cv::threshold(img, ThreshImg, body_thresh, 0, cv::THRESH_TOZERO_INV);

	arma::Col<int> thresh_vec;

	thresh_vec = ImagSegm::CVMat2Vector(ThreshImg);

	return thresh_vec;
}

arma::Col<int> ImagSegm::WingThresh(arma::Col<int> &frame_in, arma::Col<int> &body_frame_in, int N_row, int N_col, int wing_thresh, int body_dilation, arma::Col<double> body_prop) {

	cv::Mat img, img_coords_body, body_img, circle_mask, ThreshImg;

	img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);

	body_img = cv::Mat::zeros(N_row, N_col, CV_8UC1 );

	body_img = body_img+ImagSegm::Vector2CVMat(body_frame_in,N_row,N_col);

	// Dilate the image a bit:
	if (body_dilation > 0) {
		cv::dilate(body_img, body_img, cv::Mat(), cv::Point(-1, -1), body_dilation, 1, 1);
	}

	// Create a circular mask of 1.6 * wing length around the body center
	//double radius = 1.6*wing_length;
	//circle_mask = cv::Mat::zeros(N_row, N_col, CV_8UC1 );
	//cv::circle(circle_mask,cv::Point(body_prop(0),body_prop(1)),radius,cv::Scalar::all(255),-1);

	// Perform thresholding:
	cv::threshold(255-(img+body_img), ThreshImg, wing_thresh, 255, cv::THRESH_TOZERO);
	//cv::threshold(255-(img+body_img)-(255-circle_mask), ThreshImg, wing_thresh, 255, cv::THRESH_TOZERO);

	arma::Col<int> thresh_vec;

	thresh_vec = ImagSegm::CVMat2Vector(ThreshImg);

	return thresh_vec;
}

arma::Mat<double> ImagSegm::GetSegmentProperties(arma::Col<int> &frame_in, int N_row, int N_col) {

	int N_seg = frame_in.max()+1;

	arma::Mat<double> seg_prop(4,N_seg);

	cv::Mat seg_img = ImagSegm::Vector2CVMat(frame_in, N_row, N_col);

	for (int i=0; i<N_seg; i++) {

		cv::Mat bin_img;

		cv::inRange(seg_img,i,i,bin_img);

		double minVal;
		double maxVal;
		cv::Point minLoc;
		cv::Point maxLoc;

		cv::minMaxLoc(bin_img, &minVal, &maxVal, &minLoc, &maxLoc);

		double cg_x, cg_y, Area, Mx, My, border_sum;

		// Calculate area

		Area = cv::sum(bin_img)[0]/maxVal;

		Mx = 0.0;
		for (int j=0; j<N_row; j++) {
			Mx+=(cv::sum(bin_img.row(j))[0]*j)/maxVal;
		}

		My = 0.0;
		for (int k=0; k<N_col; k++) {
			My+=(cv::sum(bin_img.col(k))[0]*k)/maxVal;
		}

		// Calculate cg

		cg_x = My/Area;
		cg_y = Mx/Area;

		// Check if the segment hits one of the borders
		border_sum = cv::sum(bin_img.row(0))[0]+cv::sum(bin_img.row(N_row-1))[0]+cv::sum(bin_img.col(0))[0]+cv::sum(bin_img.col(N_col-1))[0];

		seg_prop(0,i) = cg_x;
		seg_prop(1,i) = cg_y;
		seg_prop(2,i) = Area;
		seg_prop(3,i) = border_sum;

	}
	return seg_prop;
}

arma::Col<int> ImagSegm::GraphSeg(arma::Col<int> &frame_in, int N_row, int N_col, double Sigma, int K, int minsize) {

	gs->setSigma(Sigma);
	gs->setK(K);
	gs->setMinSize(minsize);

	cv::Mat input_img, output_img;

	input_img = ImagSegm::Vector2CVMat(frame_in,N_row,N_col);

	gs->processImage(input_img,output_img);

	arma::Col<int> segmented_frame;
	segmented_frame = ImagSegm::CVMat2Vector(output_img);

	return segmented_frame;
}

arma::Col<int> ImagSegm::CVMat2Vector(cv::Mat &img) {

	arma::Col<int> img_vec;

	img.convertTo(img, CV_8UC1);
	arma::Mat<uint8_t> img_mat(reinterpret_cast<uint8_t*>(img.data), img.rows, img.cols);
	img_vec = arma::vectorise(arma::conv_to<arma::Mat<int>>::from(img_mat));

	return img_vec;
}

cv::Mat ImagSegm::Vector2CVMat(arma::Col<int> &img_vec, int N_row, int N_col) {

	arma::Mat<int> img_mat;

	img_mat = img_vec;

	img_mat.reshape(N_row,N_col);

	cv::Mat img(img_mat.n_rows, img_mat.n_cols, CV_32SC1, img_mat.memptr());

	img.convertTo(img, CV_8UC1);

	return img;
}

np::ndarray ImagSegm::ReturnSegFrame(session_param &ses_par, frames &frame_now, int cam_nr) {

	int N_row = get<0>(frame_now.image_size[cam_nr]);
	int N_col = get<1>(frame_now.image_size[cam_nr]);

	p::tuple shape = p::make_tuple(N_row,N_col);
	np::dtype dtype = np::dtype::get_builtin<int>();
	np::ndarray array_out = np::zeros(shape,dtype);

	for (int i=0; i<N_row; i++) {
		for (int j=0; j<N_col; j++) {
			array_out[i][j] = frame_now.single_seg_frame[cam_nr](i*N_row+j);
		}
	}

	return array_out;
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2014 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2020 Baidyanath Kundu, Haldia, India.

// 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)

//[from_wkt
//` Shows the usage of from_wkt

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/linestring.hpp>
#include <boost/geometry/geometries/polygon.hpp>

int main()
{
    using point_type = boost::geometry::model::d2::point_xy<double>;
    using box_type = boost::geometry::model::box<point_type>;
    using segment_type = boost::geometry::model::segment<point_type>;
    using polygon_type = boost::geometry::model::polygon<point_type>;
    using linestring_type = boost::geometry::model::linestring<point_type>;

    auto const a = boost::geometry::from_wkt<point_type>("POINT(1 2)");
    auto const d = boost::geometry::from_wkt<box_type>("BOX(0 0,3 3)");
    auto const e = boost::geometry::from_wkt<segment_type>("SEGMENT(1 0,3 4)");
    auto const c = boost::geometry::from_wkt<polygon_type>("POLYGON((0 0,0 7,4 2,2 0,0 0))");
    auto const b = boost::geometry::from_wkt<linestring_type>("LINESTRING(0 0,2 2,3 1)");

    return 0;
}

//]


// Copyright Nick Thompson, 2019
// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)
#define BOOST_TEST_MODULE test_ooura_fourier_transform

#include <cmath>
#include <iostream>
#include <boost/type_index.hpp>
#include <boost/test/included/unit_test.hpp>
#include <boost/test/tools/floating_point_comparison.hpp>
#include <boost/math/quadrature/ooura_fourier_integrals.hpp>
#include <boost/multiprecision/cpp_bin_float.hpp>

using boost::math::quadrature::ooura_fourier_sin;
using boost::math::quadrature::ooura_fourier_cos;
using boost::math::constants::pi;

float float_tol = 10*std::numeric_limits<float>::epsilon();
ooura_fourier_sin<float> float_sin_integrator(float_tol);

double double_tol = 10*std::numeric_limits<double>::epsilon();
ooura_fourier_sin<double> double_sin_integrator(double_tol);

long double long_double_tol = 10*std::numeric_limits<long double>::epsilon();
ooura_fourier_sin<long double> long_double_sin_integrator(long_double_tol);

template<class Real>
auto get_sin_integrator() {
    if constexpr (std::is_same_v<Real, float>) {
        return float_sin_integrator;
    }
    if constexpr (std::is_same_v<Real, double>) {
        return double_sin_integrator;
    }
    if constexpr (std::is_same_v<Real, long double>) {
        return long_double_sin_integrator;
    }
}

ooura_fourier_cos<float> float_cos_integrator(float_tol);
ooura_fourier_cos<double> double_cos_integrator(double_tol);
ooura_fourier_cos<long double> long_double_cos_integrator(long_double_tol);

template<class Real>
auto get_cos_integrator() {
    if constexpr (std::is_same_v<Real, float>) {
        return float_cos_integrator;
    }
    if constexpr (std::is_same_v<Real, double>) {
        return double_cos_integrator;
    }
    if constexpr (std::is_same_v<Real, long double>) {
        return long_double_cos_integrator;
    }
}

template<class Real>
void test_ooura_eta()
{
    using boost::math::quadrature::detail::ooura_eta;
    std::cout << "Testing eta function on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    {
        Real x = 0;
        Real alpha = 7;
        auto [eta, eta_prime] = ooura_eta(x, alpha);
        BOOST_CHECK_SMALL(eta, (std::numeric_limits<Real>::min)());
        BOOST_CHECK_CLOSE_FRACTION(eta_prime, 2 + alpha + Real(1)/Real(4), 10*std::numeric_limits<Real>::epsilon());
    }

    {
        Real alpha = 4;
        for (Real z = 0.125; z < 500; z += 0.125) {
            Real x = std::log(z);
            auto [eta, eta_prime] = ooura_eta(x, alpha);
            BOOST_CHECK_CLOSE_FRACTION(eta, 2*x + alpha*(1-1/z) + (z-1)/4, 10*std::numeric_limits<Real>::epsilon());
            BOOST_CHECK_CLOSE_FRACTION(eta_prime, 2 + alpha/z + z/4, 10*std::numeric_limits<Real>::epsilon());
        }
    }
}

template<class Real>
void test_ooura_sin_nodes_and_weights()
{
    using boost::math::quadrature::detail::ooura_sin_node_and_weight;
    using boost::math::quadrature::detail::ooura_eta;
    std::cout << "Testing nodes and weights on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    {
        long n = 1;
        Real alpha = 1;
        Real h = 1;
        auto [node, weight] = ooura_sin_node_and_weight(n, h, alpha);
        Real expected_node = pi<Real>()/(1-exp(-ooura_eta(n*h, alpha).first));
        BOOST_CHECK_CLOSE_FRACTION(node,  expected_node,10*std::numeric_limits<Real>::epsilon());
    }
}

template<class Real>
void test_ooura_alpha() {
    std::cout << "Testing Ooura alpha on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::sqrt;
    using std::log1p;
    using boost::math::quadrature::detail::calculate_ooura_alpha;
    Real alpha = calculate_ooura_alpha(Real(1));
    Real expected = 1/sqrt(16 + 4*log1p(pi<Real>()));
    BOOST_CHECK_CLOSE_FRACTION(alpha, expected, 10*std::numeric_limits<Real>::epsilon());
}

void test_node_weight_precision_agreement()
{
    using std::abs;
    using boost::math::quadrature::detail::ooura_sin_node_and_weight;
    using boost::math::quadrature::detail::ooura_eta;
    using boost::multiprecision::cpp_bin_float_quad;
    std::cout << "Testing agreement in two different precisions of nodes and weights\n";
    cpp_bin_float_quad alpha_quad = 1;
    long int_max = 128;
    cpp_bin_float_quad h_quad = 1/cpp_bin_float_quad(int_max);
    double alpha_dbl = 1;
    double h_dbl = static_cast<double>(h_quad);
    std::cout << std::fixed;
    for (long n = -1; n > -6*int_max; --n) {
        auto [node_dbl, weight_dbl] = ooura_sin_node_and_weight(n, h_dbl, alpha_dbl);
        auto p = ooura_sin_node_and_weight(n, h_quad, alpha_quad);
        double node_quad = static_cast<double>(p.first);
        double weight_quad = static_cast<double>(p.second);
        auto node_dist = abs(boost::math::float_distance(node_quad, node_dbl));
        if ( (weight_quad < 0 && weight_dbl > 0) || (weight_dbl < 0 && weight_quad > 0) ){
            std::cout << "Weights at different precisions have different signs!\n";
        } else {
            auto weight_dist = abs(boost::math::float_distance(weight_quad, weight_dbl));
            if (weight_dist > 100) {
                std::cout << std::fixed;
                std::cout <<"n =" << n << ", x = " << n*h_dbl << ", node distance = " << node_dist << ", weight distance = " << weight_dist << "\n";
                std::cout << std::scientific;
                std::cout << "computed weight = " << weight_dbl << ", actual weight = " << weight_quad << "\n";
            }
        }
    }

}

template<class Real>
void test_sinc()
{
    std::cout << "Testing sinc integral on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::numeric_limits;
    Real tol = 50*numeric_limits<Real>::epsilon();
    auto integrator = get_sin_integrator<Real>();
    auto f = [](Real x)->Real { return 1/x; };
    Real omega = 1;
    while (omega < 10)
    {
        auto [Is, err] = integrator.integrate(f, omega);
        BOOST_CHECK_CLOSE_FRACTION(Is, pi<Real>()/2, tol);

        auto [Isn, errn] = integrator.integrate(f, -omega);
        BOOST_CHECK_CLOSE_FRACTION(Isn, -pi<Real>()/2, tol);
        omega += 1;
    }
}

template<class Real>
void test_exp()
{
    std::cout << "Testing exponential integral on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::exp;
    using std::numeric_limits;
    Real tol = 50*numeric_limits<Real>::epsilon();
    auto integrator = get_sin_integrator<Real>();
    auto f = [](Real x)->Real {return exp(-x);};
    Real omega = 1;
    while (omega < 5)
    {
        auto [Is, err] = integrator.integrate(f, omega);
        Real exact = omega/(1+omega*omega);
        BOOST_CHECK_CLOSE_FRACTION(Is, exact, tol);
        omega += 1;
    }
}

template<class Real>
void test_root()
{
    std::cout << "Testing integral of sin(kx)/sqrt(x) on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::sqrt;
    using std::numeric_limits;
    Real tol = 10*numeric_limits<Real>::epsilon();
    auto integrator = get_sin_integrator<Real>();
    auto f = [](Real x)->Real { return 1/sqrt(x);};
    Real omega = 1;
    while (omega < 5) {
        auto [Is, err] = integrator.integrate(f, omega);
        Real exact = sqrt(pi<Real>()/(2*omega));
        BOOST_CHECK_CLOSE_FRACTION(Is, exact, 10*tol);
        omega += 1;
    }
}

// See: https://scicomp.stackexchange.com/questions/32790/numerical-evaluation-of-highly-oscillatory-integral/32799#32799
template<class Real>
Real asymptotic(Real lambda) {
    using std::sin;
    using std::cos;
    using boost::math::constants::pi;
    Real I1 = cos(lambda - pi<Real>()/4)*sqrt(2*pi<Real>()/lambda);
    Real I2 = sin(lambda - pi<Real>()/4)*sqrt(2*pi<Real>()/(lambda*lambda*lambda))/8;
    return I1 + I2;
}

template<class Real>
void test_double_osc()
{
    std::cout << "Testing double oscillation on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::sqrt;
    using std::numeric_limits;
    auto integrator = get_sin_integrator<Real>();
    Real lambda = 7;
    auto f = [&lambda](Real x)->Real { return cos(lambda*cos(x))/x; };
    Real omega = 1;
    auto [Is, err] = integrator.integrate(f, omega);
    Real exact = asymptotic(lambda);
    BOOST_CHECK_CLOSE_FRACTION(2*Is, exact, 0.05);
}

template<class Real>
void test_zero_integrand()
{
    // Make sure relative error tolerance doesn't break on zero integrand:
    std::cout << "Testing zero integrand on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::sqrt;
    using std::numeric_limits;
    auto integrator = get_sin_integrator<Real>();
    auto f = [](Real /* x */)->Real { return Real(0); };
    Real omega = 1;
    auto [Is, err] = integrator.integrate(f, omega);
    Real exact = 0;
    BOOST_CHECK_EQUAL(Is, exact);
}

// This works, but doesn't recover the precision you want in a unit test:
// template<class Real>
// void test_log()
// {
//     std::cout << "Testing integral of log(x)sin(x) on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
//     using std::log;
//     using std::exp;
//     using std::numeric_limits;
//     using boost::math::constants::euler;
//     Real tol = 1000*numeric_limits<Real>::epsilon();
//     auto f = [](Real x)->Real { return exp(-100*numeric_limits<Real>::epsilon()*x)*log(x);};
//     Real omega = 1;
//     Real Is = ooura_fourier_sin<decltype(f), Real>(f, omega, sqrt(numeric_limits<Real>::epsilon())/100);
//     BOOST_CHECK_CLOSE_FRACTION(Is, -euler<Real>(), tol);
// }

template<class Real>
void test_cos_integral1()
{
    std::cout << "Testing integral of cos(x)/(x*x+1) on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::exp;
    using boost::math::constants::half_pi;
    using boost::math::constants::e;
    using std::numeric_limits;
    Real tol = 10*numeric_limits<Real>::epsilon();

    auto integrator = get_cos_integrator<Real>();
    auto f = [](Real x)->Real { return 1/(x*x+1);};
    Real omega = 1;
    auto [Is, err] = integrator.integrate(f, omega);
    Real exact = half_pi<Real>()/e<Real>();
    BOOST_CHECK_CLOSE_FRACTION(Is, exact, tol);
}

template<class Real>
void test_cos_integral2()
{
    std::cout << "Testing integral of cos(x)/(x*x+1) on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    using std::exp;
    using boost::math::constants::half_pi;
    using boost::math::constants::e;
    using std::numeric_limits;
    Real tol = 10*numeric_limits<Real>::epsilon();

    auto integrator = get_cos_integrator<Real>();
    for (Real a = 1; a < 5; ++a) {
        auto f = [&a](Real x)->Real { return exp(-a*x);};
        for(Real omega = 1; omega < 5; ++omega) {
            auto [Is, err] = integrator.integrate(f, omega);
            Real exact = a/(a*a+omega*omega);
            BOOST_CHECK_CLOSE_FRACTION(Is, exact, 10*tol);
        }
    }
}

template<class Real>
void test_nodes()
{
    std::cout << "Testing nodes and weights on type " << boost::typeindex::type_id<Real>().pretty_name()  << "\n";
    auto sin_integrator = get_sin_integrator<Real>();

    auto const & big_nodes = sin_integrator.big_nodes();
    for (auto & node_row : big_nodes) {
        Real t0 = node_row[0];
        for (size_t i = 1; i < node_row.size(); ++i) {
            Real t1 = node_row[i];
            BOOST_CHECK(t1 > t0);
            t0 = t1;
        }
    }

    auto const & little_nodes = sin_integrator.little_nodes();
    for (auto & node_row : little_nodes) {
        Real t0 = node_row[0];
        for (size_t i = 1; i < node_row.size(); ++i) {
            Real t1 = node_row[i];
            BOOST_CHECK(t1 < t0);
            t0 = t1;
        }
    }
}

BOOST_AUTO_TEST_CASE(ooura_fourier_transform_test)
{
    test_cos_integral1<float>();
    test_cos_integral1<double>();
    test_cos_integral1<long double>();

    test_cos_integral2<float>();
    test_cos_integral2<double>();
    test_cos_integral2<long double>();

    //test_node_weight_precision_agreement();
    test_zero_integrand<float>();
    test_zero_integrand<double>();

    test_ooura_eta<float>();
    test_ooura_eta<double>();
    test_ooura_eta<long double>();

    test_ooura_sin_nodes_and_weights<float>();
    test_ooura_sin_nodes_and_weights<double>();
    test_ooura_sin_nodes_and_weights<long double>();

    test_ooura_alpha<float>();
    test_ooura_alpha<double>();
    test_ooura_alpha<long double>();

    test_sinc<float>();
    test_sinc<double>();
    test_sinc<long double>();

    test_exp<float>();
    test_exp<double>();
    test_exp<long double>();

    test_root<float>();
    test_root<double>();

    test_double_osc<float>();
    test_double_osc<double>();
    // Takes too long!
    //test_double_osc<long double>();

    // This test should be last:
    test_nodes<float>();
    test_nodes<double>();
    test_nodes<long double>();
}


#include <shift/math/interval.hpp>
#include <shift/core/algorithm.hpp>
#include <shift/platform/fpexceptions.hpp>
#include <shift/core/boost_disable_warnings.hpp>
#include <boost/test/unit_test.hpp>
#include <shift/core/boost_restore_warnings.hpp>

#include <thread>

using namespace shift;
using namespace shift::math;

BOOST_AUTO_TEST_CASE(interval_is_trivial)
{
  static_assert(std::is_trivial_v<interval<2, int>>);
  static_assert(std::is_trivial_v<interval<2, float>>);
  BOOST_CHECK((std::is_trivial_v<interval<2, int>>));
  BOOST_CHECK((std::is_trivial_v<interval<2, float>>));
}

BOOST_AUTO_TEST_CASE(interval_make_from)
{
  platform::floating_point_exceptions fpexceptions;

  auto v1 = make_vector_from(1.0f, 2.0f);
  auto v2 = make_vector_from(3.0f, 6.0f);
  auto v3 = make_vector_from(5.0f, 4.0f);

  auto r1 = make_interval_from(v1);
  BOOST_CHECK_EQUAL(r1.min.x, 1.0f);
  BOOST_CHECK_EQUAL(r1.min.y, 2.0f);
  BOOST_CHECK_EQUAL(r1.max.x, 1.0f);
  BOOST_CHECK_EQUAL(r1.max.y, 2.0f);

  auto r2 = make_interval_from(v1, v2);
  BOOST_CHECK_EQUAL(r2.min.x, 1.0f);
  BOOST_CHECK_EQUAL(r2.min.y, 2.0f);
  BOOST_CHECK_EQUAL(r2.max.x, 3.0f);
  BOOST_CHECK_EQUAL(r2.max.y, 6.0f);

  auto r3 = make_interval_from(v1, v2, v3);
  BOOST_CHECK_EQUAL(r3.min.x, 1.0f);
  BOOST_CHECK_EQUAL(r3.min.y, 2.0f);
  BOOST_CHECK_EQUAL(r3.max.x, 5.0f);
  BOOST_CHECK_EQUAL(r3.max.y, 6.0f);
}

BOOST_AUTO_TEST_CASE(interval_make_union)
{
  platform::floating_point_exceptions fpexceptions;

  interval<2, float> a(make_vector_from(1.0f, 2.0f),
                       make_vector_from(4.0f, 3.0f));
  interval<2, float> b(make_vector_from(2.0f, 1.0f),
                       make_vector_from(3.0f, 4.0f));
  interval<2, float> c(make_vector_from(6.0f, 7.0f),
                       make_vector_from(8.0f, 9.0f));
  auto r1 = make_interval_union(a);
  BOOST_CHECK_EQUAL(r1.min.x, 1.0f);
  BOOST_CHECK_EQUAL(r1.min.y, 2.0f);
  BOOST_CHECK_EQUAL(r1.max.x, 4.0f);
  BOOST_CHECK_EQUAL(r1.max.y, 3.0f);

  auto r2 = make_interval_union(a, b);
  BOOST_CHECK_EQUAL(r2.min.x, 1.0f);
  BOOST_CHECK_EQUAL(r2.min.y, 1.0f);
  BOOST_CHECK_EQUAL(r2.max.x, 4.0f);
  BOOST_CHECK_EQUAL(r2.max.y, 4.0f);

  auto r3 = make_interval_union(a, b, c);
  BOOST_CHECK_EQUAL(r3.min.x, 1.0f);
  BOOST_CHECK_EQUAL(r3.min.y, 1.0f);
  BOOST_CHECK_EQUAL(r3.max.x, 8.0f);
  BOOST_CHECK_EQUAL(r3.max.y, 9.0f);
}

BOOST_AUTO_TEST_CASE(interval_make_intersection)
{
  using std::isnan;
  platform::floating_point_exceptions fpexceptions;

  interval<2, float> a(make_vector_from(1.0f, 2.0f),
                       make_vector_from(4.0f, 3.0f));
  interval<2, float> b(make_vector_from(2.0f, 1.0f),
                       make_vector_from(3.0f, 4.0f));
  interval<2, float> c(make_vector_from(6.0f, 7.0f),
                       make_vector_from(8.0f, 9.0f));
  auto r1 = make_interval_intersection(a);
  BOOST_CHECK_EQUAL(r1.min.x, 1.0f);
  BOOST_CHECK_EQUAL(r1.min.y, 2.0f);
  BOOST_CHECK_EQUAL(r1.max.x, 4.0f);
  BOOST_CHECK_EQUAL(r1.max.y, 3.0f);

  auto r2 = make_interval_intersection(a, b);
  BOOST_CHECK_EQUAL(r2.min.x, 2.0f);
  BOOST_CHECK_EQUAL(r2.min.y, 2.0f);
  BOOST_CHECK_EQUAL(r2.max.x, 3.0f);
  BOOST_CHECK_EQUAL(r2.max.y, 3.0f);

  auto r3 = make_interval_intersection(a, b, c);
  BOOST_CHECK(isnan(r3.min.x));
  BOOST_CHECK(isnan(r3.min.y));
  BOOST_CHECK(isnan(r3.max.x));
  BOOST_CHECK(isnan(r3.max.y));
}


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

#include <iostream>
#include <fstream>
#include <string>
#include <Eigen/Dense>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>

#include "dpOptTrans/pc_helper.h"

#include <boost/program_options.hpp>
namespace po = boost::program_options;

int main(int argc, char** argv) {
  // Declare the supported options.
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help,h", "produce help message")
    ("in_a,a", po::value<std::string>(), "path to first input file")
    ("in_b,b", po::value<std::string>(), "path to second input file")
    ("out,o", po::value<std::string>(), "path to output file")
    ("cutoff,c", po::value<float>(), "cutoff for data association in ICP")
    ("transformation,t", po::value<std::string>(), "path to a transformation file with qw qx qy qz tx ty tz in the second line.")
//    ("transformation,t", po::value<std::string>(), "transformation guess")
//    ("rotation,q", po::value<std::string>(), "rotation guess in the form of a quaternion")
    ("normals,n", "use surface normals in ICP")
    ("display,d", "display results")
    ;

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  std::string pathA = "";
  std::string pathB = "";
  std::string pathOut = "";
  std::string pathTransformation  = "";
  float cutoff = 0.3;
  //  std::string mode = "";
  //  if(vm.count("mode")) mode = vm["mode"].as<std::string>();
  if(vm.count("out")) pathOut = vm["out"].as<std::string>();
  if(vm.count("in_a")) pathA = vm["in_a"].as<std::string>();
  if(vm.count("in_b")) pathB = vm["in_b"].as<std::string>();
  if(vm.count("cutoff")) cutoff = vm["cutoff"].as<float>();
  if(vm.count("transformation")) pathTransformation = vm["transformation"].as<std::string>();

  // Load point clouds.
  pcl::PointCloud<pcl::PointXYZRGBNormal> pcA, pcB, pcB_T;
  pcl::PLYReader reader;
  if (reader.read(pathA, pcA))
    std::cout << "error reading " << pathA << std::endl;
  else
    std::cout << "loaded pc from " << pathA << ": " << pcA.width << "x"
      << pcA.height << std::endl;
  if (reader.read(pathB, pcB))
    std::cout << "error reading " << pathB << std::endl;
  else
    std::cout << "loaded pc from " << pathB << ": " << pcB.width << "x"
      << pcB.height << std::endl;

  Eigen::Matrix4f T_guess = Eigen::Matrix4f::Identity();
  bool have_guess = false;
  std::ifstream in(pathTransformation);
  if (pathTransformation.size() > 1 && in.is_open()) {
    std::string dummy;
    std::getline(in, dummy);
    Eigen::Vector4f q;
    Eigen::Vector3f t;
    in >> q.w() >> q.x() >> q.y() >> q.z() >> t(0) >> t(1) >> t(2);
    std::cout << "loaded intial transformation guess from file : "
      << pathTransformation << std::endl;
    std::cout << "formatting: " << dummy << std::endl;
    std::cout << "q: " << q.transpose() << " t: " << t.transpose() << std::endl;
    T_guess.topLeftCorner<3,3>() = Eigen::Quaternionf(q).toRotationMatrix();
    T_guess.topRightCorner<3,1>() = t;
    have_guess = true;
  }

  ComputeAreaWeightsPc(pcA);
  ComputeAreaWeightsPc(pcB);

  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcA_ptr = pcA.makeShared();
  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr pcB_ptr = pcB.makeShared();

  pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal>* icp;
  if (vm.count("normals")) {
    icp = new pcl::IterativeClosestPointWithNormals<pcl::PointXYZRGBNormal,
        pcl::PointXYZRGBNormal>();
    std::cout << "Using points and surface normals in ICP" << std::endl;
  } else {
    icp = new pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal,
        pcl::PointXYZRGBNormal>();
    std::cout << "Using only points in ICP" << std::endl;
  }
  icp->setMaximumIterations(100);
  icp->setMaxCorrespondenceDistance (cutoff);
  icp->setTransformationEpsilon (1e-8);
  icp->setEuclideanFitnessEpsilon(0.01);
  icp->setInputSource(pcA_ptr);
  icp->setInputTarget(pcB_ptr);
  if (have_guess)
    icp->align(pcB_T, T_guess);
  else
    icp->align(pcB_T);

  bool success = icp->hasConverged();
  std::cout << "has converged:" << icp->hasConverged() << " score: " <<
    icp->getFitnessScore() << std::endl;
  std::cout << icp->getFinalTransformation() << std::endl;
  Eigen::Matrix4d T = icp->getFinalTransformation().cast<double>();
  Eigen::Matrix3d R = T.topLeftCorner<3,3>();
  Eigen::Quaterniond q(R);
  Eigen::Vector3d t = T.topRightCorner<3,1>();

  if(pathOut.size() > 1) {
    std::ofstream out(pathOut + std::string(".csv"));
    out << "q_w q_x q_y q_z t_x t_y t_z" << std::endl;
    out << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " " << t(0)
      << " " << t(1) << " " << t(2) << " " << std::endl;
    out.close();
  }

  if (vm.count("display")) {
    DisplayPcs(pcA, pcB, q, t, 1.0);
  }
  delete icp;

  return success ? 0 : 1;
}


/*
 *  test_swaps.hpp
 *  bondgeek
 *
 *  Created by BART MOSLEY on 6/3/12.
 *  Copyright 2012 BG Research LLC. All rights reserved.
 *
 */

#define BOOST_TEST_DYN_LINK

#include <boost/test/unit_test.hpp>
#include <bg/bondgeek.hpp>

using namespace QuantLib;
using namespace bondgeek;
using namespace boost::unit_test;

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

struct SwapTests {
	Calendar calendar;
	Date todaysDate;

    CurveMap depocurve, depocurve2;
    CurveMap futscurve;
    CurveMap swapcurve;

	RateHelperCurve acurve;
    RateHelperCurve *bcurve;
    RateHelperCurve *ccurve;

	SwapTests() :
	calendar(TARGET()),
	todaysDate(TARGET().adjust(Date(20, September, 2004)))
	{
		Settings::instance().evaluationDate() = todaysDate;
		BOOST_TEST_MESSAGE("\nSwap tests");

        string futtenors[] = {"ED1", "ED2", "ED3", "ED4", "ED5", "ED6", "ED7", "ED8"};
        double futspots[] = {96.2875, 96.7875, 96.9875, 96.6875, 96.4875, 96.3875, 96.2875, 96.0875};
		string depotenors[] = {"1W", "1M", "3M", "6M", "9M", "1y"};
		double depospots[] = {.0382, 0.0372, 0.0363, 0.0353, 0.0348, 0.0345};
		string swaptenors[] = {"2y", "3y", "5y", "10Y", "15Y"};
		double swapspots[] = {0.037125, 0.0398, 0.0443, 0.05165, 0.055175};

        for (int i=0; i<2; i++)
            depocurve2[depotenors[i]] = depospots[i];

        for (int i=0; i<6; i++)
            depocurve[depotenors[i]] = depospots[i];

        for (int i=0; i<6; i++)
            futscurve[futtenors[i]] = futspots[i];

        for (int i=0; i<5; i++)
            swapcurve[swaptenors[i]] = swapspots[i];

		acurve = RateHelperCurve(EURiborCurve("6M"));
        acurve.update(depotenors, depospots, 6,
                      swaptenors, swapspots, 5,
                      todaysDate
                      );

        bcurve = new RateHelperCurve(USDLiborCurve("3M"));
        bcurve->update(depotenors, depospots, 6,
                       swaptenors, swapspots, 5,
                       todaysDate
                       );

        ccurve = new RateHelperCurve(USDLiborCurve("3M"));
        ccurve->update(depotenors, depospots, 6,
                       swaptenors, swapspots, 5,
                       Date(21, January, 2012)
                       );

        Settings::instance().evaluationDate() = todaysDate;

	}
};

BOOST_FIXTURE_TEST_SUITE( curvetest, SwapTests )

BOOST_AUTO_TEST_CASE( swap_value2 )
{

    RateHelperCurve rhcurve( EURiborCurve("6M", Annual) );

    BOOST_TEST_MESSAGE("SWAP_VALUE2: BUILD TEST CURVE");

    rhcurve.update(depocurve2, futscurve, swapcurve);

    BOOST_TEST_MESSAGE("Test equivalence of forecastingTermStructure and discountingTermStructure");
	Real d1 = rhcurve.discountingTermStructure().currentLink()->discount(10.0);
	Real d2 = rhcurve.forecastingTermStructure().currentLink()->discount(10.0);
	Real d3 = rhcurve.discountingTermStructure().currentLink()->discount(0.0);
    BOOST_CHECK(abs(d1-d2) < 0.001);
    BOOST_CHECK(d3 > d1);

}

BOOST_AUTO_TEST_CASE( swap_value3 )
{

    BOOST_TEST_MESSAGE("SWAP_VALUE3: Update Curve");

    ccurve->update(depocurve2, futscurve, swapcurve);

    BOOST_TEST_MESSAGE("Test equivalence of forecastingTermStructure and discountingTermStructure");
	Real d1 = ccurve->discountingTermStructure().currentLink()->discount(10.0);
	Real d2 = ccurve->forecastingTermStructure().currentLink()->discount(10.0);
	Real d3 = ccurve->discountingTermStructure().currentLink()->discount(0.0);
    BOOST_CHECK(abs(d1-d2) < 0.001);
    BOOST_CHECK(d3 > d1);

}

BOOST_AUTO_TEST_CASE( evaluation_date )
{
	BOOST_TEST_MESSAGE(">>date: " << Settings::instance().evaluationDate() );
	BOOST_CHECK( Settings::instance().evaluationDate() == todaysDate );
}

BOOST_AUTO_TEST_CASE( curve_values )
{
    BOOST_TEST_MESSAGE(">>EUR curve.discount(10Y): " << acurve.discount(10.0) );
    BOOST_TEST_MESSAGE(">>USD curve.discount(10Y): " << bcurve->discount(10.0) );

    BOOST_TEST_MESSAGE(">>EUR curve.tenorquote(10Y): " << acurve.tenorquote("10Y") );
    BOOST_TEST_MESSAGE(">>USD curve.tenorquote(10Y): " << bcurve->tenorquote("10Y") );

    BOOST_CHECK(  acurve.tenorquote("10Y") ==  0.05165 );
    BOOST_CHECK(  bcurve->tenorquote("10Y") == 0.05165 );

}

BOOST_AUTO_TEST_CASE( curve_different_date )
{
    Date today0 = Settings::instance().evaluationDate();

    BOOST_TEST_MESSAGE("\nCheck under different date:" );
    Settings::instance().evaluationDate() = ccurve->curveDate();
    BOOST_TEST_MESSAGE(">>date: " << Settings::instance().evaluationDate() );

    BOOST_TEST_MESSAGE(">>USD curve.discount(10Y): " << ccurve->discount(10.0) );
    BOOST_TEST_MESSAGE(">>USD curve.tenorquote(10Y): " << ccurve->tenorquote("10Y") );
    BOOST_CHECK(  ccurve->tenorquote("10Y") ==  0.05165 );

    Settings::instance().evaluationDate() = today0;
    BOOST_CHECK( Settings::instance().evaluationDate() == todaysDate );
}

BOOST_AUTO_TEST_CASE( curve_change_quote )
{
	double df0 = ccurve->discount(10.0);
    BOOST_TEST_MESSAGE(">>USD curve.discount(10Y): " << df0 );
    BOOST_TEST_MESSAGE(">>USD curve.tenorquote(10Y): " << ccurve->tenorquote("10Y") );
    BOOST_CHECK( ccurve->tenorquote("10Y") ==  0.05165 );

    ccurve->setTenorQuote("10Y", 0.0520);
    double df1 = ccurve->discount(10.0);
    BOOST_TEST_MESSAGE(">>USD curve.discount(10Y): " << df1 );
    BOOST_TEST_MESSAGE(">>USD curve.tenorquote(10Y): " << ccurve->tenorquote("10Y") );
    BOOST_CHECK( ccurve->tenorquote("10Y") ==  0.052 );
    BOOST_CHECK( df1 < df0);

    ccurve->setTenorQuote("10Y", 0.05165);
    double df2 = ccurve->discount(10.0);
    BOOST_TEST_MESSAGE(">>USD curve.discount(10Y): " << df2 );
    BOOST_TEST_MESSAGE(">>USD curve.tenorquote(10Y): " << ccurve->tenorquote("10Y") );
    BOOST_CHECK( ccurve->tenorquote("10Y") ==  0.05165 );
    BOOST_CHECK( abs(df2-df0) < 0.0000001 );

}

BOOST_AUTO_TEST_CASE( swap_value )
{
	boost::shared_ptr<PricingEngine> swapEngine = createPriceEngine<DiscountingSwapEngine>(
																						   acurve.discountingTermStructure()
																						   );

	Real fixedRate = .04;
    Date settlementDate = acurve.referenceDate();
    Integer lengthInYears = 5;
    Date maturity = acurve.referenceDate() + lengthInYears*Years;

	SwapType<Euribor> euriborswaps(Annual,
								   Thirty360(Thirty360::European),
								   Unadjusted,
								   Semiannual,
								   Actual360(),
								   ModifiedFollowing,
								   TARGET()
								   );

	euriborswaps.linkIndexTo(acurve.yieldTermStructurePtr());

	boost::shared_ptr<FixedFloatSwap> qswp2 = euriborswaps.create(settlementDate,
																  maturity,
																  fixedRate);

	qswp2->setPricingEngine(swapEngine);

	BOOST_TEST_MESSAGE("Test equivalence of forecastingTermStructure and discountingTermStructure");
	Real d1 = acurve.discountingTermStructure().currentLink()->discount(10.0);
	Real d2 = acurve.forecastingTermStructure().currentLink()->discount(10.0);
	BOOST_CHECK(abs(d1-d2) < 0.001);

	Real p1 = qswp2->NPV();
	BOOST_TEST_MESSAGE("Check Pricing against known value");
	BOOST_CHECK( abs(19065.88-p1) < .001 );

	qswp2->setEngine(acurve);
	Real p2 = qswp2->NPV();
	BOOST_TEST_MESSAGE("Check setEngine pricing");
	BOOST_CHECK( abs(p2-p1) < .0001 );

}

BOOST_AUTO_TEST_SUITE_END()


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file find_test.cpp
/// \brief Add your file description here.

#include <boost/test/minimal.hpp>
#include "mcrl2/core/detail/print_utility.h"
#include "mcrl2/pbes/find.h"
#include "mcrl2/pbes/parse.h"
#include "mcrl2/pbes/print.h"
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

inline
data::variable nat(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_nat::nat());
}

inline
data::variable pos(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_pos::pos());
}

inline
data::variable bool_(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_bool::bool_());
}

void test_find()
{
  const std::string VARSPEC =
    "datavar         \n"
    "  m: Nat;       \n"
    "  n: Nat;       \n"
    "                \n"
    "predvar         \n"
    "  X: Bool, Pos; \n"
    "  Y: Nat;       \n"
    ;

  pbes_expression x = parse_pbes_expression("X(true, 2) && Y(n+1) && Y(m)", VARSPEC);

  //--- find_all_variables ---//
  data::variable m = nat("m");
  data::variable n = nat("n");
  std::set<data::variable> v = pbes_system::find_all_variables(x);
  BOOST_CHECK(v.find(m) != v.end());
  BOOST_CHECK(v.find(n) != v.end());

  //--- find_sort_expressions ---//
  std::set<data::sort_expression> e = pbes_system::find_sort_expressions(x);
  BOOST_CHECK(std::find(e.begin(), e.end(), data::sort_nat::nat()) != e.end());
  BOOST_CHECK(std::find(e.begin(), e.end(), data::sort_pos::pos()) != e.end());

}

void test_free_variables()
{
  const std::string VARSPEC =
    "datavar         \n"
    "  n: Nat;       \n"
    "                \n"
    "predvar         \n"
    "  X: Bool, Pos; \n"
    "  Y: Nat;       \n"
    ;

  pbes_expression x = parse_pbes_expression("forall m:Nat.(X(true, 2) && Y(n+1) && Y(m))", VARSPEC);
  data::variable m = nat("m");
  data::variable n = nat("n");

  std::set<data::variable> free_variables = pbes_system::find_free_variables(x);
  std::cout << "free variables: " << core::detail::print_set(free_variables) << std::endl;
  BOOST_CHECK(free_variables.find(m) == free_variables.end());
  BOOST_CHECK(free_variables.find(n) != free_variables.end());
}

void test_find_free_variables()
{
  std::string test1 =
    "pbes                                                                   \n"
    "                                                                       \n"
    "nu X(b:Bool, n:Nat) = (val(b) => X(!b, n)) && (val(!b) => X(!b, n+1)); \n"
    "mu Y(c:Nat, d:Bool) = forall m:Nat. Y(c, true) || X(d, m);             \n"
    "                                                                       \n"
    "init X(true, 0);                                                       \n"
    ;

  pbes p = txt2pbes(test1);

  std::set<data::variable> v = find_free_variables(p);
  std::cout << "variables: " << core::detail::print_set(v) << std::endl;
  BOOST_CHECK(v.size() == 0);

  v = find_free_variables(p.equations()[0]);
  std::cout << "variables: " << core::detail::print_set(v) << std::endl;
  BOOST_CHECK(v.size() == 0);

  v = find_free_variables(p.equations()[1]);
  std::cout << "variables: " << core::detail::print_set(v) << std::endl;
  BOOST_CHECK(v.size() == 0);

  v = find_free_variables(p.equations()[0].formula());
  std::cout << "variables: " << core::detail::print_set(v) << std::endl;
  BOOST_CHECK(v.size() == 2);

  v = find_free_variables(p.equations()[1].formula());
  std::cout << "variables: " << core::detail::print_set(v) << std::endl;
  BOOST_CHECK(v.size() == 2);

}

int test_main(int argc, char** argv)
{
  test_find();
  test_free_variables();
  test_find_free_variables();

  return 0;
}


//  (C) Copyright Gennadiy Rozental 2001-2012.
//  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)

//  See http://www.boost.org/libs/test for the library home page.
//
//  File        : $RCSfile$
//
//  Version     : $Revision$
//
//  Description : defines algoirthms for comparing 2 floating point values
// ***************************************************************************

#ifndef BOOST_TEST_FLOATING_POINT_COMPARISON_HPP_071894GER
#define BOOST_TEST_FLOATING_POINT_COMPARISON_HPP_071894GER

// Boost.Test
#include <boost/test/detail/global_typedef.hpp>
#include <boost/test/tools/predicate_result.hpp>

// Boost
#include <boost/limits.hpp>  // for std::numeric_limits
#include <boost/numeric/conversion/conversion_traits.hpp> // for numeric::conversion_traits
#include <boost/static_assert.hpp>
#include <boost/assert.hpp>

// STL
#include <iosfwd>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace math {
namespace fpc {

// ************************************************************************** //
// **************                 fpc::strength                ************** //
// ************************************************************************** //

enum strength {
    FPC_STRONG, // "Very close"   - equation 1' in docs, the default
    FPC_WEAK    // "Close enough" - equation 2' in docs.
};

// ************************************************************************** //
// **************                    details                   ************** //
// ************************************************************************** //

namespace fpc_detail {

// FPT is Floating-Point Type: float, double, long double or User-Defined.
template<typename FPT>
inline FPT
fpt_abs( FPT fpv )
{
    return fpv < static_cast<FPT>(0) ? -fpv : fpv;
}

//____________________________________________________________________________//

template<typename FPT>
struct fpt_limits {
    static FPT    min_value()
    {
        return std::numeric_limits<FPT>::is_specialized
                    ? (std::numeric_limits<FPT>::min)()
                    : static_cast<FPT>(0);
    }
    static FPT    max_value()
    {
        return std::numeric_limits<FPT>::is_specialized
                    ? (std::numeric_limits<FPT>::max)()
                    : static_cast<FPT>(1000000); // for our purposes it doesn't really matter what value is returned here
    }
};

//____________________________________________________________________________//

// both f1 and f2 are unsigned here
template<typename FPT>
inline FPT
safe_fpt_division( FPT f1, FPT f2 )
{
    // Avoid overflow.
    if( (f2 < static_cast<FPT>(1))  && (f1 > f2*fpt_limits<FPT>::max_value()) )
        return fpt_limits<FPT>::max_value();

    // Avoid underflow.
    if( (f1 == static_cast<FPT>(0)) ||
        ((f2 > static_cast<FPT>(1)) && (f1 < f2*fpt_limits<FPT>::min_value())) )
        return static_cast<FPT>(0);

    return f1/f2;
}

//____________________________________________________________________________//

} // namespace fpc_detail

// ************************************************************************** //
// **************         tolerance presentation types         ************** //
// ************************************************************************** //

template<typename ToleranceType>
struct tolerance_traits {
    template<typename FPT>
    static ToleranceType    actual_tolerance( FPT fraction_tolerance )
    {
        return static_cast<ToleranceType>( fraction_tolerance );
    }
    template<typename FPT>
    static FPT              fraction_tolerance( ToleranceType tolerance )
    {
        return static_cast<FPT>(tolerance);
    }
};

//____________________________________________________________________________//

template<typename FPT>
struct percent_tolerance_t {
    explicit    percent_tolerance_t( FPT v ) : m_value( v ) {}

    FPT m_value;
};

//____________________________________________________________________________//

template<typename FPT>
struct tolerance_traits<percent_tolerance_t<FPT> > {
    template<typename FPT2>
    static percent_tolerance_t<FPT> actual_tolerance( FPT2 fraction_tolerance )
    {
        return percent_tolerance_t<FPT>( fraction_tolerance * static_cast<FPT2>(100.) );
    }

    template<typename FPT2>
    static FPT2 fraction_tolerance( percent_tolerance_t<FPT> tolerance )
    {
        return static_cast<FPT2>(tolerance.m_value)*static_cast<FPT2>(0.01);
    }
};

//____________________________________________________________________________//

template<typename FPT>
std::ostream& operator<<( std::ostream& out, percent_tolerance_t<FPT> t )
{
    return out << t.m_value;
}

//____________________________________________________________________________//

template<typename FPT>
inline percent_tolerance_t<FPT>
percent_tolerance( FPT v )
{
    return percent_tolerance_t<FPT>( v );
}

//____________________________________________________________________________//

// ************************************************************************** //
// **************             close_at_tolerance               ************** //
// ************************************************************************** //

template<typename FPT>
class close_at_tolerance {
public:
    // Public typedefs
    typedef bool result_type;

    // Constructor
    template<typename ToleranceType>
    explicit    close_at_tolerance( ToleranceType tolerance, fpc::strength fpc_strength = FPC_STRONG )
    : m_fraction_tolerance( tolerance_traits<ToleranceType>::template fraction_tolerance<FPT>( tolerance ) )
    , m_strength( fpc_strength )
    {
        BOOST_ASSERT( m_fraction_tolerance >= 0 ); // no reason for tolerance to be negative
    }

    // Access methods
    FPT                 fraction_tolerance() const  { return m_fraction_tolerance; }
    fpc::strength       strength() const            { return m_strength; }
    FPT                 failed_fraction() const     { return m_failed_fraction; }

    // Action method
    bool                operator()( FPT left, FPT right ) const
    {
        FPT diff              = fpc_detail::fpt_abs( left - right );
        FPT fraction_of_right = fpc_detail::safe_fpt_division( diff, fpc_detail::fpt_abs( right ) );
        FPT fraction_of_left  = fpc_detail::safe_fpt_division( diff, fpc_detail::fpt_abs( left ) );

        bool res( m_strength == FPC_STRONG
            ? (fraction_of_right <= m_fraction_tolerance && fraction_of_left <= m_fraction_tolerance)
            : (fraction_of_right <= m_fraction_tolerance || fraction_of_left <= m_fraction_tolerance) );

        if( !res )
            m_failed_fraction = (fraction_of_right > m_fraction_tolerance ? fraction_of_right : fraction_of_left);

        return res;
    }

private:
    // Data members
    FPT                 m_fraction_tolerance;
    fpc::strength       m_strength;
    mutable FPT         m_failed_fraction;
};

// ************************************************************************** //
// **************                 is_close_to                  ************** //
// ************************************************************************** //

template<typename FPT1, typename FPT2, typename ToleranceType>
bool
is_close_to( FPT1 left, FPT2 right, ToleranceType tolerance )
{
    // deduce "better" type from types of arguments being compared
    // if one type is floating and the second integral we use floating type and
    // value of integral type is promoted to the floating. The same for float and double
    // But we don't want to compare two values of integral types using this tool.
    typedef typename numeric::conversion_traits<FPT1,FPT2>::supertype FPT;
    BOOST_STATIC_ASSERT( !is_integral<FPT>::value );

    return fpc::close_at_tolerance<FPT>( tolerance, FPC_STRONG )( left, right );
}

//____________________________________________________________________________//

// ************************************************************************** //
// **************             close_at_tolerance               ************** //
// ************************************************************************** //

template<typename FPT>
class small_with_tolerance {
public:
    // Public typedefs
    typedef bool result_type;

    // Constructor
    explicit    small_with_tolerance( FPT tolerance )
    : m_tolerance( tolerance )
    {
        BOOST_ASSERT( m_tolerance >= 0 ); // no reason for the tolerance to be negative
    }

    // Action method
    bool        operator()( FPT fpv ) const
    {
        return fpc::fpc_detail::fpt_abs( fpv ) < m_tolerance;
    }

private:
    // Data members
    FPT         m_tolerance;
};

// ************************************************************************** //
// **************                  is_small                    ************** //
// ************************************************************************** //

template<typename FPT>
bool
is_small( FPT fpv, FPT tolerance )
{
    return small_with_tolerance<FPT>( tolerance )( fpv );
}

//____________________________________________________________________________//

} // namespace fpc
} // namespace math

namespace test_tools {

namespace fpc = math::fpc;

// ************************************************************************** //
// **************               check_is_close                 ************** //
// ************************************************************************** //

struct BOOST_TEST_DECL check_is_close_t {
    // Public typedefs
    typedef bool result_type;

    template<typename FPT1, typename FPT2, typename ToleranceType>
    predicate_result
    operator()( FPT1 left, FPT2 right, ToleranceType tolerance ) const
    {
        predicate_result pr( fpc::is_close_to( left, right, tolerance ) );

        if( !pr )
            pr.message() << tolerance;

        return pr;
    }
};

namespace {
check_is_close_t const& check_is_close = unit_test::ut_detail::static_constant<check_is_close_t>::value;
}

//____________________________________________________________________________//

// ************************************************************************** //
// **************               check_is_small                 ************** //
// ************************************************************************** //

struct BOOST_TEST_DECL check_is_small_t {
    // Public typedefs
    typedef bool result_type;

    template<typename FPT>
    bool
    operator()( FPT fpv, FPT tolerance ) const
    {
        return fpc::is_small( fpv, tolerance );
    }
};

namespace {
check_is_small_t const& check_is_small = unit_test::ut_detail::static_constant<check_is_small_t>::value;
}

//____________________________________________________________________________//

} // namespace test_tools
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_FLOATING_POINT_COMAPARISON_HPP_071894GER