/**
* This file is part of ORB-SLAM3
*
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
*
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>.
*/

#include "KannalaBrandt8.h"

#include <boost/serialization/export.hpp>

//BOOST_CLASS_EXPORT_IMPLEMENT(ORB_SLAM3::KannalaBrandt8)

namespace ORB_SLAM3 {
//BOOST_CLASS_EXPORT_GUID(KannalaBrandt8, "KannalaBrandt8")

    cv::Point2f KannalaBrandt8::project(const cv::Point3f &p3D) {
        const float x2_plus_y2 = p3D.x * p3D.x + p3D.y * p3D.y;
        const float theta = atan2f(sqrtf(x2_plus_y2), p3D.z);
        const float psi = atan2f(p3D.y, p3D.x);

        const float theta2 = theta * theta;
        const float theta3 = theta * theta2;
        const float theta5 = theta3 * theta2;
        const float theta7 = theta5 * theta2;
        const float theta9 = theta7 * theta2;
        const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                        + mvParameters[6] * theta7 + mvParameters[7] * theta9;

        return cv::Point2f(mvParameters[0] * r * cos(psi) + mvParameters[2],
                           mvParameters[1] * r * sin(psi) + mvParameters[3]);

    }

    Eigen::Vector2d KannalaBrandt8::project(const Eigen::Vector3d &v3D) {
        const double x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
        const double theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
        const double psi = atan2f(v3D[1], v3D[0]);

        const double theta2 = theta * theta;
        const double theta3 = theta * theta2;
        const double theta5 = theta3 * theta2;
        const double theta7 = theta5 * theta2;
        const double theta9 = theta7 * theta2;
        const double r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                        + mvParameters[6] * theta7 + mvParameters[7] * theta9;

        Eigen::Vector2d res;
        res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2];
        res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];

        return res;

    }

    Eigen::Vector2f KannalaBrandt8::project(const Eigen::Vector3f &v3D) {
        const float x2_plus_y2 = v3D[0] * v3D[0] + v3D[1] * v3D[1];
        const float theta = atan2f(sqrtf(x2_plus_y2), v3D[2]);
        const float psi = atan2f(v3D[1], v3D[0]);

        const float theta2 = theta * theta;
        const float theta3 = theta * theta2;
        const float theta5 = theta3 * theta2;
        const float theta7 = theta5 * theta2;
        const float theta9 = theta7 * theta2;
        const float r = theta + mvParameters[4] * theta3 + mvParameters[5] * theta5
                         + mvParameters[6] * theta7 + mvParameters[7] * theta9;

        Eigen::Vector2f res;
        res[0] = mvParameters[0] * r * cos(psi) + mvParameters[2];
        res[1] = mvParameters[1] * r * sin(psi) + mvParameters[3];

        return res;

        /*cv::Point2f cvres = this->project(cv::Point3f(v3D[0],v3D[1],v3D[2]));

        Eigen::Vector2d res;
        res[0] = cvres.x;
        res[1] = cvres.y;

        return res;*/
    }

    Eigen::Vector2f KannalaBrandt8::projectMat(const cv::Point3f &p3D) {
        cv::Point2f point = this->project(p3D);
        return Eigen::Vector2f(point.x, point.y);
    }

    float KannalaBrandt8::uncertainty2(const Eigen::Matrix<double,2,1> &p2D)
    {
        /*Eigen::Matrix<double,2,1> c;
        c << mvParameters[2], mvParameters[3];
        if ((p2D-c).squaredNorm()>57600) // 240*240 (256)
            return 100.f;
        else
            return 1.0f;*/
        return 1.f;
    }

    Eigen::Vector3f KannalaBrandt8::unprojectEig(const cv::Point2f &p2D) {
        cv::Point3f ray = this->unproject(p2D);
        return Eigen::Vector3f(ray.x, ray.y, ray.z);
    }

    cv::Point3f KannalaBrandt8::unproject(const cv::Point2f &p2D) {
        //Use Newthon method to solve for theta with good precision (err ~ e-6)
        cv::Point2f pw((p2D.x - mvParameters[2]) / mvParameters[0], (p2D.y - mvParameters[3]) / mvParameters[1]);
        float scale = 1.f;
        float theta_d = sqrtf(pw.x * pw.x + pw.y * pw.y);
        theta_d = fminf(fmaxf(-CV_PI / 2.f, theta_d), CV_PI / 2.f);

        if (theta_d > 1e-8) {
            //Compensate distortion iteratively
            float theta = theta_d;

            for (int j = 0; j < 10; j++) {
                float theta2 = theta * theta, theta4 = theta2 * theta2, theta6 = theta4 * theta2, theta8 =
                        theta4 * theta4;
                float k0_theta2 = mvParameters[4] * theta2, k1_theta4 = mvParameters[5] * theta4;
                float k2_theta6 = mvParameters[6] * theta6, k3_theta8 = mvParameters[7] * theta8;
                float theta_fix = (theta * (1 + k0_theta2 + k1_theta4 + k2_theta6 + k3_theta8) - theta_d) /
                                  (1 + 3 * k0_theta2 + 5 * k1_theta4 + 7 * k2_theta6 + 9 * k3_theta8);
                theta = theta - theta_fix;
                if (fabsf(theta_fix) < precision)
                    break;
            }
            //scale = theta - theta_d;
            scale = std::tan(theta) / theta_d;
        }

        return cv::Point3f(pw.x * scale, pw.y * scale, 1.f);
    }

    Eigen::Matrix<double, 2, 3> KannalaBrandt8::projectJac(const Eigen::Vector3d &v3D) {
        double x2 = v3D[0] * v3D[0], y2 = v3D[1] * v3D[1], z2 = v3D[2] * v3D[2];
        double r2 = x2 + y2;
        double r = sqrt(r2);
        double r3 = r2 * r;
        double theta = atan2(r, v3D[2]);

        double theta2 = theta * theta, theta3 = theta2 * theta;
        double theta4 = theta2 * theta2, theta5 = theta4 * theta;
        double theta6 = theta2 * theta4, theta7 = theta6 * theta;
        double theta8 = theta4 * theta4, theta9 = theta8 * theta;

        double f = theta + theta3 * mvParameters[4] + theta5 * mvParameters[5] + theta7 * mvParameters[6] +
                  theta9 * mvParameters[7];
        double fd = 1 + 3 * mvParameters[4] * theta2 + 5 * mvParameters[5] * theta4 + 7 * mvParameters[6] * theta6 +
                   9 * mvParameters[7] * theta8;

        Eigen::Matrix<double, 2, 3> JacGood;
        JacGood(0, 0) = mvParameters[0] * (fd * v3D[2] * x2 / (r2 * (r2 + z2)) + f * y2 / r3);
        JacGood(1, 0) =
                mvParameters[1] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3);

        JacGood(0, 1) =
                mvParameters[0] * (fd * v3D[2] * v3D[1] * v3D[0] / (r2 * (r2 + z2)) - f * v3D[1] * v3D[0] / r3);
        JacGood(1, 1) = mvParameters[1] * (fd * v3D[2] * y2 / (r2 * (r2 + z2)) + f * x2 / r3);

        JacGood(0, 2) = -mvParameters[0] * fd * v3D[0] / (r2 + z2);
        JacGood(1, 2) = -mvParameters[1] * fd * v3D[1] / (r2 + z2);

        return JacGood;
    }

    bool KannalaBrandt8::ReconstructWithTwoViews(const std::vector<cv::KeyPoint>& vKeys1, const std::vector<cv::KeyPoint>& vKeys2, const std::vector<int> &vMatches12,
                                          Sophus::SE3f &T21, std::vector<cv::Point3f> &vP3D, std::vector<bool> &vbTriangulated){
        if(!tvr){
            Eigen::Matrix3f K = this->toK_();
            tvr = new TwoViewReconstruction(K);
        }

        //Correct FishEye distortion
        std::vector<cv::KeyPoint> vKeysUn1 = vKeys1, vKeysUn2 = vKeys2;
        std::vector<cv::Point2f> vPts1(vKeys1.size()), vPts2(vKeys2.size());

        for(size_t i = 0; i < vKeys1.size(); i++) vPts1[i] = vKeys1[i].pt;
        for(size_t i = 0; i < vKeys2.size(); i++) vPts2[i] = vKeys2[i].pt;

        cv::Mat D = (cv::Mat_<float>(4,1) << mvParameters[4], mvParameters[5], mvParameters[6], mvParameters[7]);
        cv::Mat R = cv::Mat::eye(3,3,CV_32F);
        cv::Mat K = this->toK();
        cv::fisheye::undistortPoints(vPts1,vPts1,K,D,R,K);
        cv::fisheye::undistortPoints(vPts2,vPts2,K,D,R,K);

        for(size_t i = 0; i < vKeys1.size(); i++) vKeysUn1[i].pt = vPts1[i];
        for(size_t i = 0; i < vKeys2.size(); i++) vKeysUn2[i].pt = vPts2[i];

        return tvr->Reconstruct(vKeysUn1,vKeysUn2,vMatches12,T21,vP3D,vbTriangulated);
    }

    cv::Mat KannalaBrandt8::toK() {
        cv::Mat K = (cv::Mat_<float>(3, 3)
                << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f);
        return K;
    }
    Eigen::Matrix3f KannalaBrandt8::toK_() {
        Eigen::Matrix3f K;
        K << mvParameters[0], 0.f, mvParameters[2], 0.f, mvParameters[1], mvParameters[3], 0.f, 0.f, 1.f;
        return K;
    }

    bool KannalaBrandt8::epipolarConstrain(GeometricCamera* pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2,
                                           const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc) {
        Eigen::Vector3f p3D;
        return this->TriangulateMatches(pCamera2,kp1,kp2,R12,t12,sigmaLevel,unc,p3D) > 0.0001f;
    }

    bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint& kp1, const cv::KeyPoint& kp2, GeometricCamera* pOther,
                                             Sophus::SE3f& Tcw1, Sophus::SE3f& Tcw2,
                                             const float sigmaLevel1, const float sigmaLevel2,
                                             Eigen::Vector3f& x3Dtriangulated){
        Eigen::Matrix<float,3,4> eigTcw1 = Tcw1.matrix3x4();
        Eigen::Matrix3f Rcw1 = eigTcw1.block<3,3>(0,0);
        Eigen::Matrix3f Rwc1 = Rcw1.transpose();
        Eigen::Matrix<float,3,4> eigTcw2 = Tcw2.matrix3x4();
        Eigen::Matrix3f Rcw2 = eigTcw2.block<3,3>(0,0);
        Eigen::Matrix3f Rwc2 = Rcw2.transpose();

        cv::Point3f ray1c = this->unproject(kp1.pt);
        cv::Point3f ray2c = pOther->unproject(kp2.pt);

        Eigen::Vector3f r1(ray1c.x, ray1c.y, ray1c.z);
        Eigen::Vector3f r2(ray2c.x, ray2c.y, ray2c.z);

        //Check parallax between rays
        Eigen::Vector3f ray1 = Rwc1 * r1;
        Eigen::Vector3f ray2 = Rwc2 * r2;

        const float cosParallaxRays = ray1.dot(ray2)/(ray1.norm() * ray2.norm());

        //If parallax is lower than 0.9998, reject this match
        if(cosParallaxRays > 0.9998){
            return false;
        }

        //Parallax is good, so we try to triangulate
        cv::Point2f p11,p22;

        p11.x = ray1c.x;
        p11.y = ray1c.y;

        p22.x = ray2c.x;
        p22.y = ray2c.y;

        Eigen::Vector3f x3D;

        Triangulate(p11,p22,eigTcw1,eigTcw2,x3D);

        //Check triangulation in front of cameras
        float z1 = Rcw1.row(2).dot(x3D)+Tcw1.translation()(2);
        if(z1<=0){  //Point is not in front of the first camera
            return false;
        }

        float z2 = Rcw2.row(2).dot(x3D)+Tcw2.translation()(2);
        if(z2<=0){ //Point is not in front of the first camera
            return false;
        }

        //Check reprojection error in first keyframe
        //  -Transform point into camera reference system
        Eigen::Vector3f x3D1 = Rcw1 * x3D + Tcw1.translation();
        Eigen::Vector2f uv1 = this->project(x3D1);

        float errX1 = uv1(0) - kp1.pt.x;
        float errY1 = uv1(1) - kp1.pt.y;

        if((errX1*errX1+errY1*errY1)>5.991*sigmaLevel1){   //Reprojection error is high
            return false;
        }

        //Check reprojection error in second keyframe;
        //  -Transform point into camera reference system
        Eigen::Vector3f x3D2 = Rcw2 * x3D + Tcw2.translation(); // avoid using q
        Eigen::Vector2f uv2 = pOther->project(x3D2);

        float errX2 = uv2(0) - kp2.pt.x;
        float errY2 = uv2(1) - kp2.pt.y;

        if((errX2*errX2+errY2*errY2)>5.991*sigmaLevel2){   //Reprojection error is high
            return false;
        }

        //Since parallax is big enough and reprojection errors are low, this pair of points
        //can be considered as a match
        x3Dtriangulated = x3D;

        return true;
    }

    float KannalaBrandt8::TriangulateMatches(GeometricCamera *pCamera2, const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const Eigen::Matrix3f& R12, const Eigen::Vector3f& t12, const float sigmaLevel, const float unc, Eigen::Vector3f& p3D) {

        Eigen::Vector3f r1 = this->unprojectEig(kp1.pt);
        Eigen::Vector3f r2 = pCamera2->unprojectEig(kp2.pt);

        //Check parallax
        Eigen::Vector3f r21 = R12 * r2;

        const float cosParallaxRays = r1.dot(r21)/(r1.norm() *r21.norm());

        if(cosParallaxRays > 0.9998){
            return -1;
        }

        //Parallax is good, so we try to triangulate
        cv::Point2f p11,p22;

        p11.x = r1[0];
        p11.y = r1[1];

        p22.x = r2[0];
        p22.y = r2[1];

        Eigen::Vector3f x3D;
        Eigen::Matrix<float,3,4> Tcw1;
        Tcw1 << Eigen::Matrix3f::Identity(), Eigen::Vector3f::Zero();

        Eigen::Matrix<float,3,4> Tcw2;

        Eigen::Matrix3f R21 = R12.transpose();
        Tcw2 << R21, -R21 * t12;

        Triangulate(p11,p22,Tcw1,Tcw2,x3D);
        // cv::Mat x3Dt = x3D.t();

        float z1 = x3D(2);
        if(z1 <= 0){
            return -2;
        }

        float z2 = R21.row(2).dot(x3D)+Tcw2(2,3);
        if(z2<=0){
            return -3;
        }

        //Check reprojection error
        Eigen::Vector2f uv1 = this->project(x3D);

        float errX1 = uv1(0) - kp1.pt.x;
        float errY1 = uv1(1) - kp1.pt.y;

        if((errX1*errX1+errY1*errY1)>5.991 * sigmaLevel){   //Reprojection error is high
            return -4;
        }

        Eigen::Vector3f x3D2 = R21 * x3D + Tcw2.col(3);
        Eigen::Vector2f uv2 = pCamera2->project(x3D2);

        float errX2 = uv2(0) - kp2.pt.x;
        float errY2 = uv2(1) - kp2.pt.y;

        if((errX2*errX2+errY2*errY2)>5.991 * unc){   //Reprojection error is high
            return -5;
        }

        p3D = x3D;

        return z1;
    }

    std::ostream & operator<<(std::ostream &os, const KannalaBrandt8 &kb) {
        os << kb.mvParameters[0] << " " << kb.mvParameters[1] << " " << kb.mvParameters[2] << " " << kb.mvParameters[3] << " "
           << kb.mvParameters[4] << " " << kb.mvParameters[5] << " " << kb.mvParameters[6] << " " << kb.mvParameters[7];
        return os;
    }

    std::istream & operator>>(std::istream &is, KannalaBrandt8 &kb) {
        float nextParam;
        for(size_t i = 0; i < 8; i++){
            assert(is.good());  //Make sure the input stream is good
            is >> nextParam;
            kb.mvParameters[i] = nextParam;

        }
        return is;
    }

    void KannalaBrandt8::Triangulate(const cv::Point2f &p1, const cv::Point2f &p2, const Eigen::Matrix<float,3,4> &Tcw1,
                                     const Eigen::Matrix<float,3,4> &Tcw2, Eigen::Vector3f &x3D)
    {
        Eigen::Matrix<float,4,4> A;
        A.row(0) = p1.x*Tcw1.row(2)-Tcw1.row(0);
        A.row(1) = p1.y*Tcw1.row(2)-Tcw1.row(1);
        A.row(2) = p2.x*Tcw2.row(2)-Tcw2.row(0);
        A.row(3) = p2.y*Tcw2.row(2)-Tcw2.row(1);

        Eigen::JacobiSVD<Eigen::Matrix4f> svd(A, Eigen::ComputeFullV);
        Eigen::Vector4f x3Dh = svd.matrixV().col(3);
        x3D = x3Dh.head(3)/x3Dh(3);
    }

    bool KannalaBrandt8::IsEqual(GeometricCamera* pCam)
    {
        if(pCam->GetType() != GeometricCamera::CAM_FISHEYE)
            return false;

        KannalaBrandt8* pKBCam = (KannalaBrandt8*) pCam;

        if(abs(precision - pKBCam->GetPrecision()) > 1e-6)
            return false;

        if(size() != pKBCam->size())
            return false;

        bool is_same_camera = true;
        for(size_t i=0; i<size(); ++i)
        {
            if(abs(mvParameters[i] - pKBCam->getParameter(i)) > 1e-6)
            {
                is_same_camera = false;
                break;
            }
        }
        return is_same_camera;
    }

}


#ifndef DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP
#define DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP

#include <descartes_light/bgl/bgl_dijkstra_solver.h>
#include <descartes_light/bgl/impl/event_visitors.hpp>

#include <descartes_light/descartes_macros.h>
DESCARTES_IGNORE_WARNINGS_PUSH
#include <boost/graph/dijkstra_shortest_paths.hpp>
DESCARTES_IGNORE_WARNINGS_POP

namespace descartes_light
{
template <typename FloatType, typename Visitors>
static VertexDesc<FloatType> solveDijkstra(BGLGraph<FloatType>& graph,
                                           std::vector<VertexDesc<FloatType>>& predecessors,
                                           const VertexDesc<FloatType>& source,
                                           const Visitors& event_visitors,
                                           const std::vector<std::vector<VertexDesc<FloatType>>>& ladder_rungs)
{
  // Internal properties
  auto index_prop_map = boost::get(boost::vertex_index, graph);
  auto weight_prop_map = boost::get(boost::edge_weight, graph);
  auto color_prop_map = boost::get(&Vertex<FloatType>::color, graph);
  auto distance_prop_map = boost::get(&Vertex<FloatType>::distance, graph);

  predecessors.resize(boost::num_vertices(graph), std::numeric_limits<std::size_t>::max());

  typedef typename boost::property_map<BGLGraph<FloatType>, boost::vertex_index_t>::type IndexMap;
  typedef boost::iterator_property_map<typename std::vector<VertexDesc<FloatType>>::iterator, IndexMap> PredecessorMap;
  PredecessorMap predecessor_it_map = boost::make_iterator_property_map(predecessors.begin(), index_prop_map);

  auto visitor = boost::make_dijkstra_visitor(event_visitors);

  // Perform the search
  try
  {
    boost::dijkstra_shortest_paths(graph,
                                   source,
                                   predecessor_it_map,
                                   distance_prop_map,
                                   weight_prop_map,
                                   index_prop_map,
                                   std::less<>(),
                                   std::plus<>(),
                                   std::numeric_limits<FloatType>::max(),
                                   static_cast<FloatType>(0.0),
                                   visitor,
                                   color_prop_map);

    // In the case that the visitor does not throw the target vertex descriptor, find the lowest cost vertex in last
    // rung of the ladder graph
    auto target = std::min_element(ladder_rungs.back().begin(),
                                   ladder_rungs.back().end(),
                                   [&](const VertexDesc<FloatType>& a, const VertexDesc<FloatType>& b) {
                                     return graph[a].distance < graph[b].distance;
                                   });

    // Check that the identified lowest cost vertex is valid and has a cost less than inf
    if (target != ladder_rungs.back().end() && graph[*target].distance < std::numeric_limits<FloatType>::max())
      throw *target;
  }
  catch (const VertexDesc<FloatType>& target)
  {
    return target;
  }

  // If the visitor never threw the vertex descriptor, there was an issue with the search
  throw std::runtime_error("Search failed to encounter vertex associated with the last waypoint in the trajectory");
}

template <typename FloatType, typename Visitors>
BGLDijkstraSVSESolver<FloatType, Visitors>::BGLDijkstraSVSESolver(Visitors event_visitors, unsigned num_threads)
  : BGLSolverBaseSVSE<FloatType>(num_threads), event_visitors_(std::move(event_visitors))
{
}

template <typename FloatType, typename Visitors>
SearchResult<FloatType> BGLDijkstraSVSESolver<FloatType, Visitors>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  VertexDesc<FloatType> target =
      solveDijkstra<FloatType, Visitors>(graph_, predecessors_, source_, event_visitors_, ladder_rungs_);

  SearchResult<FloatType> result;

  // Reconstruct the path from the predecesor map; remove the artificial start state
  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
  result.trajectory.erase(result.trajectory.begin());

  result.cost = graph_[target].distance;

  return result;
}

template <typename FloatType, typename Visitors>
BGLDijkstraSVDESolver<FloatType, Visitors>::BGLDijkstraSVDESolver(Visitors event_visitors, unsigned num_threads)
  : BGLSolverBaseSVDE<FloatType>(num_threads), event_visitors_(std::move(event_visitors))
{
}

template <typename FloatType, typename Visitors>
SearchResult<FloatType> BGLDijkstraSVDESolver<FloatType, Visitors>::search()
{
  // Convenience aliases
  auto& graph_ = BGLSolverBase<FloatType>::graph_;
  const auto& source_ = BGLSolverBase<FloatType>::source_;
  auto& predecessors_ = BGLSolverBase<FloatType>::predecessors_;
  const auto& ladder_rungs_ = BGLSolverBase<FloatType>::ladder_rungs_;

  // Create the dynamic edge adding event visitor
  const auto& edge_eval_ = BGLSolverBaseSVDE<FloatType>::edge_eval_;
  auto vis = std::make_pair(add_all_edges_dynamically<FloatType, boost::on_examine_vertex>(edge_eval_, ladder_rungs_),
                            event_visitors_);

  VertexDesc<FloatType> target = solveDijkstra(graph_, predecessors_, source_, vis, ladder_rungs_);

  SearchResult<FloatType> result;

  // Reconstruct the path from the predecesor map; remove the artificial start state
  const auto vd_path = BGLSolverBase<FloatType>::reconstructPath(source_, target);
  result.trajectory = BGLSolverBase<FloatType>::toStates(vd_path);
  result.trajectory.erase(result.trajectory.begin());

  result.cost = graph_[target].distance;

  return result;
}

}  // namespace descartes_light

#endif  // DESCARTES_LIGHT_SOLVERS_BGL_IMPL_BGL_DIJKSTRA_SOLVER_HPP


#include "tracer.hpp"
#include "initial_conditions/initial_conditions.hpp"
#include "ode_state.hpp"
#include "dynamics/ray_dynamics.hpp"
#include <future>
#include <chrono>

#include <boost/numeric/odeint/integrate/integrate_const.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta4.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_cash_karp54.hpp>
#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>

#include <boost/range/combine.hpp>
#include <boost/range/algorithm/min_element.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/numeric/odeint/stepper/euler.hpp>
#include "observers/energy_error_observer.hpp"
#include <utility>

Tracer::Tracer( const Potential& pot, std::shared_ptr<RayDynamics> dynamics ) :
	mDimension( pot.getDimension() ),
	mSupport( pot.getSupport() ),
	mExtents( pot.getExtents() ),
	mDynamics( std::move(dynamics) ),
    mMasterObserver( pot.getDimension(), mDynamics ),
	mEnergyErrorObs(std::make_shared<EnergyErrorObserver>())
{
	using namespace boost::range;
	using namespace boost::adaptors;

	// we choose initial step according to the potential length
	// scaling = size / support, i.e. dt = support/size

	// cannot use a lambda as older boost looks for member result_type
	struct ratio
	{
		typedef double result_type;
		double operator()(const boost::tuple<double, std::size_t>& t) const { return boost::get<0>(t) / boost::get<1>(t); }
	};

	auto ratios = combine(pot.getSupport(), pot.getExtents()) | transformed(ratio());
	mInitialDeltaT = *min_element(ratios);

	mMasterObserver.addObserverObject(mEnergyErrorObs);
}

Tracer::~Tracer() = default;

void Tracer::setErrorBounds( double abs_err, double rel_err )
{
	mAbsErrorBound = abs_err;
	mRelErrorBound = rel_err;
}

void Tracer::addObserver( obs_type observer )
{
	mMasterObserver.addObserverObject(std::move(observer));
}

TraceResult Tracer::trace(InitCondGenPtr& incoming_wave, InitialConditionConfiguration config)
{
	// fix coordinate transformation for initial condition
	auto support = mSupport;
	gen_vect offset(mDimension);
	for(unsigned i = 0; i < mDimension; ++i)
	{
		offset[i] = mSupport[i] / mExtents[i];
		support[i] -= 2*offset[i];
	}
    config.setDynamics( mDynamics ).setSupport( support ).setOffset(offset);
	incoming_wave->init( config );

	// set up master observer
	mMasterObserver.setPeriodicBoundaries( mDynamics->hasPeriodicBoundary() );
	mMasterObserver.startTracing( );

	unsigned int threadcount = std::min(mMaxThreads, (std::size_t)std::thread::hardware_concurrency());
	#ifndef NDEBUG
	std::cout << "distribute computation to " << threadcount << " threads\n";
	#endif

	std::vector<std::future<void>> threads;
	auto tf = [this](InitCondGenPtr w, bool is_printer)
	{
		return this->traceThreadFunction( std::move(w), is_printer );
	};

	for(unsigned i = 0; i < threadcount; ++i)
	{
		// start threads, only thread zero prints progress
		threads.push_back( std::async ( std::launch::async, tf, incoming_wave, i == 0) );
	}

	for( auto& f : threads)
		f.get();

	mMasterObserver.finishTracing();

	return TraceResult{mEnergyErrorObs->getMaximumError(), mEnergyErrorObs->getMeanError(), getTracedParticleCount()};
}

void Tracer::traceThreadFunction( InitCondGenPtr incoming_wave, bool printer )
{
	// check that dynamcis are set
	if(!mDynamics)
		THROW_EXCEPTION( std::runtime_error, "Cannot perform tracing when no dynamics are set!" );

	// setup types for boost odeint solver
	using namespace boost::numeric::odeint;

	if(mIntegrator == Integrator::RUNGE_KUTTA_CASH_KARP_54_ADAPTIVE) {
		typedef runge_kutta_cash_karp54<GState> error_stepper_type;
		typedef controlled_runge_kutta<error_stepper_type> controlled_stepper_type;
		typedef default_error_checker<double, range_algebra, default_operations> error_checker_type;

		// setup the integrator once!
		controlled_stepper_type stepper(error_checker_type(mAbsErrorBound, mRelErrorBound));

		traceThreadFunction_imp(stepper, incoming_wave, printer);
	} else if (mIntegrator == Integrator::EULER_CONST) {
		typedef euler<GState> stepper_type;

		// setup the integrator once!
		stepper_type stepper{};

		traceThreadFunction_imp(stepper, incoming_wave, printer);
	}
}

template<class T>
void Tracer::traceThreadFunction_imp( T&& stepper, InitCondGenPtr incoming_wave, bool printer )
{
	MasterObserver thread_observer( mMasterObserver.clone() );
	InitialCondition incoming = incoming_wave->next();

	GState p(mDimension, mDynamics->hasMonodromy());
	auto last_time = std::chrono::steady_clock::now();

	for(std::size_t i = 0; incoming; ++i, ++incoming)
	{

		if(printer && (std::chrono::steady_clock::now() - last_time) > std::chrono::seconds(10))
		{
		    last_time = std::chrono::steady_clock::now();
			std::cout << "integrate " << thread_observer.getTracedParticleCount() <<  " \n";
		}

		// generate initial condition and set up state
		p.position() = incoming.getState().getPosition();
		p.velocity() = incoming.getState().getVelocity();
		if( mDynamics->hasMonodromy() )
			p.init_monodromy();

		// notify the observer
		thread_observer.startTrajectory( incoming );

		try
		{
			/// \todo this can return... do we want to do sth with the return value?
			boost::numeric::odeint::integrate_const(
					std::ref(stepper),
					[this](const GState& s, GState& d, double t) { mDynamics->stateUpdate(s, d, t); },
					p,
					0.0, 				// start time
					mEndTime, 			// end time
					mInitialDeltaT, 	// initial time step
					std::ref(thread_observer)
			);
		} catch(int& i) {};

		thread_observer.finishTrajectory( incoming );
	}
}

void Tracer::setTimeStep(double dt)
{
	mInitialDeltaT = dt;
}

std::size_t Tracer::getDimension() const
{
	return mDimension;
}

void Tracer::setMaxThreads( std::size_t threads )
{
	if( threads == 0 )
		threads = 1;
	mMaxThreads = threads;
}

std::size_t Tracer::getMaxThreads( ) const
{
	return mMaxThreads;
}

void Tracer::setIntegrator(Integrator integrator)
{
    mIntegrator = integrator;
}

const std::vector<std::shared_ptr<Observer>>& Tracer::getObservers() const
{
	return mMasterObserver.getObservers();
}

void Tracer::setEndTime( double et )
{
	mEndTime = et;
}

double Tracer::getEndTime() const
{
	return mEndTime;
}

std::size_t Tracer::getTracedParticleCount() const
{
	return mMasterObserver.getTracedParticleCount();
}


// The configure script should define NTL_FP_CONTRACT_OFF
// for icc via the NOCONTRACT variable
#ifdef NTL_FP_CONTRACT_OFF
#pragma fp_contract(off)
#endif

#include <NTL/tools.h>

#ifdef NTL_ENABLE_AVX_FFT

// The configure script tries to prevent this, but we
// double check here.  Note that while it is strongly
// discouraged, other parts of NTL probably work even with
// "fast math"; however, quad_float will definitely break.

#if (defined(__GNUC__) && __FAST_MATH__)
#error "do not compile pd_FFT.cpp with -ffast-math!!"
#endif

#include <NTL/PD.h>
#include <NTL/pd_FFT.h>
#include <NTL/FFT_impl.h>

#if (defined(__GNUC__) && __FAST_MATH__)
#error "do not compile pd_FFT.cpp with -ffast-math!!"
#endif

#if (NTL_FMA_DETECTED && !defined(NTL_CONTRACTION_FIXED))
#error "contraction not fixed"
#endif

NTL_START_IMPL

#define NTL_CSR_NEAREST (0x00000000)
#define NTL_CSR_DOWN    (0x00002000)
#define NTL_CSR_UP      (0x00004000)
#define NTL_CSR_TRUNC   (0x00006000)
#define NTL_CSR_MASK    (0x00006000)

CSRPush::CSRPush()
{
   // save current register value
   reg = _mm_getcsr();
   // set rounding mode to "down"
   _mm_setcsr((reg & ~NTL_CSR_MASK) | NTL_CSR_DOWN);
}

CSRPush::~CSRPush()
{
   _mm_setcsr(reg);
}

void
pd_LazyPrepMulModPrecon_impl(double *bninv, const double *b, double n, long len)
{
   for (long i = 0; i < len; i++) bninv[i] = b[i]/n;
}

template<class pd> pd
pd_LazyReduce1(pd a, double q)
{
   return correct_excess(a, q);
}

template<class pd> pd
pd_LazyReduce2(pd a, double q)
{
   return correct_excess(a, 2*q);
}

// inputs in [0, 2*n), output in [0, 4*n)
template<class pd> pd
pd_LazyAddMod(pd a, pd b, double n)
{
   return a+b;
}

// inputs in [0, 2*n), output in [0, 4*n)
template<class pd> pd
pd_LazySubMod(pd a, pd b, double n)
{
   return a-b+2*n;
}

// inputs in [0, 2*n), output in [0, 2*n)
template<class pd> pd
pd_LazyAddMod2(pd a, pd b, double n)
{
   pd r = a+b;
   return correct_excess(r, 2*n);
}

// inputs in [0, 2*n), output in [0, 2*n)
template<class pd> pd
pd_LazySubMod2(pd a, pd b, double n)
{
   pd r = a-b;
   return correct_deficit(r, 2*n);
}

// inputs in [0, 4*n), output in [0, 4*n)
template<class pd> pd
pd_LazyAddMod4(pd a, pd b, double n)
{
   pd r = a+b;
   return correct_excess(r, 4*n);
}

// inputs in [0, 4*n), output in [0, 4*n)
template<class pd> pd
pd_LazySubMod4(pd a, pd b, double n)
{
   pd r = a-b;
   return correct_deficit(r, 4*n);
}

// Input and output in [0, 4*n)
template<class pd> pd
pd_LazyDoubleMod4(pd a, double n)
{
   return 2 * pd_LazyReduce2(a, n);
}

// Input and output in [0, 2*n)
template<class pd> pd
pd_LazyDoubleMod2(pd a, double n)
{
   return 2 * pd_LazyReduce1(a, n);
}

// n in [0,2^50), b in [0,n), a in [0,4*n), bninv = RoundDown(b/n)
// returns a*b mod n in [0, 2*n)
template<class pd> pd
pd_LazyMulModPrecon(pd a, pd b, double n, pd bninv)
{
   pd hi = a*b;
   pd lo = fused_mulsub(a, b, hi);  // hi+lo == a*b (exactly)
   pd q =  fused_muladd(a, bninv, 1L << 52);
   q -= (1L << 52);             // q is the correct quotient, or one too small
   pd d = fused_negmuladd(q, n, hi);   // d == hi - q*n (exactly)
   pd r = d + lo;           // r is the remainder, or the remainder plus n

   return r;
}

// return (a[0] + a[1], a[0] - a[1], a[2] + a[3], a[2] - a[3], ...)
// all inputs and outputs in [0, 2*n)
template<class pd> pd
pd_fwd_butterfly_packed2(pd a, double n)
{
   pd b = swap2(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend2(sum, diff);
   res = pd_LazyReduce2(res, n);
   return res;
}

// return (a[0] + a[2], a[1] + a[3], (a[0] - a[2]), (a[1] - a[3])*root, ...)
// all inputs and outputs in [0, 2*n)
// it is also assumed that w = (1,1,1,root,...) and wninv = RoundDown(w/n)
template<class pd> pd
pd_fwd_butterfly_packed4(pd a, pd w, double n, pd wninv)
{
   pd b = swap4(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend4(sum, diff);
   res = pd_LazyMulModPrecon(res, w, n, wninv);
   return res;
}

static double
pd_LazyPrepMulModPrecon(long b, long n)
{
   return double(b)/double(n);
}

//===================================

#define NTL_PD_FFT_THRESH (11)

#define PDLGSZ NTL_LG2_PDSZ
#define PDSZ NTL_PDSZ

#if (PDSZ == 8)
typedef PD<8> pd_full;
typedef PD<4> pd_half;
typedef PD<2> pd_qrtr;
#else
typedef PD<4> pd_full;
typedef PD<2> pd_half;
#endif

#define PDLD pd_full::load

// this assumes xx0, xx1, w, qinv are pd_half's
#define fwd_butterfly_half(xx0, xx1, w, q, wqinv)  \
do \
{ \
   pd_half x0_ = xx0; \
   pd_half x1_ = xx1; \
   pd_half t_  = pd_LazySubMod(x0_, x1_, q); \
   xx0 = pd_LazyAddMod2(x0_, x1_, q); \
   xx1 = pd_LazyMulModPrecon(t_, w, q, wqinv); \
}  \
while (0)

// this assumes xx0, xx1, w, qinv are pd_full's
#define fwd_butterfly_full(xx0, xx1, w, q, wqinv)  \
do \
{ \
   pd_full x0_ = xx0; \
   pd_full x1_ = xx1; \
   pd_full t_  = pd_LazySubMod(x0_, x1_, q); \
   xx0 = pd_LazyAddMod2(x0_, x1_, q); \
   xx1 = pd_LazyMulModPrecon(t_, w, q, wqinv); \
}  \
while (0)

// this assumes xx0_ptr, xx1_ptr, w_ptr, wqinv_ptr are double pointers
// which are read/written as pd_full's.
// In gcc, restrict keyword will help code gen.
#define fwd_butterfly(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do \
{ \
   pd_full x0_     = PDLD(xx0_ptr); \
   pd_full x1_     = PDLD(xx1_ptr); \
   pd_full w_      = PDLD(w_ptr); \
   pd_full wqinv_  = PDLD(wqinv_ptr); \
   pd_full t_      = pd_LazySubMod(x0_, x1_, q); \
   store(xx0_ptr, pd_LazyAddMod2(x0_, x1_, q)); \
   store(xx1_ptr, pd_LazyMulModPrecon(t_, w_, q, wqinv_)); \
}  \
while (0)

#if 0
#define fwd_butterfly_x4(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   pd_full xx0_0_ = PDLD(xx0_ptr+0*PDSZ);  pd_full xx1_0_ = PDLD(xx1_ptr+0*PDSZ);  \
   pd_full xx0_1_ = PDLD(xx0_ptr+1*PDSZ);  pd_full xx1_1_ = PDLD(xx1_ptr+1*PDSZ);  \
   pd_full xx0_2_ = PDLD(xx0_ptr+2*PDSZ);  pd_full xx1_2_ = PDLD(xx1_ptr+2*PDSZ);  \
   pd_full xx0_3_ = PDLD(xx0_ptr+3*PDSZ);  pd_full xx1_3_ = PDLD(xx1_ptr+3*PDSZ);  \
   fwd_butterfly_full(xx0_0_, xx1_0_, PDLD(w_ptr+0*PDSZ), q, PDLD(wqinv_ptr+0*PDSZ));  \
   fwd_butterfly_full(xx0_1_, xx1_1_, PDLD(w_ptr+1*PDSZ), q, PDLD(wqinv_ptr+1*PDSZ));  \
   fwd_butterfly_full(xx0_2_, xx1_2_, PDLD(w_ptr+2*PDSZ), q, PDLD(wqinv_ptr+2*PDSZ));  \
   fwd_butterfly_full(xx0_3_, xx1_3_, PDLD(w_ptr+3*PDSZ), q, PDLD(wqinv_ptr+3*PDSZ));  \
   store(xx0_ptr+0*PDSZ, xx0_0_);  store(xx1_ptr+0*PDSZ, xx1_0_);  \
   store(xx0_ptr+1*PDSZ, xx0_1_);  store(xx1_ptr+1*PDSZ, xx1_1_);  \
   store(xx0_ptr+2*PDSZ, xx0_2_);  store(xx1_ptr+2*PDSZ, xx1_2_);  \
   store(xx0_ptr+3*PDSZ, xx0_3_);  store(xx1_ptr+3*PDSZ, xx1_3_);  \
}  \
while(0)
#else
#define fwd_butterfly_x4(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   fwd_butterfly(xx0_ptr+0*PDSZ, xx1_ptr+0*PDSZ, w_ptr+0*PDSZ, q, wqinv_ptr+0*PDSZ);  \
   fwd_butterfly(xx0_ptr+1*PDSZ, xx1_ptr+1*PDSZ, w_ptr+1*PDSZ, q, wqinv_ptr+1*PDSZ);  \
   fwd_butterfly(xx0_ptr+2*PDSZ, xx1_ptr+2*PDSZ, w_ptr+2*PDSZ, q, wqinv_ptr+2*PDSZ);  \
   fwd_butterfly(xx0_ptr+3*PDSZ, xx1_ptr+3*PDSZ, w_ptr+3*PDSZ, q, wqinv_ptr+3*PDSZ);  \
}  \
while(0)
#endif

static inline NTL_ALWAYS_INLINE void
pd_fft_layer_inner_loop(double* NTL_RESTRICT xp0,
                        double* NTL_RESTRICT xp1,
                        long size,
                        const double* NTL_RESTRICT wtab,
                        const double* NTL_RESTRICT wqinvtab,
                        double q)

{
   long j = 0;
   do {
     fwd_butterfly_x4(xp0+j, xp1+j, wtab+j, q, wqinvtab+j);
     j += 4*PDSZ;
   } while (j < size);
}

// assumes size >= 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer(double* xp, long blocks, long size,
	     const double* wtab,
	     const double* wqinvtab,
	     double q)
{
   size /= 2;

   do {
      pd_fft_layer_inner_loop(xp, xp+size, size, wtab, wqinvtab, q);
      xp += 2 * size;
   } while (--blocks != 0);
}

// size == 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size8(double* NTL_RESTRICT xp, long blocks,
		  const double* NTL_RESTRICT wtab,
		  const double* NTL_RESTRICT wqinvtab,
		  double q)
{
   do {
      fwd_butterfly_x4(xp+0*PDSZ, xp+4*PDSZ, wtab, q, wqinvtab);
      xp += 8*PDSZ;
   } while (--blocks != 0);
}

// size == 4*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size4(double* NTL_RESTRICT xp, long blocks,
		   const double* NTL_RESTRICT wtab,
		   const double* NTL_RESTRICT wqinvtab,
		   double q)
{
   do {
      fwd_butterfly(xp+0*PDSZ, xp+2*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+1*PDSZ, xp+3*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      fwd_butterfly(xp+4*PDSZ, xp+6*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+5*PDSZ, xp+7*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      xp += 8*PDSZ;
      blocks -= 2;
   } while (blocks != 0);
}

// size == 2*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size2(double* NTL_RESTRICT xp, long blocks,
		   const double* NTL_RESTRICT wtab,
		   const double* NTL_RESTRICT wqinvtab,
		   double q)
{
   do {
      fwd_butterfly(xp+0*PDSZ, xp+1*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+2*PDSZ, xp+3*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+4*PDSZ, xp+5*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      fwd_butterfly(xp+6*PDSZ, xp+7*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);

      xp += 8*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#if (PDSZ == 8)
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1_one_block(double* x,
                   pd_half w8, pd_half w8qinv,
                   pd_full w4, pd_full w4qinv,
		   double q)
{
   pd_half x0 = pd_half::load(x);
   pd_half x1 = pd_half::load(x+PDSZ/2);
   fwd_butterfly_half(x0, x1, w8, q, w8qinv);
   pd_full y = join(x0, x1);

   y = pd_fwd_butterfly_packed4(y, w4, q, w4qinv);
   y = pd_fwd_butterfly_packed2(y, q);

   store(x, y);
}

// size == PDSZ == 8
// processes last three levels, of size 8, 4, and 2.
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w8_ptr = *w_pp;
   const double *w8qinv_ptr = *wqinv_pp;

   const double *w4_ptr = *(w_pp-1);
   const double *w4qinv_ptr = *(wqinv_pp-1);

   pd_half w8 = pd_half::load(w8_ptr);

   pd_half w8qinv = pd_half::load(w8qinv_ptr);

   pd_qrtr w4_qrtr = pd_qrtr::load(w4_ptr);
   pd_half w4_half = join(w4_qrtr, w4_qrtr);
   pd_full w4      = join(w4_half, w4_half);
   w4 = blend4(dup2even(w4), w4);

   pd_qrtr w4qinv_qrtr = pd_qrtr::load(w4qinv_ptr);
   pd_half w4qinv_half = join(w4qinv_qrtr, w4qinv_qrtr);
   pd_full w4qinv      = join(w4qinv_half, w4qinv_half);
   w4qinv = blend4(dup2even(w4qinv), w4qinv);

   do {
      pd_fft_layer_size1_one_block(xp+0*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+1*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+2*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+3*PDSZ, w8, w8qinv, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}
#else
// PDSZ == 4

static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1_one_block(double* x,
                   pd_half w4, pd_half w4qinv,
		   double q)
{
   pd_half x0 = pd_half::load(x);
   pd_half x1 = pd_half::load(x+PDSZ/2);
   fwd_butterfly_half(x0, x1, w4, q, w4qinv);
   pd_full y = join(x0, x1);

   y = pd_fwd_butterfly_packed2(y, q);

   store(x, y);
}

// size == PDSZ == 4
// processes last two levels, of size 4 and 2.
static inline NTL_ALWAYS_INLINE void
pd_fft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w4_ptr = *w_pp;
   const double *w4qinv_ptr = *wqinv_pp;

   pd_half w4 = pd_half::load(w4_ptr);
   pd_half w4qinv = pd_half::load(w4qinv_ptr);

   do {
      pd_fft_layer_size1_one_block(xp+0*PDSZ, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+1*PDSZ, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+2*PDSZ, w4, w4qinv, q);
      pd_fft_layer_size1_one_block(xp+3*PDSZ, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#endif

void
pd_fft_base(double* xp, long lgN, const pd_mod_t& mod)
{
  double q = mod.q;
  const double** wtab = mod.wtab;
  const double** wqinvtab = mod.wqinvtab;

  long N = 1L << lgN;

  long j, size, blocks;
  for (j = lgN, size = N, blocks = 1;
       size > 8*PDSZ; j--, blocks <<= 1, size >>= 1)
    pd_fft_layer(xp, blocks, size, wtab[j], wqinvtab[j], q);

  pd_fft_layer_size8(xp, blocks, wtab[j], wqinvtab[j], q);
  j--, blocks <<= 1, size >>= 1;

  pd_fft_layer_size4(xp, blocks, wtab[j], wqinvtab[j], q);
  j--, blocks <<= 1, size >>= 1;

  pd_fft_layer_size2(xp, blocks, wtab[j], wqinvtab[j], q);
  j--, blocks <<= 1, size >>= 1;

  pd_fft_layer_size1(xp, blocks, wtab+j, wqinvtab+j, q);

}

static inline NTL_ALWAYS_INLINE void
pd_move(double *x, const long *a)
{
   pd_full r;
   loadu(r, a);
   store(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_move(long *x, const double *a)
{
   pd_full r;
   load(r, a);
   storeu(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_reduce1_move(long *x, const double *a, double q)
{
   pd_full r;
   load(r, a);
   r = pd_LazyReduce1(r, q);
   storeu(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_reduce2_move(long *x, const double *a, double q)
{
   pd_full r;
   load(r, a);
   r = pd_LazyReduce2(r, q);
   r = pd_LazyReduce1(r, q);
   storeu(x, r);
}

static inline NTL_ALWAYS_INLINE void
pd_mul_move(long *x, const double *a, pd_full b, double q, pd_full bqinv)
{
   pd_full r;
   load(r, a);
   r = pd_LazyMulModPrecon(r, b, q, bqinv);
   r = pd_LazyReduce1(r, q);
   storeu(x, r);
}

static
void pd_fft_short(double* xp, long yn, long xn, long lgN,
                   const pd_mod_t& mod)
{
  long N = 1L << lgN;

  if (yn == N)
    {
      if (xn == N && lgN <= NTL_PD_FFT_THRESH)
	{
	  // no truncation
	  pd_fft_base(xp, lgN, mod);
	  return;
	}
    }

  // divide-and-conquer algorithm

  long half = N >> 1;
  double q = mod.q;

  if (yn <= half)
    {
      if (xn <= half)
	{
	  pd_fft_short(xp, yn, xn, lgN - 1, mod);
	}
      else
	{
	  xn -= half;

	  // (X, Y) -> X + Y
	  for (long j = 0; j < xn; j+=PDSZ)
	    store(xp+j, pd_LazyAddMod2(PDLD(xp+j), PDLD(xp+j+half), q));

	  pd_fft_short(xp, yn, half, lgN - 1, mod);
	}
    }
  else
    {
      yn -= half;

      double* xp0 = xp;
      double* xp1 = xp + half;
      const double* wtab = mod.wtab[lgN];
      const double* wqinvtab = mod.wqinvtab[lgN];

      if (xn <= half)
	{
	  // X -> (X, w*X)
	  for (long j = 0; j < xn; j+=PDSZ)
	    store(xp1+j, pd_LazyMulModPrecon(PDLD(xp0+j), PDLD(wtab+j), q, PDLD(wqinvtab+j)));

	  pd_fft_short(xp0, half, xn, lgN - 1, mod);
	  pd_fft_short(xp1, yn, xn, lgN - 1, mod);
	}
      else
	{
	  xn -= half;

	  // (X, Y) -> (X + Y, w*(X - Y))
          pd_fft_layer_inner_loop(xp0, xp1, xn, wtab, wqinvtab, q);

	  // X -> (X, w*X)
	  for (long j = xn; j < half; j+=PDSZ)
	    store(xp1+j, pd_LazyMulModPrecon(PDLD(xp0+j), PDLD(wtab+j), q, PDLD(wqinvtab+j)));

	  pd_fft_short(xp0, half, half, lgN - 1, mod);
	  pd_fft_short(xp1, yn, half, lgN - 1, mod);
	}
    }
}

void
pd_fft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                  long yn, long xn)

{
   for (long i = 0; i < xn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_fft_short(xp, yn, xn, lgN, mod);

   double q = mod.q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_reduce1_move(A+i+0*PDSZ, xp+i+0*PDSZ, q);
      pd_reduce1_move(A+i+1*PDSZ, xp+i+1*PDSZ, q);
      pd_reduce1_move(A+i+2*PDSZ, xp+i+2*PDSZ, q);
      pd_reduce1_move(A+i+3*PDSZ, xp+i+3*PDSZ, q);
   }
}

void
pd_fft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                  long yn, long xn, double fac)

{
   for (long i = 0; i < xn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_fft_short(xp, yn, xn, lgN, mod);

   double q = mod.q;
   double facqinv = fac/q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_mul_move(A+i+0*PDSZ, xp+i+0*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+1*PDSZ, xp+i+1*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+2*PDSZ, xp+i+2*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+3*PDSZ, xp+i+3*PDSZ, fac, q, facqinv);
   }
}

//================ ifft ==============

// return (a[0] + a[1], a[0] - a[1], a[2] + a[3], a[2] - a[3], ...)
// all inputs and outputs in [0, 4*n)
template<class pd> pd
pd_inv_butterfly_packed2(pd a, double n)
{
   a = pd_LazyReduce2(a, n);
   pd b = swap2(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend2(sum, diff);
   return res;
}

// return (a[0] + a[2], a[1] + a[3]*root, a[0] - a[2], a[1] - a[3]*root, ...)
// all inputs and outputs in [0, 4*n)
// it is also assumed that w = (1,1,1,root,...) and wninv = RoundDown(w/n)
template<class pd> pd
pd_inv_butterfly_packed4(pd a, pd w, double n, pd wninv)
{
   a = pd_LazyMulModPrecon(a, w, n, wninv);
   pd b = swap4(a);
   pd sum = pd_LazyAddMod(a, b, n);
   pd diff = pd_LazySubMod(b, a, n);
   pd res = blend4(sum, diff);
   return res;
}

#define inv_butterfly_half(xx0, xx1, w, q, wqinv)  \
do  \
{  \
   pd_half x0_ = pd_LazyReduce2(xx0, q);  \
   pd_half x1_ = xx1;  \
   pd_half t_ = pd_LazyMulModPrecon(x1_, w, q, wqinv);   \
   xx0 = pd_LazyAddMod(x0_, t_, q);    \
   xx1 = pd_LazySubMod(x0_, t_, q);    \
} while (0)

#define inv_butterfly(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   pd_full x0_ = pd_LazyReduce2(PDLD(xx0_ptr), q);  \
   pd_full x1_ = PDLD(xx1_ptr);  \
   pd_full t_ = pd_LazyMulModPrecon(x1_, PDLD(w_ptr), q, PDLD(wqinv_ptr));   \
   store(xx0_ptr, pd_LazyAddMod(x0_, t_, q));    \
   store(xx1_ptr, pd_LazySubMod(x0_, t_, q));    \
} while (0)

#define inv_butterfly_x4(xx0_ptr, xx1_ptr, w_ptr, q, wqinv_ptr)  \
do  \
{  \
   inv_butterfly(xx0_ptr+0*PDSZ, xx1_ptr+0*PDSZ, w_ptr+0*PDSZ, q, wqinv_ptr+0*PDSZ);  \
   inv_butterfly(xx0_ptr+1*PDSZ, xx1_ptr+1*PDSZ, w_ptr+1*PDSZ, q, wqinv_ptr+1*PDSZ);  \
   inv_butterfly(xx0_ptr+2*PDSZ, xx1_ptr+2*PDSZ, w_ptr+2*PDSZ, q, wqinv_ptr+2*PDSZ);  \
   inv_butterfly(xx0_ptr+3*PDSZ, xx1_ptr+3*PDSZ, w_ptr+3*PDSZ, q, wqinv_ptr+3*PDSZ);  \
}  \
while(0)

static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_inner_loop(double* NTL_RESTRICT xp0,
                         double* NTL_RESTRICT xp1,
                         long size,
                         const double* NTL_RESTRICT wtab,
                         const double* NTL_RESTRICT wqinvtab,
                         double q)

{
   long j = 0;
   do {
     inv_butterfly_x4(xp0+j, xp1+j, wtab+j, q, wqinvtab+j);
     j += 4*PDSZ;
   } while (j < size);
}

// assumes size >= 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer(double* xp, long blocks, long size,
	      const double* wtab,
	      const double* wqinvtab,
	      double q)
{
   size /= 2;

   do {
      pd_ifft_layer_inner_loop(xp, xp+size, size, wtab, wqinvtab, q);
      xp += 2 * size;
   } while (--blocks != 0);
}

// size == 8*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size8(double* NTL_RESTRICT xp, long blocks,
		    const double* NTL_RESTRICT wtab,
		    const double* NTL_RESTRICT wqinvtab,
		    double q)
{
   do {
      inv_butterfly_x4(xp+0*PDSZ, xp+4*PDSZ, wtab, q, wqinvtab);
      xp += 8*PDSZ;
   } while (--blocks != 0);
}

// size == 4*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size4(double* NTL_RESTRICT xp, long blocks,
		    const double* NTL_RESTRICT wtab,
		    const double* NTL_RESTRICT wqinvtab,
		    double q)
{
   do {
      inv_butterfly(xp+0*PDSZ, xp+2*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+1*PDSZ, xp+3*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      inv_butterfly(xp+4*PDSZ, xp+6*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+5*PDSZ, xp+7*PDSZ, wtab+1*PDSZ, q, wqinvtab+1*PDSZ);

      xp += 8*PDSZ;
      blocks -= 2;
   } while (blocks != 0);
}

// size == 2*PDSZ
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size2(double* NTL_RESTRICT xp, long blocks,
		    const double* NTL_RESTRICT wtab,
		    const double* NTL_RESTRICT wqinvtab,
		    double q)
{
   do {
      inv_butterfly(xp+0*PDSZ, xp+1*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+2*PDSZ, xp+3*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+4*PDSZ, xp+5*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);
      inv_butterfly(xp+6*PDSZ, xp+7*PDSZ, wtab+0*PDSZ, q, wqinvtab+0*PDSZ);

      xp += 8*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#if (PDSZ == 8)
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1_one_block(double* x,
                   pd_half w8, pd_half w8qinv,
                   pd_full w4, pd_full w4qinv,
		   double q)
{
   pd_full y = PDLD(x);
   y = pd_inv_butterfly_packed2(y, q);
   y = pd_inv_butterfly_packed4(y, w4, q, w4qinv);

   pd_half x0 = get_lo(y);
   pd_half x1 = get_hi(y);
   inv_butterfly_half(x0, x1, w8, q, w8qinv);

   store(x, x0);
   store(x+PDSZ/2, x1);
}

// size == PDSZ == 8
// processes last three levels, of size 8, 4, and 2.
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w8_ptr = *w_pp;
   const double *w8qinv_ptr = *wqinv_pp;

   const double *w4_ptr = *(w_pp-1);
   const double *w4qinv_ptr = *(wqinv_pp-1);

   pd_half w8 = pd_half::load(w8_ptr);

   pd_half w8qinv = pd_half::load(w8qinv_ptr);

   pd_qrtr w4_qrtr = pd_qrtr::load(w4_ptr);
   pd_half w4_half = join(w4_qrtr, w4_qrtr);
   pd_full w4      = join(w4_half, w4_half);
   w4 = blend4(dup2even(w4), w4);

   pd_qrtr w4qinv_qrtr = pd_qrtr::load(w4qinv_ptr);
   pd_half w4qinv_half = join(w4qinv_qrtr, w4qinv_qrtr);
   pd_full w4qinv      = join(w4qinv_half, w4qinv_half);
   w4qinv = blend4(dup2even(w4qinv), w4qinv);

   do {
      pd_ifft_layer_size1_one_block(xp+0*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+1*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+2*PDSZ, w8, w8qinv, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+3*PDSZ, w8, w8qinv, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}
#else
// PDSZ == 4

static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1_one_block(double* x,
                   pd_half w4, pd_half w4qinv,
		   double q)
{
   pd_full y = PDLD(x);
   y = pd_inv_butterfly_packed2(y, q);

   pd_half x0 = get_lo(y);
   pd_half x1 = get_hi(y);
   inv_butterfly_half(x0, x1, w4, q, w4qinv);

   store(x, x0);
   store(x+PDSZ/2, x1);
}

// size == PDSZ == 4
// processes last two levels, of size 4 and 2.
static inline NTL_ALWAYS_INLINE void
pd_ifft_layer_size1(double* xp, long blocks,
                   const double **w_pp, const double **wqinv_pp,
		   double q)
{
   const double *w4_ptr = *w_pp;
   const double *w4qinv_ptr = *wqinv_pp;

   pd_half w4 = pd_half::load(w4_ptr);
   pd_half w4qinv = pd_half::load(w4qinv_ptr);

   do {
      pd_ifft_layer_size1_one_block(xp+0*PDSZ, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+1*PDSZ, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+2*PDSZ, w4, w4qinv, q);
      pd_ifft_layer_size1_one_block(xp+3*PDSZ, w4, w4qinv, q);

      xp += 4*PDSZ;
      blocks -= 4;
   } while (blocks != 0);
}

#endif

void
pd_ifft_base(double* xp, long lgN, const pd_mod_t& mod)
{
  double q = mod.q;
  const double** wtab = mod.wtab;
  const double** wqinvtab = mod.wqinvtab;

  long N = 1L << lgN;

  long j=PDLGSZ, size=PDSZ, blocks=N/PDSZ;

  pd_ifft_layer_size1(xp, blocks, wtab+j, wqinvtab+j, q);
  j++, blocks >>= 1, size <<= 1;

  pd_ifft_layer_size2(xp, blocks, wtab[j], wqinvtab[j], q);
  j++, blocks >>= 1, size <<= 1;

  pd_ifft_layer_size4(xp, blocks, wtab[j], wqinvtab[j], q);
  j++, blocks >>= 1, size <<= 1;

  pd_ifft_layer_size8(xp, blocks, wtab[j], wqinvtab[j], q);
  j++, blocks >>= 1, size <<= 1;

  for (; size <= N; j++, blocks >>= 1, size <<= 1)
    pd_ifft_layer(xp, blocks, size, wtab[j], wqinvtab[j], q);

}

static void
pd_ifft_short2(double* xp, long yn, long lgN, const pd_mod_t& mod);

static void
pd_ifft_short1(double* xp, long yn, long lgN, const pd_mod_t& mod)

// Implements truncated inverse FFT interface, but with xn==yn.
// All computations are done in place.

{
  long N = 1L << lgN;

  if (yn == N && lgN <= NTL_PD_FFT_THRESH)
    {
      // no truncation
      pd_ifft_base(xp, lgN, mod);
      return;
    }

  // divide-and-conquer algorithm

  long half = N >> 1;
  double q = mod.q;

  if (yn <= half)
    {
      // X -> 2X
      for (long j = 0; j < yn; j+=PDSZ)
      	store(xp+j, pd_LazyDoubleMod4(PDLD(xp+j), q));

      pd_ifft_short1(xp, yn, lgN - 1, mod);
    }
  else
    {
      double* xp0 = xp;
      double* xp1 = xp + half;

      pd_ifft_short1(xp0, half, lgN - 1, mod);

      yn -= half;

      if (yn < half) {
        const double* wtab1 = mod.wtab1[lgN];
        const double* wqinvtab1 = mod.wqinvtab1[lgN];

	// X -> (2X, w*X)
	for (long j = yn; j < half; j+=PDSZ)
	  {
	    pd_full x0 = PDLD(xp0+j);
	    store(xp0+j, pd_LazyDoubleMod4(x0, q));
	    store(xp1+j, pd_LazyMulModPrecon(x0, PDLD(wtab1+j), q, PDLD(wqinvtab1+j)));
	  }
      }

      pd_ifft_short2(xp1, yn, lgN - 1, mod);

      // (X, Y) -> (X + Y/w, X - Y/w)
      pd_ifft_layer_inner_loop(xp0, xp1, yn, mod.wtab[lgN], mod.wqinvtab[lgN], q);
    }
}

static void
pd_ifft_short2(double* xp, long yn, long lgN, const pd_mod_t& mod)

// Implements truncated inverse FFT interface, but with xn==N.
// All computations are done in place.

{
  long N = 1L << lgN;

  if (yn == N && lgN <= NTL_PD_FFT_THRESH)
    {
      // no truncation
      pd_ifft_base(xp, lgN, mod);
      return;
    }

  // divide-and-conquer algorithm

  long half = N >> 1;
  double q = mod.q;

  if (yn <= half)
    {
      // X -> 2X
      for (long j = 0; j < yn; j+=PDSZ)
     	store(xp+j, pd_LazyDoubleMod4(PDLD(xp+j), q));

      // (X, Y) -> X + Y
      for (long j = yn; j < half; j+=PDSZ)
	store(xp+j, pd_LazyAddMod4(PDLD(xp+j), PDLD(xp+j+half), q));

      pd_ifft_short2(xp, yn, lgN - 1, mod);

      // (X, Y) -> X - Y
      for (long j = 0; j < yn; j+=PDSZ)
	store(xp+j, pd_LazySubMod4(PDLD(xp+j), PDLD(xp+j+half), q));
    }
  else
    {
      double* xp0 = xp;
      double* xp1 = xp + half;

      pd_ifft_short1(xp0, half, lgN - 1, mod);

      yn -= half;

      if (yn < half) {
        const double* wtab1 = mod.wtab1[lgN];
        const double* wqinvtab1 = mod.wqinvtab1[lgN];

	// (X, Y) -> (2X - Y, w*(X - Y))
	for (long j = yn; j < half; j+=PDSZ)
	  {
	    pd_full x0 = PDLD(xp0+j);
	    pd_full x1 = PDLD(xp1+j);
	    pd_full u  = pd_LazySubMod4(x0, x1, q);
	    store(xp0+j, pd_LazyAddMod4(x0, u, q));
	    store(xp1+j, pd_LazyMulModPrecon(u, PDLD(wtab1+j), q, PDLD(wqinvtab1+j)));
	  }
      }

      pd_ifft_short2(xp1, yn, lgN - 1, mod);

      // (X, Y) -> (X + Y/w, X - Y/w)
      pd_ifft_layer_inner_loop(xp0, xp1, yn, mod.wtab[lgN], mod.wqinvtab[lgN], q);
    }
}

void
pd_ifft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                   long yn, double fac)
{
   long N = 1L << lgN;

   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_ifft_short1(xp, yn, lgN, mod);

   double q = mod.q;
   double facqinv = fac/q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_mul_move(A+i+0*PDSZ, xp+i+0*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+1*PDSZ, xp+i+1*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+2*PDSZ, xp+i+2*PDSZ, fac, q, facqinv);
      pd_mul_move(A+i+3*PDSZ, xp+i+3*PDSZ, fac, q, facqinv);
   }
}

void
pd_ifft_trunc_impl(long* A, const long* a, double* xp, long lgN, const pd_mod_t& mod,
                   long yn)
{
   long N = 1L << lgN;

   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_move(xp+i+0*PDSZ, a+i+0*PDSZ);
      pd_move(xp+i+1*PDSZ, a+i+1*PDSZ);
      pd_move(xp+i+2*PDSZ, a+i+2*PDSZ);
      pd_move(xp+i+3*PDSZ, a+i+3*PDSZ);
   }

   pd_ifft_short1(xp, yn, lgN, mod);

   double q = mod.q;
   for (long i = 0; i < yn; i += 4*PDSZ) {
      pd_reduce2_move(A+i+0*PDSZ, xp+i+0*PDSZ, q);
      pd_reduce2_move(A+i+1*PDSZ, xp+i+1*PDSZ, q);
      pd_reduce2_move(A+i+2*PDSZ, xp+i+2*PDSZ, q);
      pd_reduce2_move(A+i+3*PDSZ, xp+i+3*PDSZ, q);
   }
}

NTL_END_IMPL

#else

void _ntl_pd_FFT_dummy() { }

#endif


// MIT License
//
// Copyright (c) 2021 Oliver Schick
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef MPO19_PCURE_02082020
#define MPO19_PCURE_02082020

#include <mpo19/typedefs.hpp>
#include <mpo19/random.hpp>
#include <mpo19/clustering.hpp>
#include <mpo19/linkage.hpp>
#include <mpo19/paillier_wrapper.hpp>
#include <boost/phoenix.hpp>

#include <iostream>
#include <type_traits>

namespace mpo19
{

constexpr struct calc_distance_t {
    size_t operator()(point<int64_t> const& lhs
                      , point<int64_t> const& rhs) const
    {
        using namespace boost::phoenix::placeholders;
        assert(lhs.dimension() == rhs.dimension());
        return apply_on_elements(arg1 * arg1, lhs - rhs).fold();
    }

    double operator()(point<double> const& lhs
                      , point<int64_t> const& rhs) const
    {
        using namespace boost::phoenix::placeholders;
        assert(lhs.dimension() == rhs.dimension());
        point<double> res(lhs.dimension());
        apply_on_elements(res, arg1 - arg2, lhs, rhs);
        apply_on_elements(res, arg1 * arg1, res);
        return res.fold();
    }

} calc_distance;

template<typename T, typename LinkagePolicy>
struct cluster_dist_t : public LinkagePolicy {

    template<typename... Args>
    cluster_dist_t(std::vector<point<T>> const& values, Args&&... args)
        : LinkagePolicy{std::forward<Args>(args)...}, values_{values} {}

    auto operator()(cluster const& c1, cluster const& c2)
    {
        auto c1_indices = get_indices(c1);
        auto c2_indices = get_indices(c2);

        size_t c1_indices_size = c1_indices.size();
        size_t c2_indices_size = c2_indices.size();

        assert(0 < c1_indices_size);
        assert(0 < c2_indices_size);

        auto res = calc_distance(values_[c1_indices[0]], values_[c2_indices[0]]);

        size_t i = 0, j = 1;
        for(; i != c1_indices_size; ++i, j = 0) {
            for(; j != c2_indices_size; ++j) {
                assert( !(i == 0 && j == 0) &&
                        "We already calculated the distance for (0, 0)");
                res =
                    LinkagePolicy::cluster_distance(
                        res, calc_distance(values_[c1_indices[i]], values_[c2_indices[j]]));
            }
        }

        return res;
    }

private:
    std::vector<point<T>> const& values_;
};

template<typename T>
struct pcure_clusters {
    //We use a vector<vector>, as the rows might have different sizes
    std::vector<cluster> partition_dendrograms;
    std::vector<point<T>> values;
};

template<typename T>
std::ostream& operator<<(std::ostream& os, pcure_clusters<T> const& clusters)
{
    os << '(';
    for(cluster const& c : clusters.partition_dendrograms) {
        os << c;
    }
    os << ')';
    os << '(';
    for(point<T> const& p : clusters.values) {
        os << p;
    }
    os << ')';
    return os;
}

template<typename T>
std::istream& operator>>(std::istream& is, pcure_clusters<T>& clusters)
{
    clusters.partition_dendrograms.clear();
    clusters.values.clear();

    char c = is.get();
    assert(c == '(');

    while(is.peek() != ')') {
        cluster clust;
        is >> clust;
        clusters.partition_dendrograms.emplace_back(std::move(clust));
    }

    c = is.get();
    assert(c == ')');
    c = is.get();
    assert(c == '(');

    while(is.peek() != ')') {
        point<T> p;
        is >> p;
        clusters.values.emplace_back(std::move(p));
    }

    return is;
}

template<template<class> class LinkagePolicy>
struct clear_clustering_t {

    template<typename RandomAccessRange>
    auto operator()(RandomAccessRange const& partition, size_t t)
    {
        using dist_table_t = table_t<decltype(calc_distance(partition[0], partition[0]))>;
        size_t n = partition.size();
        dist_table_t dist_matrix{boost::extents[n][n]};

        for(index_t  i = 0; i != index_t(n); ++i) {
            for(index_t j = 0; j != index_t(n); ++j) {
                dist_matrix[i][j] = calc_distance(partition[i], partition[j]);
            }
        }

        return hierarchical_clustering<LinkagePolicy<dist_table_t>> {dist_matrix}(t);
    }

    template<typename T>
    auto operator()(pcure_clusters<T> const& clusters, size_t t)
    {
        size_t n = clusters.partition_dendrograms.size();
        table_t<T> dist_matrix{boost::extents[n][n]};

        hierarchical_clustering<cluster_dist_t<T, LinkagePolicy<table_t<T>>>>
        hc{clusters.values, dist_matrix};

        cluster_dist_t<T, LinkagePolicy<table_t<T>>>& cluster_dist = hc;

        for(index_t i = 0; i != index_t(n); ++i) {
            for(index_t j = 0; j != index_t(n); ++j) {
                if(i == j) {
                    dist_matrix[i][j] = 0;
                } else {
                    dist_matrix[i][j] = cluster_dist(
                                            clusters.partition_dendrograms[i]
                                            , clusters.partition_dendrograms[j]);
                }
            }
        }

        return hc(t);
    }
};

template<typename Range>
std::vector<cluster> assign_clusters(
    Range const& input
    , std::vector<point<double>> const& centroids)
{
    assert(!centroids.empty());
    std::vector<cluster> res(centroids.size());
    size_t rank = 0;

    for(size_t i = 0; i != input.size(); ++i) {
        size_t min_index = 0;
        double min_distance = calc_distance(centroids[0], input[i]);
        for(size_t j = 1; j != centroids.size(); ++j) {
            double distance = calc_distance(centroids[j], input[i]);
            if(distance < min_distance) {
                min_index = j;
                min_distance = distance;
            }
        }
        if(res[min_index].is_cluster()) ++rank;
        cluster singleton{cluster::leaf_t(i)};
        merge(res[min_index], singleton, rank);
        assert(!singleton.is_cluster());
    }

    res.erase(std::remove_if(res.begin(), res.end(), [](cluster const& c){
        return !c.is_cluster();
    }), res.end());

    return res;
}

template<typename T>
void transform_pcure_dendrogram(
    pcure_clusters<T>& clusters
    , std::vector<cluster>& dendrogram
    , size_t base_rank)
{
    for(cluster& c : dendrogram) {
        assert(c.is_cluster());
        std::vector<index_t> indices = get_indices(c);
        assert(clusters.partition_dendrograms[indices[0]].is_cluster());
        c = std::move(clusters.partition_dendrograms[indices[0]]);

        for(size_t i = 1, rank = base_rank; i != indices.size(); ++i, ++rank) {
            index_t idx = indices[i];
            assert(idx < clusters.partition_dendrograms.size());
            assert(clusters.partition_dendrograms[idx].is_cluster());
            merge(c, clusters.partition_dendrograms[idx], rank);
        }

        assert(c.is_cluster());
    }
}

template<typename ClusteringPolicy>
struct pcure : public ClusteringPolicy {

    template<typename... Args>
    pcure(size_t s, size_t p, size_t q, size_t t1, size_t t2, size_t t, Args&&... args)
        : ClusteringPolicy{std::forward<Args>(args)...}, s{s}, p{p}, q{q}, t1{t1}, t2{t2}, t{t} {}

    template<typename T>
    pcure_clusters<T> A_clustering(std::vector<point<T>>& input)
    {
        using boost::adaptors::sliced;
        using boost::adaptors::filtered;
        using boost::range::copy;

        struct add_partition_idx_t : boost::static_visitor<void> {

            add_partition_idx_t(size_t partition_idx)
                : partition_idx_{partition_idx} {}

            result_type operator()(cluster::union_t& u) const
            {
                u.get_left().visit(*this);
                u.get_right().visit(*this);
            }

            result_type operator()(cluster::leaf_t& l) const
            {
                l += partition_idx_;
            }

        private:
            size_t partition_idx_;
        };

        assert(input.size() > s &&
               "Cannot take a sample of size s with s being bigger than the input size");
        assert(s > p &&
               "Cannot partition s into more than s partitions");
        assert(s/(p*q) > 0 &&
               "The target clusters shall be grater than zero, i.e. s/(p*q) > 0");

        size_t const target_clusters = s/(p*q);

        pcure_clusters<T> res;

        res.partition_dendrograms.reserve(s/q);

        //if s is not a multiple of p, then the last s % p items won't be included in any partition
        auto sample = random_sample(input, s);

        for(size_t i = 0; i != p; ++i) {
            size_t const slice_first = i * s/p;
            size_t const slice_last = (i+1) * s/p;
            assert(slice_first < slice_last);

            auto partition = sample | sliced(slice_first, slice_last);

            auto dendrogram = static_cast<ClusteringPolicy&>(*this)(partition, target_clusters);
            auto filtered_dendrogram = dendrogram | filtered([&](cluster const& c) -> bool {
                return get_indices(c).size() >= t1;
            });

            for(cluster& c : filtered_dendrogram) {
                if(slice_first != 0)
                    c.visit(add_partition_idx_t{slice_first});
                res.partition_dendrograms.emplace_back(std::move(c));
            }
        }
        res.values = std::move(sample);

        return res;
    }

    template<typename T>
    pcure_clusters<T> B_clustering(pcure_clusters<T>& clusters)
    {
        using boost::range::remove_if;

        assert(clusters.partition_dendrograms.size() <= s/q);

        //The result in this context is the merge history of input clusters,
        //therefore we need to convert it to a merge history of indices
        auto dendrogram = static_cast<ClusteringPolicy&>(*this)(clusters, t);
        size_t base_rank = s/p - s/(p*q) + 1;

        for(cluster& c : dendrogram) {
            assert(c.is_cluster());
            std::vector<index_t> indices = get_indices(c);
            assert(clusters.partition_dendrograms[indices[0]].is_cluster());
            c = std::move(clusters.partition_dendrograms[indices[0]]);

            for(size_t i = 1, rank = base_rank; i != indices.size(); ++i, ++rank) {
                assert(clusters.partition_dendrograms[indices[i]].is_cluster());
                merge(c, clusters.partition_dendrograms[indices[i]], rank);
            }

            assert(c.is_cluster());
        }

        dendrogram.erase(remove_if(dendrogram, [&](cluster const& c) {
            return get_indices(c).size() < t2;
        })
        , dendrogram.end());

        clusters.partition_dendrograms = std::move(dendrogram);

        return std::move(clusters);
    }

    template<typename T>
    std::vector<point<double>> get_cluster_centroids(std::vector<point<T>> const& input)
    {
        using namespace boost::phoenix::placeholders;

        std::vector<point<T>> in{input};
        auto C_A = A_clustering(in);
        auto C_B = B_clustering(C_A);

        std::vector<point<double>> centroids;
        centroids.reserve(C_B.partition_dendrograms.size());
        for(cluster const& c : C_B.partition_dendrograms) {
            std::vector<index_t> indices = get_indices(c);
            point<double> sum(C_B.values.front().dimension(), 0.0);
            for(index_t i : indices) {
                apply_on_elements(sum, arg1 + arg2, sum, C_B.values[i]);
            }
            apply_on_elements(sum, arg1/indices.size(), sum);
            centroids.emplace_back(std::move(sum));
        }

        return centroids;
    }

    template<typename T>
    std::vector<cluster> assign_clusters(
        std::vector<point<T>> const& input
        , std::vector<point<double>> const& centroids)
    {
        return ::mpo19::assign_clusters(input, centroids);
    }

private:
    size_t s, p, q, t1, t2, t;
};

}

#endif //MPO19_PCURE_02082020


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California 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 HOLDERS 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.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/camera/camera.h"

#include <ceres/rotation.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include "theia/sfm/camera/projection_matrix_utils.h"
#include "theia/sfm/camera/project_point_to_image.h"
#include "theia/sfm/camera/radial_distortion.h"

namespace theia {

using Eigen::AngleAxisd;
using Eigen::Map;
using Eigen::Matrix;
using Eigen::Matrix3d;
using Eigen::Vector2d;
using Eigen::Vector3d;
using Eigen::Vector4d;

Camera::Camera() {
  // Set rotation and position to zero (i.e. identity).
  Map<Matrix<double, 1, 6> >(mutable_extrinsics()).setZero();

  SetFocalLength(1.0);
  SetAspectRatio(1.0);
  SetSkew(0.0);
  SetPrincipalPoint(0.0, 0.0);
  SetRadialDistortion(0.0, 0.0);

  image_size_[0] = 0;
  image_size_[1] = 0;
}

bool Camera::InitializeFromProjectionMatrix(
      const int image_width,
      const int image_height,
      const Matrix3x4d projection_matrix) {
  DCHECK_GT(image_width, 0);
  DCHECK_GT(image_height, 0);
  image_size_[0] = image_width;
  image_size_[1] = image_height;

  Vector3d orientation, position;
  Matrix3d calibration_matrix;
  DecomposeProjectionMatrix(projection_matrix,
                            &calibration_matrix,
                            &orientation,
                            &position);

  Map<Vector3d>(mutable_extrinsics() + ORIENTATION) = orientation;
  Map<Vector3d>(mutable_extrinsics() + POSITION) = position;

  if (calibration_matrix(0, 0) == 0 || calibration_matrix(1, 1) == 0) {
    LOG(INFO) << "Cannot set focal lengths to zero!";
    return false;
  }

  CalibrationMatrixToIntrinsics(calibration_matrix,
                                mutable_intrinsics() + FOCAL_LENGTH,
                                mutable_intrinsics() + SKEW,
                                mutable_intrinsics() + ASPECT_RATIO,
                                mutable_intrinsics() + PRINCIPAL_POINT_X,
                                mutable_intrinsics() + PRINCIPAL_POINT_Y);
  return true;
}

void Camera::GetProjectionMatrix(Matrix3x4d* pmatrix) const {
  Matrix3d calibration_matrix;
  GetCalibrationMatrix(&calibration_matrix);
  ComposeProjectionMatrix(calibration_matrix,
                          GetOrientationAsAngleAxis(),
                          GetPosition(),
                          pmatrix);
}

void Camera::GetCalibrationMatrix(Matrix3d* kmatrix) const {
  IntrinsicsToCalibrationMatrix(FocalLength(),
                                Skew(),
                                AspectRatio(),
                                PrincipalPointX(),
                                PrincipalPointY(),
                                kmatrix);
}

double Camera::ProjectPoint(const Vector4d& point, Vector2d* pixel) const {
  return ProjectPointToImage(extrinsics(),
                             intrinsics(),
                             point.data(),
                             pixel->data());
}

Vector3d Camera::PixelToUnitDepthRay(const Vector2d& pixel) const {
  // Remove the effect of calibration.
  const Vector3d undistorted_point = PixelToNormalizedCoordinates(pixel);

  // Apply rotation.
  const Matrix3d& rotation = GetOrientationAsRotationMatrix();
  const Vector3d direction = rotation.transpose() * undistorted_point;
  return direction;
}

Vector3d Camera::PixelToNormalizedCoordinates(const Vector2d& pixel) const {
  // First, undo the calibration.
  const double focal_length_y = FocalLength() * AspectRatio();
  const double y_normalized = (pixel[1] - PrincipalPointY()) / focal_length_y;
  const double x_normalized =
      (pixel[0] - PrincipalPointX() - y_normalized * Skew()) / FocalLength();

  // Undo radial distortion.
  const Vector2d normalized_point(x_normalized, y_normalized);
  Vector2d undistorted_pixel;
  RadialUndistortPoint(normalized_point,
                       RadialDistortion1(),
                       RadialDistortion2(),
                       &undistorted_pixel);
  const Vector3d undistorted_point = undistorted_pixel.homogeneous();
  return undistorted_point;
}

  // ----------------------- Getter and Setter methods ---------------------- //
void Camera::SetPosition(const Vector3d& position) {
  Map<Vector3d>(mutable_extrinsics() + POSITION) = position;
}

Vector3d Camera::GetPosition() const {
  return Map<const Vector3d>(extrinsics() + POSITION);
}

void Camera::SetOrientationFromRotationMatrix(const Matrix3d& rotation) {
  ceres::RotationMatrixToAngleAxis(
      ceres::ColumnMajorAdapter3x3(rotation.data()),
      mutable_extrinsics() + ORIENTATION);
}

void Camera::SetOrientationFromAngleAxis(const Vector3d& angle_axis) {
  Map<Vector3d>(mutable_extrinsics() + ORIENTATION) = angle_axis;
}

Matrix3d Camera::GetOrientationAsRotationMatrix() const {
  Matrix3d rotation;
  ceres::AngleAxisToRotationMatrix(
      extrinsics() + ORIENTATION,
      ceres::ColumnMajorAdapter3x3(rotation.data()));
  return rotation;
}

Vector3d Camera::GetOrientationAsAngleAxis() const {
  return Map<const Vector3d>(extrinsics() + ORIENTATION);
}

void Camera::SetFocalLength(const double focal_length) {
  mutable_intrinsics()[FOCAL_LENGTH] = focal_length;
}

double Camera::FocalLength() const {
  return intrinsics()[FOCAL_LENGTH];
}

void Camera::SetAspectRatio(const double aspect_ratio) {
  mutable_intrinsics()[ASPECT_RATIO] = aspect_ratio;
}
double Camera::AspectRatio() const {
  return intrinsics()[ASPECT_RATIO];
}

void Camera::SetSkew(const double skew) {
  mutable_intrinsics()[SKEW] = skew;
}

double Camera::Skew() const {
  return intrinsics()[SKEW];
}

void Camera::SetPrincipalPoint(const double principal_point_x,
                               const double principal_point_y) {
  mutable_intrinsics()[PRINCIPAL_POINT_X] = principal_point_x;
  mutable_intrinsics()[PRINCIPAL_POINT_Y] = principal_point_y;
}

double Camera::PrincipalPointX() const {
  return intrinsics()[PRINCIPAL_POINT_X];
}

double Camera::PrincipalPointY() const {
  return intrinsics()[PRINCIPAL_POINT_Y];
}

void Camera::SetRadialDistortion(const double radial_distortion_1,
                                 const double radial_distortion_2) {
  mutable_intrinsics()[RADIAL_DISTORTION_1] = radial_distortion_1;
  mutable_intrinsics()[RADIAL_DISTORTION_2] = radial_distortion_2;
}

double Camera::RadialDistortion1() const {
  return intrinsics()[RADIAL_DISTORTION_1];
}

double Camera::RadialDistortion2() const {
  return intrinsics()[RADIAL_DISTORTION_2];
}

void Camera::SetImageSize(const int image_width, const int image_height) {
  image_size_[0] = image_width;
  image_size_[1] = image_height;
}

}  // namespace theia


// based on Eigen unsupported bvh KDTree

#pragma once

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <algorithm>
#include <queue>

#include "rpxdock/bvh/bvh_algo.hpp"
#include "rpxdock/geom/primitive.hpp"
#include "rpxdock/util/types.hpp"

namespace rpxdock {
namespace bvh {

using namespace geom;
using namespace Eigen;

// internal pair class for the BVH--used instead of std::pair because of
// alignment
template <class F, int DIM>
struct VintPair {
  using first_type = Matrix<F, DIM, 1>;
  using secont_type = int;
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(F, DIM)
  VintPair(const first_type &v, int i) : first(v), second(i) {}
  first_type first;
  int second;
};

// these templates help the tree initializer get the bounding spheres either in
// from a provided iterator range or using bounding_vol in a unified way
template <typename Objs, typename Vols, typename Iter>
struct get_bvols_helper {
  void operator()(const Objs &objs, Iter sphbeg, Iter sphend, Vols &out) {
    out.insert(out.end(), sphbeg, sphend);
    eigen_assert(out.size() == objs.size());
  }
};

template <typename Objs, typename Vols>
struct get_bvols_helper<Objs, Vols, int> {
  void operator()(const Objs &objs, int, int, Vols &out) {
    out.reserve(objs.size());
    for (int i = 0; i < (int)objs.size(); ++i)
      out.push_back(bounding_vol(objs[i]));
  }
};

template <class RAiter>
struct P1Range {
  using value_type = typename RAiter::value_type::first_type;
  RAiter a, b;
  auto const &operator[](size_t i) const { return (a + i)->first; }
  auto &operator[](size_t i) { return (a + i)->first; }
  size_t size() const { return b - a; }
  auto get_index(size_t i) const { return (a + i)->second; }
};
template <class RAiter>
auto p1range(RAiter a, RAiter b) {
  P1Range<RAiter> r;
  r.a = a;
  r.b = b;
  return r;
}

template <typename F, bool idrange = false>
struct WelzlBoundingSphere {
  static Sphere<F> bound(auto subtree_objs) {
    return welzl_bounding_sphere<true>(subtree_objs);
  }
};

template <typename _Scalar, typename _Object, int _DIM = 3,
          typename _Volume = Sphere<_Scalar>,
          typename BoundingSphere = WelzlBoundingSphere<_Scalar, true>>
class SphereBVH {
 public:
  static int const DIM = _DIM;
  typedef _Object Object;
  typedef std::vector<Object, Eigen::aligned_allocator<Object>> Objs;
  typedef _Scalar F;
  // typedef Eigen::AlignedBox<F, DIM> Volume;
  typedef _Volume Volume;
  typedef std::vector<Volume, Eigen::aligned_allocator<Volume>> Vols;
  typedef int Index;
  typedef const int *VolumeIterator;  // the iterators are just pointers into
                                      // the tree's vectors
  typedef const Object *ObjectIterator;

  std::vector<int> child;  // child of x are child[2x] and
                           // child[2x+1], indices bigger than
                           // vols.size() index into objs.
  Vols vols;
  Objs objs;

  SphereBVH() {}

  template <typename Iter>
  SphereBVH(Iter begin, Iter end) {
    init(begin, end, 0, 0);
  }  // int is recognized by init as not being an iterator type

  template <typename OIter, typename BIter>
  SphereBVH(OIter begin, OIter end, BIter sphbeg, BIter sphend) {
    init(begin, end, sphbeg, sphend);
  }

  size_t size() const { return objs.size(); }

  /** Given an iterator range over \a Object references, constructs the BVH,
   * overwriting whatever is in there currently.
   * Requires that bounding_vol(Object) return a Volume. */
  template <typename Iter>
  void init(Iter begin, Iter end) {
    init(begin, end, 0, 0);
  }

  /** Given an iterator range over \a Object references and an iterator range
   * over their bounding vols,
   * constructs the BVH, overwriting whatever is in there currently. */
  template <typename OIter, typename BIter>
  void init(OIter begin, OIter end, BIter sphbeg, BIter sphend) {
    objs.clear();
    vols.clear();
    child.clear();

    objs.insert(objs.end(), begin, end);
    int n = static_cast<int>(objs.size());

    // if we have at most one object, we don't need any internal nodes
    if (n < 2) return;

    Vols ovol;
    VIPairs ocen;

    // compute the bounding vols depending on BIter type
    get_bvols_helper<Objs, Vols, BIter>()(objs, sphbeg, sphend, ovol);

    ocen.reserve(n);
    vols.reserve(n - 1);
    child.reserve(2 * n - 2);

    for (int i = 0; i < n; ++i) ocen.push_back(VIPair(ovol[i].cen, i));

    // the recursive part of the algorithm
    build(ocen, 0, n, ovol, 0);

    Objs tmp(n);
    tmp.swap(objs);
    for (int i = 0; i < n; ++i) objs[i] = tmp[ocen[i].second];
  }

  /** \returns the index of the root of the hierarchy */
  inline Index getRootIndex() const { return (int)vols.size() - 1; }

  /** Given an \a index of a node, on exit, \a vbeg and \a vend range
   * over the indices of the volume children of the node
   * and \a obeg and \a oend range over the object children of the node
   */
  EIGEN_STRONG_INLINE
  void getChildren(Index index, VolumeIterator &vbeg, VolumeIterator &vend,
                   ObjectIterator &obeg, ObjectIterator &oend) const {
    // inlining this function should open lots of optimization opportunities to
    // the compiler
    if (index < 0) {
      vbeg = vend;
      if (!objs.empty()) obeg = &(objs[0]);
      oend = obeg + objs.size();  // output all objs--necessary
                                  // when the tree has only one
                                  // object
      return;
    }

    int nvol = static_cast<int>(vols.size());

    int idx = index * 2;
    if (child[idx + 1] < nvol) {  // second index is always bigger
      vbeg = &(child[idx]);
      vend = vbeg + 2;
      obeg = oend;
    } else if (child[idx] >= nvol) {  // if both child are objs
      vbeg = vend;
      obeg = &(objs[child[idx] - nvol]);
      oend = obeg + 2;
    } else {  // if the first child is a volume and the second is an object
      vbeg = &(child[idx]);
      vend = vbeg + 1;
      obeg = &(objs[child[idx + 1] - nvol]);
      oend = obeg + 1;
    }
    // std::cout << " gc" << vend - vbeg;
  }

  inline const Volume &getVolume(Index index) const { return vols[index]; }

 private:
  typedef VintPair<F, DIM> VIPair;
  typedef std::vector<VIPair, Eigen::aligned_allocator<VIPair>> VIPairs;
  typedef Eigen::Matrix<F, DIM, 1> VectorType;

  struct AxisComparator {
    int dim;
    AxisComparator(int inDim) : dim(inDim) {}
    inline bool operator()(const VIPair &v1, const VIPair &v2) const {
      return v1.first[dim] < v2.first[dim];
    }
  };
  struct DotComparator {
    Matrix<F, DIM, 1> normal;
    DotComparator(Matrix<F, DIM, 1> n) : normal(n) {}
    template <class Pair>
    DotComparator(Pair p) : normal(p.second - p.first) {}
    inline bool operator()(const VIPair &v1, const VIPair &v2) const {
      return v1.first.dot(normal) < v2.first.dot(normal);
    }
  };

  // Build the part of the tree between objs[from] and objs[to] (not
  // including objs[to]). This routine partitions the ocen in [from, to) along
  // the dimension dim, recursively constructs the two halves, and adds their
  // parent node.  TODO: a cache-friendlier layout
  void build(VIPairs &ocen, int from, int to, Vols const &ovol,
             int dim) noexcept {
    eigen_assert(to - from > 1);
    if (to - from == 2) {
      auto merge = ovol[ocen[from].second].merged(ovol[ocen[from + 1].second]);
      vols.push_back(merge);
      child.push_back(from + (int)objs.size() - 1);
      child.push_back(from + (int)objs.size());
    } else if (to - from == 3) {
      int mid = from + 2;
      auto subtree_objs = p1range(ocen.begin() + from, ocen.begin() + to);
      nth_element(ocen.begin() + from, ocen.begin() + mid, ocen.begin() + to,
                  DotComparator(most_separated_points_on_AABB(subtree_objs)));
      // AxisComparator(dim));
      build(ocen, from, mid, ovol, (dim + 1) % DIM);
      int idx1 = (int)vols.size() - 1;
      Volume bound = BoundingSphere::bound(subtree_objs);
      vols.push_back(bound);
      // Volume merge = vols[idx1].merged(ovol[ocen[mid].second]);
      // if (merge.rad + 0.0001 < bound.rad)
      // std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
      // vols.push_back(bound.rad < merge.rad ? bound : merge);
      child.push_back(idx1);
      child.push_back(mid + (int)objs.size() - 1);
    } else {
      int mid = from + (to - from) / 2;
      auto subtree_objs = p1range(ocen.begin() + from, ocen.begin() + to);
      nth_element(ocen.begin() + from, ocen.begin() + mid, ocen.begin() + to,
                  DotComparator(most_separated_points_on_AABB(subtree_objs)));
      // AxisComparator(dim));
      build(ocen, from, mid, ovol, (dim + 1) % DIM);
      int idx1 = (int)vols.size() - 1;
      build(ocen, mid, to, ovol, (dim + 1) % DIM);
      int idx2 = (int)vols.size() - 1;
      Volume bound = BoundingSphere::bound(subtree_objs);
      vols.push_back(bound);
      // Volume merge = vols[idx1].merged(vols[idx2]);
      // if (merge.rad + 0.0001 < bound.rad)
      // std::cout << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << std::endl;
      // vols.push_back(bound.rad < merge.rad ? bound : merge);
      child.push_back(idx1);
      child.push_back(idx2);
    }
  }
};
}  // namespace bvh
}  // namespace rpxdock


#include "Rational.h"

#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>

template <typename Archive>
void Rational::serialize(Archive & ar, unsigned int const version) {
  if (version > 0) {
  } else {
  }
  ar & fraction;
}

template void Rational::serialize<boost::archive::text_oarchive>(boost::archive::text_oarchive&, unsigned int const);
template void Rational::serialize<boost::archive::text_iarchive>(boost::archive::text_iarchive&, unsigned int const);

Rational::Rational() : Rational(std::make_pair(0, 1)) { }

Rational::Rational(Fraction const & fraction_set) : fraction(Cancel(fraction_set)){ }

Rational::Rational(int numerator, unsigned int denominator) : Rational(std::make_pair(numerator, denominator)) { }

Rational::Rational(mpq_class q) : Rational(q.get_num().get_si(), q.get_den().get_ui()) { }

Fraction Rational::get_fraction() const { return fraction; }

bool Rational::IsPositive() const { return fraction.first > 0; }
bool Rational::IsNegative() const { return fraction.first < 0; }
bool Rational::IsZero()     const { return fraction.first == 0; }

void Rational::DivideByTwo() {
  if (fraction.first % 2 == 0) {
    fraction.first = fraction.first / 2;
  } else {
    fraction.second = fraction.second * 2;
  }
}

void Rational::Negate() {
  fraction.first = -1 * fraction.first;
}

Fraction Rational::Cancel(Fraction const & fraction_set) {
  unsigned int gcd { GreatestCommonDivisor(fraction_set) };
  return std::make_pair(fraction_set.first / static_cast<int>(gcd), fraction_set.second / gcd);
}

unsigned int Rational::LeastCommonMultiple(Fraction const & fraction) {
  if (fraction.first == 0) {
    return 0;
  }
  unsigned int gcd = GreatestCommonDivisor(fraction);

  if (fraction.first < 0) {
    return (static_cast<unsigned int>(-fraction.first) * fraction.second) / gcd;
  } else {
    return (static_cast<unsigned int>(fraction.first) * fraction.second) / gcd;
  }
}

unsigned int Rational::GreatestCommonDivisor(Fraction const & fraction) {
  return (fraction.first < 0 ? GreatestCommonDivisor(std::make_pair(-fraction.first, fraction.second)) : (fraction.second == 0 ? fraction.first : GreatestCommonDivisor(std::make_pair(fraction.second, fraction.first % fraction.second))));
}

Rational Rational::AddOther (Rational const & other) const {
  return Rational(fraction.first * other.fraction.second + fraction.second * other.fraction.first, fraction.second * other.fraction.second);
}

Rational Rational::SubtractOther (Rational const & other) const {
  return Rational(fraction.first * other.fraction.second - fraction.second * other.fraction.first, fraction.second * other.fraction.second);
}

Rational Rational::DivideOther (Rational const & other) const {
  int numerator = fraction.first * static_cast<int>(other.fraction.second);
  int denominator = static_cast<int>(fraction.second) * other.fraction.first;

  if (numerator * denominator < 0) {
    return Rational(-1 * std::abs(numerator), static_cast<unsigned int>(std::abs(denominator)));
  } else {
    return Rational(std::abs(numerator), static_cast<unsigned int>(std::abs(denominator)));
  }
}

Rational Rational::MultiplyOther (Rational const & other) const {
  return Rational(fraction.first * other.fraction.first, fraction.second * other.fraction.second);
}

std::string const Rational::ToString(bool plus_sign) const {
  std::string ret_string = "";
  if (fraction.first == 0) {
    if (plus_sign) {
      return "+ 0";
    } else {
      return "0";
    }
  } else if (fraction.first > 0) {
    if (plus_sign) {
      ret_string += "+ ";
    }
  } else {
    ret_string += "- ";
  }

  ret_string += std::to_string(std::abs(fraction.first));

  if (fraction.second == 1) {
    return ret_string;
  }

  ret_string += "/";
  ret_string += std::to_string(fraction.second);

  return ret_string;
}

bool Rational::operator== (Rational const & other) const { return (fraction.first == other.fraction.first) && (fraction.second == other.fraction.second); }
bool Rational::operator!= (Rational const & other) const { return !(*this == other); }
bool Rational::operator< (Rational const & other) const { return this->SubtractOther(other).fraction.first < 0; }
bool Rational::operator> (Rational const & other) const { return other < *this; }
bool Rational::operator<= (Rational const & other) const { return !(*this > other); }
bool Rational::operator>= (Rational const & other) const { return !(*this < other); }


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

#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <NTL/GF2E.h>
#include <NTL/GF2EX.h>
#include <NTL/GF2EXFactoring.h>

#include "gcm.h"

using namespace std;
using namespace NTL;

int main(int argc, char **argv)
{
    struct slice a1, c1, t1, a2, c2, h;
    struct slice *sp[] = {&a1, &c1, &t1, &a2, &c2, &h};
    int i;
    GF2E x, y, z;
    GF2EX p1, p2;
    char out[2*blocklen + 1];

    argc -= 1;
    argv += 1;

    assert(argc == 6);

    for (i = 0; i < argc; i += 1) {
        hex2bytes(sp[i], argv[i]);
    }

    initfield();

    block2felem(x, &h);
    block2felem(y, &t1);
    buildpoly(p1, &a1, &c1, &t1);
    eval(z, p1, x);

    buildpoly(p2, &a2, &c2, &t1);
    add(p2, p2, y);
    add(p2, p2, z);
    eval(z, p2, x);

    felem2hex(out, z);
    printf("%s\n", out);

    return 0;
}