/*****************************************************************************/
/*  Copyright (c) 2015, Alessandro Pieropan                                  */
/*  All rights reserved.                                                     */
/*                                                                           */
/*  Redistribution and use in source and binary forms, with or without       */
/*  modification, are permitted provided that the following conditions       */
/*  are met:                                                                 */
/*                                                                           */
/*  1. Redistributions of source code must retain the above copyright        */
/*  notice, this list of conditions and the following disclaimer.            */
/*                                                                           */
/*  2. Redistributions in binary form must reproduce the above copyright     */
/*  notice, this list of conditions and the following disclaimer in the      */
/*  documentation and/or other materials provided with the distribution.     */
/*                                                                           */
/*  3. Neither the name of the copyright holder nor the names of its         */
/*  contributors may be used to endorse or promote products derived from     */
/*  this software without specific prior written permission.                 */
/*                                                                           */
/*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS      */
/*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT        */
/*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR    */
/*  A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT     */
/*  HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,   */
/*  SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT         */
/*  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,    */
/*  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY    */
/*  THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT      */
/*  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE    */
/*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.     */
/*****************************************************************************/

#include "../include/pose_estimation.h"
#include <opencv2/calib3d/calib3d.hpp>
#include <boost/math/constants/constants.hpp>
#include "../include/utilities.h"
#include <eigen3/Eigen/Geometry>
#include <algorithm>
#include <stdexcept>
#include <iostream>

using namespace std;
using namespace cv;
using namespace Eigen;

namespace fato {

void getPoseRansac(const std::vector<cv::Point3f>& model_points,
                   const std::vector<cv::Point2f>& tracked_points,
                   const cv::Mat& camera_model, int iterations, float distance,
                   vector<int>& inliers, Mat& rotation, Mat& translation) {
  if (model_points.size() > 4) {
    Mat rotation_vect;
    solvePnPRansac(model_points, tracked_points, camera_model,
                   Mat::zeros(1, 8, CV_32F), rotation_vect, translation, false,
                   iterations, distance, model_points.size(), inliers, CV_P3P);

    try {
      Rodrigues(rotation_vect, rotation);
      rotation.convertTo(rotation, CV_32FC1);
    } catch (cv::Exception& e) {
      cout << "Error estimating ransac rotation: " << e.what() << endl;
    }

  } else {
    rotation = Mat(3, 3, CV_32FC1, 0.0f);
    translation = Mat(1, 3, CV_32FC1, 0.0f);
  }
}

void getMatrices(const std::vector<cv::Point2f>& prev_pts,
                 const std::vector<float>& prev_depth,
                 const std::vector<cv::Point2f>& next_pts, float nodal_x,
                 float nodal_y, float focal_x, float focal_y,
                 Eigen::MatrixXf& A, Eigen::VectorXf& b) {
  int valid_points = prev_pts.size();

  A = Eigen::MatrixXf(valid_points * 2, 6);
  b = Eigen::VectorXf(valid_points * 2);

  float focal = (focal_x + focal_y) / 2.0;

  for (auto i = 0; i < valid_points; ++i) {
    float mz = prev_depth.at(i);
    float x = next_pts.at(i).x - nodal_x;
    float y = next_pts.at(i).y - nodal_y;
    float xy = x * y / focal;

    int id = 2 * i;
    int id2 = id + 1;
    // first equation for X, u flow
    A(id, 0) = focal / mz;
    A(id, 1) = 0;
    A(id, 2) = -x / mz;
    A(id, 3) = -xy;
    A(id, 4) = focal + (x * x) / focal;
    A(id, 5) = -y;
    // second equation for X, v flow
    A(id2, 0) = 0;
    A(id2, 1) = focal / mz;
    A(id2, 2) = -y / mz;
    A(id2, 3) = -(focal + (x * x) / focal);
    A(id2, 4) = xy;
    A(id2, 5) = x;
    // setting u,v flow in b vector
    b[id] = next_pts.at(i).x - prev_pts.at(i).x;
    b[id2] = next_pts.at(i).y - prev_pts.at(i).y;
  }
}

void getPoseFromFlow(const std::vector<cv::Point2f>& prev_pts,
                     const std::vector<float>& prev_depth,
                     const std::vector<cv::Point2f>& next_pts, float nodal_x,
                     float nodal_y, float focal_x, float focal_y,
                     vector<float>& translation, vector<float>& rotation) {
  int valid_points = prev_pts.size();

  Eigen::MatrixXf A(valid_points * 2, 6);
  Eigen::VectorXf b(valid_points * 2);

  float focal = (focal_x + focal_y) / 2.0;

  for (auto i = 0; i < prev_pts.size(); ++i) {
    float mz = prev_depth.at(i);
    float x = next_pts.at(i).x - nodal_x;
    float y = next_pts.at(i).y - nodal_y;
    float xy = x * y / focal;

    int id = 2 * i;
    int id2 = id + 1;
    // first equation for X, u flow
    A(id, 0) = focal / mz;
    A(id, 1) = 0;
    A(id, 2) = -x / mz;
    A(id, 3) = -xy;
    A(id, 4) = focal + (x * x) / focal;
    A(id, 5) = -y;
    // second equation for X, v flow
    A(id2, 0) = 0;
    A(id2, 1) = focal / mz;
    A(id2, 2) = -y / mz;
    A(id2, 3) = -(focal + (x * x) / focal);
    A(id2, 4) = xy;
    A(id2, 5) = x;
    // setting u,v flow in b vector
    b[id] = next_pts.at(i).x - prev_pts.at(i).x;
    b[id2] = next_pts.at(i).y - prev_pts.at(i).y;
  }

  translation.resize(3, 0);
  rotation.resize(3, 0);
  // solving least square to find the 6 unknown parameters: tx,ty, tz, wx, wy,
  // wz
  Eigen::VectorXf Y = (A.transpose() * A).ldlt().solve(A.transpose() * b);

  //  cout << "LQ: " << fixed << setprecision(3) << Y[0] << " " << Y[1] << " "
  //  << Y[2] << " "
  //                 << Y[3] << " " << Y[4] << " " << Y[5] << endl;

  translation[0] = Y[0];
  translation[1] = Y[1];
  translation[2] = Y[2];

  rotation[0] = Y[3];
  rotation[1] = Y[4];
  rotation[2] = Y[5];
}

Eigen::VectorXf getPoseFromFlowRobust(const std::vector<cv::Point2f>& prev_pts,
                           const std::vector<float>& prev_depth,
                           const std::vector<cv::Point2f>& next_pts,
                           float nodal_x, float nodal_y, float focal_x,
                           float focal_y, int num_iters,
                           std::vector<float>& translation,
                           std::vector<float>& rotation,
                           std::vector<int>& outliers) {
  Eigen::MatrixXf X;
  Eigen::VectorXf y;

  getMatrices(prev_pts, prev_depth, next_pts, nodal_x, nodal_y, focal_x,
              focal_y, X, y);

  // during tracking possible to initialize with the previous estimation???
  Eigen::VectorXf beta = (X.transpose() * X).ldlt().solve(X.transpose() * y);

  Eigen::VectorXf W;
  Eigen::MatrixXf XW(X.rows(), X.cols());

  for (auto i = 0; i < num_iters; ++i) {
    // cout << "iter " << i;// << endl;

    Eigen::VectorXf residuals = ((X * beta) - y);
    residuals = residuals.array().abs();

    // cout << residuals << endl;

    std::vector<float> res_vector(residuals.rows(), 0);

    for (int j = 0; j < residuals.rows(); ++j) {
      res_vector.at(j) = residuals(j);
    }

    sort(res_vector.begin(), res_vector.end());

    float residual_scale;
    if (res_vector.size() % 2 != 0)
      residual_scale = res_vector.at(res_vector.size() / 2);
    else
      residual_scale = res_vector.at(res_vector.size() / 2) +
                       res_vector.at(res_vector.size() / 2 + 1) / 2.0f;

    // avoid division by 0
    residual_scale = max(residual_scale, 0.0001f);

    residual_scale *=
        6.9460;  // constant defined in the IRLS and bisquare distance

    W = residuals / residual_scale;

    for (auto j = 0; j < W.rows(); ++j) {
      if (W(j) > 1) {
        W(j) = 0;
      } else {
        auto tmp = 1 - W(j) * W(j);
        W(j) = tmp * tmp;
      }
    }

    for (auto j = 0; j < XW.rows(); ++j) {
      for (auto k = 0; k < XW.cols(); ++k) {
        XW(j, k) = W(j) * X(j, k);
      }
    }

    beta = (XW.transpose() * X).ldlt().solve(XW.transpose() * y);
  }

  // cout << "\n here" << endl;

  int num_points = W.rows() / 2;

  for (auto j = 0; j < num_points; ++j) {
    int id = 2 * j;
    if (W(id) == 0 || W(id + 1) == 0) {
      outliers.push_back(j);
    }
  }

  translation.resize(3, 0);
  rotation.resize(3, 0);

  translation[0] = beta[0];
  translation[1] = beta[1];
  translation[2] = beta[2];

  rotation[0] = beta[3];
  rotation[1] = beta[4];
  rotation[2] = beta[5];

  return beta;
}

void getPose2D(const std::vector<cv::Point2f*>& model_points,
               const std::vector<cv::Point2f*>& tracked_points, float& scale,
               float& angle) {
  vector<double> angles;
  vector<float> scales;

  angles.reserve(model_points.size() * 2);
  scales.reserve(model_points.size() * 2);

  const double pi = boost::math::constants::pi<double>();

  for (size_t i = 0; i < model_points.size(); ++i) {
    for (size_t j = 0; j < model_points.size(); j++) {
      // computing angle
      Point2f a = *tracked_points.at(i) - *tracked_points.at(j);
      Point2f b = *model_points.at(i) - *model_points.at(j);
      double val = atan2(a.y, a.x) - atan2(b.y, b.x);

      if (abs(val) > pi) {
        int sign = (val < 0) ? -1 : 1;
        val = val - sign * 2 * pi;
      }
      angles.push_back(val);

      if (i == j) continue;

      // computing scale
      auto tracked_dis =
          getDistance(tracked_points.at(i), tracked_points.at(j));
      auto model_dis = getDistance(model_points.at(i), model_points.at(j));
      if (model_dis != 0) scales.push_back(tracked_dis / model_dis);
    }
  }

  sort(angles.begin(), angles.end());
  sort(scales.begin(), scales.end());

  auto angles_size = angles.size();
  auto scale_size = scales.size();

  if (angles_size == 0)
    angle = 0;

  else if (angles_size % 2 == 0)
    angle = (angles[angles_size / 2 - 1] + angles[angles_size / 2]) / 2;
  else
    angle = angles[angles_size / 2];

  if (scale_size == 0)
    scale = 1;
  else if (scale_size % 2 == 0)
    scale = (scales[scale_size / 2 - 1] + scales[scale_size / 2]) / 2;
  else
    scale = scales[scale_size / 2];
}

// TODO: implement ransac approach for better SVD estimation
Mat getRigidTransform(Mat& a, Mat& b) {
  int numRows = a.rows;

  vector<float> srcM = {0, 0, 0};
  vector<float> dstM = {0, 0, 0};

  // compute centroid of the two sets of points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      srcM[j] += a.at<float>(i, j);
      dstM[j] += b.at<float>(i, j);
    }
  }

  for (int i = 0; i < 3; i++) {
    srcM[i] = srcM[i] / static_cast<float>(numRows);
    dstM[i] = dstM[i] / static_cast<float>(numRows);
  }

  Mat AA, BB, AA_T, BB_T;
  a.copyTo(AA);
  b.copyTo(BB);
  // subtracting centroid from all the points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      AA.at<float>(i, j) -= srcM[j];
      BB.at<float>(i, j) -= dstM[j];
    }
  }

  transpose(AA, AA_T);

  Mat3f H;
  H = AA_T * BB;

  // cout << "H\n" << H << endl;

  Mat w, u, vt, vt_T, u_T;
  SVD::compute(H, w, u, vt);

  transpose(vt, vt_T);
  transpose(u, u_T);

  Mat3f R;
  R = vt_T * u_T;

  // reflection case
  if (determinant(R) < 0) {
    for (size_t i = 0; i < 3; i++) {
      vt.at<float>(2, i) *= -1;
    }
    transpose(vt, vt_T);
    R = vt_T * u_T;
  }

  return R;
}

Mat getRigidTransform(Mat& a, Mat& b, vector<float>& cA, vector<float>& cB) {
  int numRows = a.rows;

  vector<float>& srcM = cA;
  vector<float>& dstM = cB;

  Mat AA, BB, AA_T, BB_T;
  a.copyTo(AA);
  b.copyTo(BB);
  // subtracting centroid from all the points
  for (int i = 0; i < numRows; i++) {
    for (int j = 0; j < 3; j++) {
      AA.at<float>(i, j) -= srcM[j];
      BB.at<float>(i, j) -= dstM[j];
    }
  }

  transpose(AA, AA_T);

  Mat3f H;
  H = AA_T * BB;

  Mat w, u, vt, vt_T, u_T;
  SVD::compute(H, w, u, vt);

  transpose(vt, vt_T);
  transpose(u, u_T);

  Mat3f R;
  R = vt_T * u_T;

  // reflection case
  if (determinant(R) < 0) {
    for (size_t i = 0; i < 3; i++) {
      vt.at<float>(2, i) *= -1;
    }
    transpose(vt, vt_T);
    R = vt_T * u_T;
  }

  return R;
}

void rotateBBox(const vector<Point3f>& bBox, const Mat& rotation,
                vector<Point3f>& updatedBBox) {
  Mat a(4, 3, CV_32FC1);
  Mat b, b_T, a_T;

  for (size_t i = 0; i < 4; i++) {
    a.at<float>(i, 0) = bBox.at(i).x;
    a.at<float>(i, 1) = bBox.at(i).y;
    a.at<float>(i, 2) = bBox.at(i).z;
  }

  transpose(a, a_T);

  b_T = rotation * a_T;
  transpose(b_T, b);

  for (size_t i = 0; i < 4; i++) {
    updatedBBox.push_back(
        Point3f(b.at<float>(i, 0), b.at<float>(i, 1), b.at<float>(i, 2)));
  }
}

void rotatePoint(const Point3f& point, const Mat& rotation,
                 Point3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point.x;
  a.at<float>(1) = point.y;
  a.at<float>(2) = point.z;
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint.x = b.at<float>(0);
  updatedPoint.y = b.at<float>(1);
  updatedPoint.z = b.at<float>(2);
}

void rotatePoint(const Vec3f& point, const Mat& rotation, Vec3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point[0];
  a.at<float>(1) = point[1];
  a.at<float>(2) = point[2];
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint[0] = b.at<float>(0);
  updatedPoint[1] = b.at<float>(1);
  updatedPoint[2] = b.at<float>(2);
}

void rotatePoint(const Vec3f& point, const Mat& rotation,
                 Point3f& updatedPoint) {
  Mat a(1, 3, CV_32FC1);
  a.at<float>(0) = point[0];
  a.at<float>(1) = point[1];
  a.at<float>(2) = point[2];
  Mat b, b_T, a_T;

  transpose(a, a_T);
  b_T = rotation * a_T;
  transpose(b_T, b);

  updatedPoint.x = b.at<float>(0);
  updatedPoint.y = b.at<float>(1);
  updatedPoint.z = b.at<float>(2);
}

void rotationVecToMat(const Mat& vec, Mat& mat) {}

/**************************************************************/
/*           POSE CLASS                                       */
/**************************************************************/

Pose::Pose() { pose_ = Matrix4d(4, 4); }

Pose::Pose(Mat& r_mat, Mat& t_vect) {
  pose_ = Matrix4d(4, 4);

  if (t_vect.cols != 3 && t_vect.rows != 3) {
    cout << t_vect.cols << " " << t_vect.rows << endl;
    throw std::runtime_error(
        "Pose: bad translation vector format, 3x1 accepted");
  }

  Mat tmp_rot, tmp_t;
  r_mat.convertTo(tmp_rot, CV_64FC1);
  t_vect.convertTo(tmp_t, CV_64FC1);

  if (tmp_rot.cols == 3 && tmp_rot.rows == 3) {
    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = tmp_rot.at<double>(i, j);
      }
      pose_(i, 3) = tmp_t.at<double>(i);
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;
  } else if (r_mat.cols == 3 && r_mat.cols == 1) {
    Eigen::Matrix3d rot_view;
    rot_view =
        Eigen::AngleAxisd(tmp_rot.at<double>(2), Eigen::Vector3d::UnitZ()) *
        Eigen::AngleAxisd(tmp_rot.at<double>(1), Eigen::Vector3d::UnitY()) *
        Eigen::AngleAxisd(tmp_rot.at<double>(0), Eigen::Vector3d::UnitX());

    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = rot_view(i, j);
      }
      pose_(i, 3) = tmp_t.at<double>(i);
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;
  } else {
    throw std::runtime_error(
        "Pose: bad rotation matrix format, 3x3 or 1x3 accepted");
  }

  init_rot = r_mat;
  init_tr = t_vect;
}

Pose::Pose(Matrix4d& pose) {
  pose_ = Matrix4d(4, 4);

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      pose_(i, j) = pose(i, j);
    }
  }
}

Pose::Pose(std::vector<double>& beta) {
  if (beta.size() != 6) {
    throw std::runtime_error("Pose: bad parameters size, should be 6");
  }

  pose_ = Matrix4d(4, 4);

  Eigen::Matrix3d rot_view;
  rot_view = Eigen::AngleAxisd(beta.at(5), Eigen::Vector3d::UnitZ()) *
             Eigen::AngleAxisd(beta.at(4), Eigen::Vector3d::UnitY()) *
             Eigen::AngleAxisd(beta.at(3), Eigen::Vector3d::UnitX());

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      pose_(i, j) = rot_view(i, j);
    }
    pose_(3, i) = 0;
  }
  pose_(3, 3) = 1;

  pose_(0, 3) = beta.at(0);
  pose_(1, 3) = beta.at(1);
  pose_(2, 3) = beta.at(2);

  init_beta_ = beta;
}

Pose::Pose(VectorXf &beta)
{
    if (beta.rows() != 6) {
      throw std::runtime_error("Pose: bad parameters size, should be 6");
    }

    init_beta_.resize(6,0);

    for(auto i = 0; i < 6; ++i)
        init_beta_[i] = static_cast<double>(beta[i]);

    pose_ = Matrix4d(4, 4);

    Eigen::Matrix3d rot_view;
    rot_view = Eigen::AngleAxisd(init_beta_.at(5), Eigen::Vector3d::UnitZ()) *
               Eigen::AngleAxisd(init_beta_.at(4), Eigen::Vector3d::UnitY()) *
               Eigen::AngleAxisd(init_beta_.at(3), Eigen::Vector3d::UnitX());

    for (int i = 0; i < 3; ++i) {
      for (int j = 0; j < 3; ++j) {
        pose_(i, j) = rot_view(i, j);
      }
      pose_(3, i) = 0;
    }
    pose_(3, 3) = 1;

    pose_(0, 3) = init_beta_.at(0);
    pose_(1, 3) = init_beta_.at(1);
    pose_(2, 3) = init_beta_.at(2);
}

std::pair<cv::Mat, cv::Mat> Pose::toCV() const {
  cv::Mat rotation = cv::Mat(3, 3, CV_64FC1, 0.0f);
  cv::Mat translation = cv::Mat(1, 3, CV_64FC1, 0.0f);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      rotation.at<double>(i, j) = pose_(i, j);
    }
    translation.at<double>(i) = pose_(i, 3);
  }

  return pair<cv::Mat, cv::Mat>(rotation, translation);
}

std::pair<Eigen::Matrix3d, Eigen::Vector3d> Pose::toEigen() const
{
    Eigen::Matrix3d rotation_temp;
    Eigen::Vector3d translation_vect;
    for(auto i = 0; i < 3; ++i)
    {
        for(auto j = 0; j < 3; ++j)
        {
          rotation_temp(i,j) = pose_(i,j);
        }
        translation_vect(i) = pose_(i,3);
    }

    return pair<Eigen::Matrix3d, Eigen::Vector3d>(rotation_temp, translation_vect);
}

glm::mat4 Pose::toGL() const
{

    Eigen::Matrix4d Rx_180 = Eigen::Matrix<double, 4, 4>::Identity();
          Rx_180(1, 1) = -1.0;
          Rx_180(2, 2) = -1.0;
    auto pose = Rx_180 * pose_;

    glm::mat4 pose_glm;

    for(auto i = 0; i < 4; ++i)
    {
        for(auto j = 0; j < 4; ++j)
        {
          pose_glm[j][i] = pose(i,j);
        }
    }

    return pose_glm;
}

void Pose::transform(Eigen::Matrix4d& transform) { pose_ = transform * pose_; }

void Pose::transform(std::vector<double> &beta)
{

    Eigen::Matrix3d rot_view;
    rot_view = Eigen::AngleAxisd(beta.at(5), Eigen::Vector3d::UnitZ()) *
               Eigen::AngleAxisd(beta.at(4), Eigen::Vector3d::UnitY()) *
               Eigen::AngleAxisd(beta.at(3), Eigen::Vector3d::UnitX());

    Eigen::Matrix4d transform;

    for(auto i = 0; i < 3; ++i)
    {
        for(auto j = 0; j < 3; ++j)
        {
            transform(i,j) = rot_view(i,j);
        }
        transform(i,3) = beta[i];
        transform(3,i) = 0;
    }
    transform(3,3) = 1;

//    Eigen::Matrix4d Rx_180 = Eigen::Matrix<double, 4, 4>::Identity();
//    Rx_180(1, 1) = -1.0;
//    Rx_180(2, 2) = -1.0;

//    transform = Rx_180 * transform;

    pose_ = transform * pose_;
}

vector<double> Pose::getBeta() {
  Eigen::Matrix3d tmp_mat(3, 3);

  for (auto i = 0; i < 3; ++i) {
    for (auto j = 0; j < 3; ++j) tmp_mat(i, j) = pose_(i, j);
  }

  Eigen::Vector3d angles = tmp_mat.eulerAngles(2, 1, 0);

  vector<double> tmp = {pose_(0, 3), pose_(1, 3), pose_(2, 3),
                        angles(2),   angles(1),   angles(0)};

  return tmp;
}

vector<double> Pose::translation() const
{
    return vector<double>{pose_(0, 3), pose_(1, 3), pose_(2, 3)};
}

Eigen::Quaternionf Pose::rotation() const
{
    Eigen::Matrix3f tmp_mat(3, 3);

    for (auto i = 0; i < 3; ++i) {
      for (auto j = 0; j < 3; ++j) tmp_mat(i, j) = (float)pose_(i, j);
    }

    return Quaternionf(tmp_mat);
}

string Pose::str() const {
  stringstream ss;
  ss << fixed << setprecision(3);

  for (auto i = 0; i < 4; ++i) {
    for (auto j = 0; j < 4; ++j) {
      ss << pose_(i, j) << " ";
    }
    ss << "\n";
  }

  return ss.str();
}

}  // end namespace


// polyvec
#include <polyvec/geometry/winding_number.hpp>
#include <polyvec/mc/raster_image_connectivity.hpp>
#include <polyvec/mc/get_bounding_box.hpp>
#include <polyvec/mc/bit_vector.hpp>
#include <polyvec/mc/associate_holes.hpp>

// libc++
#include <cstdio>
#include <memory>

// eigen
#include <Eigen/Geometry>

NAMESPACE_BEGIN ( polyfit )
NAMESPACE_BEGIN ( mc )

constexpr int RasterImageConnectivity::infinite_region;

Eigen::Vector2i
RasterImageConnectivity::pixel_vertex_index_to_pos ( const int index ) {
    return Eigen::Vector2i ( index % pixel_vertex_dims().x(), index / pixel_vertex_dims().x() );
}

int
RasterImageConnectivity::pixel_vertex_pos_to_index ( const Eigen::Vector2i& pos ) {
    return pos.x() + pos.y() * pixel_vertex_dims().x();
}

int
RasterImageConnectivity::n_pixel_vertices() {
    return pixel_vertex_dims().x() * pixel_vertex_dims().y();
}

const Eigen::Vector2i&
RasterImageConnectivity::pixel_vertex_dims() const {
    return _pixel_vertex_dims;
}

Mesh_connectivity&
RasterImageConnectivity::connectivity() {
    return _connectivity;
}

int
RasterImageConnectivity::n_polygons() {
    return connectivity().n_active_faces();
}

int
RasterImageConnectivity::n_regions() {
    return _n_regions;
}

int
RasterImageConnectivity::n_region_polygons ( const int region_id ) {
    assert_break ( region_id < n_regions() );
    const int ans= _region_to_polygon_xval[region_id+1]-_region_to_polygon_xval[region_id];
    assert_break ( ans >= 1 );
    return ans;
}

int
RasterImageConnectivity::polygon_to_region ( const int polygon_id ) {
    assert_break ( polygon_id < n_polygons() );
    return _polygon_to_region[ polygon_id ];
}

void
RasterImageConnectivity::region_to_polygon ( const int region_id, int& outer_boundary, std::vector<int>& holes ) {
    outer_boundary = _region_to_polygon_val[ _region_to_polygon_xval[ region_id ] ];
    assert_break ( outer_boundary == region_id );

    holes.resize ( 0 );
    holes.reserve ( std::max ( 0, n_region_polygons ( region_id )-1 ) );

    for ( int j= _region_to_polygon_xval[ region_id ]+1 ; j < _region_to_polygon_xval[ region_id+1 ] ; ++j ) {
        holes.push_back (  _region_to_polygon_val [j] );
    }
}

RasterImageConnectivity::PolygonType
RasterImageConnectivity::get_polygon_type ( const int polygon_id ) {
    if ( polygon_id < n_regions() ) {
        return POLYGON_OUTER_BOUNDARY;
    } else if ( polygon_id < n_polygons() ) {
        return POLGYON_HOLE ;
    } else {
        assert_break ( 0 );
        return POLYGON_INVALID;
    }
}

Eigen::Matrix2Xi
RasterImageConnectivity::get_polygon_points ( const int pid ) {
    // Lambda to extract the points on polygon
    std::vector<int> points;

    Mesh_connectivity::Face_iterator face = connectivity().face_at ( pid );
    Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
    Mesh_connectivity::Half_edge_iterator he = he_end;

    int safe_guard = 0;
    const int safe_gaurd_cap = 10000;

    do {
        Eigen::Vector2i pos =  pixel_vertex_index_to_pos ( he.origin().index() );
        points.push_back ( pos.x() );
        points.push_back ( pos.y() );
        assert_break ( safe_guard < safe_gaurd_cap );
        ++safe_guard;
        he = he.next();
    } while ( !he.is_equal ( he_end ) );

    return Eigen::Matrix2Xi::ConstMapType ( points.data(), 2, points.size() /2 );
}

std::vector<int>
RasterImageConnectivity::regions_sorted_by_area() {
    std::vector<int> by_area(n_regions());
    std::vector<double> areas(n_regions());
    for (int i = 0; i < (int)by_area.size(); ++i) {
      by_area[i] = i;
      bool _1;
      Eigen::Matrix2Xi bdry = get_polygon_points(i);
      polyvec::WindingNumber::compute_orientation(bdry.cast<double>(), _1,
                                                  areas[i]);
    }
    std::sort(by_area.begin(), by_area.end(),
              [areas](int i, int j) { return areas[i] > areas[j]; });

    return by_area;
}

// dump as vtk for debugging
void
RasterImageConnectivity::dump_as_vtk ( const std::string filename ) {

    // Defrag the mesh
    Mesh_connectivity::Defragmentation_maps defrag;
    connectivity().compute_defragmention_maps ( defrag );

    FILE* fl = fopen ( filename.c_str(), "w" );
    assert_break ( fl && "FILE should be open" );

    /*
     * Write the vtk file.
     */

    // write the header
    fprintf ( fl, "# vtk DataFile Version 2.0\n" );
    fprintf ( fl, "Shayan's output mesh\n" );
    fprintf ( fl, "ASCII\n" );
    fprintf ( fl, "DATASET UNSTRUCTURED_GRID\n" );
    fprintf ( fl, "\n" );

    // write the vertices
    fprintf ( fl, "POINTS %d float\n", connectivity().n_active_vertices() );

    for ( int vnidx = 0; vnidx < connectivity().n_active_vertices(); vnidx++ ) {
        int voidx = defrag.new2old_vertices[vnidx];
        Eigen::Vector2i pos = pixel_vertex_index_to_pos ( voidx );
        fprintf ( fl, "%d %d 0 \n",  pos.x(), pos.y() );
    }

    fprintf ( fl, "\n" );

    //
    // write the faces
    //

    // count their total number of vertices.
    int total_vert_duplicated_per_face = 0;

    for ( int fn = 0; fn < connectivity().n_active_faces(); ++fn ) {
        int fo = defrag.new2old_faces[fn];
        Mesh_connectivity::Face_iterator face = connectivity().face_at ( fo );
        Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
        Mesh_connectivity::Half_edge_iterator he = face.half_edge();

        do {
            ++total_vert_duplicated_per_face;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );
    }

    int face_verts_cache[4096]; // this must be more than maximum number of edges per face.
    fprintf ( fl, "CELLS %d %d \n", connectivity().n_active_faces(), connectivity().n_active_faces() + total_vert_duplicated_per_face );

    for ( int fn = 0; fn < connectivity().n_active_faces(); ++fn ) {
        int fo = defrag.new2old_faces[fn];
        Mesh_connectivity::Face_iterator face = connectivity().face_at ( fo );
        int n_verts = 0;
        Mesh_connectivity::Half_edge_iterator he_end = face.half_edge();
        Mesh_connectivity::Half_edge_iterator he = face.half_edge();

        do {
            face_verts_cache[n_verts] = defrag.old2new_vertices[he.origin().index()];
            ++n_verts;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );

        fprintf ( fl, "%d ", n_verts );

        for ( int voffset = 0; voffset < n_verts; ++voffset ) {
            //
            fprintf ( fl, "%d ", face_verts_cache[voffset] );
        }

        fprintf ( fl, "\n" );
    }

    fprintf ( fl, "\n" );

    // write the face types
    fprintf ( fl, "CELL_TYPES %d \n", connectivity().n_active_faces() );

    for ( int f = 0; f < connectivity().n_active_faces(); f++ ) {
        fprintf ( fl, "7 \n" ); // VTK POLYGON
    }

    fprintf ( fl, "\n" );

    // Now write the pixel indices
    fprintf ( fl, "POINT_DATA %d \n", connectivity().n_active_vertices() );

//
    fprintf ( fl, "SCALARS %s int 1 \n", "pixel_point_index" );
    fprintf ( fl, "LOOKUP_TABLE default \n" );

    for ( int i = 0; i < connectivity().n_active_vertices(); i++ ) {
        int voidx = defrag.new2old_vertices[i];
        fprintf ( fl, "%d \n", voidx );
    }

// Write region colors?
    fprintf ( fl, "CELL_DATA %d \n", connectivity().n_active_faces() );

//
    fprintf ( fl, "SCALARS %s int 1 \n", "region_index" );
    fprintf ( fl, "LOOKUP_TABLE default \n" );

    for ( int i = 0; i < connectivity().n_active_faces(); i++ ) {
        fprintf ( fl, "%d \n", i );
    }

    fclose ( fl );
}

void
RasterImageConnectivity::enumerate_holes ( Mesh_connectivity& pixconn, std::vector<int>& half_edge_on_hole ) {
    assert_break ( pixconn.n_active_half_edges() == pixconn.n_total_half_edges() );
    assert_break ( pixconn.n_active_faces() == pixconn.n_total_faces() );

    // Start marking all the half-edges
    Bit_vector is_half_edge_marked ( pixconn.n_active_half_edges() );

    // A lambda to traverse a hole and mark all the half edges on it
    auto traverse_bounday = [&] ( const int seed_he ) {
        Mesh_connectivity::Half_edge_iterator he_end = pixconn.half_edge_at ( seed_he );
        Mesh_connectivity::Half_edge_iterator he = he_end;
        assert_break ( he.face().is_equal ( pixconn.hole() ) );

        int safe_guard = 0;
        const int safe_gaurd_cap = 10000;

        do {
            is_half_edge_marked.set ( he.index() );
            assert_break ( safe_guard < safe_gaurd_cap );
            ++safe_guard;
            he = he.next();
        } while ( !he.is_equal ( he_end ) );
    };

    // Loop over all half-edges, if not marked and boundary, it is a hole
    half_edge_on_hole.resize ( 0 );

    for ( int heid = 0 ; heid < pixconn.n_total_half_edges() ; ++heid ) {
        Mesh_connectivity::Half_edge_iterator he = pixconn.half_edge_at ( heid );

        if ( he.face().is_equal ( pixconn.hole() ) && ( !is_half_edge_marked[heid] ) ) {
            half_edge_on_hole.push_back ( heid );
            traverse_bounday ( heid );
        }
    }
}

// constructor
RasterImageConnectivity
RasterImageConnectivity::build ( const std::vector<Eigen::Matrix2Xi>& region_boundaries ) {

    RasterImageConnectivity ans;

    //
    // First find the bounding box of the image
    //
    {
        Eigen::AlignedBox<int, 2> bbox = get_bounding_box ( region_boundaries );
        // Make sure min is 0,0. This must have been done in get_pixel_regions()
        assert_break ( bbox.min() == Eigen::Vector2i ( 0, 0 ) );
        // Set the dims to the max
        ans._pixel_vertex_dims = bbox.max()+Eigen::Vector2i ( 1, 1 );
    }

    //
    // Now bould the connectivity by treating each region as a BIG polygon
    //
    {
        std::vector<int> polygon_verts;
        std::vector<int> polygon_xadj = {0};

        for ( int i = 0 ; i < ( int ) region_boundaries.size() ; ++i ) {
            polygon_xadj.push_back ( polygon_xadj.back() );
            const Eigen::Matrix2Xi& region_boundary = region_boundaries[i];

            for ( int j = 0 ; j < ( int ) region_boundary.cols() ; ++j ) {
                polygon_verts.push_back ( ans.pixel_vertex_pos_to_index ( region_boundary.col ( j ) ) );
                ++polygon_xadj.back();
            }
        }

        ans._connectivity.build_from_polygons ( ans.n_pixel_vertices(), polygon_verts, polygon_xadj );
    }

    //
    // Now get all the holes and add them
    // Even the outer boundary -- it should be fine
    //
    {
        Mesh_connectivity& pixconn = ans.connectivity();

        // Set number of regions
        ans._n_regions = ( int ) region_boundaries.size();

        // A lambda to modify the connectivity
        auto associate_face_and_he = [&] ( const int seed_he, const int parent_face ) {
            Mesh_connectivity::Half_edge_iterator he_end = pixconn.half_edge_at ( seed_he );
            Mesh_connectivity::Half_edge_iterator he = he_end;
            Mesh_connectivity::Face_iterator face = pixconn.face_at ( parent_face );
            assert_break ( he.face().is_equal ( pixconn.hole() ) );

            int safe_guard = 0;
            const int safe_gaurd_cap = 10000;

            // Make the association
            face.data().half_edge = he.index();

            do {
                he.data().face = face.index();
                assert_break ( safe_guard < safe_gaurd_cap );
                ++safe_guard;
                he = he.next();
            } while ( !he.is_equal ( he_end ) );
        };

        // Get all the holes
        std::vector<int> he_on_holes;
        enumerate_holes ( pixconn,  he_on_holes );

        for ( int heid: he_on_holes ) {
            Mesh_connectivity::Face_iterator face =  pixconn.add_face();
            associate_face_and_he ( heid, face.index() );
        }

    } // End of creating the hole polygons

    // Now associate holes and boundary (polygons to regions)
    {
        Mesh_connectivity& pixconn = ans.connectivity();

        // No extract the points
        // Get the holes and polygons as CCW paths
        // And also find the areas
        std::vector<Eigen::Matrix2Xd> outer_boundaries;
        std::vector<double> outer_boundary_areas;
        std::vector<Eigen::Matrix2Xd> holes;
        std::vector<double> hole_areas;

        for ( int i = 0 ; i < ans.n_regions() ; ++i ) {
            outer_boundaries.push_back ( ans.get_polygon_points ( i ).cast<double>() );
            bool is_ccw;
            double area;
            polyvec::WindingNumber::compute_orientation ( outer_boundaries.back(), is_ccw, area );
            assert_break ( is_ccw );
            outer_boundary_areas.push_back ( area );
        }

        for ( int i = ans.n_regions()  ; i < ans.n_polygons() ; ++i ) {
            holes.push_back ( ans.get_polygon_points ( i ).cast<double>() );
            holes.back().rowwise().reverseInPlace(); // make CCW
            bool is_ccw;
            double area;
            polyvec::WindingNumber::compute_orientation ( holes.back(), is_ccw, area );
            assert_break ( is_ccw );
            hole_areas.push_back ( area );
        }

        //
        // Create lambdas that say if a polygon is inside another
        //  assumes that out and in are both pixel boundaries
        // and are ccw
        // Only works if the area of out is bigger than in
        //
        auto is_inside = [&] ( const Eigen::Matrix2Xd &out, const Eigen::Matrix2Xd &in ) {
            const Eigen::Vector2d mid = (in.col ( 0 ) + in.col ( 1 )) /2.;
            const Eigen::Vector2d tang = in.col ( 1 ) -  in.col ( 0 );
            const Eigen::Vector2d normal ( -tang.y(), tang.x() );
            const Eigen::Vector2d definitely_inside_in = normal/2. + mid;

            bool is_trustable;
            double winding;
            ::polyvec::WindingNumber::compute_winding ( out, definitely_inside_in, winding, is_trustable );
            assert_break ( is_trustable );
            return std::abs ( winding ) > 1e-5;
        };

        // Now call the association function
        std::vector<std::vector<int>> boundary_holes;
        associate_holes (
            ( int ) outer_boundaries.size(),
            ( int ) holes.size(),
        [&] ( const int bout, const int hin ) {
            if ( ( int ) outer_boundary_areas[bout] <= ( int ) hole_areas[hin] ) {
                return false;
            } else {
                return  is_inside ( outer_boundaries[bout], holes[hin] );
            }
        },
        [&] ( const int hout, const int hin ) {
            if ( ( int ) hole_areas[hout] <= ( int ) hole_areas[hin] ) {
                return false;
            } else {
                return  is_inside ( holes[hout], holes[hin] );
            }
        },
        boundary_holes );

        // Now build the adj and xadj
        ans._polygon_to_region.resize ( ans.n_polygons(), infinite_region );
        ans._region_to_polygon_xval.resize ( 1, 0 );
        ans._region_to_polygon_val.resize ( 0 );

        for ( int i = 0 ; i < ans.n_regions() ; ++i ) {
            const int region_id = i;
            ans._region_to_polygon_xval.push_back ( ans._region_to_polygon_xval.back() );

            // Don't forget yourself :)
            ans._polygon_to_region[region_id] = region_id;
            ++ans._region_to_polygon_xval.back();
            ans._region_to_polygon_val.push_back ( region_id );

            for ( int  j = 0 ; j < ( int ) boundary_holes[region_id].size() ; ++j ) {
                const int polygon_id = ans.n_regions() + boundary_holes[region_id][j];
                ans._polygon_to_region[polygon_id] = region_id;
                ++ans._region_to_polygon_xval.back();
                ans._region_to_polygon_val.push_back ( polygon_id );
            }
        }
    } // ALL DONE WITH REGION HOLE ASSOCIATION

    return ans;
}

NAMESPACE_END ( mc )
NAMESPACE_END ( polyfit )


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

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.

// This file was modified by Oracle on 2015-2017.
// Modifications copyright (c) 2015-2017, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle
// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_SEGMENT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_SEGMENT_HPP

#include <cstddef>
#include <utility>

#include <boost/numeric/conversion/cast.hpp>

#include <boost/geometry/core/assert.hpp>
#include <boost/geometry/core/coordinate_system.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/srs.hpp>
#include <boost/geometry/core/point_type.hpp>
#include <boost/geometry/core/radian_access.hpp>
#include <boost/geometry/core/tags.hpp>

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

#include <boost/geometry/geometries/helper_geometry.hpp>

#include <boost/geometry/formulas/vertex_latitude.hpp>

#include <boost/geometry/algorithms/detail/assign_indexed_point.hpp>

#include <boost/geometry/algorithms/detail/envelope/point.hpp>
#include <boost/geometry/algorithms/detail/envelope/transform_units.hpp>

#include <boost/geometry/algorithms/detail/expand/point.hpp>

#include <boost/geometry/algorithms/dispatch/envelope.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace envelope
{

template <typename CalculationType, typename CS_Tag>
struct envelope_segment_call_vertex_latitude
{
    template <typename T1, typename T2, typename Strategy>
    static inline CalculationType apply(T1 const& lat1,
                                        T2 const& alp1,
                                        Strategy const& )
    {
        return geometry::formula::vertex_latitude<CalculationType, CS_Tag>
            ::apply(lat1, alp1);
    }
};

template <typename CalculationType>
struct envelope_segment_call_vertex_latitude<CalculationType, geographic_tag>
{
    template <typename T1, typename T2, typename Strategy>
    static inline CalculationType apply(T1 const& lat1,
                                        T2 const& alp1,
                                        Strategy const& strategy)
    {
        return geometry::formula::vertex_latitude<CalculationType, geographic_tag>
            ::apply(lat1, alp1, strategy.model());
    }
};

template <typename CS_Tag>
class envelope_segment_impl
{
private:

    // degrees or radians
    template <typename CalculationType>
    static inline void swap(CalculationType& lon1,
                            CalculationType& lat1,
                            CalculationType& lon2,
                            CalculationType& lat2)
    {
        std::swap(lon1, lon2);
        std::swap(lat1, lat2);
    }

    // radians
    template <typename CalculationType>
    static inline bool contains_pi_half(CalculationType const& a1,
                                        CalculationType const& a2)
    {
        // azimuths a1 and a2 are assumed to be in radians
        BOOST_GEOMETRY_ASSERT(! math::equals(a1, a2));

        static CalculationType const pi_half = math::half_pi<CalculationType>();

        return (a1 < a2)
                ? (a1 < pi_half && pi_half < a2)
                : (a1 > pi_half && pi_half > a2);
    }

    // radians or degrees
    template <typename Units, typename CoordinateType>
    static inline bool crosses_antimeridian(CoordinateType const& lon1,
                                            CoordinateType const& lon2)
    {
        typedef math::detail::constants_on_spheroid
            <
                CoordinateType, Units
            > constants;

        return math::abs(lon1 - lon2) > constants::half_period(); // > pi
    }

    // degrees or radians
    template <typename Units, typename CalculationType, typename Strategy>
    static inline void compute_box_corners(CalculationType& lon1,
                                           CalculationType& lat1,
                                           CalculationType& lon2,
                                           CalculationType& lat2,
                                           Strategy const& strategy)
    {
        // coordinates are assumed to be in radians
        BOOST_GEOMETRY_ASSERT(lon1 <= lon2);

        CalculationType lon1_rad = math::as_radian<Units>(lon1);
        CalculationType lat1_rad = math::as_radian<Units>(lat1);
        CalculationType lon2_rad = math::as_radian<Units>(lon2);
        CalculationType lat2_rad = math::as_radian<Units>(lat2);

        CalculationType a1, a2;
        strategy.apply(lon1_rad, lat1_rad, lon2_rad, lat2_rad, a1, a2);

        if (lat1 > lat2)
        {
            std::swap(lat1, lat2);
            std::swap(lat1_rad, lat2_rad);
            std::swap(a1, a2);
        }

        if (math::equals(a1, a2))
        {
            // the segment must lie on the equator or is very short
            return;
        }

        if (contains_pi_half(a1, a2))
        {
            CalculationType p_max = envelope_segment_call_vertex_latitude
                <CalculationType, CS_Tag>::apply(lat1_rad, a1, strategy);

            CalculationType const mid_lat = lat1 + lat2;
            if (mid_lat < 0)
            {
                // update using min latitude
                CalculationType const lat_min_rad = -p_max;
                CalculationType const lat_min
                    = math::from_radian<Units>(lat_min_rad);

                if (lat1 > lat_min)
                {
                    lat1 = lat_min;
                }
            }
            else if (mid_lat > 0)
            {
                // update using max latitude
                CalculationType const lat_max_rad = p_max;
                CalculationType const lat_max
                    = math::from_radian<Units>(lat_max_rad);

                if (lat2 < lat_max)
                {
                    lat2 = lat_max;
                }
            }
        }
    }

    template <typename Units, typename CalculationType, typename Strategy>
    static inline void apply(CalculationType& lon1,
                             CalculationType& lat1,
                             CalculationType& lon2,
                             CalculationType& lat2,
                             Strategy const& strategy)
    {
        typedef math::detail::constants_on_spheroid
            <
                CalculationType, Units
            > constants;

        bool is_pole1 = math::equals(math::abs(lat1), constants::max_latitude());
        bool is_pole2 = math::equals(math::abs(lat2), constants::max_latitude());

        if (is_pole1 && is_pole2)
        {
            // both points are poles; nothing more to do:
            // longitudes are already normalized to 0
            // but just in case
            lon1 = 0;
            lon2 = 0;
        }
        else if (is_pole1 && !is_pole2)
        {
            // first point is a pole, second point is not:
            // make the longitude of the first point the same as that
            // of the second point
            lon1 = lon2;
        }
        else if (!is_pole1 && is_pole2)
        {
            // second point is a pole, first point is not:
            // make the longitude of the second point the same as that
            // of the first point
            lon2 = lon1;
        }

        if (lon1 == lon2)
        {
            // segment lies on a meridian
            if (lat1 > lat2)
            {
                std::swap(lat1, lat2);
            }
            return;
        }

        BOOST_GEOMETRY_ASSERT(!is_pole1 && !is_pole2);

        if (lon1 > lon2)
        {
            swap(lon1, lat1, lon2, lat2);
        }

        if (crosses_antimeridian<Units>(lon1, lon2))
        {
            lon1 += constants::period();
            swap(lon1, lat1, lon2, lat2);
        }

        compute_box_corners<Units>(lon1, lat1, lon2, lat2, strategy);
    }

public:
    template <
            typename Units,
            typename CalculationType,
            typename Box,
            typename Strategy
            >
    static inline void apply(CalculationType lon1,
                             CalculationType lat1,
                             CalculationType lon2,
                             CalculationType lat2,
                             Box& mbr,
                             Strategy const& strategy)
    {
        typedef typename coordinate_type<Box>::type box_coordinate_type;

        typedef typename helper_geometry
            <
                Box, box_coordinate_type, Units
            >::type helper_box_type;

        helper_box_type radian_mbr;

        apply<Units>(lon1, lat1, lon2, lat2, strategy);

        geometry::set
            <
                min_corner, 0
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lon1));

        geometry::set
            <
                min_corner, 1
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lat1));

        geometry::set
            <
                max_corner, 0
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lon2));

        geometry::set
            <
                max_corner, 1
            >(radian_mbr, boost::numeric_cast<box_coordinate_type>(lat2));

        transform_units(radian_mbr, mbr);
    }
};

template <std::size_t Dimension, std::size_t DimensionCount>
struct envelope_one_segment
{
    template<typename Point, typename Box, typename Strategy>
    static inline void apply(Point const& p1,
                             Point const& p2,
                             Box& mbr,
                             Strategy const& strategy)
    {
        envelope_one_point<Dimension, DimensionCount>::apply(p1, mbr, strategy);
        detail::expand::point_loop
            <
                strategy::compare::default_strategy,
                strategy::compare::default_strategy,
                Dimension,
                DimensionCount
            >::apply(mbr, p2, strategy);
    }
};

template <std::size_t DimensionCount>
struct envelope_segment
{
    template <typename Point, typename Box, typename Strategy>
    static inline void apply(Point const& p1,
                             Point const& p2,
                             Box& mbr,
                             Strategy const& strategy)
    {
        // first compute the envelope range for the first two coordinates
        strategy.apply(p1, p2, mbr);

        // now compute the envelope range for coordinates of
        // dimension 2 and higher
        envelope_one_segment<2, DimensionCount>::apply(p1, p2, mbr, strategy);
    }

    template <typename Segment, typename Box>
    static inline void apply(Segment const& segment, Box& mbr)
    {
        typename point_type<Segment>::type p[2];
        detail::assign_point_from_index<0>(segment, p[0]);
        detail::assign_point_from_index<1>(segment, p[1]);
        apply(p[0], p[1], mbr);
    }
};

}} // namespace detail::envelope
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template <typename Segment>
struct envelope<Segment, segment_tag>
{
    template <typename Box, typename Strategy>
    static inline void apply(Segment const& segment,
                             Box& mbr,
                             Strategy const& strategy)
    {
        typename point_type<Segment>::type p[2];
        detail::assign_point_from_index<0>(segment, p[0]);
        detail::assign_point_from_index<1>(segment, p[1]);
        detail::envelope::envelope_segment
            <
               dimension<Segment>::value
            >::apply(p[0], p[1], mbr, strategy);
    }
};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_ENVELOPE_SEGMENT_HPP


///////////////////////////////////////////////////////////////////////////////
// marginal_cells.hpp                                                        //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_MARGINAL_CELLS_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_MARGINAL_CELLS_HPP_ER_2010
#include <boost/mpl/fold.hpp>
#include <boost/mpl/vector/vector0.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/mpl/push_back.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/cells.hpp>

namespace boost {
namespace statistics{
namespace detail{
namespace contingency_table{

    template<typename Keys>
    struct marginal_cells : boost::mpl::fold<
        Keys,
        boost::mpl::vector0<>,
        boost::mpl::push_back<
            boost::mpl::_1,
            contingency_table::tag::cells<
                boost::mpl::vector1<boost::mpl::_2>
            >
        >
    >{};

}// contingency_table
}// detail
}// statistics
}// boost

#endif