#ifndef GPSOINN_HXX
#define GPSOINN_HXX

#include "graph/graph.hxx"

#include <Eigen/Dense>
#include <Eigen/StdVector>
#include <algorithm>
#include <array>
#include <cmath>
#include <random>

namespace GPSOINN {

template <unsigned dimension> class GPNet {
  private:
    typedef Eigen::Matrix<double, dimension, 1> NodeVector;
    struct Node {
        NodeVector vector;
        size_t win_count;
    };
    typedef std::array<double, dimension> array;
    typedef UndirectedGraph<Node, unsigned, std::less<NodeVector>,
                            Eigen::aligned_allocator<NodeVector>>
        UGraph;

  public:
    GPNet(unsigned lambda = 20000, unsigned age_max = 50, unsigned k = 1,
          double sigma_2 = 1e-6);
    ~GPNet() {}

    void train(array &data);
    double predict(array &data);

  private:
    UGraph m_graph;
    double m_sigma_2;
    unsigned m_local_opt_coeff = 100;
    unsigned m_age_max;
    unsigned m_lambda;
    unsigned m_k;
    unsigned m_cycles = 0;
    const double m_const_coeff;

    std::pair<double, double> threshold(size_t index,
                                        const NodeVector &x_vector);
}; // class GPNet

} // namespace GPSOINN

namespace GPSOINN {

template <unsigned dimension>
GPNet<dimension>::GPNet(unsigned lambda, unsigned age_max, unsigned k,
                        double sigma_2)
    : m_lambda(lambda), m_age_max(age_max), m_k(k), m_sigma_2(sigma_2),
      m_const_coeff(std::sqrt(std::pow(2 * M_PI, dimension))) {
    m_graph.insert_vertex(Node{.vector = NodeVector::Random(), .win_count = 0});
    m_graph.insert_vertex(Node{.vector = NodeVector::Random(), .win_count = 0});
}

template <unsigned dimension> void GPNet<dimension>::train(array &data) {
    using namespace Eigen;
    using NodeM = Matrix<double, dimension, Dynamic>;

    if (m_graph.vertex_count() < 2) {
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(), .win_count = 0});
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(), .win_count = 0});
    }
    std::random_device rand_dev;
    std::srand(rand_dev());

    typename NodeM::Index min_index, min2_index;
    Map<NodeVector> input(data.data());

    {
        // construct the data matrix
        NodeM nodes(dimension, m_graph.vertex_count());

        size_t col = 0;
        for (auto &node : m_graph) {
            nodes.col(col) = node.value().vector;
            ++col;
        }

        // find the winner and the second winner
        RowVectorXd distances =
            (nodes.colwise() - input).colwise().squaredNorm();
        distances.minCoeff(&min_index);

        if (min_index == 0) {
            distances(0) = distances(1);
            distances.minCoeff(&min2_index);
            if (min2_index == 0)
                min2_index = 1;
        } else {
            distances(min_index) = distances(min_index - 1);
            distances.minCoeff(&min2_index);
            if (min2_index == min_index)
                ++min2_index;
        }
    }

    auto min_iter = m_graph.begin();
    auto min2_iter = m_graph.begin();
    for (size_t i = 0; i != min_index; ++i)
        ++min_iter;
    for (size_t i = 0; i != min2_index; ++i)
        ++min2_iter;

    auto[threshold1, prob1] = threshold(min_index, input);
    auto[threshold2, prob2] = threshold(min2_index, input);

    if ((prob1 > prob2 && prob1 > threshold1) || prob2 > threshold2) {
        m_graph.insert_edge(min_iter, min2_iter, 0);

        auto &win_count = min_iter->value().win_count;
        NodeVector &winner_vec = min_iter->value().vector;
        ++win_count;
        winner_vec.array() += ((input - winner_vec).array() / (win_count + 1));

        for (auto &edge : *min_iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            node_vec.array() += ((input - node_vec).array() /
                                 m_local_opt_coeff / (win_count + 1));
            ++edge.weight;
        }

        for (auto niter = m_graph.cbegin(); niter != m_graph.cend(); ++niter) {
            for (auto edge = niter->cbegin(), pre = niter->cbefore_begin();
                 edge != niter->cend();) {
                if (edge->weight > m_age_max) {
                    edge = m_graph.erase_after_edge(niter, pre);
                } else {
                    ++edge;
                    ++pre;
                }
            }
        }
    } else {
        m_graph.insert_vertex(Node{.vector = input, .win_count = 0});
    }

    ++m_cycles;
    if (m_cycles == m_lambda) {
        m_cycles = 0;
        for (auto iter = m_graph.cbegin(); iter != m_graph.cend();) {
            if (std::distance(iter->cbegin(), iter->cend()) < m_k)
                iter = m_graph.erase_vertex(iter);
            else
                ++iter;
        }
    }
} // namespace GPSOINN

template <unsigned dimension>
std::pair<double, double>
GPNet<dimension>::threshold(size_t index, const NodeVector &x_vector) {
    using namespace Eigen;

    Matrix<double, dimension, dimension> local_cov;
    local_cov.setZero();
    auto iter = m_graph.begin();
    for (size_t i = 0; i != index; ++i)
        ++iter;

    NodeVector &winner_vec = iter->value().vector;
    size_t win_sum = 0;
    size_t neighbour_cnt = 0;
    for (auto edge : *iter) {
        NodeVector &node_vec = m_graph[edge.head].value().vector;
        local_cov.array() +=
            ((node_vec - winner_vec) * (node_vec - winner_vec).transpose())
                .array() *
            edge.weight;
        win_sum += edge.weight;
        ++neighbour_cnt;
    }
    if (win_sum)
        local_cov.array() /= win_sum;
    local_cov += MatrixXd::Identity(dimension, dimension) * m_sigma_2;

    // if (neighbour_cnt > dimension) {
    //     Matrix<bool, dimension, 1> comp;
    //     SelfAdjointEigenSolver<Matrix<double, dimension, dimension>>
    //     eigens(local_cov);
    //     comp = ((eigens.eigenvalues()).array() >=
    //     m_sigma_2)
    //                .template cast<bool>();

    //     unsigned count = comp.count();
    //     Matrix<double, dimension, Dynamic> p_components(dimension, count);
    //     VectorXd eigenvals(count);

    //     {
    //         unsigned cnt = 0;
    //         for (unsigned i = 0; i != dimension; ++i) {
    //             if (comp(i)) {
    //                 p_components.col(cnt) =
    //                 eigens.eigenvectors().col(i); eigenvals(cnt) =
    //                 eigens.eigenvalues()(i);
    //                 ++cnt;
    //             }
    //         }
    //     }
    //     local_cov =
    //         p_components * eigenvals.asDiagonal() * p_components.transpose()
    //         + MatrixXd::Identity(dimension, dimension) * m_sigma_2;
    // }

    SelfAdjointEigenSolver<Matrix<double, dimension, dimension>> eigens(
        local_cov);
    double determinant_sqrt = std::sqrt(std::abs(eigens.eigenvalues().prod()));
    double threshold = 1;
    Matrix<double, dimension, dimension> cov_inv =
        eigens.eigenvectors() *
        eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
        eigens.eigenvectors().transpose();
    if (neighbour_cnt) {
        for (auto edge : *iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            double prob = std::exp(-((x_vector - node_vec).transpose() *
                                     cov_inv * (x_vector - node_vec))(0) /
                                   2) /
                          m_const_coeff / determinant_sqrt;

            if (prob < threshold)
                threshold = prob;
        }
    } else
        threshold = 0.55;

    double xprob = std::exp(-((x_vector - winner_vec).transpose() * cov_inv *
                              (x_vector - winner_vec))(0) /
                            2) /
                   m_const_coeff / determinant_sqrt;
    return {
        threshold,
        xprob,
    };
}
template <unsigned dimension> double GPNet<dimension>::predict(array &data) {
    using namespace Eigen;

    Map<NodeVector> input(data.data());

    double prob = 0;
    size_t wins = 0;
    for (auto &node : m_graph) {
        Matrix<double, dimension, dimension> local_cov;
        local_cov.setZero();
        size_t win_sum = 0;
        for (auto edge : node) {
            NodeVector &edge_vec = m_graph[edge.head].value().vector;
            NodeVector &node_vec = node.value().vector;
            local_cov.array() +=
                ((edge_vec - node_vec) * (edge_vec - node_vec).transpose())
                    .array() *
                edge.weight;
            win_sum += edge.weight;
        }
        if (win_sum)
            local_cov.array() /= win_sum;
        local_cov += MatrixXd::Identity(dimension, dimension) * m_sigma_2;

        SelfAdjointEigenSolver<Matrix<double, dimension, dimension>> eigens(
            local_cov);
        double determinant_sqrt = std::sqrt(eigens.eigenvalues().prod());
        Matrix<double, dimension, dimension> cov_inv =
            eigens.eigenvectors() *
            eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
            eigens.eigenvectors().transpose();

        NodeVector &node_vec = node.value().vector;
        double xprob = std::exp(-((input - node_vec).transpose() * cov_inv *
                                  (input - node_vec))(0) /
                                2) /
                       m_const_coeff / determinant_sqrt;
        prob += xprob * node.value().win_count;
        wins += node.value().win_count;
    }

    if (wins)
        prob /= wins;
    return prob;
}

// specialization
template <> class GPNet<0> {
  public:
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> NodeVector;
    struct Node {
        NodeVector vector;
        size_t win_count;
    };

  private:
    typedef std::vector<double> vector_d;
    typedef UndirectedGraph<Node, unsigned, std::less<NodeVector>,
                            Eigen::aligned_allocator<NodeVector>>
        UGraph;

  public:
    GPNet(unsigned dimension = 1, unsigned lambda = 20000,
          unsigned age_max = 50, unsigned k = 1, double sigma_2 = 1e-6);
    ~GPNet() {}

    void train(vector_d &data);
    double predict(vector_d &data);

  private:
    UGraph m_graph;
    double m_sigma_2;
    unsigned m_local_opt_coeff = 100;
    unsigned m_age_max;
    unsigned m_lambda;
    unsigned m_k;
    unsigned m_cycles = 0;
    double m_const_coeff;
    unsigned m_dimension;

    std::pair<double, double> threshold(size_t index,
                                        const NodeVector &x_vector);
}; // class GPNet

GPNet<0>::GPNet(unsigned dimension, unsigned lambda, unsigned age_max,
                unsigned k, double sigma_2)
    : m_lambda(lambda), m_age_max(age_max), m_k(k), m_sigma_2(sigma_2),
      m_const_coeff(std::sqrt(std::pow(2 * M_PI, dimension))),
      m_dimension(dimension) {
    m_graph.insert_vertex(
        Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
    m_graph.insert_vertex(
        Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
}

void GPNet<0>::train(vector_d &data) {
    using namespace Eigen;
    using NodeM = Matrix<double, Dynamic, Dynamic>;

    if (m_graph.vertex_count() < 2) {
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
        m_graph.insert_vertex(
            Node{.vector = NodeVector::Random(m_dimension, 1), .win_count = 0});
    }
    std::random_device rand_dev;
    std::srand(rand_dev());

    typename NodeM::Index min_index, min2_index;
    Map<NodeVector> input(data.data(), m_dimension);

    {
        // construct the data matrix
        NodeM nodes(m_dimension, m_graph.vertex_count());

        size_t col = 0;
        for (auto &node : m_graph) {
            nodes.col(col) = node.value().vector;
            ++col;
        }

        // find the winner and the second winner
        RowVectorXd distances =
            (nodes.colwise() - input).colwise().squaredNorm();
        distances.minCoeff(&min_index);

        if (min_index == 0) {
            distances(0) = distances(1);
            distances.minCoeff(&min2_index);
            if (min2_index == 0)
                min2_index = 1;
        } else {
            distances(min_index) = distances(min_index - 1);
            distances.minCoeff(&min2_index);
            if (min2_index == min_index)
                ++min2_index;
        }
    }

    auto min_iter = m_graph.begin();
    auto min2_iter = m_graph.begin();
    for (size_t i = 0; i != min_index; ++i)
        ++min_iter;
    for (size_t i = 0; i != min2_index; ++i)
        ++min2_iter;

    auto[threshold1, prob1] = threshold(min_index, input);
    auto[threshold2, prob2] = threshold(min2_index, input);

    if ((prob1 > prob2 && prob1 > threshold1) || prob2 > threshold2) {
        m_graph.insert_edge(min_iter, min2_iter, 0);

        auto &win_count = min_iter->value().win_count;
        NodeVector &winner_vec = min_iter->value().vector;
        ++win_count;
        winner_vec.array() += ((input - winner_vec).array() / (win_count + 1));

        for (auto &edge : *min_iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            node_vec.array() += ((input - node_vec).array() /
                                 m_local_opt_coeff / (win_count + 1));
            ++edge.weight;
        }

        for (auto niter = m_graph.cbegin(); niter != m_graph.cend(); ++niter) {
            for (auto edge = niter->cbegin(), pre = niter->cbefore_begin();
                 edge != niter->cend();) {
                if (edge->weight > m_age_max) {
                    edge = m_graph.erase_after_edge(niter, pre);
                } else {
                    ++edge;
                    ++pre;
                }
            }
        }
    } else {
        m_graph.insert_vertex(Node{.vector = input, .win_count = 0});
    }

    ++m_cycles;
    if (m_cycles == m_lambda) {
        m_cycles = 0;
        for (auto iter = m_graph.cbegin(); iter != m_graph.cend();) {
            if (std::distance(iter->cbegin(), iter->cend()) < m_k)
                iter = m_graph.erase_vertex(iter);
            else
                ++iter;
        }
    }
} // namespace GPSOINN

std::pair<double, double> GPNet<0>::threshold(size_t index,
                                              const NodeVector &x_vector) {
    using namespace Eigen;

    Matrix<double, Dynamic, Dynamic> local_cov(m_dimension, m_dimension);
    local_cov.setZero();
    auto iter = m_graph.begin();
    for (size_t i = 0; i != index; ++i)
        ++iter;

    NodeVector &winner_vec = iter->value().vector;
    size_t win_sum = 0;
    size_t neighbour_cnt = 0;
    for (auto edge : *iter) {
        NodeVector &node_vec = m_graph[edge.head].value().vector;
        local_cov.array() +=
            ((node_vec - winner_vec) * (node_vec - winner_vec).transpose())
                .array() *
            edge.weight;
        win_sum += edge.weight;
        ++neighbour_cnt;
    }
    if (win_sum)
        local_cov.array() /= win_sum;
    local_cov += MatrixXd::Identity(m_dimension, m_dimension) * m_sigma_2;

    // if (neighbour_cnt > m_dimension) {
    //     Matrix<bool, Dynamic, 1> comp(m_dimension, 1);
    //     SelfAdjointEigenSolver<Matrix<double, Dynamic, Dynamic>>
    //     eigens(local_cov);
    //     comp = ((eigens.eigenvalues()).array() >=
    //     m_sigma_2)
    //                .template cast<bool>();

    //     unsigned count = comp.count();
    //     Matrix<double, Dynamic, Dynamic> p_components(m_dimension, count);
    //     VectorXd eigenvals(count);

    //     {
    //         unsigned cnt = 0;
    //         for (unsigned i = 0; i != m_dimension; ++i) {
    //             if (comp(i)) {
    //                 p_components.col(cnt) =
    //                 eigens.eigenvectors().col(i); eigenvals(cnt) =
    //                 eigens.eigenvalues()(i);
    //                 ++cnt;
    //             }
    //         }
    //     }
    //     local_cov =
    //         p_components * eigenvals.asDiagonal() * p_components.transpose()
    //         + MatrixXd::Identity(m_dimension, m_dimension) * m_sigma_2;
    // }

    SelfAdjointEigenSolver<Matrix<double, Dynamic, Dynamic>> eigens(local_cov);
    double determinant_sqrt = std::sqrt(eigens.eigenvalues().prod());
    double threshold = 1;
    Matrix<double, Dynamic, Dynamic> cov_inv =
        eigens.eigenvectors() *
        eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
        eigens.eigenvectors().transpose();
    if (neighbour_cnt) {
        for (auto edge : *iter) {
            NodeVector &node_vec = m_graph[edge.head].value().vector;
            double prob = std::exp(-((x_vector - node_vec).transpose() *
                                     cov_inv * (x_vector - node_vec))(0) /
                                   2) /
                          m_const_coeff / determinant_sqrt;

            if (prob < threshold)
                threshold = prob;
        }
    } else
        threshold = 0.55;

    double xprob = std::exp(-((x_vector - winner_vec).transpose() * cov_inv *
                              (x_vector - winner_vec))(0) /
                            2) /
                   m_const_coeff / determinant_sqrt;
    return {
        threshold,
        xprob,
    };
}
double GPNet<0>::predict(vector_d &data) {
    using namespace Eigen;

    Map<NodeVector> input(data.data(), m_dimension);

    double prob = 0;
    size_t wins = 0;
    for (auto &node : m_graph) {
        Matrix<double, Dynamic, Dynamic> local_cov(m_dimension, m_dimension);
        local_cov.setZero();
        size_t win_sum = 0;
        for (auto edge : node) {
            NodeVector &edge_vec = m_graph[edge.head].value().vector;
            NodeVector &node_vec = node.value().vector;
            local_cov.array() +=
                ((edge_vec - node_vec) * (edge_vec - node_vec).transpose())
                    .array() *
                edge.weight;
            win_sum += edge.weight;
        }
        if (win_sum)
            local_cov.array() /= win_sum;
        local_cov += MatrixXd::Identity(m_dimension, m_dimension) * m_sigma_2;

        SelfAdjointEigenSolver<Matrix<double, Dynamic, Dynamic>> eigens(
            local_cov);
        double determinant_sqrt =
            std::sqrt(std::abs(eigens.eigenvalues().prod()));
        Matrix<double, Dynamic, Dynamic> cov_inv =
            eigens.eigenvectors() *
            eigens.eigenvalues().array().inverse().matrix().asDiagonal() *
            eigens.eigenvectors().transpose();

        NodeVector &node_vec = node.value().vector;
        double xprob = std::exp(-((input - node_vec).transpose() * cov_inv *
                                  (input - node_vec))(0) /
                                2) /
                       m_const_coeff / determinant_sqrt;
        prob += xprob * node.value().win_count;
        wins += node.value().win_count;
    }

    if (wins)
        prob /= wins;
    return prob;
}
} // namespace GPSOINN

#endif // GPSOINN_HXX


#include <iostream>
#include <vector>
#include <deque>
#include <typeinfo>
#include <set>
#include <cstdlib>
// -------------------- OpenMesh
#include <OpenMesh/Core/IO/MeshIO.hh>
#include <OpenMesh/Core/Mesh/PolyMesh_ArrayKernelT.hh>
#include <OpenMesh/Core/Mesh/PolyMeshT.hh>
#include <imgui/imgui.h>
#include <igl/opengl/glfw/imgui/ImGuiMenu.h>
#include <igl/opengl/glfw/imgui/ImGuiHelpers.h>
#include <igl/avg_edge_length.h>
#include <igl/gaussian_curvature.h>
#include <igl/principal_curvature.h>
#include <igl/massmatrix.h>
#include <igl/invert_diag.h>
#include <igl/readOFF.h>
#include <igl/opengl/glfw/Viewer.h>
#include <igl/jet.h>
#include <igl/pinv.h>
#include <Eigen/Eigenvalues>
#include <Eigen/QR>
#include <string>
#include <iostream>

using namespace Eigen;
using namespace std;

// ----------------------------------------------------------------------------
typedef OpenMesh::PolyMesh_ArrayKernelT<> _Mesh;
typedef OpenMesh::Vec3d v3d_t;
typedef vector<v3d_t> vv3d_t;
// ----------------------------------------------------------------------------
// Build a simple cube and write it to std::cout
bool is_visible = false;
bool is_visible_offset = false;
bool is_visible_vert = false;

void view_f_normals(_Mesh mesh_in, MatrixXd PD, MatrixXd f_centers) {
    mesh_in.triangulate();
    MatrixXd V;
    MatrixXi F;

    V.resize(mesh_in.n_vertices(), 3);
    F.resize(mesh_in.n_faces(), 3);

    for (size_t i = 0; i < mesh_in.n_vertices(); ++i) {
        auto p = mesh_in.point(mesh_in.vertex_handle(i));
        V(i, 0) = p[0];
        V(i, 1) = p[1];
        V(i, 2) = p[2];
    }

    for (size_t i = 0; i < mesh_in.n_faces(); ++i) {
        auto fh = mesh_in.face_handle(i);
        int vi = 0;
        for (_Mesh::ConstFaceVertexCCWIter fvi = mesh_in.cfv_ccwbegin(fh); fvi != mesh_in.cfv_ccwend(fh); ++fvi) {
//            cout << vi << " " << endl;
            F(i, vi) = fvi->idx();
            vi++;
        }
//        cout << endl;
    }

    // Plot the mesh_in with pseudocolors
    igl::opengl::glfw::Viewer viewer;
    igl::opengl::glfw::imgui::ImGuiMenu menu;
    viewer.plugins.push_back(&menu);
    // Customize the menu
    double doubleVariable = 0.1f; // Shared between two menus
    // Add content to the default menu windowOh
    menu.callback_draw_viewer_menu = [&]() {
        // Draw parent menu content
        menu.draw_viewer_menu();

        // Add new group
        if (ImGui::CollapsingHeader("New Group", ImGuiTreeNodeFlags_DefaultOpen)) {
            // Expose variable directly ...
            ImGui::InputDouble("double", &doubleVariable, 0, 0, "%.4f");

            // ... or using a custom callback
            static bool boolVariable = true;
            if (ImGui::Checkbox("bool", &boolVariable)) {
                // do something
                cout << "boolVariable: " << boolalpha << boolVariable << endl;
            }

            // Expose an enumeration type
            enum Orientation {
                Up = 0, Down, Left, Right
            };
            static Orientation dir = Up;
            ImGui::Combo("Direction", (int *) (&dir), "Up\0Down\0Left\0Right\0\0");

            // We can also use a vector<string> defined dynamically
            static int num_choices = 3;
            static vector<string> choices;
            static int idx_choice = 0;
            if (ImGui::InputInt("Num letters", &num_choices)) {
                num_choices = max(1, min(26, num_choices));
            }
            if (num_choices != (int) choices.size()) {
                choices.resize(num_choices);
                for (int i = 0; i < num_choices; ++i)
                    choices[i] = string(1, 'A' + i);
                if (idx_choice >= num_choices)
                    idx_choice = num_choices - 1;
            }
            ImGui::Combo("Letter", &idx_choice, choices);

            // Add a button
            if (ImGui::Button("Print Hello", ImVec2(-1, 0))) {
                cout << "Hello\n";
            }
        }
    };

    viewer.data(0).set_mesh(V, F);

    // for showing the direction of normals.
    const RowVector3d red(0.8, 0.2, 0.2);
    const RowVector3d blue(0.2, 0.4, 0.87);
    // Average edge length for sizing
    const double avg = igl::avg_edge_length(V, F);
    cout << avg << "the avg is " << endl;  //testing the length

    viewer.data(0).add_edges(f_centers + PD * avg, f_centers - PD * avg, red);
    viewer.launch();
}

void view_v_normal(_Mesh mesh_in, MatrixXd PD) {
    mesh_in.triangulate();
    MatrixXd V;
    MatrixXi F;

    V.resize(mesh_in.n_vertices(), 3);
    F.resize(mesh_in.n_faces(), 3);

    for (size_t i = 0; i < mesh_in.n_vertices(); ++i) {
        auto p = mesh_in.point(mesh_in.vertex_handle(i));
        V(i, 0) = p[0];
        V(i, 1) = p[1];
        V(i, 2) = p[2];
    }

    for (size_t i = 0; i < mesh_in.n_faces(); ++i) {
        auto fh = mesh_in.face_handle(i);
        int vi = 0;
        for (_Mesh::ConstFaceVertexCCWIter fvi = mesh_in.cfv_ccwbegin(fh); fvi != mesh_in.cfv_ccwend(fh); ++fvi) {
            F(i, vi) = fvi->idx();
            vi++;
        }
    }

    // Plot the mesh_in with pseudocolors
    igl::opengl::glfw::Viewer viewer;
    igl::opengl::glfw::imgui::ImGuiMenu menu;
    viewer.plugins.push_back(&menu);
    // Customize the menu
    double doubleVariable = 0.1f; // Shared between two menus
    // Add content to the default menu windowOh
    menu.callback_draw_viewer_menu = [&]() {
        // Draw parent menu content
        menu.draw_viewer_menu();

        // Add new group
        if (ImGui::CollapsingHeader("New Group", ImGuiTreeNodeFlags_DefaultOpen)) {
            // Expose variable directly ...
            ImGui::InputDouble("double", &doubleVariable, 0, 0, "%.4f");

            // ... or using a custom callback
            static bool boolVariable = true;
            if (ImGui::Checkbox("bool", &boolVariable)) {
                // do something
                cout << "boolVariable: " << boolalpha << boolVariable << endl;
            }

            // Expose an enumeration type
            enum Orientation {
                Up = 0, Down, Left, Right
            };
            static Orientation dir = Up;
            ImGui::Combo("Direction", (int *) (&dir), "Up\0Down\0Left\0Right\0\0");

            // We can also use a vector<string> defined dynamically
            static int num_choices = 3;
            static vector<string> choices;
            static int idx_choice = 0;
            if (ImGui::InputInt("Num letters", &num_choices)) {
                num_choices = max(1, min(26, num_choices));
            }
            if (num_choices != (int) choices.size()) {
                choices.resize(num_choices);
                for (int i = 0; i < num_choices; ++i)
                    choices[i] = string(1, 'A' + i);
                if (idx_choice >= num_choices)
                    idx_choice = num_choices - 1;
            }
            ImGui::Combo("Letter", &idx_choice, choices);

            // Add a button
            if (ImGui::Button("Print Hello", ImVec2(-1, 0))) {
                cout << "Hello\n";
            }
        }
    };

    viewer.data(0).set_mesh(V, F);

    // for showing the direction of normals.
    const RowVector3d red(0.8, 0.2, 0.2);
    const RowVector3d blue(0.2, 0.4, 0.87);
    // Average edge length for sizing
    const double avg = igl::avg_edge_length(V, F);
    cout << avg << "the avg is " << endl;  //testing the length

    viewer.data(0).add_edges(V + PD * avg, V - PD * avg, red);
    viewer.launch();
}

void triangle(_Mesh mesh_in, MatrixXi *F) {
    F->resize(mesh_in.n_faces() * 2, 3);

    int count = 0;
    int fcount = 0;
    for (auto f_it = mesh_in.faces_begin(); f_it != mesh_in.faces_end(); f_it++) { // face
        int vidx = 0;
        int first=0;
        int second=0;
        for (auto fh_it = mesh_in.fv_iter(*f_it); fh_it.is_valid(); ++fh_it) {
            if(vidx != 3)
            {
                (F->row(f_it->idx()*2))(first) = fh_it->idx();
                first++;
            }

            if(vidx != 1) {
                (F->row(f_it->idx()*2+1))(second) = fh_it->idx();
                second++;
            }
            vidx++;
        }
        fcount += 2;
    }
}

//void black_rec(_Mesh mesh_in, MatrixXd *V, MatrixXi *F) {
//    V->resize(mesh_in.n_vertices(), 3);
//    F->resize(mesh_in.n_faces() * 2, 3);
//
//    int count = 0;
//    int fcount = 0;
//    for (auto f_it = mesh_in.faces_begin(); f_it != mesh_in.faces_end(); f_it++) { // face
//        int vidx = 0;
//        int first=0;
//        int second=0;
//        for (auto fh_it = mesh_in.fh_iter(*f_it); fh_it.is_valid(); ++fh_it) {
//            auto vert_idx = (mesh_in.to_vertex_handle(*fh_it)).idx();
//            (*V)(vert_idx, 0) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[0] +
//                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[0]) / 2;
//            (*V)(vert_idx, 1) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[1] +
//                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[1]) / 2;
//            (*V)(vert_idx, 2) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[2] +
//                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[2]) / 2;
//            count++;
//
//            if(vidx != 3)
//            {
//                (F->row(fcount))(first) = vert_idx;
//                first++;
//            }
//
//
//            if(vidx != 1) {
//                (F->row(fcount+1))(second) = vert_idx;
//                second++;
//            }
//            vidx++;
//        }
//        fcount += 2;
//    }
//}

void black_rec(_Mesh mesh_in, MatrixXd *V, MatrixXi *F) {
    V->resize(mesh_in.n_faces() * 4, 3);
    F->resize(mesh_in.n_faces() * 2, 3);

    int count = 0;
    int fcount = 0;
    for (auto f_it = mesh_in.faces_begin(); f_it != mesh_in.faces_end(); f_it++) { // face
        for (auto fh_it = mesh_in.fh_iter(*f_it); fh_it.is_valid(); ++fh_it) {
            (*V)(count, 0) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[0] +
                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[0]) / 2;
            (*V)(count, 1) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[1] +
                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[1]) / 2;
            (*V)(count, 2) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[2] +
                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[2]) / 2;
            count++;
        }
        auto idx_base = fcount * 2;
        F->row(fcount) << idx_base, idx_base + 1, idx_base + 2;
        F->row(++fcount) << idx_base, idx_base + 2, idx_base + 3;
        fcount++;
    }
}

void offset_black_rec(_Mesh mesh_in, MatrixXd f_normals, MatrixXd *V, MatrixXi *F) {
    V->resize(mesh_in.n_faces() * 4, 3);
    F->resize(mesh_in.n_faces() * 2, 3);

    int count = 0;
    int fcount = 0;
    for (auto f_it = mesh_in.faces_begin(); f_it != mesh_in.faces_end(); f_it++) { // face
        for (auto fh_it = mesh_in.fh_iter(*f_it); fh_it.is_valid(); ++fh_it) {
            (*V)(count, 0) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[0] +
                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[0]) / 2 +
                             f_normals.row(f_it->idx())[0];
            (*V)(count, 1) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[1] +
                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[1]) / 2 +
                             f_normals.row(f_it->idx())[1];
            (*V)(count, 2) = ((mesh_in.point(mesh_in.to_vertex_handle(*fh_it)))[2] +
                              (mesh_in.point(mesh_in.from_vertex_handle(*fh_it)))[2]) / 2 +
                             f_normals.row(f_it->idx())[2];
            count++;
        }
        auto idx_base = fcount * 2;
        F->row(fcount) << idx_base, idx_base + 1, idx_base + 2;
        F->row(++fcount) << idx_base, idx_base + 2, idx_base + 3;
        fcount++;
    }
}

void offset_black_v_normals(MatrixXd Vorig, MatrixXi Forig, MatrixXd v_normals, MatrixXd *V, MatrixXi *F) {
    V->resize(Vorig.rows(), 3);
    F->resize(Forig.rows(), 3);

    *V = Vorig - v_normals;
    *F = Forig;
}

// mesh_in, init_v_normals, v_normals, f_centers, f_normals, v_normals, gaussian_curvatures, mean_curvatures
void view_total_info(_Mesh mesh_in, MatrixXd init_v_normals, MatrixXd v_normals, MatrixXd f_centers,
                     MatrixXd f_normals, MatrixXd gaussian_curvatures, MatrixXd mean_curvatures,
                     MatrixXd pc1, MatrixXd pc2) {
    igl::opengl::glfw::Viewer viewer;
    viewer.data().line_width = 4.0; //line width

    // for showing the direction of normals.
    const RowVector3d red(0.8, 0.2, 0.2);
    const RowVector3d blue(0, 1, 0);
    const RowVector3d black(0, 0, 0);
    MatrixXd to_vert, from_vert;
    to_vert.resize(mesh_in.n_vertices() * 4, 3);
    from_vert.resize(mesh_in.n_vertices() * 4, 3);

    // for offsetting the faces and balck rec
    MatrixXd Vinner, Voffset, Vvert;
    MatrixXi Finner, Foffset, Fvert;
    black_rec(mesh_in, &Vinner, &Finner);
    offset_black_rec(mesh_in, f_normals, &Voffset, &Foffset);

    // for showing quad mesh
    MatrixXi F;
    triangle(mesh_in, &F);
//    mesh_in.triangulate();
    MatrixXd V;
//    MatrixXi F;
    V.resize(mesh_in.n_vertices(), 3);
//    F.resize(mesh_in.n_faces(), 3);

    for (size_t i = 0; i < mesh_in.n_vertices(); ++i) {
        auto p = mesh_in.point(mesh_in.vertex_handle(i));
        V(i, 0) = p[0];
        V(i, 1) = p[1];
        V(i, 2) = p[2];
    }

    // offset
    offset_black_v_normals(V, F, v_normals, &Vvert, &Fvert);

    // Plot the mesh_in with pseudocolors
    // Average edge length for sizing
    const double avg = igl::avg_edge_length(V, F);
//    cout << avg << "the avg is " << endl;  //testing the length

    // set mesh and add edges. multi-layer drawing,
    // by concealing the trangle mesh (cancle the wireframe) we will show the the quad mesh in the higher layer
    // show everything we have step by step by pressing different keys
    // default. initial v_normal and optimized v_normal
    // 1. show black midpoint division
    // 2. show face normals.
    // 3. show offset faces.
    // 4. curvatures of face guassian
    // 5. mean curvatures.

    viewer.data(0).set_mesh(V, F);
//    viewer.data(0).add_edges(from_vert, to_vert, black);
    viewer.data(0).add_edges(V, V - init_v_normals * avg, red);
    viewer.data(0).add_edges(V, V - v_normals * avg, blue);
    viewer.append_mesh(false);
    viewer.append_mesh(false);
    viewer.append_mesh(false);
    viewer.append_mesh(false);
    viewer.append_mesh(false);
    viewer.append_mesh(false);

    // cout<< "vinner"<<Vinner <<endl<< "Finner"<<Finner<<endl;  // bebuging
    viewer.data_list[1].set_mesh(Vinner, Finner);  // show black
//    viewer.data_list[2].set_mesh(Voffset, Foffset);
    viewer.data_list[2].set_mesh(V, F);
    viewer.data_list[2].add_edges(f_centers + f_normals * avg, f_centers - f_normals * avg, black);
    viewer.data_list[3].set_mesh(Vvert, Fvert);

    viewer.data_list[4].set_mesh(V, F);
    MatrixXd C;
    igl::jet(gaussian_curvatures, true, C);
//    cout<<"\n---------"<<"C size is "<< C.size() << "mesh"<<mesh_in.n_vertices()<<"\n"<<C<<endl;
    viewer.data_list[4].set_colors(C);
    // cout << "Vertex size, " << V.rows() << "Face size, : " << F.rows() << endl;
    // cout << "COlor size << " <<  gaussian_curvatures.rows() << "x" << gaussian_curvatures.cols() << endl;

    viewer.data_list[5].set_mesh(V, F);
    MatrixXd mean_cur_color;
    igl::jet(mean_curvatures, true, mean_cur_color);
    viewer.data_list[5].set_colors(mean_cur_color);

    viewer.data_list[6].set_mesh(V, F);
    viewer.data_list[6].add_edges(f_centers + 1.2*pc1 * avg, f_centers - 1.2*pc1 * avg, red);
    viewer.data_list[6].add_edges(f_centers + 1.2*pc2 * avg, f_centers - 1.2*pc2 * avg, blue);
    viewer.data(6).line_width = 3.0; //line width

    viewer.selected_data_index = 0;
    viewer.callback_key_down =
            [&](igl::opengl::glfw::Viewer &, unsigned int key, int mod) {
                if (key == GLFW_KEY_0) {
                    cout << "got 0" << endl;
                    viewer.data(1).set_visible(false);
                    viewer.data(2).set_visible(false);
                    viewer.data(3).set_visible(false);
                    viewer.data(4).set_visible(false);
                    viewer.data(5).set_visible(false);
                    viewer.data(0).set_visible(true);
                    return true;
                }

                if (key == GLFW_KEY_1) {
                    cout << "got 1" << endl;
                    int old_id = viewer.data().id;
                    cout << old_id << endl;
                    cout << "selected idx " << viewer.selected_data_index << endl;
                    viewer.selected_data_index = 1;
                    viewer.data_list[viewer.selected_data_index].set_colors(RowVector3d(0.1, 0.1, 0.9));
                    is_visible = !is_visible;
                    viewer.data(viewer.selected_data_index).set_visible(is_visible);
                    return true;
                }
                else if (key == GLFW_KEY_SPACE)
                {
                    viewer.selected_data_index = (viewer.selected_data_index+1) %6;
                    return true;
                }
                else if (key == GLFW_KEY_ENTER)
                {
                    is_visible = !is_visible;
                    viewer.data(0).set_visible(is_visible);
                    return true;
                }

                else if (key == GLFW_KEY_2) {
                    cout << "got enter" << endl;
                    is_visible_offset = !is_visible_offset;
                    viewer.data(2).set_visible(!is_visible_offset);
                    viewer.data(0).set_visible(false);
                    return true;
                } else if (key == GLFW_KEY_3) {
                    // close 1
                    viewer.selected_data_index = 0;
                    viewer.data(viewer.selected_data_index).set_visible(true);

                    cout << "got 3" << endl;
                    int old_id = viewer.data().id;
                    cout << old_id << endl;
                    cout << "selected idx " << viewer.selected_data_index << endl;

                    viewer.selected_data_index = 3;
                    viewer.data_list[viewer.selected_data_index].set_colors(RowVector3d(0.8, 0.5, 0.9));
                    is_visible_vert = !is_visible_vert;
                    viewer.data(viewer.selected_data_index).set_visible(is_visible_vert);
                    return true;
                } else if (key == GLFW_KEY_4) {
                    cout << "got 4" << endl;
                    int old_id = viewer.data().id;
                    cout << old_id << endl;
                    cout << "selected idx " << viewer.selected_data_index << endl;
                    viewer.selected_data_index = 4;
                    is_visible_offset = !is_visible_offset;
                    viewer.data(viewer.selected_data_index).set_visible(!is_visible_offset);

                    // close 0
                    viewer.selected_data_index = 0;
                    viewer.data(viewer.selected_data_index).set_visible(false);
                    return true;
                } else if (key == GLFW_KEY_5) {
                    cout << "got 5" << endl;
                    viewer.selected_data_index = 5;
                    is_visible_offset = !is_visible_offset;
                    viewer.data(viewer.selected_data_index).set_visible(!is_visible_offset);

                    // close 0
                    viewer.selected_data_index = 0;
                    viewer.data(viewer.selected_data_index).set_visible(false);
                    return true;
                }
                else if (key == GLFW_KEY_6) {
                    cout << "got 6" << endl;
                    viewer.selected_data_index = 6;
                    is_visible_offset = !is_visible_offset;
                    viewer.data(viewer.selected_data_index).set_visible(!is_visible_offset);

                    // close 0
                    viewer.selected_data_index = 0;
                    viewer.data(viewer.selected_data_index).set_visible(false);
                    return true;

                }
                return false;
            };

    igl::opengl::glfw::imgui::ImGuiMenu menu;
    viewer.plugins.push_back(&menu);
    // Customize the menu
    double doubleVariable = 0.1f; // Shared between two menus
    // Add content to the default menu windowOh
    menu.callback_draw_viewer_menu = [&]() {
        // Draw parent menu content
        menu.draw_viewer_menu();
        // Add new group
        if (ImGui::CollapsingHeader("New Group", ImGuiTreeNodeFlags_DefaultOpen)) {
            // Expose variable directly ...
            ImGui::InputDouble("double", &doubleVariable, 0, 0, "%.4f");

            // ... or using a custom callback
            static bool boolVariable = true;
            if (ImGui::Checkbox("bool", &boolVariable)) {
                // do something
                cout << "boolVariable: " << boolalpha << boolVariable << endl;
            }

            // Expose an enumeration type
            enum Orientation {
                Up = 0, Down, Left, Right
            };
            static Orientation dir = Up;
            ImGui::Combo("Direction", (int *) (&dir), "Up\0Down\0Left\0Right\0\0");

            // We can also use a vector<string> defined dynamically
            static int num_choices = 3;
            static vector<string> choices;
            static int idx_choice = 0;
            if (ImGui::InputInt("Num letters", &num_choices)) {
                num_choices = max(1, min(26, num_choices));
            }
            if (num_choices != (int) choices.size()) {
                choices.resize(num_choices);
                for (int i = 0; i < num_choices; ++i)
                    choices[i] = string(1, 'A' + i);
                if (idx_choice >= num_choices)
                    idx_choice = num_choices - 1;
            }
            ImGui::Combo("Letter", &idx_choice, choices);

            // Add a button
            if (ImGui::Button("Print Hello", ImVec2(-1, 0))) {
                cout << "Hello\n";
            }
        }
    };

    viewer.launch();
}

template<class MatT>
Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime>
pseudoinverse(const MatT &mat, typename MatT::Scalar tolerance = typename MatT::Scalar{1e-7}) // choose appropriately
{
    typedef typename MatT::Scalar Scalar;
    auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
    const auto &singularValues = svd.singularValues();
    Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime> singularValuesInv(mat.cols(), mat.rows());
    singularValuesInv.setZero();
    for (unsigned int i = 0; i < singularValues.size(); ++i) {
        if (singularValues(i) > tolerance) {
            singularValuesInv(i, i) = Scalar{1} / singularValues(i);
        } else {
            singularValuesInv(i, i) = Scalar{0};
        }
    }
    return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
}

int main(int argc, char *argv[]) {
    std::cout << "Implementation of quad mesh curvature estimation via optimization" << std::endl;

    //// --------------------- read meashes
    _Mesh mesh;
    if (!OpenMesh::IO::read_mesh(mesh, "../../../model/FlyingCarpet_quad.obj")) {
        std::cerr << "read error\n";
        exit(1);
    }

    //// --------------------- normal initialize
    // initilize the normal for each vertex as the cross product of two edges.
    // which is the initial guess by normalized cross product of two edges
    vv3d_t one_ring_edges;
    vector<size_t> valence;
    v3d_t center;
    MatrixXd v_normals = MatrixXd::Zero(mesh.n_vertices(), 3);
    int n_neighbor;
    for (auto v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); v_it++) { // vertex
        center = mesh.point(*v_it);
        n_neighbor = 0;
        for (_Mesh::VertexVertexIter vv_it = mesh.vv_iter(*v_it); vv_it.is_valid(); ++vv_it) {
            auto curr = mesh.point(*vv_it);
            one_ring_edges.push_back(v3d_t(curr - center).normalized());
            n_neighbor++;
        }
        valence.push_back(one_ring_edges.size());
        if (n_neighbor > 2) {
            one_ring_edges.push_back(one_ring_edges[0]);
        }

        v3d_t normal = v3d_t(0);
        for (size_t ii = 0; ii < one_ring_edges.size() - 1; ii++) {
            normal += one_ring_edges[ii] % one_ring_edges[ii + 1]; // % is the cross product
        }
        normal /= one_ring_edges.size() - 1;
        one_ring_edges.clear();
        v_normals(v_it->idx(), 0) = normal[0];
        v_normals(v_it->idx(), 1) = normal[1];
        v_normals(v_it->idx(), 2) = normal[2];
    }

    // Now we have all the vertex normals (but just the simple guess).
    // check if v_normals is correct or not.
    // todo: why so many zeros.
//    cout << "\n-------------------" << endl << "v_normals" << endl << v_normals << "\n-------------------" << endl;
//    view_v_normal(mesh, v_normals);

    //// --------------------- face_normals, which is always the same during optimization
    // get the normal for each face using the cross product between the intercross lines of the quad.
    MatrixXd f_normals;
    MatrixXd f_centers;
    f_normals.resize(mesh.n_faces(), 3);
    f_centers.resize(mesh.n_faces(), 3);
    vv3d_t quad_vertices;
    for (auto f_it = mesh.faces_begin(); f_it != mesh.faces_end(); f_it++) { // face
        for (auto fv_it = mesh.fv_iter(*f_it); fv_it.is_valid(); ++fv_it) {
            quad_vertices.push_back(v3d_t(mesh.point(*fv_it)));
        }

        auto normal = ((quad_vertices[0] - quad_vertices[2]) %
                       (quad_vertices[1] - quad_vertices[3])).normalized();

        auto f_center = quad_vertices[2] + (quad_vertices[0] - quad_vertices[2]) / 2;
        quad_vertices.clear();

        f_normals(f_it->idx(), 0) = normal[0];
        f_normals(f_it->idx(), 1) = normal[1];
        f_normals(f_it->idx(), 2) = normal[2];

        f_centers(f_it->idx(), 0) = f_center[0];
        f_centers(f_it->idx(), 1) = f_center[1];
        f_centers(f_it->idx(), 2) = f_center[2];
    }

    //  Now we have all the vertex normals (initial guess), and face normals
    // cout << "\n-------------------" << endl << "f_normals" << endl << f_normals << "\n-------------------" << endl;
    // view_f_normals(mesh, f_normals, f_centers);

    //// ----------------------- lets get the face normal of the adjacent faces of each quad.
    MatrixXd adj_f_normals;
    MatrixXi edg_orders;
    adj_f_normals.resize(mesh.n_faces(), 12);
    edg_orders.resize(mesh.n_faces(), 4);
    for (auto f_it = mesh.faces_begin(); f_it != mesh.faces_end(); f_it++) { // face
        int face_count = 0;
        for (auto fh_it = mesh.fh_iter(*f_it); fh_it.is_valid(); ++fh_it) {
            edg_orders(f_it->idx(), face_count) = fh_it->idx();
            auto opposite_idx = (mesh.opposite_face_handle(*fh_it)).idx();
            if (!mesh.is_boundary(*fh_it) and opposite_idx != -1) {
                adj_f_normals(f_it->idx(), 3 * face_count) = f_normals.row(opposite_idx)(0);
                adj_f_normals(f_it->idx(), 3 * face_count + 1) = f_normals.row(opposite_idx)(1);
                adj_f_normals(f_it->idx(), 3 * face_count + 2) = f_normals.row(opposite_idx)(2);
                face_count++;
            }
        }
    }

    //  Now we have all the vertex normals, and face normals, adjacent face normals
    // cout << "\n-------------------" << endl << "edg_orders" << endl << edg_orders << "\n-------------------" << endl;
    // cout << "\n-------------------" << endl << "adjacent f_normals" << endl << adj_f_normals << "\n-------------------" << endl;

    //// ----------------- optimization, update the vertices normals, and then calculate the vertex curvatures.
    /// -> shape operator -> egien value -> principal curvatures
    // for 12 constraints
    // MatrixXd lin_sys_a = MatrixXd::Zero(12, 12);
    // VectorXd lin_sys_b; lin_sys_b.resize(12, 1);
    // lin_sys_b << 2.0 ,  2.0 ,  2.0 ,  0 ,  2.0 ,  2.0 ,  2.0 ,  2.0, 0.0, 0.0, 0.0, 0.0;
    // for 16 constraints (over constraint, default)

    MatrixXd lin_sys_a = MatrixXd::Zero(16, 12);
    VectorXd lin_sys_b;
    lin_sys_b.resize(16, 1);
    lin_sys_b << 2.0, 2.0, 2.0, 0, 2.0, 2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

    MatrixXd solution;

    MatrixXd init_v_normals = v_normals;
    MatrixXd new_v_normals = v_normals;
    deque<Vector3d> vertices_;
    deque<v3d_t> adj_normals;
    Vector3d f_normal;
    Vector3d of_normal; //opposite_face_normal
    MatrixXd v_normals_diff = MatrixXd::Zero(4, 3);
    MatrixXd v_normals_diff2 = MatrixXd::Zero(4, 3);
    MatrixXd opp_normals = MatrixXd::Zero(4, 3);
    int f_it_idx;
    vector<size_t> v_idx_of_face;
    v3d_t v_normal;
    set<int, greater<int>> neighbours;
    set<int, greater<int>> vtx_neighbours;

    //// optimization
    // argv is the input argument for optimization steps.
    for (int k = 0; k < atoi(argv[1]); k++) {   // optimization steps
        for (auto f_it = mesh.faces_begin(); f_it != mesh.faces_end(); f_it++) { // faces
            f_it_idx = f_it->idx();
            //  cout << "=====> curr_face " << f_it_idx << "/" << mesh.n_faces() << endl;
            for (size_t ii = 0; ii < 4; ii++) {  // number of edges of each face is 4
                int e = edg_orders(f_it_idx, ii);   // handle No. of edge
                auto edge = mesh.halfedge_handle(e);    // half edge handle
                auto v_from = mesh.from_vertex_handle(edge);    // v_from handle
                auto f_idx = mesh.face_handle(edge);

                f_normal = f_normals.row(f_idx.idx());
                // get the opposite face normals, when there is a boundary (no adj face), of_normal will be zero
                of_normal(0) = adj_f_normals.row(f_it_idx)(ii * 3);
                of_normal(1) = adj_f_normals.row(f_it_idx)(ii * 3 + 1);
                of_normal(2) = adj_f_normals.row(f_it_idx)(ii * 3 + 2);

                v_idx_of_face.push_back(v_from.idx());  // 4 vertices of a face
                auto vtx = mesh.point(v_from);
                // get 4 vertices and 4 adj_normals
                vertices_.push_back(Vector3d(vtx[0], vtx[1], vtx[2]));
                adj_normals.push_back(v3d_t(of_normal(0), of_normal(1), of_normal(2)));

                //// this part is a little bit confusing at the first time,
                /// this is for getting the vertice v_j along the same line of v.
                // get the neighbors of vertex on the inside and outside
                // indexed properly. so that we get the constraints on the vertex normals
                // in the two PC directions
                // for opp face, add vtx
                //for curr face add vtx
                // find out = set\curr_vtx_neigh
                //pair is out, mesh.from(he)
                auto opposite_fidx = mesh.opposite_face_handle(edge);
                auto curr_fidx = mesh.face_handle(edge);
                int opp_vtx_idx, in_vtx_idx;
                if (!mesh.is_boundary(edge) and opposite_fidx.idx() != -1) {
                    for (auto curr_fvit = mesh.fv_iter(opposite_fidx); curr_fvit.is_valid(); ++curr_fvit) {
                        neighbours.insert(curr_fvit->idx());
                    }
                }

                if (!mesh.is_boundary(edge) and curr_fidx.idx() != -1) {
                    for (auto curr_fvit = mesh.fv_iter(curr_fidx); curr_fvit.is_valid(); ++curr_fvit) {
                        neighbours.insert(curr_fvit->idx());
                    }
                }

                for (auto vv_it = mesh.vv_iter(v_from); vv_it.is_valid(); ++vv_it) {
                    vtx_neighbours.insert(vv_it->idx());
                }

                // you may have nothing here.
                for (auto jj: vtx_neighbours) {
                    if (!neighbours.count(jj)) {
                        opp_vtx_idx = jj;
                        in_vtx_idx = mesh.to_vertex_handle(edge).idx();
//                        in_vtx_idx = v_from.idx();
                        v_normals_diff(ii, 0) = v_normals(opp_vtx_idx, 0) - v_normals(in_vtx_idx, 0);
                        v_normals_diff(ii, 1) = v_normals(opp_vtx_idx, 1) - v_normals(in_vtx_idx, 1);
                        v_normals_diff(ii, 2) = v_normals(opp_vtx_idx, 2) - v_normals(in_vtx_idx, 2);
                    }
                }

                // find the second pair. If not exists, zeros
                neighbours.clear(); // do not clear vtx_neighbours, we will use them.
                if (ii == 0) { e = edg_orders(f_it_idx, 3); }
                else { e = edg_orders(f_it_idx, ii - 1); }

                //// todo: the only thing missing is error checking in the edg order function as sometiomes the edge handles are useless when the edge to visit is a boundary edge
                // if (e >= mesh.n_halfedges() || e < 0 ){
                //     continue;
                // }

                edge = mesh.halfedge_handle(e);
                opposite_fidx = mesh.opposite_face_handle(edge);
                if (!mesh.is_boundary(edge) and opposite_fidx.idx() != -1) {
                    for (auto curr_fvit = mesh.fv_iter(opposite_fidx); curr_fvit.is_valid(); ++curr_fvit) {
                        neighbours.insert(curr_fvit->idx());
                    }
                }

                if (!mesh.is_boundary(edge) and curr_fidx.idx() != -1) {
                    for (auto curr_fvit = mesh.fv_iter(curr_fidx); curr_fvit.is_valid(); ++curr_fvit) {
                        neighbours.insert(curr_fvit->idx());
                    }
                }

                for (auto jj: vtx_neighbours) {
                    if (!neighbours.count(jj)) {
                        opp_vtx_idx = jj;
                        in_vtx_idx = mesh.to_vertex_handle(edge).idx();
                        v_normals_diff2(ii, 0) = v_normals(opp_vtx_idx, 0) - v_normals(in_vtx_idx, 0);
                        v_normals_diff2(ii, 1) = v_normals(opp_vtx_idx, 1) - v_normals(in_vtx_idx, 1);
                        v_normals_diff2(ii, 2) = v_normals(opp_vtx_idx, 2) - v_normals(in_vtx_idx, 2);

                        // below is for another method, keep it.
                        //v_normal[0] = v_normals(opp_vtx_idx, 0);
                        //v_normal[1] = v_normals(opp_vtx_idx, 1);
                        //v_normal[2] = v_normals(opp_vtx_idx, 2);
                        //opp_normals(ii) = 2- of_normal % v_normal;
                    }
                }
            }

            /// now we build the linear system and solve it. Here we construct A and b matrix one by one element-wise
            /// Black-rectangle constraints
            lin_sys_a(0, 0) = f_normal[0];
            lin_sys_a(0, 1) = f_normal[1];
            lin_sys_a(0, 2) = f_normal[2];
            lin_sys_a(0, 3) = f_normal[0];
            lin_sys_a(0, 4) = f_normal[1];
            lin_sys_a(0, 5) = f_normal[2];

            lin_sys_a(1, 3) = f_normal[0];
            lin_sys_a(1, 4) = f_normal[1];
            lin_sys_a(1, 5) = f_normal[2];
            lin_sys_a(1, 6) = f_normal[0];
            lin_sys_a(1, 7) = f_normal[1];
            lin_sys_a(1, 8) = f_normal[2];

            lin_sys_a(2, 6) = f_normal[0];
            lin_sys_a(2, 7) = f_normal[1];
            lin_sys_a(2, 8) = f_normal[2];
            lin_sys_a(2, 9) = f_normal[0];
            lin_sys_a(2, 10) = f_normal[1];
            lin_sys_a(2, 11) = f_normal[2];

            /// Shape operator constraint
            auto v13 = vertices_[1] - vertices_[3];
            auto v20 = vertices_[2] - vertices_[0];
            lin_sys_a(3, 0) = v13[0];
            lin_sys_a(3, 1) = v13[1];
            lin_sys_a(3, 2) = v13[2];
            lin_sys_a(3, 3) = v20[0];
            lin_sys_a(3, 4) = v20[1];
            lin_sys_a(3, 5) = v20[2];
            lin_sys_a(3, 6) = -v13[0];
            lin_sys_a(3, 7) = -v13[1];
            lin_sys_a(3, 8) = -v13[2];
            lin_sys_a(3, 9) = -v20[0];
            lin_sys_a(3, 10) = -v20[1];
            lin_sys_a(3, 11) = -v20[2];

            /// Adjacent face constraints
            lin_sys_a(4, 0) = adj_normals[0][0];
            lin_sys_a(4, 1) = adj_normals[0][1];
            lin_sys_a(4, 2) = adj_normals[0][2];
            lin_sys_a(4, 3) = adj_normals[0][0];
            lin_sys_a(4, 4) = adj_normals[0][1];
            lin_sys_a(4, 5) = adj_normals[0][2];

            lin_sys_a(5, 3) = adj_normals[1][0];
            lin_sys_a(5, 4) = adj_normals[1][1];
            lin_sys_a(5, 5) = adj_normals[1][2];
            lin_sys_a(5, 6) = adj_normals[1][0];
            lin_sys_a(5, 7) = adj_normals[1][1];
            lin_sys_a(5, 8) = adj_normals[1][2];

            lin_sys_a(6, 6) = adj_normals[2][0];
            lin_sys_a(6, 7) = adj_normals[2][1];
            lin_sys_a(6, 8) = adj_normals[2][2];
            lin_sys_a(6, 9) = adj_normals[2][0];
            lin_sys_a(6, 10) = adj_normals[2][1];
            lin_sys_a(6, 11) = adj_normals[2][2];

            lin_sys_a(7, 0) = adj_normals[3][0];
            lin_sys_a(7, 1) = adj_normals[3][1];
            lin_sys_a(7, 2) = adj_normals[3][2];
            lin_sys_a(7, 9) = adj_normals[3][0];
            lin_sys_a(7, 10) = adj_normals[3][1];
            lin_sys_a(7, 11) = adj_normals[3][2];

            /// Adjacent face constraints. This is for the second method, have not finished
            // lin_sys_a(8, 0) = opp_normals[0][0];
            // lin_sys_a(8, 1) = adj_normals[0][1];
            // lin_sys_a(8, 2) = adj_normals[0][2];
            // lin_sys_a(8, 3) = adj_normals[0][0];
            // lin_sys_a(8, 4) = adj_normals[0][1];
            // lin_sys_a(8, 5) = adj_normals[0][2];

            /// gordon's inline constraint
            lin_sys_a(8, 0) = v_normals_diff(0, 0);
            lin_sys_a(8, 1) = v_normals_diff(0, 1);
            lin_sys_a(8, 2) = v_normals_diff(0, 2);

            lin_sys_a(9, 3) = v_normals_diff(1, 0);
            lin_sys_a(9, 4) = v_normals_diff(1, 1);
            lin_sys_a(9, 5) = v_normals_diff(1, 2);

            lin_sys_a(10, 6) = v_normals_diff(2, 0);
            lin_sys_a(10, 7) = v_normals_diff(2, 1);
            lin_sys_a(10, 8) = v_normals_diff(2, 2);

            lin_sys_a(11, 9) = v_normals_diff(3, 0);
            lin_sys_a(11, 10) = v_normals_diff(3, 1);
            lin_sys_a(11, 11) = v_normals_diff(3, 2);

            lin_sys_a(12, 0) = v_normals_diff2(0, 0);
            lin_sys_a(12, 1) = v_normals_diff2(0, 1);
            lin_sys_a(12, 2) = v_normals_diff2(0, 2);

            lin_sys_a(13, 3) = v_normals_diff2(1, 0);
            lin_sys_a(13, 4) = v_normals_diff2(1, 1);
            lin_sys_a(13, 5) = v_normals_diff2(1, 2);

            lin_sys_a(14, 6) = v_normals_diff2(2, 0);
            lin_sys_a(14, 7) = v_normals_diff2(2, 1);
            lin_sys_a(14, 8) = v_normals_diff2(2, 2);

            lin_sys_a(15, 9) = v_normals_diff2(3, 0);
            lin_sys_a(15, 10) = v_normals_diff2(3, 1);
            lin_sys_a(15, 11) = v_normals_diff2(3, 2);

            solution = lin_sys_a.colPivHouseholderQr().solve(lin_sys_b);

            /// now we get the new local v_normals : solution.
            new_v_normals(v_idx_of_face[0], 0) = solution(0);
            new_v_normals(v_idx_of_face[0], 1) = solution(1);
            new_v_normals(v_idx_of_face[0], 2) = solution(2);
            new_v_normals(v_idx_of_face[1], 0) = solution(3);
            new_v_normals(v_idx_of_face[1], 1) = solution(4);
            new_v_normals(v_idx_of_face[1], 2) = solution(5);
            new_v_normals(v_idx_of_face[2], 0) = solution(6);
            new_v_normals(v_idx_of_face[2], 1) = solution(7);
            new_v_normals(v_idx_of_face[2], 2) = solution(8);
            new_v_normals(v_idx_of_face[3], 0) = solution(9);
            new_v_normals(v_idx_of_face[3], 1) = solution(10);
            new_v_normals(v_idx_of_face[3], 2) = solution(11);

            // cout << "\t\tRelative error " << (lin_sys_a * solution - lin_sys_b).norm() / lin_sys_b.norm() << endl;

            v_idx_of_face.clear();
            adj_normals.clear();
            // lin_sys_a = MatrixXd::Zero(12, 12); // this is for the second method, comgaussian_curvaturesment it if not used.
            lin_sys_a = MatrixXd::Zero(16, 12);
            neighbours.clear();
            vtx_neighbours.clear();
            v_normals_diff = MatrixXd::Zero(4, 3);
        }
        v_normals = v_normals + 0.001 * new_v_normals;  /// important. The step size is 0.001
    }

    /// Now we have everthing.
    /// calculate curvatures locally by solving the local shape operator and get its egenvalues.
    MatrixXd lin_sys_v = MatrixXd::Zero(2, 3);  // vertices corrdinates
    MatrixXd lin_sys_n = MatrixXd::Zero(2, 3);  // v_normals.
    MatrixXd shape_operator = MatrixXd::Zero(2, 2);
    vector<RowVector3d> quad_v_normals;
    quad_vertices.clear(); // we use the quad_vertices defined before.

    MatrixXd curvature = MatrixXd::Zero(1, 2); //local curvature
    MatrixXd principle_vector = MatrixXd::Zero(2, 2);
    MatrixXd gaussian_curvatures = MatrixXd::Zero(mesh.n_faces()*2, 1); // we have to change to traingle.
    MatrixXd mean_curvatures = MatrixXd::Zero(mesh.n_faces()*2, 1);
    MatrixXd pc1 = MatrixXd::Zero(mesh.n_faces(), 3);
    MatrixXd pc2 = MatrixXd::Zero(mesh.n_faces(), 3);
    MatrixXd coor_trans = MatrixXd::Zero(3, 3);

    for (auto f_it = mesh.faces_begin(); f_it != mesh.faces_end(); f_it++) { // face
        for (auto fv_it = mesh.fv_iter(*f_it); fv_it.is_valid(); ++fv_it) {  // v
            quad_vertices.push_back(v3d_t(mesh.point(*fv_it)));
            quad_v_normals.push_back(v_normals.row((fv_it->idx())));
        }
        lin_sys_v(0, 0) = quad_vertices[0][0] - quad_vertices[2][0];
        lin_sys_v(0, 1) = quad_vertices[0][1] - quad_vertices[2][1];
        lin_sys_v(0, 2) = quad_vertices[0][2] - quad_vertices[2][2];
        lin_sys_v(1, 0) = quad_vertices[1][0] - quad_vertices[3][0];
        lin_sys_v(1, 1) = quad_vertices[1][1] - quad_vertices[3][1];
        lin_sys_v(1, 2) = quad_vertices[1][2] - quad_vertices[3][2];

        lin_sys_n(0, 0) = quad_v_normals[2][0] - quad_v_normals[0][0];
        lin_sys_n(0, 1) = quad_v_normals[2][1] - quad_v_normals[0][1];
        lin_sys_n(0, 2) = quad_v_normals[2][2] - quad_v_normals[0][2];
        lin_sys_n(1, 0) = quad_v_normals[3][0] - quad_v_normals[1][0];
        lin_sys_n(1, 1) = quad_v_normals[3][1] - quad_v_normals[1][1];
        lin_sys_n(1, 2) = quad_v_normals[3][2] - quad_v_normals[1][2];

        cout << "----------------\n"<<"lin_sys\n"<<lin_sys_v << endl << lin_sys_n <<endl;  //debuging
        shape_operator = lin_sys_n * pseudoinverse(lin_sys_v);
        cout << "shape operator \n"<< shape_operator<<endl;
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(shape_operator);
        if (eigensolver.info() != Eigen::Success) abort();

        curvature = eigensolver.eigenvalues();
        cout << "egienvalues \n"<< curvature<<endl;  //debuging
        principle_vector = eigensolver.eigenvectors();
        auto f_idx = f_it->idx();
        gaussian_curvatures(f_idx*2, 0) = curvature(0, 0) * curvature(1,0);
        gaussian_curvatures(f_idx*2+1, 0) = curvature(0,0) * curvature(1,0);
        mean_curvatures(f_idx*2, 0) = (curvature(0,0) + curvature(1,0)) / 2;
        mean_curvatures(f_idx*2+1, 0) = (curvature(0,0) + curvature(1,0)) / 2;

        coor_trans(0, 2) = f_normals(f_idx, 0);
        coor_trans(1, 2) = f_normals(f_idx, 1);
        coor_trans(2, 2) = f_normals(f_idx, 2);

        coor_trans(0, 0) = quad_vertices[0][0] - quad_vertices[2][0];
        coor_trans(1, 0) = quad_vertices[0][1] - quad_vertices[2][1];
        coor_trans(2, 0) = quad_vertices[0][2] - quad_vertices[2][2];

        coor_trans( 0, 1) = quad_vertices[1][0] - quad_vertices[3][0];
        coor_trans( 1, 1) = quad_vertices[1][1] - quad_vertices[3][1];
        coor_trans( 2, 1) = quad_vertices[1][2] - quad_vertices[3][2];

        pc1.row(f_idx) = coor_trans * Vector3d(principle_vector(0,0), principle_vector(1,0), 0);
        pc2.row(f_idx) = coor_trans * Vector3d(principle_vector(0,1), principle_vector(1,1), 0);

        quad_vertices.clear();
        quad_v_normals.clear();
    }
    /// curvarues.
    //cout << "\n-------------------" << endl << "guassian_curvatures" << endl << gaussian_curvatures
     //    << "\n-------------------" << endl;
    //cout << "\n-------------------" << endl << "mean_curvatures" << endl << mean_curvatures << "\n-------------------"<< endl;
    //cout << "\n-------------------" << endl << "principal direction 1"<<pc1 <<endl;
    //cout << "\n-------------------" << endl << "principal direction 2"<<pc2 <<endl;

    /// now we have the curvatures, we will show the curvatures of each vertices.
    // show everything we have step by step by pressing different keys
    // 1: face normals.
    // 2. initial v_normal and optimized v_normal
    // 3. offset faces.
    // 4. curvatures of face guassian
    // 5. mean curvatures.
    view_total_info(mesh, init_v_normals, v_normals, f_centers, f_normals, gaussian_curvatures, mean_curvatures, pc1, pc2);
    return 0;
}


/**
 *  Safe landing control of DC9-30 with the abstraction-based engine.
 *
 *  Created by Yinan Li on Jan 11, 2021.
 *  Hybrid Systems Group, University of Waterloo.
 */

#include <iostream>
#include <boost/algorithm/string.hpp>
#include <cmath>
#include <sys/stat.h>

#include "src/system.hpp"
#include "src/abstraction.hpp"
#include "src/DBAparser.h"
#include "src/bsolver.hpp"
#include "src/hdf5io.h"

/* Parameters of the model */
double mg = 60000.0*9.81;
double mi = 1.0/60000; /* weight inverse: 1/m */

/* ODE of the longitudinal equation of motions for DC9-30 */
struct eomlong {
    static const int n = 3;  // state dimension
    static const int m = 2;  // control dimension

    /**
     * Constructors:
     * @param[out] dx (dV, dgamma, dh)
     * @param[in] x (V,gamma,h).
     * @param[in] u (T, alpha) control input (thrust, angle of attack)
     */
    template<typename S>
    eomlong(S *dx, const S *x, rocs::Rn u) {
	double c = 1.25+4.2*u[1];
	dx[0] = mi*(u[0]*cos(u[1])-(2.7+3.08*c*c)*x[0]*x[0]-mg*sin(x[1]));
	dx[1] = (1.0/(60000*x[0]))*(u[0]*sin(u[1])+68.6*c*x[0]*x[0]-mg*cos(x[1]));
	dx[2] = x[0]*sin(x[1]);
    }

};

/** A target set in the form of f(x)<=0:
 * x(0)*sin(x(1)) >= -0.91
 *
 **/
template<typename T>
T target_area(const T &x) {
    T y(7);
    y[0] = -0.91-x[0]*sin(x[1]);
    y[1] = 63 - x[0];
    y[2] = x[0] - 75;
    y[3] = -3*M_PI/180 - x[1];
    y[4] = x[1];
    y[5] = -x[2];
    y[6] = x[2] - 2.5;
    return y;
}

int main(int argc, char *argv[])
{
    /**
     * Define the control system
     **/
    /* Set sampling time and disturbance */
    double tau = 0.25;
    double delta = 10;
    /* Set parameters for computation */
    int kmax = 5;
    double tol = 0.01;
    double alpha = 0.5;
    double beta = 2;
    rocs::params controlparams(kmax, tol, alpha, beta);
    rocs::CTCntlSys<eomlong> aircraft("landing", tau, eomlong::n,
				      eomlong::m, delta, &controlparams);
    /* set the state space */
    double xlb[] = {58, -3*M_PI/180, 0};
    double xub[] = {83, 0, 56};
    double ulb[] = {0,0};
    double uub[] = {32000, 8*M_PI/180};
    double mu[] = {32000, 9.0/8.0*M_PI/180};
    aircraft.init_workspace(xlb, xub);
    aircraft.init_inputset(mu, ulb, uub);
    // std::cout << "# of u= " << aircraft._ugrid._nv
    // 	      << ", dimension= " << aircraft._ugrid._dim << '\n';
    // for(int i = 0; i < aircraft._ugrid._nv; ++i) {
    // 	std::cout << aircraft._ugrid._data[i][0] << ','
    // 		  << aircraft._ugrid._data[i][1] << '\n';
    // }
    aircraft.allocate_flows();

    /**
     * Compute abstraction
     */
    /* Set the target set */
    const double eta[] = {25.0/362, 3*M_PI/180/66, 56.0/334};
    double glb[] = {63, -3*M_PI/180, 0};
    double gub[] = {75, 0, 2.5};
    rocs::abstraction< rocs::CTCntlSys<eomlong> > abst(&aircraft);
    abst.init_state(eta, xlb, xub);
    std::cout << "The number of abstraction states: " << abst._x._nv << '\n';
    auto target = [&abst, &eta, &glb, &gub](size_t i) {
		      std::vector<double> x(abst._x._dim);
		      abst._x.id_to_val(x, i);
		      double c[]{eta[0]/2.0, eta[1]/2.0, eta[2]/2.0}; //+1e-10;
		      if(x[0]-c[0] >= glb[0] && x[0]+c[0] <= gub[0] &&
			 x[1]-c[1] >= glb[1] && x[1]+c[1] <= gub[1] &&
			 x[2]-c[2] >= glb[2] && x[2]+c[2] <= gub[2])
			  return 1;
		      else
			  return 0;
		  };
    abst.assign_labels(target);
    abst.assign_label_outofdomain(0);
    /* Compute abstraction */
    clock_t tb, te;
    std::string transfile = "abstraction.h5";
    struct stat buffer;
    float tabst;
    if(stat(transfile.c_str(), &buffer) == 0) {
	/* Read from a file */
	std::cout << "Transitions have been computed. Reading transitions...\n";
	rocs::h5FileHandler transRdr(transfile, H5F_ACC_RDONLY);
	tb = clock();
	transRdr.read_transitions(abst._ts);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of reading abstraction: " << tabst << '\n';
    } else {
	std::cout << "Transitions haven't been computed. Computing transitions...\n";
	/* Robustness margins */
	double e1[] = {0,0,0};
	double e2[] = {0,0,0};
	tb = clock();
	abst.assign_transitions(e1, e2);
	te = clock();
	tabst = (float)(te - tb)/CLOCKS_PER_SEC;
	std::cout << "Time of computing abstraction: " << tabst << '\n';
	/* Write transitions to file */
	rocs::h5FileHandler transWtr(transfile, H5F_ACC_TRUNC);
	transWtr.write_transitions(abst._ts);
    }
    std::cout << "# of transitions: " << abst._ts._ntrans << '\n';

    std::vector<size_t> targetIDs;
    std::vector< std::vector<double> > targetPts;   //initial invariant set
    std::vector<double> x(abst._x._dim);
    for(size_t i = 0; i < abst._labels.size(); ++i) {
    	if(abst._labels[i] > 0) {
    	    targetIDs.push_back(i);
    	    abst._x.id_to_val(x, i);
    	    targetPts.push_back(x);
    	}
    }

    /**
     * Reachability control synthesis
     */
    rocs::DSolver solver(&abst._ts);
    tb = clock();
    solver.reachability(targetIDs);
    te = clock();
    float tsyn = (float)(te - tb)/CLOCKS_PER_SEC;
    std::cout << "Time of control synthesis: " << tsyn << '\n';
    std::cout << "The size of winning set: " << solver._nw << '\n';

    /**
     * Write the control synthesis result into .h5 file.
     */
    std::string datafile = "controller_abst_safelanding.h5";
    rocs::h5FileHandler ctlrWtr(datafile, H5F_ACC_TRUNC);
    ctlrWtr.write_problem_setting< rocs::CTCntlSys<eomlong> >(aircraft);
    ctlrWtr.write_2d_array<double>(targetPts, "G");
    ctlrWtr.write_array<double>(eta, 3, "eta");
    ctlrWtr.write_2d_array<double>(abst._x._data, "xgrid");
    ctlrWtr.write_discrete_controller(solver);

    aircraft.release_flows();
    return 0;
}


#ifndef KERNELS_HPP
#define KERNELS_HPP

#include <skelly_sim.hpp>

#include <Eigen/Dense>
#include <STKFMM/STKFMM.hpp>

/// Namespace for miscellaneous "kernel" functions and related convenience FMM class
namespace kernels {
typedef Eigen::MatrixXd (*fmm_kernel_func_t)(const int n_trg, MatrixRef &f_sl, MatrixRef &f_dl, stkfmm::STKFMM *);

Eigen::MatrixXd oseen_tensor_contract_direct(MatrixRef &r_src, MatrixRef &r_trg, MatrixRef &density, double eta = 1.0,
                                             double reg = 5E-3, double epsilon_distance = 1E-5);

Eigen::MatrixXd stokes_vel_fmm(const int n_trg, MatrixRef &f_sl, MatrixRef &f_dl, stkfmm::STKFMM *fmmPtr);

Eigen::MatrixXd stokes_pvel_fmm(const int n_trg, MatrixRef &f_sl, MatrixRef &f_dl, stkfmm::STKFMM *fmmPtr);

Eigen::MatrixXd oseen_tensor_direct(MatrixRef &r_src, MatrixRef &r_trg, double eta = 1.0, double reg = 5E-3,
                                    double epsilon_distance = 1E-5);

Eigen::MatrixXd rotlet(MatrixRef &r_src, MatrixRef &r_trg, MatrixRef &density, double eta = 1.0, double reg = 5E-3,
                       double epsilon_distance = 1E-5);

Eigen::MatrixXd stresslet_times_normal(MatrixRef &r_src, MatrixRef &normals, double eta = 1.0, double reg = 5E-3,
                                       double epsilon_distance = 1E-5);

Eigen::MatrixXd stresslet_times_normal_times_density(MatrixRef &r_src, MatrixRef &normals, MatrixRef &density,
                                                     double eta = 1.0, double reg = 5E-3,
                                                     double epsilon_distance = 1E-5);

/// Convenience class to represent an FMM interaction, which stores the STKFMM pointer. This
/// setup allows for a direct call to the FMM object which returns the relevant target kernel
/// evaluation matrix to each MPI rank.
template <typename stkfmm_type>
class FMM {
  public:
    template <typename F>
    FMM(const int order, const int maxPoints, const stkfmm::PAXIS paxis, const stkfmm::KERNEL k, const F &kernel_func)
        : fmmPtr_(new stkfmm_type(order, maxPoints, paxis, static_cast<unsigned>(k))), k_(k),
          kernel_func_(kernel_func){};

    /// @brief Set flag to force next call to set up tree, regardless of cache variables
    void force_setup_tree() { force_setup_tree_ = true; };

    /// @brief Evaluate the FMM kernel given the given sources/targets
    ///
    /// Repeated calls to the FMM object with the same source/target positions will maintain
    /// the FMM tree and therefore avoid the costly STKFMM::setupTree() call.
    ///
    /// @param[in] r_sl [ 3 x n_src ] matrix of 'single-layer' source coordinates
    /// @param[in] r_dl [ 3 x n_src ] matrix of 'double-layer' source coordinates
    /// @param[in] r_trg [ 3 x n_trg ] matrix of target coordinates
    /// @param[in] f_sl [ k_dim_sl x n_src ] matrix of 'single-layer' source strengths
    /// @param[in] f_sl [ k_dim_dl x n_src ] matrix of 'double-layer' source strengths
    /// @returns [ k_dim_trg x n_trg ] matrix of kernel evaluated at target positions given the sources
    Eigen::MatrixXd operator()(MatrixRef &r_sl, MatrixRef &r_dl, MatrixRef &r_trg, MatrixRef &f_sl, MatrixRef &f_dl) {
        // Check if LOCAL source/target points have changed, and then broadcast that for a GLOBAL update
        char setup_flag_local =
            (force_setup_tree_ || r_sl_old_.size() != r_sl.size() || r_dl_old_.size() != r_dl.size() ||
             r_trg_old_.size() != r_trg.size() || r_sl_old_ != r_sl || r_dl_old_ != r_dl || r_trg_old_ != r_trg);
        char setup_flag;
        MPI_Allreduce(&setup_flag_local, &setup_flag, 1, MPI_CHAR, MPI_LOR, MPI_COMM_WORLD);

        if (setup_flag) {
            double sl_min = r_sl.size() ? r_sl.minCoeff() : std::numeric_limits<double>::max();
            double dl_min = r_dl.size() ? r_dl.minCoeff() : std::numeric_limits<double>::max();
            double trg_min = r_trg.size() ? r_trg.minCoeff() : std::numeric_limits<double>::max();
            double sl_max = r_sl.size() ? r_sl.maxCoeff() : std::numeric_limits<double>::min();
            double dl_max = r_dl.size() ? r_dl.maxCoeff() : std::numeric_limits<double>::min();
            double trg_max = r_trg.size() ? r_trg.maxCoeff() : std::numeric_limits<double>::min();

            // Find most extreme points to define our box, which is required to be a cube
            double local_min = std::min(std::min(sl_min, dl_min), trg_min);
            double local_max = std::max(std::max(sl_max, dl_max), trg_max);

            double global_min, global_max;
            MPI_Allreduce(&local_min, &global_min, 1, MPI_DOUBLE, MPI_MIN, MPI_COMM_WORLD);
            MPI_Allreduce(&local_max, &global_max, 1, MPI_DOUBLE, MPI_MAX, MPI_COMM_WORLD);

            // Scale box coordinates so that no source/target lies on the box boundary
            global_min *= 1.01;
            global_max *= 1.01;
            const double L = global_max - global_min;

            // Update FMM tree and cache coordinates
            double origin[3] = {global_min, global_min, global_min};
            fmmPtr_->setBox(origin, L);
            fmmPtr_->setPoints(r_sl.size() / 3, r_sl.data(), r_trg.size() / 3, r_trg.data(), r_dl.size() / 3,
                               r_dl.data());
            fmmPtr_->setupTree(k_);
            r_sl_old_ = r_sl;
            r_dl_old_ = r_dl;
            r_trg_old_ = r_trg;
            force_setup_tree_ = false;
        }

        int n_trg = r_trg.size() / 3;
        return kernel_func_(n_trg, f_sl, f_dl, fmmPtr_.get());
    }

  private:
    std::unique_ptr<stkfmm::STKFMM> fmmPtr_; ///< Pointer to underlying STKFMM object
    bool force_setup_tree_ = true; ///< When set, forces tree to rebuild on next call, then is cleared. Useful for
                                   ///< testing/benchmarking
    Eigen::MatrixXd r_sl_old_;     ///< cached 'single-layer' source positions to check for FMM tree invalidation
    Eigen::MatrixXd r_dl_old_;     ///< cache 'double-layer' source positions to check for FMM tree invalidation
    Eigen::MatrixXd r_trg_old_;    ///< cache target positions to check for FMM tree invalidation
    stkfmm::KERNEL k_;             ///< Kernel enum from STKFMM that this interaction calls
    fmm_kernel_func_t
        kernel_func_; ///< Kernel function pointer from our own kernels namespace for the kernel this object will call
};
}; // namespace kernels

#endif


#include <cstring>
#include <cstdio>
#include <cstdlib>
#include <cmath>
#include <ctime>
#include <string>
#include <algorithm>
#include <pthread.h>
#include <unordered_map>
#include <unordered_set>
#include <list>
#include <vector>
#include <tuple>
#include "hashing.h"
#include <boost/functional/hash.hpp>
using namespace std;

const double pi = 3.141592653589793238462643383;

int ptranseThreads = 1;
int ptranseTrainTimes = 3000;
int nbatches = 1;
int dimension = 50;
double ptranseAlpha = 0.001;
double margin = 1;

string inPath = "data/";
string outPath = "res/";

int *lefHead, *rigHead;
int *lefTail, *rigTail;

struct Triple {
    int h, r, t;
    list<pair<vector<int>, double> > pathList;
};

Triple **trainHead, **trainTail, ** trainList;

// restore (h, r, t)s
unordered_set<tuple<int, int, int>> isInTrain;
unordered_map<pair<vector<int>, int>, double, pair_hash> pathConfidence;

struct cmp_head {
    bool operator()(const Triple* const &a, const Triple* const &b) {
        return (a->h < b->h)||(a->h == b->h && a->r < b->r)||(a->h == b->h && a->r == b->r && a->t < b->t);
    }
};

struct cmp_tail {
    bool operator()(const Triple* const &a, const Triple* const &b) {
        return (a->t < b->t)||(a->t == b->t && a->r < b->r)||(a->t == b->t && a->r == b->r && a->h < b->h);
    }
};

/*
    There are some math functions for the program initialization.
*/
unsigned long long *next_random;

unsigned long long randd(int id) {
    next_random[id] = next_random[id] * (unsigned long long)25214903917 + 11;
    return next_random[id];
}

int rand_max(int id, int x) {
    int res = randd(id) % x;
    while (res<0)
        res+=x;
    return res;
}

double rand(double min, double max) {
    return min + (max - min) * rand() / (RAND_MAX + 1.0);
}

double normal(double x, double miu,double sigma) {
    return 1.0/sqrt(2*pi)/sigma*exp(-1*(x-miu)*(x-miu)/(2*sigma*sigma));
}

double randn(double miu,double sigma, double min ,double max) {
    double x, y, dScope;
    do {
        x = rand(min,max);
        y = normal(x,miu,sigma);
        dScope=rand(0.0,normal(miu,miu,sigma));
    } while (dScope > y);
    return x;
}

void norm(double * con) {
    double x = 0;
    for (int  ii = 0; ii < dimension; ii++)
        x += (*(con + ii)) * (*(con + ii));
    x = sqrt(x);
    if (x>1)
        for (int ii=0; ii < dimension; ii++)
            *(con + ii) /= x;
}

double sqr(double x){
    return x * x;
}

/*
    Read triples from the training file.
*/

int relationTotal, entityTotal, tripleTotal;
double *relationVec, *entityVec;

int ptranseBatch, ptranseLen;
double res;

void gradient(int e1_a, int e2_a, int rel_a, int e1_b, int e2_b, int rel_b) {
    int lasta1 = e1_a * dimension;
    int lasta2 = e2_a * dimension;
    int lastar = rel_a * dimension;
    int lastb1 = e1_b * dimension;
    int lastb2 = e2_b * dimension;
    int lastbr = rel_b * dimension;
    for (int ii=0; ii  < dimension; ii++) {
        double x;
        x = (entityVec[lasta2 + ii] - entityVec[lasta1 + ii] - relationVec[lastar + ii]);
        if (x > 0)
            x = -ptranseAlpha;
        else
            x = ptranseAlpha;
        relationVec[lastar + ii] -= x;
        entityVec[lasta1 + ii] -= x;
        entityVec[lasta2 + ii] += x;
        x = (entityVec[lastb2 + ii] - entityVec[lastb1 + ii] - relationVec[lastbr + ii]);
        if (x > 0)
            x = ptranseAlpha;
        else
            x = -ptranseAlpha;
        relationVec[lastbr + ii] -=  x;
        entityVec[lastb1 + ii] -= x;
        entityVec[lastb2 + ii] += x;
    }
}

double calc_sum(int e1, int e2, int rel) {
    double sum=0;
    int last1 = e1 * dimension;
    int last2 = e2 * dimension;
    int lastr = rel * dimension;
    for (int ii=0; ii < dimension; ii++) {
                    sum += fabs(entityVec[last2 + ii] - entityVec[last1 + ii] - relationVec[lastr + ii]);
                }
    return sum;
}

void train_kb(int e1_a, int e2_a, int rel_a, int e1_b, int e2_b, int rel_b) {
    double sum1 = calc_sum(e1_a, e2_a, rel_a);
    double sum2 = calc_sum(e1_b, e2_b, rel_b);
    if (sum1 + margin > sum2) {
        res += margin + sum1 - sum2;
        gradient(e1_a, e2_a, rel_a, e1_b, e2_b, rel_b);
    }
}

int corrupt_head(int id, int h, int r) {
    int lef, rig, mid, ll, rr;
    lef = lefHead[h] - 1;
    rig = rigHead[h];
    while (lef + 1 < rig) {
        mid = (lef + rig) >> 1;
        if (trainHead[mid]->r >= r) rig = mid; else
        lef = mid;
    }
    ll = rig;
    lef = lefHead[h];
    rig = rigHead[h] + 1;
    while (lef + 1 < rig) {
        mid = (lef + rig) >> 1;
        if (trainHead[mid]->r <= r) lef = mid; else
        rig = mid;
    }
    rr = lef;
    int tmp = rand_max(id, entityTotal - (rr - ll + 1));
    if (tmp < trainHead[ll]->t) return tmp;
    if (tmp > trainHead[rr]->t - rr + ll - 1) return tmp + rr - ll + 1;
    lef = ll, rig = rr + 1;
    while (lef + 1 < rig) {
        mid = (lef + rig) >> 1;
        if (trainHead[mid]->t - mid + ll - 1 < tmp)
            lef = mid;
        else
            rig = mid;
    }
    return tmp + lef - ll + 1;
}

int corrupt_tail(int id, int t, int r) {
    int lef, rig, mid, ll, rr;
    lef = lefTail[t] - 1;
    rig = rigTail[t];
    while (lef + 1 < rig) {
        mid = (lef + rig) >> 1;
        if (trainTail[mid]->r >= r) rig = mid; else
        lef = mid;
    }
    ll = rig;
    lef = lefTail[t];
    rig = rigTail[t] + 1;
    while (lef + 1 < rig) {
        mid = (lef + rig) >> 1;
        if (trainTail[mid]->r <= r) lef = mid; else
        rig = mid;
    }
    rr = lef;
    int tmp = rand_max(id, entityTotal - (rr - ll + 1));
    if (tmp < trainTail[ll]->h) return tmp;
    if (tmp > trainTail[rr]->h - rr + ll - 1) return tmp + rr - ll + 1;
    lef = ll, rig = rr + 1;
    while (lef + 1 < rig) {
        mid = (lef + rig) >> 1;
        if (trainTail[mid]->h - mid + ll - 1 < tmp)
            lef = mid;
        else
            rig = mid;
    }
    return tmp + lef - ll + 1;
}

double calc_path(int r1, const vector<int>& relPath) {
    double sum = 0;
    for (int ii = 0; ii < dimension; ii++) {
        double tmp = relationVec[r1 * dimension + ii];
        for (auto &j : relPath)
            tmp -= relationVec[j * dimension + ii];
            // L1 norm as default
            sum+=fabs(tmp);
    }
    return sum;
}

void gradient_path(int r1, const vector<int>& relPath, double belta) {
    for (int ii=0; ii < dimension; ii++) {
        double x = relationVec[r1 * dimension + ii];
        for (auto &j : relPath)
            x -= relationVec[j * dimension + ii];
        if (x>0) x=1;
            else x=-1;
        relationVec[r1 * dimension + ii]+=belta*ptranseAlpha*x;
        for (auto &j : relPath)
            relationVec[j * dimension + ii]-=belta*ptranseAlpha*x;
    }
}

void train_path(int rel, int relNeg, const vector<int>& relPath, double margin,double x) {
    double sum1 = calc_path(rel,relPath);
    double sum2 = calc_path(relNeg,relPath);
    double lambda = 1;
    if (sum1+margin>sum2) {
        res+=x*lambda*(margin+sum1-sum2);
        gradient_path(rel,relPath, -x*lambda);
        gradient_path(relNeg,relPath, x*lambda);
    }
}

void* ptransetrainMode(void *con) {
    int id;
    id = (unsigned long long)(con);
    next_random[id] = rand();
    for (int k = ptranseBatch / ptranseThreads; k >= 0; k--) {
        int i = rand_max(id, ptranseLen);

        // transe part
        int j; // corrupted part
        int pr = 500;
        if (randd(id) % 1000 < pr){
            j = corrupt_head(id, trainList[i]->h, trainList[i]->r);
            train_kb(trainList[i]->h, trainList[i]->t, trainList[i]->r, trainList[i]->h, j, trainList[i]->r);
        }
        else {
            j = corrupt_tail(id, trainList[i]->t, trainList[i]->r);
            train_kb(trainList[i]->h, trainList[i]->t, trainList[i]->r, j, trainList[i]->t, trainList[i]->r);
        }

        // ptranse part
        j = rand_max(id, relationTotal);
        while (isInTrain.find(make_tuple(trainList[i]->h, j, trainList[i]->t)) != isInTrain.end())
            j = rand_max(id, relationTotal);
        for (auto & pathList : trainList[i]->pathList){
            vector<int> relPath = pathList.first;
            double pr = pathList.second;
            double pr_path = 0;
            if (pathConfidence.count(make_pair(relPath, trainList[i]->r))>0)
                pr_path = pathConfidence[make_pair(relPath, trainList[i]->r)];
            pr_path = 0.99*pr_path + 0.01;
            train_path(trainList[i]->r, j, relPath, 2*margin, pr*pr_path);
        }

        //norm
        norm(relationVec + dimension * trainList[i]->r);
        norm(entityVec + dimension * trainList[i]->h);
        norm(entityVec + dimension * trainList[i]->t);
        norm(entityVec + dimension * j);
    }
}

void* train_ptranse(void *con) {
    ptranseLen = tripleTotal;
    ptranseBatch = ptranseLen / nbatches;
    next_random = (unsigned long long *)calloc(ptranseThreads, sizeof(unsigned long long));
    for (int epoch = 0; epoch < ptranseTrainTimes; epoch++) {
        printf("epoch %d started.\n", epoch);
        res = 0;
        for (int batch = 0; batch < nbatches; batch++) {
            pthread_t *pt = (pthread_t *)malloc(ptranseThreads * sizeof(pthread_t));
            for (int a = 0; a < ptranseThreads; a++)
                pthread_create(&pt[a], NULL, ptransetrainMode,  (void*)a);
            for (int a = 0; a < ptranseThreads; a++)
                pthread_join(pt[a], NULL);
            free(pt);
        }
        printf("epoch %d %f\n", epoch, res);
    }
}

void init() {

    FILE *fin;
    int tmp;

    // fin = fopen((inPath + "relation2id.txt").c_str(), "r");
    // tmp = fscanf(fin, "%d", &relationTotal);
    // fclose(fin);
    relationTotal = 1345;
    printf("Relations:\t%d\n", relationTotal);

    relationTotal <<= 1;
    relationVec = (double *)calloc(relationTotal * dimension, sizeof(double));
    for (int i = 0; i < relationTotal; i++) {
        for (int ii=0; ii<dimension; ii++)
            relationVec[i * dimension + ii] = randn(0, 1.0 / dimension, -6 / sqrt(dimension), 6 / sqrt(dimension));
    }

    // fin = fopen((inPath + "entity2id.txt").c_str(), "r");
    // tmp = fscanf(fin, "%d", &entityTotal);
    // fclose(fin);
    entityTotal = 14951;
    printf("Entities:\t%d\n", entityTotal);

    entityVec = (double *)calloc(entityTotal * dimension, sizeof(double));
    for (int i = 0; i < entityTotal; i++) {
        for (int ii=0; ii<dimension; ii++)
            entityVec[i * dimension + ii] = randn(0, 1.0 / dimension, -6 / sqrt(dimension), 6 / sqrt(dimension));
        norm(entityVec+i*dimension);
    }
    printf("Entities' vectors initialized.\n");

    fin = fopen((inPath + "train_pra.txt").c_str(), "r");
    tmp = fscanf(fin, "%d", &tripleTotal);
    printf("Triples:\t%d\n", tripleTotal);
    trainHead = (Triple **)calloc(tripleTotal, sizeof(Triple*));
    trainTail = (Triple **)calloc(tripleTotal, sizeof(Triple*));
    trainList = (Triple **)calloc(tripleTotal, sizeof(Triple*));
    tripleTotal = 0;
    // establish new triple in advance
    trainHead[tripleTotal] = new Triple();
    trainTail[tripleTotal] = new Triple();
    trainList[tripleTotal] = new Triple();
    while (fscanf(fin, "%d", &trainList[tripleTotal]->h) == 1) {

        //input (h, t, r)
        tmp = fscanf(fin, "%d", &trainList[tripleTotal]->t);
        tmp = fscanf(fin, "%d", &trainList[tripleTotal]->r);

        //input paths
        //input path_amount
        int pathsAmount;
        tmp = fscanf(fin, "%d", &pathsAmount);
        //input each path
        for(int i = 0;i<pathsAmount;i++){
            int pathLength, pathElement;
            double pathProbability;
            vector<int> relPath;
            relPath.clear();
            tmp = fscanf(fin, "%d", &pathLength);
            for(int j = 0;j<pathLength;j++){
                tmp = fscanf(fin, "%d", &pathElement);
                relPath.push_back(pathElement);
            }
            tmp = fscanf(fin, "%lf", &pathProbability);
            trainList[tripleTotal]->pathList.push_back(make_pair(relPath, pathProbability));
        }

        //put (h, r, t) into training set
        isInTrain.insert(make_tuple(trainList[tripleTotal]->h, trainList[tripleTotal]->r, trainList[tripleTotal]->t));

        //copy trainList to trainHead trainTail
        (*trainHead[tripleTotal]) = (*trainTail[tripleTotal]) = (*trainList[tripleTotal]);
        tripleTotal++;
        trainHead[tripleTotal] = new Triple();
        trainTail[tripleTotal] = new Triple();
        trainList[tripleTotal] = new Triple();

    }
    fclose(fin);

    sort(trainHead, trainHead + tripleTotal, cmp_head());
    sort(trainTail, trainTail + tripleTotal, cmp_tail());

    lefHead = (int *)calloc(entityTotal, sizeof(int));
    rigHead = (int *)calloc(entityTotal, sizeof(int));
    lefTail = (int *)calloc(entityTotal, sizeof(int));
    rigTail = (int *)calloc(entityTotal, sizeof(int));
    memset(rigHead, -1, entityTotal * sizeof(int));
    memset(rigTail, -1, entityTotal * sizeof(int));
    memset(lefHead, -1, entityTotal * sizeof(int));
    memset(lefTail, -1, entityTotal * sizeof(int));
    // tripleTotal should be larger than 0, otherwise could cause errors.
    lefTail[trainTail[0]->t] = 0;
    lefHead[trainHead[0]->h] = 0;
    for (int i = 1; i < tripleTotal; i++) {
        if (trainTail[i]->t != trainTail[i - 1]->t) {
            rigTail[trainTail[i - 1]->t] = i - 1;
            lefTail[trainTail[i]->t] = i;
        }
        if (trainHead[i]->h != trainHead[i - 1]->h) {
            rigHead[trainHead[i - 1]->h] = i - 1;
            lefHead[trainHead[i]->h] = i;
        }
    }
    rigHead[trainHead[tripleTotal - 1]->h] = tripleTotal - 1;
    rigTail[trainTail[tripleTotal - 1]->t] = tripleTotal - 1;

    // input confidence file
    fin = fopen((inPath + "confidence.txt").c_str(), "r");
    int pathLength;
    while (fscanf(fin, "%d", &pathLength)==1){
        int pathElement;
        vector<int> relPath;
        relPath.clear();
        for (int i=0; i<pathLength; i++)
        {
            tmp = fscanf(fin, "%d", &pathElement);
            relPath.push_back(pathElement);
        }
        int relationsAmount;
        fscanf(fin, "%d", &relationsAmount);
        for (int i=0; i<relationsAmount; i++)
        {
            int relation;
            double pr;
            tmp = fscanf(fin, "%d%lf", &relation, &pr);
            pathConfidence[make_pair(relPath, relation)] = pr;
        }
    }
    fclose(fin);
    printf("Initialization completed.\n");
}

void destruct(){
    for(int i = 0;i<=tripleTotal;i++){
        delete(trainHead[i]);
        delete(trainTail[i]);
        delete(trainList[i]);
    }
}

void out_ptranse() {
		FILE* f2 = fopen((outPath + "relation2vec.bern").c_str(), "w");
		FILE* f3 = fopen((outPath + "entity2vec.bern").c_str(), "w");
		for (int i=0; i < relationTotal; i++) {
			int last = dimension * i;
			for (int ii = 0; ii < dimension; ii++)
				fprintf(f2, "%.6lf\t", relationVec[last + ii]);
			fprintf(f2,"\n");
		}
		for (int  i = 0; i < entityTotal; i++) {
			int last = i * dimension;
			for (int ii = 0; ii < dimension; ii++)
				fprintf(f3, "%.6lf\t", entityVec[last + ii] );
			fprintf(f3,"\n");
		}
		fclose(f2);
		fclose(f3);
}

int main() {
    init();
    train_ptranse(NULL);
    out_ptranse();
    destruct();
    return 0;
}