// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2018-2021 www.open3d.org
//
// 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.
// ----------------------------------------------------------------------------

#include <Eigen/Dense>
#include <iostream>
#include <list>

#include "open3d/geometry/IntersectionTest.h"
#include "open3d/geometry/KDTreeFlann.h"
#include "open3d/geometry/PointCloud.h"
#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Logging.h"

namespace open3d {
namespace geometry {

class BallPivotingVertex;
class BallPivotingEdge;
class BallPivotingTriangle;

typedef BallPivotingVertex* BallPivotingVertexPtr;
typedef std::shared_ptr<BallPivotingEdge> BallPivotingEdgePtr;
typedef std::shared_ptr<BallPivotingTriangle> BallPivotingTrianglePtr;

class BallPivotingVertex {
public:
    enum Type { Orphan = 0, Front = 1, Inner = 2 };

    BallPivotingVertex(int idx,
                       const Eigen::Vector3d& point,
                       const Eigen::Vector3d& normal)
        : idx_(idx), point_(point), normal_(normal), type_(Orphan) {}

    void UpdateType();

public:
    int idx_;
    const Eigen::Vector3d& point_;
    const Eigen::Vector3d& normal_;
    std::unordered_set<BallPivotingEdgePtr> edges_;
    Type type_;
};

class BallPivotingEdge {
public:
    enum Type { Border = 0, Front = 1, Inner = 2 };

    BallPivotingEdge(BallPivotingVertexPtr source, BallPivotingVertexPtr target)
        : source_(source), target_(target), type_(Type::Front) {}

    void AddAdjacentTriangle(BallPivotingTrianglePtr triangle);
    BallPivotingVertexPtr GetOppositeVertex();

public:
    BallPivotingVertexPtr source_;
    BallPivotingVertexPtr target_;
    BallPivotingTrianglePtr triangle0_;
    BallPivotingTrianglePtr triangle1_;
    Type type_;
};

class BallPivotingTriangle {
public:
    BallPivotingTriangle(BallPivotingVertexPtr vert0,
                         BallPivotingVertexPtr vert1,
                         BallPivotingVertexPtr vert2,
                         Eigen::Vector3d ball_center)
        : vert0_(vert0),
          vert1_(vert1),
          vert2_(vert2),
          ball_center_(ball_center) {}

public:
    BallPivotingVertexPtr vert0_;
    BallPivotingVertexPtr vert1_;
    BallPivotingVertexPtr vert2_;
    Eigen::Vector3d ball_center_;
};

void BallPivotingVertex::UpdateType() {
    if (edges_.empty()) {
        type_ = Type::Orphan;
    } else {
        for (const BallPivotingEdgePtr& edge : edges_) {
            if (edge->type_ != BallPivotingEdge::Type::Inner) {
                type_ = Type::Front;
                return;
            }
        }
        type_ = Type::Inner;
    }
}

void BallPivotingEdge::AddAdjacentTriangle(BallPivotingTrianglePtr triangle) {
    if (triangle != triangle0_ && triangle != triangle1_) {
        if (triangle0_ == nullptr) {
            triangle0_ = triangle;
            type_ = Type::Front;
            // update orientation
            if (BallPivotingVertexPtr opp = GetOppositeVertex()) {
                Eigen::Vector3d tr_norm =
                        (target_->point_ - source_->point_)
                                .cross(opp->point_ - source_->point_);
                tr_norm /= tr_norm.norm();
                Eigen::Vector3d pt_norm =
                        source_->normal_ + target_->normal_ + opp->normal_;
                pt_norm /= pt_norm.norm();
                if (pt_norm.dot(tr_norm) < 0) {
                    std::swap(target_, source_);
                }
            } else {
                utility::LogError("GetOppositeVertex() returns nullptr.");
            }
        } else if (triangle1_ == nullptr) {
            triangle1_ = triangle;
            type_ = Type::Inner;
        } else {
            utility::LogDebug("!!! This case should not happen");
        }
    }
}

BallPivotingVertexPtr BallPivotingEdge::GetOppositeVertex() {
    if (triangle0_ != nullptr) {
        if (triangle0_->vert0_->idx_ != source_->idx_ &&
            triangle0_->vert0_->idx_ != target_->idx_) {
            return triangle0_->vert0_;
        } else if (triangle0_->vert1_->idx_ != source_->idx_ &&
                   triangle0_->vert1_->idx_ != target_->idx_) {
            return triangle0_->vert1_;
        } else {
            return triangle0_->vert2_;
        }
    } else {
        return nullptr;
    }
}

class BallPivoting {
public:
    BallPivoting(const PointCloud& pcd)
        : has_normals_(pcd.HasNormals()), kdtree_(pcd) {
        mesh_ = std::make_shared<TriangleMesh>();
        mesh_->vertices_ = pcd.points_;
        mesh_->vertex_normals_ = pcd.normals_;
        mesh_->vertex_colors_ = pcd.colors_;
        for (size_t vidx = 0; vidx < pcd.points_.size(); ++vidx) {
            vertices.emplace_back(new BallPivotingVertex(static_cast<int>(vidx),
                                                         pcd.points_[vidx],
                                                         pcd.normals_[vidx]));
        }
    }

    virtual ~BallPivoting() {
        for (auto vert : vertices) {
            delete vert;
        }
    }

    bool ComputeBallCenter(int vidx1,
                           int vidx2,
                           int vidx3,
                           double radius,
                           Eigen::Vector3d& center) {
        const Eigen::Vector3d& v1 = vertices[vidx1]->point_;
        const Eigen::Vector3d& v2 = vertices[vidx2]->point_;
        const Eigen::Vector3d& v3 = vertices[vidx3]->point_;
        double c = (v2 - v1).squaredNorm();
        double b = (v1 - v3).squaredNorm();
        double a = (v3 - v2).squaredNorm();

        double alpha = a * (b + c - a);
        double beta = b * (a + c - b);
        double gamma = c * (a + b - c);
        double abg = alpha + beta + gamma;

        if (abg < 1e-16) {
            return false;
        }

        alpha = alpha / abg;
        beta = beta / abg;
        gamma = gamma / abg;

        Eigen::Vector3d circ_center = alpha * v1 + beta * v2 + gamma * v3;
        double circ_radius2 = a * b * c;

        a = std::sqrt(a);
        b = std::sqrt(b);
        c = std::sqrt(c);
        circ_radius2 = circ_radius2 /
                       ((a + b + c) * (b + c - a) * (c + a - b) * (a + b - c));

        double height = radius * radius - circ_radius2;
        if (height >= 0.0) {
            Eigen::Vector3d tr_norm = (v2 - v1).cross(v3 - v1);
            tr_norm /= tr_norm.norm();
            Eigen::Vector3d pt_norm = vertices[vidx1]->normal_ +
                                      vertices[vidx2]->normal_ +
                                      vertices[vidx3]->normal_;
            pt_norm /= pt_norm.norm();
            if (tr_norm.dot(pt_norm) < 0) {
                tr_norm *= -1;
            }

            height = sqrt(height);
            center = circ_center + height * tr_norm;
            return true;
        }
        return false;
    }

    BallPivotingEdgePtr GetLinkingEdge(const BallPivotingVertexPtr& v0,
                                       const BallPivotingVertexPtr& v1) {
        for (BallPivotingEdgePtr edge0 : v0->edges_) {
            for (BallPivotingEdgePtr edge1 : v1->edges_) {
                if (edge0->source_->idx_ == edge1->source_->idx_ &&
                    edge0->target_->idx_ == edge1->target_->idx_) {
                    return edge0;
                }
            }
        }
        return nullptr;
    }

    void CreateTriangle(const BallPivotingVertexPtr& v0,
                        const BallPivotingVertexPtr& v1,
                        const BallPivotingVertexPtr& v2,
                        const Eigen::Vector3d& center) {
        utility::LogDebug(
                "[CreateTriangle] with v0.idx={}, v1.idx={}, v2.idx={}",
                v0->idx_, v1->idx_, v2->idx_);
        BallPivotingTrianglePtr triangle =
                std::make_shared<BallPivotingTriangle>(v0, v1, v2, center);

        BallPivotingEdgePtr e0 = GetLinkingEdge(v0, v1);
        if (e0 == nullptr) {
            e0 = std::make_shared<BallPivotingEdge>(v0, v1);
        }
        e0->AddAdjacentTriangle(triangle);
        v0->edges_.insert(e0);
        v1->edges_.insert(e0);

        BallPivotingEdgePtr e1 = GetLinkingEdge(v1, v2);
        if (e1 == nullptr) {
            e1 = std::make_shared<BallPivotingEdge>(v1, v2);
        }
        e1->AddAdjacentTriangle(triangle);
        v1->edges_.insert(e1);
        v2->edges_.insert(e1);

        BallPivotingEdgePtr e2 = GetLinkingEdge(v2, v0);
        if (e2 == nullptr) {
            e2 = std::make_shared<BallPivotingEdge>(v2, v0);
        }
        e2->AddAdjacentTriangle(triangle);
        v2->edges_.insert(e2);
        v0->edges_.insert(e2);

        v0->UpdateType();
        v1->UpdateType();
        v2->UpdateType();

        Eigen::Vector3d face_normal =
                ComputeFaceNormal(v0->point_, v1->point_, v2->point_);
        if (face_normal.dot(v0->normal_) > -1e-16) {
            mesh_->triangles_.emplace_back(
                    Eigen::Vector3i(v0->idx_, v1->idx_, v2->idx_));
        } else {
            mesh_->triangles_.emplace_back(
                    Eigen::Vector3i(v0->idx_, v2->idx_, v1->idx_));
        }
        mesh_->triangle_normals_.push_back(face_normal);
    }

    Eigen::Vector3d ComputeFaceNormal(const Eigen::Vector3d& v0,
                                      const Eigen::Vector3d& v1,
                                      const Eigen::Vector3d& v2) {
        Eigen::Vector3d normal = (v1 - v0).cross(v2 - v0);
        double norm = normal.norm();
        if (norm > 0) {
            normal /= norm;
        }
        return normal;
    }

    bool IsCompatible(const BallPivotingVertexPtr& v0,
                      const BallPivotingVertexPtr& v1,
                      const BallPivotingVertexPtr& v2) {
        utility::LogDebug("[IsCompatible] v0.idx={}, v1.idx={}, v2.idx={}",
                          v0->idx_, v1->idx_, v2->idx_);
        Eigen::Vector3d normal =
                ComputeFaceNormal(v0->point_, v1->point_, v2->point_);
        if (normal.dot(v0->normal_) < -1e-16) {
            normal *= -1;
        }
        bool ret = normal.dot(v0->normal_) > -1e-16 &&
                   normal.dot(v1->normal_) > -1e-16 &&
                   normal.dot(v2->normal_) > -1e-16;
        utility::LogDebug("[IsCompatible] returns = {}", ret);
        return ret;
    }

    BallPivotingVertexPtr FindCandidateVertex(
            const BallPivotingEdgePtr& edge,
            double radius,
            Eigen::Vector3d& candidate_center) {
        utility::LogDebug("[FindCandidateVertex] edge=({}, {}), radius={}",
                          edge->source_->idx_, edge->target_->idx_, radius);
        BallPivotingVertexPtr src = edge->source_;
        BallPivotingVertexPtr tgt = edge->target_;

        const BallPivotingVertexPtr opp = edge->GetOppositeVertex();
        if (opp == nullptr) {
            utility::LogError("edge->GetOppositeVertex() returns nullptr.");
        }
        utility::LogDebug("[FindCandidateVertex] edge=({}, {}), opp={}",
                          src->idx_, tgt->idx_, opp->idx_);
        utility::LogDebug("[FindCandidateVertex] src={} => {}", src->idx_,
                          src->point_.transpose());
        utility::LogDebug("[FindCandidateVertex] tgt={} => {}", tgt->idx_,
                          tgt->point_.transpose());
        utility::LogDebug("[FindCandidateVertex] src={} => {}", opp->idx_,
                          opp->point_.transpose());

        Eigen::Vector3d mp = 0.5 * (src->point_ + tgt->point_);
        utility::LogDebug("[FindCandidateVertex] edge=({}, {}), mp={}",
                          edge->source_->idx_, edge->target_->idx_,
                          mp.transpose());

        BallPivotingTrianglePtr triangle = edge->triangle0_;
        const Eigen::Vector3d& center = triangle->ball_center_;
        utility::LogDebug("[FindCandidateVertex] edge=({}, {}), center={}",
                          edge->source_->idx_, edge->target_->idx_,
                          center.transpose());

        Eigen::Vector3d v = tgt->point_ - src->point_;
        v /= v.norm();

        Eigen::Vector3d a = center - mp;
        a /= a.norm();

        std::vector<int> indices;
        std::vector<double> dists2;
        kdtree_.SearchRadius(mp, 2 * radius, indices, dists2);
        utility::LogDebug("[FindCandidateVertex] found {} potential candidates",
                          indices.size());

        BallPivotingVertexPtr min_candidate = nullptr;
        double min_angle = 2 * M_PI;
        for (auto nbidx : indices) {
            utility::LogDebug("[FindCandidateVertex] nbidx {:d}", nbidx);
            const BallPivotingVertexPtr& candidate = vertices[nbidx];
            if (candidate->idx_ == src->idx_ || candidate->idx_ == tgt->idx_ ||
                candidate->idx_ == opp->idx_) {
                utility::LogDebug(
                        "[FindCandidateVertex] candidate {:d} is a triangle "
                        "vertex of the edge",
                        candidate->idx_);
                continue;
            }
            utility::LogDebug("[FindCandidateVertex] candidate={:d} => {}",
                              candidate->idx_, candidate->point_.transpose());

            bool coplanar = IntersectionTest::PointsCoplanar(
                    src->point_, tgt->point_, opp->point_, candidate->point_);
            if (coplanar && (IntersectionTest::LineSegmentsMinimumDistance(
                                     mp, candidate->point_, src->point_,
                                     opp->point_) < 1e-12 ||
                             IntersectionTest::LineSegmentsMinimumDistance(
                                     mp, candidate->point_, tgt->point_,
                                     opp->point_) < 1e-12)) {
                utility::LogDebug(
                        "[FindCandidateVertex] candidate {:d} is intersecting "
                        "the existing triangle",
                        candidate->idx_);
                continue;
            }

            Eigen::Vector3d new_center;
            if (!ComputeBallCenter(src->idx_, tgt->idx_, candidate->idx_,
                                   radius, new_center)) {
                utility::LogDebug(
                        "[FindCandidateVertex] candidate {:d} can not compute "
                        "ball",
                        candidate->idx_);
                continue;
            }
            utility::LogDebug("[FindCandidateVertex] candidate {:d} center={}",
                              candidate->idx_, new_center.transpose());

            Eigen::Vector3d b = new_center - mp;
            b /= b.norm();
            utility::LogDebug(
                    "[FindCandidateVertex] candidate {:d} v={}, a={}, b={}",
                    candidate->idx_, v.transpose(), a.transpose(),
                    b.transpose());

            double cosinus = a.dot(b);
            cosinus = std::min(cosinus, 1.0);
            cosinus = std::max(cosinus, -1.0);
            utility::LogDebug(
                    "[FindCandidateVertex] candidate {:d} cosinus={:f}",
                    candidate->idx_, cosinus);

            double angle = std::acos(cosinus);

            Eigen::Vector3d c = a.cross(b);
            if (c.dot(v) < 0) {
                angle = 2 * M_PI - angle;
            }

            if (angle >= min_angle) {
                utility::LogDebug(
                        "[FindCandidateVertex] candidate {:d} angle {:f} > "
                        "min_angle {:f}",
                        candidate->idx_, angle, min_angle);
                continue;
            }

            bool empty_ball = true;
            for (auto nbidx2 : indices) {
                const BallPivotingVertexPtr& nb = vertices[nbidx2];
                if (nb->idx_ == src->idx_ || nb->idx_ == tgt->idx_ ||
                    nb->idx_ == candidate->idx_) {
                    continue;
                }
                if ((new_center - nb->point_).norm() < radius - 1e-16) {
                    utility::LogDebug(
                            "[FindCandidateVertex] candidate {:d} not an empty "
                            "ball",
                            candidate->idx_);
                    empty_ball = false;
                    break;
                }
            }

            if (empty_ball) {
                utility::LogDebug("[FindCandidateVertex] candidate {:d} works",
                                  candidate->idx_);
                min_angle = angle;
                min_candidate = vertices[nbidx];
                candidate_center = new_center;
            }
        }

        if (min_candidate == nullptr) {
            utility::LogDebug("[FindCandidateVertex] returns nullptr");
        } else {
            utility::LogDebug("[FindCandidateVertex] returns {:d}",
                              min_candidate->idx_);
        }
        return min_candidate;
    }

    void ExpandTriangulation(double radius) {
        utility::LogDebug("[ExpandTriangulation] radius={}", radius);
        while (!edge_front_.empty()) {
            BallPivotingEdgePtr edge = edge_front_.front();
            edge_front_.pop_front();
            if (edge->type_ != BallPivotingEdge::Front) {
                continue;
            }

            Eigen::Vector3d center;
            BallPivotingVertexPtr candidate =
                    FindCandidateVertex(edge, radius, center);
            if (candidate == nullptr ||
                candidate->type_ == BallPivotingVertex::Type::Inner ||
                !IsCompatible(candidate, edge->source_, edge->target_)) {
                edge->type_ = BallPivotingEdge::Type::Border;
                border_edges_.push_back(edge);
                continue;
            }

            BallPivotingEdgePtr e0 = GetLinkingEdge(candidate, edge->source_);
            BallPivotingEdgePtr e1 = GetLinkingEdge(candidate, edge->target_);
            if ((e0 != nullptr && e0->type_ != BallPivotingEdge::Type::Front) ||
                (e1 != nullptr && e1->type_ != BallPivotingEdge::Type::Front)) {
                edge->type_ = BallPivotingEdge::Type::Border;
                border_edges_.push_back(edge);
                continue;
            }

            CreateTriangle(edge->source_, edge->target_, candidate, center);

            e0 = GetLinkingEdge(candidate, edge->source_);
            e1 = GetLinkingEdge(candidate, edge->target_);
            if (e0->type_ == BallPivotingEdge::Type::Front) {
                edge_front_.push_front(e0);
            }
            if (e1->type_ == BallPivotingEdge::Type::Front) {
                edge_front_.push_front(e1);
            }
        }
    }

    bool TryTriangleSeed(const BallPivotingVertexPtr& v0,
                         const BallPivotingVertexPtr& v1,
                         const BallPivotingVertexPtr& v2,
                         const std::vector<int>& nb_indices,
                         double radius,
                         Eigen::Vector3d& center) {
        utility::LogDebug(
                "[TryTriangleSeed] v0.idx={}, v1.idx={}, v2.idx={}, "
                "radius={}",
                v0->idx_, v1->idx_, v2->idx_, radius);

        if (!IsCompatible(v0, v1, v2)) {
            return false;
        }

        BallPivotingEdgePtr e0 = GetLinkingEdge(v0, v2);
        BallPivotingEdgePtr e1 = GetLinkingEdge(v1, v2);
        if (e0 != nullptr && e0->type_ == BallPivotingEdge::Type::Inner) {
            utility::LogDebug(
                    "[TryTriangleSeed] returns {} because e0 is inner edge",
                    false);
            return false;
        }
        if (e1 != nullptr && e1->type_ == BallPivotingEdge::Type::Inner) {
            utility::LogDebug(
                    "[TryTriangleSeed] returns {} because e1 is inner edge",
                    false);
            return false;
        }

        if (!ComputeBallCenter(v0->idx_, v1->idx_, v2->idx_, radius, center)) {
            utility::LogDebug(
                    "[TryTriangleSeed] returns {} could not compute ball "
                    "center",
                    false);
            return false;
        }

        // test if no other point is within the ball
        for (const auto& nbidx : nb_indices) {
            const BallPivotingVertexPtr& v = vertices[nbidx];
            if (v->idx_ == v0->idx_ || v->idx_ == v1->idx_ ||
                v->idx_ == v2->idx_) {
                continue;
            }
            if ((center - v->point_).norm() < radius - 1e-16) {
                utility::LogDebug(
                        "[TryTriangleSeed] returns {} computed ball is not "
                        "empty",
                        false);
                return false;
            }
        }

        utility::LogDebug("[TryTriangleSeed] returns {}", true);
        return true;
    }

    bool TrySeed(BallPivotingVertexPtr& v, double radius) {
        utility::LogDebug("[TrySeed] with v.idx={}, radius={}", v->idx_,
                          radius);
        std::vector<int> indices;
        std::vector<double> dists2;
        kdtree_.SearchRadius(v->point_, 2 * radius, indices, dists2);
        if (indices.size() < 3u) {
            return false;
        }

        for (size_t nbidx0 = 0; nbidx0 < indices.size(); ++nbidx0) {
            const BallPivotingVertexPtr& nb0 = vertices[indices[nbidx0]];
            if (nb0->type_ != BallPivotingVertex::Type::Orphan) {
                continue;
            }
            if (nb0->idx_ == v->idx_) {
                continue;
            }

            int candidate_vidx2 = -1;
            Eigen::Vector3d center;
            for (size_t nbidx1 = nbidx0 + 1; nbidx1 < indices.size();
                 ++nbidx1) {
                const BallPivotingVertexPtr& nb1 = vertices[indices[nbidx1]];
                if (nb1->type_ != BallPivotingVertex::Type::Orphan) {
                    continue;
                }
                if (nb1->idx_ == v->idx_) {
                    continue;
                }
                if (TryTriangleSeed(v, nb0, nb1, indices, radius, center)) {
                    candidate_vidx2 = nb1->idx_;
                    break;
                }
            }

            if (candidate_vidx2 >= 0) {
                const BallPivotingVertexPtr& nb1 = vertices[candidate_vidx2];

                BallPivotingEdgePtr e0 = GetLinkingEdge(v, nb1);
                if (e0 != nullptr &&
                    e0->type_ != BallPivotingEdge::Type::Front) {
                    continue;
                }
                BallPivotingEdgePtr e1 = GetLinkingEdge(nb0, nb1);
                if (e1 != nullptr &&
                    e1->type_ != BallPivotingEdge::Type::Front) {
                    continue;
                }
                BallPivotingEdgePtr e2 = GetLinkingEdge(v, nb0);
                if (e2 != nullptr &&
                    e2->type_ != BallPivotingEdge::Type::Front) {
                    continue;
                }

                CreateTriangle(v, nb0, nb1, center);

                e0 = GetLinkingEdge(v, nb1);
                e1 = GetLinkingEdge(nb0, nb1);
                e2 = GetLinkingEdge(v, nb0);
                if (e0->type_ == BallPivotingEdge::Type::Front) {
                    edge_front_.push_front(e0);
                }
                if (e1->type_ == BallPivotingEdge::Type::Front) {
                    edge_front_.push_front(e1);
                }
                if (e2->type_ == BallPivotingEdge::Type::Front) {
                    edge_front_.push_front(e2);
                }

                if (edge_front_.size() > 0) {
                    utility::LogDebug(
                            "[TrySeed] edge_front_.size() > 0 => return "
                            "true");
                    return true;
                }
            }
        }

        utility::LogDebug("[TrySeed] return false");
        return false;
    }

    void FindSeedTriangle(double radius) {
        for (size_t vidx = 0; vidx < vertices.size(); ++vidx) {
            utility::LogDebug("[FindSeedTriangle] with radius={}, vidx={}",
                              radius, vidx);
            if (vertices[vidx]->type_ == BallPivotingVertex::Type::Orphan) {
                if (TrySeed(vertices[vidx], radius)) {
                    ExpandTriangulation(radius);
                }
            }
        }
    }

    std::shared_ptr<TriangleMesh> Run(const std::vector<double>& radii) {
        if (!has_normals_) {
            utility::LogError("ReconstructBallPivoting requires normals");
        }

        mesh_->triangles_.clear();

        for (double radius : radii) {
            utility::LogDebug("[Run] ################################");
            utility::LogDebug("[Run] change to radius {:.4f}", radius);
            if (radius <= 0) {
                utility::LogError(
                        "got an invalid, negative radius as parameter");
            }

            // update radius => update border edges
            for (auto it = border_edges_.begin(); it != border_edges_.end();) {
                BallPivotingEdgePtr edge = *it;
                BallPivotingTrianglePtr triangle = edge->triangle0_;
                utility::LogDebug(
                        "[Run] try edge {:d}-{:d} of triangle {:d}-{:d}-{:d}",
                        edge->source_->idx_, edge->target_->idx_,
                        triangle->vert0_->idx_, triangle->vert1_->idx_,
                        triangle->vert2_->idx_);

                Eigen::Vector3d center;
                if (ComputeBallCenter(triangle->vert0_->idx_,
                                      triangle->vert1_->idx_,
                                      triangle->vert2_->idx_, radius, center)) {
                    utility::LogDebug("[Run]   yes, we can work on this");
                    std::vector<int> indices;
                    std::vector<double> dists2;
                    kdtree_.SearchRadius(center, radius, indices, dists2);
                    bool empty_ball = true;
                    for (auto idx : indices) {
                        if (idx != triangle->vert0_->idx_ &&
                            idx != triangle->vert1_->idx_ &&
                            idx != triangle->vert2_->idx_) {
                            utility::LogDebug(
                                    "[Run]   but no, the ball is not empty");
                            empty_ball = false;
                            break;
                        }
                    }

                    if (empty_ball) {
                        utility::LogDebug(
                                "[Run]   yeah, add edge to edge_front_: {:d}",
                                edge_front_.size());
                        edge->type_ = BallPivotingEdge::Type::Front;
                        edge_front_.push_back(edge);
                        it = border_edges_.erase(it);
                        continue;
                    }
                }
                ++it;
            }

            // do the reconstruction
            if (edge_front_.empty()) {
                FindSeedTriangle(radius);
            } else {
                ExpandTriangulation(radius);
            }

            utility::LogDebug("[Run] mesh_ has {:d} triangles",
                              mesh_->triangles_.size());
            utility::LogDebug("[Run] ################################");
        }
        return mesh_;
    }

private:
    bool has_normals_;
    KDTreeFlann kdtree_;
    std::list<BallPivotingEdgePtr> edge_front_;
    std::list<BallPivotingEdgePtr> border_edges_;
    std::vector<BallPivotingVertexPtr> vertices;
    std::shared_ptr<TriangleMesh> mesh_;
};

std::shared_ptr<TriangleMesh> TriangleMesh::CreateFromPointCloudBallPivoting(
        const PointCloud& pcd, const std::vector<double>& radii) {
    BallPivoting bp(pcd);
    return bp.Run(radii);
}

}  // namespace geometry
}  // namespace open3d


#include "Frame.h"
#include "FeatureFrontEndCV.h"
#include "FrameWiseGeometry.h"

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <geometry_msgs/QuaternionStamped.h> //for DJI.

#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Quaternion.h> //for apm and pixhawk.

#include <visualization_msgs/Marker.h> //for visualization.

#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <ros/duration.h>
#include <math.h>

#include <geometry_msgs/PoseStamped.h> //for px4's external pose estimate
#include <sensor_msgs/Imu.h>

#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

#include <Eigen/Core>
#include <Eigen/Dense>  // linear algebra
#include <Eigen/StdVector>

#include <deque>
#include <chrono>
#include "SLAM_simple.h"
using namespace std;

SLAM_simple* pSLAM;
void FetchImageCallback(const sensor_msgs::ImageConstPtr& img1,const sensor_msgs::ImageConstPtr& img2,const sensor_msgs::ImageConstPtr& img3,const sensor_msgs::ImageConstPtr& img4)
{
    LOG(INFO)<<"In Fetch Image Callback:"<<endl;
    cv_bridge::CvImageConstPtr p1,p2,p3,p4;
    shared_ptr<cv::Mat> m1,m2,m3,m4;
    try
    {
        p1 = cv_bridge::toCvShare(img1);
        p2 = cv_bridge::toCvShare(img2);
        p3 = cv_bridge::toCvShare(img3);
        p4 = cv_bridge::toCvShare(img4);
        m1 = shared_ptr<cv::Mat> (new cv::Mat(p1->image.clone()));
        m2 = shared_ptr<cv::Mat> (new cv::Mat(p2->image.clone()));
        m3 = shared_ptr<cv::Mat> (new cv::Mat(p3->image.clone()));
        m4 = shared_ptr<cv::Mat> (new cv::Mat(p4->image.clone()));
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        LOG(ERROR)<<"cv_bridge exception: %s"<<e.what()<<endl;
        cout<<"cv_bridge exception: %s"<<e.what()<<endl;
        return;
    }
    LOG(INFO)<<"Images caught! Will call iterateWith4Imgs."<<endl;
    pSLAM->iterateWith4Imgs(m1,m2,m3,m4);
    LOG(INFO)<<"iterateWith4Imgs() finished."<<endl;
}
void FetchIMUCallBack(const sensor_msgs::ImuConstPtr& imu)
{
    sensor_msgs::Imu imu_copy = *imu;
    pSLAM->addIMUInfo(imu_copy);
}

int main(int argc,char** argv)
{
    google::InitGoogleLogging(argv[0]);
    ros::init(argc,argv,"test_VO_node");
    ros::NodeHandle nh;
    std::string front_left_topic("/gi/forward/left/image_raw"),front_right_topic("/gi/forward/right/image_raw"),left_left_topic("/gi/leftward/left/image_raw"),left_right_topic("/gi/leftward/right/image_raw");

    message_filters::Subscriber<sensor_msgs::Image> front_left_sub(nh, front_left_topic, 10);
    message_filters::Subscriber<sensor_msgs::Image> front_right_sub(nh, front_right_topic, 10);
    message_filters::Subscriber<sensor_msgs::Image> down_left_sub(nh, left_left_topic, 10);
    message_filters::Subscriber<sensor_msgs::Image> down_right_sub(nh, left_right_topic, 10);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image,sensor_msgs::Image,sensor_msgs::Image> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), front_left_sub, front_right_sub,down_left_sub,down_right_sub);
    sync.setMaxIntervalDuration(ros::Duration(0.01));
    sync.registerCallback(boost::bind(FetchImageCallback, _1, _2,_3,_4));
    ros::Subscriber sub = nh.subscribe("/mavros/imu/data_raw",100,FetchIMUCallBack);
    pSLAM = new SLAM_simple(argc,argv);
    ros::spin();
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_DETAIL_SHUFFLE_DEFAULT_MATCHER_HPP_INCLUDED
#define BOOST_SIMD_DETAIL_SHUFFLE_DEFAULT_MATCHER_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/function/extract.hpp>
#include <boost/simd/function/combine.hpp>
#include <boost/simd/function/slice.hpp>
#include <boost/simd/arch/common/simd/function/shuffle/identity.hpp>
#include <boost/simd/arch/common/simd/function/shuffle/broadcast.hpp>

namespace boost { namespace simd { namespace detail
{
  // -----------------------------------------------------------------------------------------------
  // Half-permutation snatcher
  template<bool isUpper, int... Ps> struct half_
  {
    using perm = brigand::integral_list<int,Ps...>;
    template<typename I, typename C>
    struct apply : brigand::at_c < perm, isUpper ? I::value+C::value : I::value>
    {};
  };

  // -----------------------------------------------------------------------------------------------
  // Default matcher - don't do anything special but shuffle manually
  struct  default_matcher
        : identity_shuffle
        , broadcast_shuffle
  {
    // please, trigger on identity & broadcast pattern to do the correct thing
    using identity_shuffle::process;
    using broadcast_shuffle::process;

    // Unary helpers
    template<typename T, int N> BOOST_FORCEINLINE static typename T::value_type
    fill_( const T& a0, std::integral_constant<int,N> const& )
    {
      return boost::simd::extract<N>(a0);
    }

    template<typename T> BOOST_FORCEINLINE static typename T::value_type
    fill_(const T&, std::integral_constant<int,-1> const&)
    {
      return typename T::value_type{0};
    }

    // Unary permutation handler
    template<typename T, int... P>
    static BOOST_FORCEINLINE T process(T const& a0, pattern_<P...> const& p)
    {
      return process(a0, p, typename T::storage_kind{});
    }

    template<typename T, int... P>
    static BOOST_FORCEINLINE T process(T const& a0, pattern_<P...> const&, aggregate_storage const&)
    {
      // Aggregate storage has a chance to be the combination of two regular shuffles
      auto s = slice(a0);
      return combine( shuffle<half_<false,P...>>(s[0],s[1])
                    , shuffle<half_<true ,P...>>(s[0],s[1])
                    );
    }

    template<typename K, typename T, int... P>
    static BOOST_FORCEINLINE T process(T const& a0, pattern_<P...> const&, K const&)
    { return T( fill_ (a0 , std::integral_constant<int,P>{} )... );
    }

    // Binary permutation handler
    template<typename T, int... P>
    static BOOST_FORCEINLINE T process(T const& a0, T const& a1, pattern_<P...> const&)
    {
      return T( fill_ (a0 ,a1 , std::integral_constant<int,P>{}
                              , brigand::bool_<(P<T::static_size)>{}
                      )...
              );
    }

    // Binary helpers
    template<typename T, int N>
    BOOST_FORCEINLINE static typename T::value_type
    fill_ ( const T& a0, const T&
          , std::integral_constant<int,N> const&, std::true_type const&
          )
    {
      return  boost::simd::extract<N>(a0);
    }

    template<typename T>
    BOOST_FORCEINLINE static typename T::value_type
    fill_ ( const T&, const T&
          , std::integral_constant<int,-1> const&, std::true_type const&
          )
    {
      return 0;
    }

    template<typename T, int N>
    BOOST_FORCEINLINE static typename T::value_type
    fill_ (const T&, const T & a1
          , std::integral_constant<int,N> const&, std::false_type const&
          )
    {
      return  boost::simd::extract<N-T::static_size>(a1);
    }
  };
} } }

#endif


#include "annotator.hpp"
#include "extractor.hpp"

#include <cstddef>

// For boost RTree
#include <boost/geometry.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/geometry/strategies/spherical/distance_haversine.hpp>
//#include <boost/log/trivial.hpp>

RouteAnnotator::RouteAnnotator(const Database &db) : db(db) {}

std::vector<internal_nodeid_t>
RouteAnnotator::coordinates_to_internal(const std::vector<point_t> &points)
{

    static const boost::geometry::strategy::distance::haversine<double> haversine(6372795.0);
    static const double MAX_DISTANCE = 5;

    if (!db.rtree)
        throw RtreeError("RTree is null - call build_rtree() on database before use");

    std::vector<internal_nodeid_t> internal_nodeids;
    for (const auto &point : points)
    {
        std::vector<value_t> rtree_results;

        // Search for the nearest point to the one supplied
        db.rtree->query(boost::geometry::index::nearest(point, 1),
                        std::back_inserter(rtree_results));

        // If we got exactly one hit (we should), append it to our nodeids list, if it
        // was within 1m
        /*
        BOOST_LOG_TRIVIAL(debug) << "Distance from " << point.get<0>() << "," << point.get<1>()
                                 << " to  " << rtree_results[0].first.get<0>() << ","
                                 << rtree_results[0].first.get<1>() << " is "
                                 << boost::geometry::distance(point, rtree_results[0].first,
                                                              haversine)
                                 << "\n";
        */
        if (rtree_results.size() == 1 &&
            boost::geometry::distance(point, rtree_results[0].first, haversine) < MAX_DISTANCE)
        {
            internal_nodeids.push_back(rtree_results[0].second);
        }
        // otherwise, insert a 0 value, this coordinate didn't match
        else
        {
            internal_nodeids.push_back(INVALID_INTERNAL_NODEID);
        }
    }
    return internal_nodeids;
}

std::vector<internal_nodeid_t>
RouteAnnotator::external_to_internal(const std::vector<external_nodeid_t> &external_nodeids)
{
    // Convert external node ids into internal ones
    std::vector<internal_nodeid_t> results;
    std::for_each(external_nodeids.begin(), external_nodeids.end(),
                  [this, &results](const external_nodeid_t n) {
                      const auto internal_node_id = db.external_internal_map.find(n);
                      if (internal_node_id == db.external_internal_map.end())
                      {
                          // Push an invalid nodeid into the list if we didn't match
                          results.push_back(INVALID_INTERNAL_NODEID);
                      }
                      else
                      {
                          results.push_back(internal_node_id->second);
                      }
                  });
    return results;
}

annotated_route_t RouteAnnotator::annotateRoute(const std::vector<internal_nodeid_t> &route)
{
    annotated_route_t result;

    for (std::size_t i = 0; i < route.size() - 1; i++)
    {
        const auto way_id = [&]() {
            if (route[i] < route[i + 1])
            {
                return db.pair_way_map.find(std::make_pair(route[i], route[i + 1]));
            }
            else
            {
                return db.pair_way_map.find(std::make_pair(route[i + 1], route[i]));
            }
        }();
        if (way_id != db.pair_way_map.end())
        {
            result.push_back(way_id->second.id);
        }
        else
        {
            result.push_back(INVALID_WAYID);
        }
    }
    return result;
}

std::string RouteAnnotator::get_tag_key(const std::size_t index)
{
    return db.getstring(db.key_value_pairs[index].first);
}

std::string RouteAnnotator::get_tag_value(const std::size_t index)
{
    return db.getstring(db.key_value_pairs[index].second);
}

tagrange_t RouteAnnotator::get_tag_range(const wayid_t way_id) { return db.way_tag_ranges[way_id]; }

wayid_t RouteAnnotator::get_external_way_id(const wayid_t way_id)
{
    return db.internal_to_external_way_id_map[way_id];
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>

#include <Eigen/Dense>
#include <boost/hana.hpp>
namespace hana = boost::hana;

#include <kontiki/types.h>
#include <kontiki/measurements/newton_rscamera_measurement.h>
#include <kontiki/sfm/landmark.h>
#include <kontiki/sfm/observation.h>

#include "../camera_defs.h"
#include "../trajectory_defs.h"

#include "measurement_helper.h"

namespace py = pybind11;

PYBIND11_MODULE(_newton_rscamera_measurement, m) {
  m.doc() = "Newton rolling shutter projection";

  // Create one StaticRsMeasurement class for each camera type
  hana::for_each(camera_types, [&](auto ct) {
    using CameraModel = typename decltype(ct)::type;

    using Class = kontiki::measurements::NewtonRsCameraMeasurement<CameraModel>;
    std::string pyclass_name = "NewtonRsCameraMeasurement_" + std::string(CameraModel::CLASS_ID);
    auto cls = py::class_<Class, std::shared_ptr<Class>>(m, pyclass_name.c_str());

    cls.doc() = R"pbdoc( Newton-based rolling shutter projection

    Projects a landmark into a rolling shutter camera using the Newton method.
    This method should produce measurements such that the projected image row
    is consistent with the projection time.
    However, this is not a guarantee, so if you really need to make sure that the
    rolling shutter prjection time constraint is fulfilled, we recommend to at least
    check the result before accepting it.
    )pbdoc";

    declare_measurement_common<Class>(cls);
    cls.def(py::init<std::shared_ptr<CameraModel>, std::shared_ptr<kontiki::sfm::Observation>, double, double>());
    cls.def(py::init<std::shared_ptr<CameraModel>, std::shared_ptr<kontiki::sfm::Observation>, double>());
    cls.def(py::init<std::shared_ptr<CameraModel>, std::shared_ptr<kontiki::sfm::Observation>>());

    cls.def_readonly("camera", &Class::camera);
    cls.def_readonly("observation", &Class::observation);
    cls.def_readwrite("weight", &Class::weight, "Image residual weight (applied before loss)");

    // Declare the project() function for all trajectory types
    hana::for_each(trajectory_types, [&](auto tt) {
      using TrajectoryModel = typename decltype(tt)::type;

      // Use temporary to extract TrajectoryModel from TrajectoryImpl
      cls.def("project", [](Class &self, const TrajectoryModel &trajectory) {
        return self.template Project<TrajectoryModel>(trajectory);
        });

    }); // for_each(trajectory_types)
  }); // for_each(camera_types)
}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <ored/model/lgmdata.hpp>
#include <ored/utilities/correlationmatrix.hpp>
#include <ored/utilities/log.hpp>
#include <ored/utilities/parsers.hpp>
#include <ored/utilities/to_string.hpp>

#include <qle/models/fxbsconstantparametrization.hpp>
#include <qle/models/fxbspiecewiseconstantparametrization.hpp>
#include <qle/models/fxeqoptionhelper.hpp>
#include <qle/models/irlgm1fconstantparametrization.hpp>
#include <qle/models/irlgm1fpiecewiseconstanthullwhiteadaptor.hpp>
#include <qle/models/irlgm1fpiecewiseconstantparametrization.hpp>
#include <qle/models/irlgm1fpiecewiselinearparametrization.hpp>
#include <qle/pricingengines/analyticcclgmfxoptionengine.hpp>
#include <qle/pricingengines/analyticlgmswaptionengine.hpp>

#include <ql/math/optimization/levenbergmarquardt.hpp>
#include <ql/models/shortrate/calibrationhelpers/swaptionhelper.hpp>
#include <ql/quotes/simplequote.hpp>
#include <ql/utilities/dataformatters.hpp>

#include <boost/algorithm/string/case_conv.hpp>
#include <boost/lexical_cast.hpp>

namespace ore {
namespace data {

bool LgmData::operator==(const LgmData& rhs) {

    if (qualifier_ != rhs.qualifier_ || calibrationType_ != rhs.calibrationType_ || revType_ != rhs.revType_ ||
        volType_ != rhs.volType_ || calibrateH_ != rhs.calibrateH_ || hType_ != rhs.hType_ || hTimes_ != rhs.hTimes_ ||
        hValues_ != rhs.hValues_ || calibrateA_ != rhs.calibrateA_ || aType_ != rhs.aType_ || aTimes_ != rhs.aTimes_ ||
        aValues_ != rhs.aValues_ || shiftHorizon_ != rhs.shiftHorizon_ || scaling_ != rhs.scaling_ ||
        optionExpiries_ != rhs.optionExpiries_ || optionTerms_ != rhs.optionTerms_ ||
        optionStrikes_ != rhs.optionStrikes_) {
        return false;
    }
    return true;
}

bool LgmData::operator!=(const LgmData& rhs) { return !(*this == rhs); }

std::ostream& operator<<(std::ostream& oss, const ParamType& type) {
    if (type == ParamType::Constant)
        oss << "CONSTANT";
    else if (type == ParamType::Piecewise)
        oss << "PIECEWISE";
    else
        QL_FAIL("Parameter type not covered by <<");
    return oss;
}

ParamType parseParamType(const string& s) {
    if (boost::algorithm::to_upper_copy(s) == "CONSTANT")
        return ParamType::Constant;
    else if (boost::algorithm::to_upper_copy(s) == "PIECEWISE")
        return ParamType::Piecewise;
    else
        QL_FAIL("Parameter type " << s << " not recognized");
}

CalibrationType parseCalibrationType(const string& s) {
    if (boost::algorithm::to_upper_copy(s) == "BOOTSTRAP")
        return CalibrationType::Bootstrap;
    else if (boost::algorithm::to_upper_copy(s) == "BESTFIT")
        return CalibrationType::BestFit;
    else if (boost::algorithm::to_upper_copy(s) == "NONE")
        return CalibrationType::None;
    else
        QL_FAIL("Calibration type " << s << " not recognized");
}

std::ostream& operator<<(std::ostream& oss, const CalibrationType& type) {
    if (type == CalibrationType::Bootstrap)
        oss << "BOOTSTRAP";
    else if (type == CalibrationType::BestFit)
        oss << "BESTFIT";
    else if (type == CalibrationType::None)
        oss << "NONE";
    else
        QL_FAIL("Calibration type not covered");
    return oss;
}

LgmData::ReversionType parseReversionType(const string& s) {
    if (boost::algorithm::to_upper_copy(s) == "HULLWHITE")
        return LgmData::ReversionType::HullWhite;
    else if (boost::algorithm::to_upper_copy(s) == "HAGAN")
        return LgmData::ReversionType::Hagan;
    else
        QL_FAIL("Reversion type " << s << " not recognized");
}

std::ostream& operator<<(std::ostream& oss, const LgmData::ReversionType& type) {
    if (type == LgmData::ReversionType::HullWhite)
        oss << "HULLWHITE";
    else if (type == LgmData::ReversionType::Hagan)
        oss << "HAGAN";
    else
        QL_FAIL("Reversion type not covered");
    return oss;
}

LgmData::VolatilityType parseVolatilityType(const string& s) {
    if (boost::algorithm::to_upper_copy(s) == "HULLWHITE")
        return LgmData::VolatilityType::HullWhite;
    else if (boost::algorithm::to_upper_copy(s) == "HAGAN")
        return LgmData::VolatilityType::Hagan;
    else
        QL_FAIL("Volatility type " << s << " not recognized");
}

std::ostream& operator<<(std::ostream& oss, const LgmData::VolatilityType& type) {
    if (type == LgmData::VolatilityType::HullWhite)
        oss << "HULLWHITE";
    else if (type == LgmData::VolatilityType::Hagan)
        oss << "HAGAN";
    else
        QL_FAIL("Volatility type not covered");
    return oss;
}

CalibrationStrategy parseCalibrationStrategy(const string& s) {
    if (boost::algorithm::to_upper_copy(s) == "COTERMINALATM")
        return CalibrationStrategy::CoterminalATM;
    else if (boost::algorithm::to_upper_copy(s) == "COTERMINALDEALSTRIKE")
        return CalibrationStrategy::CoterminalDealStrike;
    else if (boost::algorithm::to_upper_copy(s) == "UNDERLYINGATM")
        return CalibrationStrategy::UnderlyingATM;
    else if (boost::algorithm::to_upper_copy(s) == "UNDERLYINGDEALSTRIKE")
        return CalibrationStrategy::UnderlyingDealStrike;
    else if (boost::algorithm::to_upper_copy(s) == "NONE")
        return CalibrationStrategy::None;
    else
        QL_FAIL("Calibration strategy " << s << " not recognized");
}

std::ostream& operator<<(std::ostream& oss, const CalibrationStrategy& type) {
    if (type == CalibrationStrategy::CoterminalATM)
        oss << "COTERMINALATM";
    else if (type == CalibrationStrategy::CoterminalDealStrike)
        oss << "COTERMINALDEALSTRIKE";
    else if (type == CalibrationStrategy::UnderlyingATM)
        oss << "UNDERLYINGATM";
    else if (type == CalibrationStrategy::UnderlyingDealStrike)
        oss << "UNDERLYINGDEALSTRIKE";
    else if (type == CalibrationStrategy::None)
        oss << "NONE";
    else
        QL_FAIL("Calibration strategy not covered");
    return oss;
}

// TODO: use fromXML here, filtering is not done during parsing.
void LgmData::fromFile(const std::string& fileName, const std::string& qualifier) {
    LOG("load model configuration from " << fileName);
    clear();
    XMLDocument doc(fileName);
    XMLNode* root = doc.getFirstNode("Models");
    Size count = 0;
    for (XMLNode* child = XMLUtils::getChildNode(root, "LGM"); child; child = XMLUtils::getNextSibling(child, "LGM")) {
        std::string childCcy = XMLUtils::getAttribute(child, "qualifier");
        if (qualifier == childCcy) {
            fromXML(child);
            count++;
            break;
        }
    }
    QL_REQUIRE(count == 1, "LGM configuration not found for qualifier '" << qualifier << "'");
    LOG("load model configuration from " << fileName << " done.");
}

void LgmData::clear() {

    optionExpiries_.clear();
    optionTerms_.clear();
    optionStrikes_.clear();
}

void LgmData::reset() {
    clear();

    qualifier_ = "";
    calibrationType_ = CalibrationType::Bootstrap;
    revType_ = ReversionType::HullWhite;
    volType_ = VolatilityType::HullWhite;
    calibrateH_ = false;
    hType_ = ParamType::Constant;
    hTimes_ = {};
    hValues_ = {0.03};
    calibrateA_ = false;
    aType_ = ParamType::Constant;
    aTimes_ = {};
    aValues_ = {0.01};
    shiftHorizon_ = 0.0;
    scaling_ = 1.0;
}

void LgmData::fromXML(XMLNode* node) {
    // XMLUtils::checkNode(node, "Models");
    // XMLNode* modelNode = XMLUtils::getChildNode(node, "LGM");

    std::string calibTypeString = XMLUtils::getChildValue(node, "CalibrationType", true);
    calibrationType_ = parseCalibrationType(calibTypeString);
    LOG("LGM calibration type = " << calibTypeString);

    // Volatility config

    XMLNode* volNode = XMLUtils::getChildNode(node, "Volatility");

    calibrateA_ = XMLUtils::getChildValueAsBool(volNode, "Calibrate", true);
    LOG("LGM Volatility calibrate = " << calibrateA_);

    std::string volTypeString = XMLUtils::getChildValue(volNode, "VolatilityType", true);
    volType_ = parseVolatilityType(volTypeString);
    LOG("LGM Volatility type = " << volTypeString);

    std::string alphaTypeString = XMLUtils::getChildValue(volNode, "ParamType", true);
    aType_ = parseParamType(alphaTypeString);
    LOG("LGM Volatility param type = " << alphaTypeString);

    aTimes_ = XMLUtils::getChildrenValuesAsDoublesCompact(volNode, "TimeGrid", true);
    LOG("LGM Volatility time grid size = " << aTimes_.size());

    aValues_ = XMLUtils::getChildrenValuesAsDoublesCompact(volNode, "InitialValue", true);
    LOG("LGM Volatility initial values size = " << aValues_.size());

    // Reversion config

    XMLNode* revNode = XMLUtils::getChildNode(node, "Reversion");

    calibrateH_ = XMLUtils::getChildValueAsBool(revNode, "Calibrate", true);
    LOG("LGM Reversion calibrate = " << calibrateH_);

    std::string revTypeString = XMLUtils::getChildValue(revNode, "ReversionType", true);
    revType_ = parseReversionType(revTypeString);
    LOG("LGM Reversion type = " << revTypeString);

    std::string hTypeString = XMLUtils::getChildValue(revNode, "ParamType", true);
    hType_ = parseParamType(hTypeString);
    LOG("LGM Reversion parameter type = " << hTypeString);

    hTimes_ = XMLUtils::getChildrenValuesAsDoublesCompact(revNode, "TimeGrid", true);
    LOG("LGM Reversion time grid size = " << hTimes_.size());

    hValues_ = XMLUtils::getChildrenValuesAsDoublesCompact(revNode, "InitialValue", true);
    LOG("LGM Reversion initial values size = " << hValues_.size());

    // Parameter transformation config

    XMLNode* tranformNode = XMLUtils::getChildNode(node, "ParameterTransformation");
    shiftHorizon_ = XMLUtils::getChildValueAsDouble(tranformNode, "ShiftHorizon", true);
    LOG("LGM shift horizon = " << shiftHorizon_);

    scaling_ = XMLUtils::getChildValueAsDouble(tranformNode, "Scaling", true);
    LOG("LGM scaling = " << scaling_);

    LOG("LgmData done");
}

XMLNode* LgmData::toXML(XMLDocument& doc) {

    XMLNode* lgmNode = doc.allocNode("LGM");

    XMLUtils::addGenericChild(doc, lgmNode, "CalibrationType", calibrationType_);

    // volatility
    XMLNode* volatilityNode = XMLUtils::addChild(doc, lgmNode, "Volatility");
    XMLUtils::addChild(doc, volatilityNode, "Calibrate", calibrateA_);

    XMLNode* volatilityTypeNode = doc.allocNode("VolatilityType", to_string(volType_));
    XMLUtils::appendNode(volatilityNode, volatilityTypeNode);

    XMLUtils::addGenericChild(doc, volatilityNode, "ParamType", aType_);
    XMLUtils::addGenericChildAsList(doc, volatilityNode, "TimeGrid", aTimes_);
    XMLUtils::addGenericChildAsList(doc, volatilityNode, "InitialValue", aValues_);

    // reversion
    XMLNode* reversionNode = XMLUtils::addChild(doc, lgmNode, "Reversion");
    XMLUtils::addChild(doc, reversionNode, "Calibrate", calibrateH_);

    XMLNode* reversionTypeNode = doc.allocNode("ReversionType", to_string(revType_));
    XMLUtils::appendNode(reversionNode, reversionTypeNode);

    XMLUtils::addGenericChild(doc, reversionNode, "ParamType", hType_);
    XMLUtils::addGenericChildAsList(doc, reversionNode, "TimeGrid", hTimes_);
    XMLUtils::addGenericChildAsList(doc, reversionNode, "InitialValue", hValues_);

    // parameter transformation
    XMLNode* parameterTransformationNode = XMLUtils::addChild(doc, lgmNode, "ParameterTransformation");
    XMLUtils::addChild(doc, parameterTransformationNode, "ShiftHorizon", shiftHorizon_);
    XMLUtils::addChild(doc, parameterTransformationNode, "Scaling", scaling_);

    return lgmNode;
}
} // namespace data
} // namespace ore


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include "../Lstm.hpp"

#include <boost/test/data/test_case.hpp>

BOOST_AUTO_TEST_SUITE(LstmTests)

using namespace armnn_driver;

BOOST_DATA_TEST_CASE(LstmNoCifgNoPeepholeNoProjectionTest, COMPUTE_DEVICES)
{
    LstmNoCifgNoPeepholeNoProjection<hal_1_1::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmCifgPeepholeNoProjectionTest, COMPUTE_DEVICES)
{
    LstmCifgPeepholeNoProjection<hal_1_1::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmNoCifgPeepholeProjectionTest, COMPUTE_DEVICES)
{
    LstmNoCifgPeepholeProjection<hal_1_1::HalPolicy>(sample);
}

BOOST_DATA_TEST_CASE(LstmCifgPeepholeNoProjectionBatch2Test, COMPUTE_DEVICES)
{
    LstmCifgPeepholeNoProjectionBatch2<hal_1_1::HalPolicy>(sample);
}

BOOST_AUTO_TEST_SUITE_END()


#include "shift/rc/optimizer_mesh/filter.hpp"
#include <shift/resource_db/mesh.hpp>
#include <shift/log/log.hpp>
#include <shift/math/vector.hpp>
#include <shift/core/hash_table.hpp>
#include <boost/endian/conversion.hpp>
#include <filesystem>
#include <queue>
#include <memory>

namespace std
{
/// Implementation of the 64bit Murmur hash algorithm to hash vertex data of
/// arbitrary size.
template <>
struct hash<const void*>
{
  std::size_t operator()(const void* vertex) const
  {
    constexpr auto m = static_cast<std::size_t>(0xc6a4a7935bd1e995ULL);
    constexpr int r = 47;

    std::size_t remaining_size = vertex_size;
    std::size_t h = vertex_size;

    // Mix 4 bytes at a time into the hash
    const auto* data = reinterpret_cast<const std::uint8_t*>(vertex);

    while (remaining_size >= sizeof(std::size_t))
    {
      auto k = *reinterpret_cast<const std::size_t*>(data);

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

      h *= m;
      h ^= k;

      data += sizeof(std::size_t);
      remaining_size -= sizeof(std::size_t);
    }

    // Handle the last few bytes of the input array
    switch (remaining_size)
    {
    case 7:
      h ^= static_cast<std::size_t>(data[6]) << 48;
      [[fallthrough]];
    case 6:
      h ^= static_cast<std::size_t>(data[5]) << 40;
      [[fallthrough]];
    case 5:
      h ^= static_cast<std::size_t>(data[4]) << 32;
      [[fallthrough]];
    case 4:
      h ^= *reinterpret_cast<const std::uint32_t*>(&data[0]);
      break;
    case 3:
      h ^= static_cast<std::size_t>(data[2]) << 16;
      [[fallthrough]];
    case 2:
      h ^= *reinterpret_cast<const std::uint16_t*>(&data[0]);
      break;
    case 1:
      h ^= static_cast<std::size_t>(data[0]);
    };
    h *= m;

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

    return h;
  }

  std::size_t vertex_size;
};

/// An overload of std::equal_to to compare arbitrary vertices.
template <>
struct equal_to<const void*>
{
  using first_argument_type = const void*;
  using second_argument_type = const void*;
  using result_type = bool;

  bool operator()(const first_argument_type lhs, const second_argument_type rhs)
  {
    return std::memcmp(lhs, rhs, vertex_size) == 0;
  }

  std::size_t vertex_size;
};
}

namespace shift::rc
{
namespace fs = std::filesystem;

bool optimize_mesh(const job_description& /*job*/)
{
  // auto input_file_path = job.input_base_path / job.input_file_path;
  // if (!fs::exists(input_file_path) ||
  // !fs::is_regular_file(input_file_path))
  //{
  //  log::error() << "Cannot find input file " << input_file_path << ".";
  //  return false;
  //}

  // std::ifstream file;
  // file.open(input_file_path.generic_string(),
  //          std::ios_base::in | std::ios_base::binary);
  // if (!file.is_open())
  //{
  //  /// ToDo: Throw exception.
  //  return false;
  //}

  // std::vector<std::uint32_t> indices;
  // indices.resize(1000);  // total_indices
  // std::size_t unique_vertex_count;
  // unique_vertex_count =
  //  generate_index_buffer(indices, nullptr, sizeof(vertex));
  // generate_vertex_buffer(indices, vertices.data(), indices.size(),
  //                       sizeof(vertex));

  //// Store resource into repository.
  // resource_db::repository::singleton_instance().save(*mesh,

  return true;
}

template <typename Index>
std::vector<std::uint8_t> resolve_vertex_indices(resource_db::mesh& mesh,
                                                 /* Index* indices, */
                                                 std::size_t index_count)
{
  std::size_t vertex_size = 0;
  for (auto& attribute : mesh.vertex_attributes)
  {
    vertex_size += resource_db::vertex_attribute_size(attribute.component_type,
                                                      attribute.data_type);
  }
  const auto& index_buffer = mesh.index_buffer_view.buffer->storage;

  std::vector<std::uint8_t> destination;
  destination.resize(index_count * vertex_size);
  for (std::size_t i = 0; i < index_count; ++i)
  {
    for (auto& attribute : mesh.vertex_attributes)
    {
      const auto& vertex_buffer = attribute.vertex_buffer_view.buffer->storage;
      std::memcpy(
        destination.data() + i * attribute.stride + attribute.offset,
        vertex_buffer.data() +
          *reinterpret_cast<const Index*>(&index_buffer[i * sizeof(Index)]) *
            attribute.stride +
          attribute.offset,
        attribute.size);
    }
  }
  return destination;
}

bool optimize_mesh(resource_db::mesh& mesh)
{
  if (mesh.sub_meshes.size() != 1)
    return true;

  for (auto& attribute : mesh.vertex_attributes)
  {
    if (!attribute.vertex_buffer_view.buffer)
      return false;
  }
  std::vector<std::uint8_t> vertex_buffer;

  if (!mesh.index_buffer_view.buffer)
    return false;

  std::size_t index_count = 0;
  for (auto sub_mesh : mesh.sub_meshes)
  {
    switch (sub_mesh.topology)
    {
    case resource_db::primitive_topology::triangle_list:
      index_count = sub_mesh.index_count / 3;
      break;

    case resource_db::primitive_topology::triangle_strip:
      /// ToDo: Find and subtract empty triangles.
      BOOST_ASSERT(false);  // Not yet implemented.
      return false;

    case resource_db::primitive_topology::triangle_fan:
      BOOST_ASSERT(false);  // Not yet implemented.
      return false;

    default:
      BOOST_ASSERT(false);  // Unsupported.
      return false;
    }
  }

  switch (mesh.index_data_type)
  {
  case resource_db::vertex_index_data_type::uint8:
    resolve_vertex_indices<std::uint8_t>(
      mesh, /*mesh.index_buffer_view.buffer->storage.data(),*/ index_count);
    break;

  default:
    // Ignore other cases for now.
    break;
  }

  // resolve_vertex_indices(mesh.
  return true;
}

std::pair<std::vector<std::uint32_t>, std::size_t> generate_index_buffer(
  std::vector<std::uint8_t>& vertex_buffer, std::size_t vertex_size)
{
  std::size_t vertex_count = vertex_buffer.size() / vertex_size;
  std::vector<std::uint32_t> indices;
  indices.resize(vertex_count);

  std::hash<const void*> hasher{vertex_size};
  std::equal_to<const void*> compare{vertex_size};
  core::hash_table<const void*, std::size_t> table(vertex_count, hasher,
                                                   compare);

  std::size_t next_vertex = 0;
  for (std::size_t i = 0; i < vertex_count; ++i)
  {
    const void* vertex = vertex_buffer.data() + i * vertex_size;
    bool unique;
    std::size_t* value;
    std::tie(unique, value) = table[vertex];
    if (unique)
      *value = next_vertex++;
    indices[i] = *value;
  }

  return std::make_pair(std::move(indices), next_vertex);
}

void compress_vertex_buffer(std::vector<std::uint8_t>& vertex_buffer,
                            std::size_t vertex_size,
                            std::size_t unique_vertex_count,
                            const std::vector<std::uint32_t>& indices)
{
  std::uint32_t next_vertex = 0;
  for (std::size_t i = 0; i < unique_vertex_count; ++i)
  {
    BOOST_ASSERT(indices[i] <= next_vertex);
    if (indices[i] == next_vertex)
    {
      if (next_vertex != i)
      {
        std::memcpy(vertex_buffer.data() + next_vertex * vertex_size,
                    vertex_buffer.data() + i * vertex_size, vertex_size);
      }
      ++next_vertex;
    }
  }
}
}


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SIMD_COSD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SIMD_COSD_HPP_INCLUDED

#include <boost/simd/function/scalar/cosd.hpp>
#include <boost/simd/arch/common/generic/function/autodispatcher.hpp>
#include <boost/simd/arch/common/simd/function/cosd.hpp>

#endif