/*!
 * \file plane.cpp
 *
 * \author Han
 * \date 2017/06/21
 *
 * 平面操作，主要包括坐标的三维和二维变换等
 */
#include <MathGeoLib/MathGeoLib.h>

#include <modelpro/mesh.h>
#include <modelpro/plane.h>
#include <sketchup/sketchup.h>

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

namespace h2o {
Plane::Plane(const Vector3d &p, const Vector3d &n) {
    Vector3d normal = n;
    normal.normalize();
    Vector3d z_axis = {0.0, 0.0, 1.0};

    // 平面二三维变换的实质是一个旋转加平移
    R_ = Quaterniond::FromTwoVectors(normal, z_axis);
    T_ = -R_ * p;
    Rinv_ = R_.inverse();

    plane_ = Plane3d(n, p);
}

Plane::~Plane() {}

Vector2d Plane::to_2d(const Vector3d &point) const {
    Vector3d plane = R_ * point + T_;
    return plane.head(2);
}

Vector3d Plane::to_3d(const Vector2d &point) const {
    Vector3d point3d = {point.x(), point.y(), 0.0};
    Vector3d object3d = Rinv_ * (point3d - T_);
    return object3d;
}

double Plane::signed_distance(const Vector3d &point) const { return plane_.signedDistance(point); }

bool Plane::on_plane(const Vector3d &point) const {
    double distance = signed_distance(point);
    // EPSILON_LENGTH 的精度是 0.001
    if (distance < EPSILON_LENGTH && distance > -EPSILON_LENGTH) {
        return true;
    } else {
        return false;
    }
}

Vector3d Plane::get_normal() const { return plane_.normal(); }

PlaneRotated::PlaneRotated(const std::vector<Vector3d> &points3d, const Vector3d &n) : Plane() {
    CHECK(points3d.size() >= 3);

    Vector3d normal = n;
    normal.normalize();
    Vector3d z_axis(0.0, 0.0, 1.0);
    Vector3d y_axis(0.0, 1.0, 0.0);
    // 平面二三维变换的实质是一个旋转加平移
    R_ = Quaterniond::FromTwoVectors(normal, z_axis);

    T_ = -R_ * points3d[0];
    Rinv_ = R_.inverse();

    plane_ = Plane3d(n, points3d[0]);
    // 将所有点变换到二维，计算 rotated rect
    std::vector<cv::Point2f> plane2d(points3d.size());
    std::transform(begin(points3d), end(points3d), begin(plane2d), [&](const Vector3d &p) {
        Vector3d plane3d = R_ * p + T_;
        CHECK(std::abs(plane3d.z()) < 0.1)
            << "Project to 2d plane failed, the z coord of the point is : " << plane3d.z();
        return cv::Point2f(plane3d.x(), plane3d.y());
    });

    /*
     * OPENCV 的 rotated rect 的四个角点分别为
     * point = center + R * corner
     * R 为 Rotation2d 得到的
     * 0 ----- 3
     * |       |
     * 1 ----- 2
     */
    cv::RotatedRect rect = cv::minAreaRect(plane2d);

    Matrix3d R_z;
    Vector3d T2;
    Vector3d direction_y;

    std::vector<cv::Point2f> points_rect(4);
    std::vector<Vector3d> bb_3d(4);
    rect.points(points_rect.data());

    std::transform(begin(points_rect), end(points_rect), begin(bb_3d), [&](const cv::Point2f &p) {
        Vector3d plane3d(p.x, p.y, 0.0);
        Vector3d object3d = Rinv_ * (plane3d - T_);
        return object3d;
    });

    // 绕z轴再旋转一定量，将二维平面的xy轴和他对其
    // 这个 旋转的大小的计算是两种
    if (std::abs(std::abs(normal.dot(z_axis)) - 1.0) < 1e-6) {
        direction_y = Vector3d(points_rect[0].x - points_rect[1].x, points_rect[0].y - points_rect[1].y, 0.0);
        R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
        T2 = -R_z * Vector3d(points_rect[1].x, points_rect[1].y, 0.0);
    } else {
        // 需要保证 y 轴是朝上的，将某个朝上的轴，旋转到y轴
        if (compare_z(bb_3d[0], bb_3d[1]) == 1) {
            direction_y = Vector3d(points_rect[0].x - points_rect[1].x, points_rect[0].y - points_rect[1].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[1].x, points_rect[1].y, 0.0);
        } else if (compare_z(bb_3d[0], bb_3d[1]) == -1) {
            direction_y = Vector3d(points_rect[1].x - points_rect[0].x, points_rect[1].y - points_rect[0].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[3].x, points_rect[3].y, 0.0);
        }
        if (compare_z(bb_3d[0], bb_3d[3]) == 1) {
            direction_y = Vector3d(points_rect[0].x - points_rect[3].x, points_rect[0].y - points_rect[3].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[2].x, points_rect[2].y, 0.0);
        } else if (compare_z(bb_3d[0], bb_3d[3]) == -1) {
            direction_y = Vector3d(points_rect[3].x - points_rect[0].x, points_rect[3].y - points_rect[0].y, 0.0);
            R_z = Quaterniond::FromTwoVectors(direction_y, y_axis);
            T2 = -R_z * Vector3d(points_rect[0].x, points_rect[0].y, 0.0);
        }
    }
    // plane3d = R_p + T
    // texture = Rz * plane3d + T2, T2 用于将点 0 的坐标移动到 0
    // texture = Rz * R_ * p + Rz * T + T2
    T_ = R_z * T_ + T2;
    R_ = R_z * R_;
    Rinv_ = R_.inverse();
    bb_3d_ = bb_3d;

    init_plane_bb();
}

void PlaneRotated::set_gsd(double gsd) {
    // 左上角的纹理影像坐标为 0，0，但是他的plane2d的坐标是最大的
    io_(0, 0) = 1.0 / gsd;
    io_(0, 1) = 0.0;
    io_(0, 2) = 0.0;
    io_(1, 0) = 0.0;
    io_(1, 1) = -1.0 / gsd;
    io_(1, 2) = bb_plane2d_.sizes()(1) / gsd;

    Matrix2d A;
    Vector2d b;
    A = io_.block(0, 0, 2, 2);
    b = io_.col(2);

    iob_.block(0, 0, 2, 2) = A.inverse();
    iob_.col(2) = -A.inverse() * b;
}

std::vector<Vector2d> PlaneRotated::get_corners2d() const {
    std::vector<Vector2d> bb_plane2d(4);
    std::transform(begin(bb_3d_), end(bb_3d_), begin(bb_plane2d), [&](const Vector3d &p) { return to_2d(p); });
    return bb_plane2d;
}

std::vector<Vector3d> PlaneRotated::get_corners3d() const { return bb_3d_; }

Vector2d PlaneRotated::to_texture(const Vector3d &point) const {
    Vector2d plane2d = to_2d(point);
    Vector2d tex2d = io_ * plane2d.homogeneous();
    return tex2d;
}

Vector3d PlaneRotated::from_texture(const Vector2d &point) const {
    Vector2d plane2d = iob_ * point.homogeneous();
    Vector3d object3d = to_3d(plane2d);
    return object3d;
}

OBB3d PlaneRotated::get_obb(double thickness) {
    Vector3d normal = plane_.normal();

    std::vector<Vector3d> corners3d = get_corners3d();
    std::vector<Vector3d> points(corners3d.size() * 2);
    for (int i = 0; i < corners3d.size(); ++i) {
        Vector3d point = corners3d[i] + thickness * normal;
        points[2 * i + 0] = point;

        point = corners3d[i] - thickness * normal;
        points[2 * i + 1] = point;
    }

    Vector3d dir1 = (corners3d[1] - corners3d[0]).normalized();
    Vector3d dir2 = (corners3d[3] - corners3d[0]).normalized();

    OBB3d obb = obb_compute(points, dir1, dir2);

    return obb;
}

int PlaneRotated::compare_z(const Vector3d &p1, const Vector3d &p2) const {
    if (p1.z() - p2.z() > EPSILON_LENGTH) {
        return 1;
    } else if (p1.z() - p2.z() < -EPSILON_LENGTH) {
        return -1;
    } else {
        return 0;
    }
    return 0;
}

void PlaneRotated::init_plane_bb() {
    // 把所有的 bb_3d 换算到平面坐标
    for (const auto &corner : bb_3d_) {
        bb_plane2d_.extend(to_2d(corner));
    }
}

Matrix3d estimate_homography(const std::vector<Vector2d> &object, const std::vector<Vector2d> &image) {
    std::vector<cv::Point2f> object2(object.size()), image2(image.size());
    for (int i = 0; i < object.size(); ++i) {
        object2[i] = cv::Point2f(object[i].x(), object[i].y());
        image2[i] = cv::Point2f(image[i].x(), image[i].y());
    }

    cv::Mat mask;
    cv::Mat H2 = cv::findHomography(image2, object2, mask, 0);
    Matrix3d H;
    cv::cv2eigen(H2, H);

    return H;
}

double estimate_gsd(const std::vector<Vector2d> &object, const std::vector<Vector2d> &image) {
    double gsd = 0.0;
    for (int i = 0; i < image.size(); ++i) {

        int j = (i + 1) % image.size();

        Vector2d o1 = object[i];
        Vector2d o2 = object[j];

        Vector2d i1 = image[i];
        Vector2d i2 = image[j];

        gsd += (o1 - o2).norm() / (i1 - i2).norm();
    }
    gsd /= image.size();
    return gsd;
}

PlaneRotated plane_from_face(VALUE face) {
    Vector3d normal = su_face_normal(face);
    std::vector<Vector3d> points = su_entity_vertices(face);
    PlaneRotated plane(points, normal);

    return plane;
}

} // namespace h2o


#include <wav2midi/scale.hpp>
#include <cmath>
#include <boost/format.hpp>

// scale::item
namespace wav2midi {
    namespace {
        const std::vector<std::string> scale_names{{
            "A%d",
            "A#%d",
            "B%d",
            "C%d",
            "C#%d",
            "D%d",
            "D#%d",
            "E%d",
            "F%d",
            "F#%d",
            "G%d",
            "G#%d",
        }};
    }

    scale::item::item(uint32_t no) {
        using namespace std;
        no_ = no;
        local_no_ = no_ % 12u;
        octave_ = (no_ + 9u) / 12u;
        frequency_ = scale::frequency_to_no(no_);
        name_ = (boost::format(scale_names[local_no_]) % octave_).str();
    }

    uint32_t scale::item::no() const { return no_; }
    uint32_t scale::item::local_no() const { return local_no_; }
    uint32_t scale::item::octave() const { return octave_; }
    double scale::item::frequency() const { return frequency_; }
    const std::string & scale::item::name() const { return name_; }

}

// scale
namespace wav2midi {
    scale::scale() {
        for (auto no = 0u; no < scale::n; ++no) {
            items_.emplace_back(no);
        }
    }

    const scale::item & scale::operator [](uint32_t no) const {
        static item unknown{0};
        return no < 88u ? items_[no] : unknown;
    }

    const scale::item & scale::match(double frequency) const {
        return (*this)[scale::frequency_to_no(frequency)];
    }

    uint32_t scale::frequency_to_no(double frequency) {
        using namespace std;
        return round(12.0 * log2(frequency / 27.5));
    }

    double scale::no_to_frequency(uint32_t no) {
        using namespace std;
        return 27.5 * pow(2.0, no / 12.0);
    }
}


#include <cstdio>
#include <cmath>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>
#include <map>
#include <cfloat>
#include <filesystem>

#if WIN32
#define NOMINMAX
#endif

namespace fs = std::filesystem;

// Eigen
#include <Eigen/Dense>
#include <unsupported/Eigen/CXX11/Tensor>

// OpenMP
#include <omp.h>

// Argument parser
#include "argparse.h"

// Progress bar
#include "progress.h"

#define TINYPLY_IMPLEMENTATION
#include "triangle_point/tinyply.h"
#include "triangle_point/vec.h"
#include "triangle_point/poitri.h"

/** \brief Compute triangle point distance and corresponding closest point.
 * \param[in] point point
 * \param[in] v1 first vertex
 * \param[in] v2 second vertex
 * \param[in] v3 third vertex
 * \param[out] ray corresponding closest point
 * \return distance
 */
float triangle_point_distance(const Eigen::Vector3f point, const Eigen::Vector3f v1, const Eigen::Vector3f v2, const Eigen::Vector3f v3,
    Eigen::Vector3f& closest_point) {

    Vec3f x0(point.data());
    Vec3f x1(v1.data());
    Vec3f x2(v2.data());
    Vec3f x3(v3.data());

    Vec3f r(0);
    float distance = point_triangle_distance(x0, x1, x2, x3, r);

    for (int d = 0; d < 3; d++) {
        closest_point(d) = r[d];
    }

    return distance;
}

/** \brief Point cloud class forward declaration. */
class PointCloud;

/** \brief Just encapsulating vertices and faces. */
class Mesh {
public:
    /** \brief Empty constructor. */
    Mesh() {

    }

    /** \brief Reading an off file and returning the vertices x, y, z coordinates and the
     * face indices.
     * \param[in] filepath path to the OFF file
     * \param[out] mesh read mesh with vertices and faces
     * \return success
     */
    static bool from_off(const std::string filepath, Mesh& mesh) {

        std::ifstream* file = new std::ifstream(filepath.c_str());
        std::string line;
        std::stringstream ss;
        int line_nb = 0;

        std::getline(*file, line);
        ++line_nb;

        if (line != "off" && line != "OFF") {
            std::cout << "[Error] Invalid header: \"" << line << "\", " << filepath << std::endl;
            return false;
        }

        size_t n_edges;
        std::getline(*file, line);
        ++line_nb;

        int n_vertices;
        int n_faces;
        ss << line;
        ss >> n_vertices;
        ss >> n_faces;
        ss >> n_edges;

        for (size_t v = 0; v < n_vertices; ++v) {
            std::getline(*file, line);
            ++line_nb;

            ss.clear();
            ss.str("");

            Eigen::Vector3f vertex;
            ss << line;
            ss >> vertex(0);
            ss >> vertex(1);
            ss >> vertex(2);

            mesh.add_vertex(vertex);
        }

        size_t n;
        for (size_t f = 0; f < n_faces; ++f) {
            std::getline(*file, line);
            ++line_nb;

            ss.clear();
            ss.str("");

            size_t n;
            ss << line;
            ss >> n;

            if (n != 3) {
                std::cout << "[Error] Not a triangle (" << n << " points) at " << (line_nb - 1) << std::endl;
                return false;
            }

            Eigen::Vector3i face;
            ss >> face(0);
            ss >> face(1);
            ss >> face(2);

            mesh.add_face(face);
        }

        if (n_vertices != mesh.num_vertices()) {
            std::cout << "[Error] Number of vertices in header differs from actual number of vertices." << std::endl;
            return false;
        }

        if (n_faces != mesh.num_faces()) {
            std::cout << "[Error] Number of faces in header differs from actual number of faces." << std::endl;
            return false;
        }

        file->close();
        delete file;

        return true;
    }

    static bool from_ply(const std::string& filename, Mesh& mesh) {
        using tinyply::PlyFile;
        using tinyply::PlyData;

        try {
            // Open
            std::ifstream reader(filename.c_str(), std::ios::binary);
            if (reader.fail()) {
                std::cerr << ("Failed to open file: " + filename) << std::endl;
                return false;
            }

            // Read header
            PlyFile file;
            file.parse_header(reader);

            // Request vertex data
            std::shared_ptr<PlyData> vert_data, norm_data, uv_data, face_data;
            try {
                vert_data = file.request_properties_from_element("vertex", { "x", "y", "z" });
            }
            catch (std::exception& e) {
                std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            try {
                norm_data = file.request_properties_from_element("vertex", { "nx", "ny", "nz" });
            }
            catch (std::exception& e) {
                // std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            try {
                uv_data = file.request_properties_from_element("vertex", { "u", "v" });
            }
            catch (std::exception& e) {
                // std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            try {
                face_data = file.request_properties_from_element("face", { "vertex_indices" }, 3);
            }
            catch (std::exception& e) {
                std::cerr << "tinyply exception: " << e.what() << std::endl;
            }

            // Read vertex data
            file.read(reader);

            // Copy vertex data
            const size_t numVerts = vert_data->count;
            std::vector<double> raw_vertices(numVerts * 3);
            std::memcpy(raw_vertices.data(), vert_data->buffer.get(), sizeof(double) * numVerts * 3);

            const size_t numFaces = face_data->count;
            std::vector<uint32_t> raw_indices(numFaces * 3);
            std::memcpy(raw_indices.data(), face_data->buffer.get(), sizeof(uint32_t) * numFaces * 3);

            // Store in mesh
            for (int i = 0; i < raw_vertices.size(); i += 3) {
                Eigen::Vector3f v;
                v << raw_vertices[i + 0], raw_vertices[i + 1], raw_vertices[i + 2];
                mesh.add_vertex(v);
            }

            for (int i = 0; i < raw_indices.size(); i += 3) {
                Eigen::Vector3i f;
                f << raw_indices[i + 0], raw_indices[i + 1], raw_indices[i + 2];
                mesh.add_face(f);
            }
        }
        catch (const std::exception& e) {
            std::cerr << "Caught tinyply exception: " << e.what() << std::endl;
            return false;
        }

        return true;
    }

    /** \brief Write mesh to OFF file.
     * \param[in] filepath path to OFF file to write
     * \return success
     */
    bool to_off(const std::string filepath) const {
        std::ofstream* out = new std::ofstream(filepath, std::ofstream::out);
        if (!static_cast<bool>(out)) {
            return false;
        }

        (*out) << "OFF" << std::endl;
        (*out) << this->num_vertices() << " " << this->num_faces() << " 0" << std::endl;

        for (unsigned int v = 0; v < this->num_vertices(); v++) {
            (*out) << this->vertices[v](0) << " " << this->vertices[v](1) << " " << this->vertices[v](2) << std::endl;
        }

        for (unsigned int f = 0; f < this->num_faces(); f++) {
            (*out) << "3 " << this->faces[f](0) << " " << this->faces[f](1) << " " << this->faces[f](2) << std::endl;
        }

        out->close();
        delete out;

        return true;
    }

    /** \brief Add a vertex.
     * \param[in] vertex vertex to add
     */
    void add_vertex(Eigen::Vector3f& vertex) {
        this->vertices.push_back(vertex);
    }

    /** \brief Get the number of vertices.
     * \return number of vertices
     */
    int num_vertices() const {
        return static_cast<int>(this->vertices.size());
    }

    /** \brief Get a vertex.
     * \param[in] v vertex index
     * \return vertex
     */
    Eigen::Vector3f vertex(int v) const {
        assert(v >= 0 && v < this->num_vertices());
        return this->vertices[v];
    }

    /** \brief Add a face.
     * \param[in] face face to add
     */
    void add_face(Eigen::Vector3i& face) {
        this->faces.push_back(face);
    }

    /** \brief Get the number of faces.
     * \return number of faces
     */
    int num_faces() const {
        return static_cast<int>(this->faces.size());
    }

    /** \brief Get a face.
     * \param[in] f face index
     * \return face
     */
    Eigen::Vector3i face(int f) const {
        assert(f >= 0 && f < this->num_faces());
        return this->faces[f];
    }

    /** \brief Sample points from the mesh
     * \param[in] mesh mesh to sample from
     * \param[in] n batch index in points
     * \param[in] points pre-initialized tensor holding points
     */
    bool sample(const int N, PointCloud& point_cloud) const;

private:

    /** \brief Vertices as (x,y,z)-vectors. */
    std::vector<Eigen::Vector3f> vertices;

    /** \brief Faces as list of vertex indices. */
    std::vector<Eigen::Vector3i> faces;
};

/** \brief Class representing a point cloud in 3D. */
class PointCloud {
public:
    /** \brief Constructor. */
    PointCloud() {

    }

    /** \brief Copy constructor.
     * \param[in] point_cloud point cloud to copy
     */
    PointCloud(const PointCloud& point_cloud) {
        this->points.clear();

        for (unsigned int i = 0; i < point_cloud.points.size(); i++) {
            this->points.push_back(point_cloud.points[i]);
        }
    }

    /** \brief Destructor. */
    ~PointCloud() {

    }

    /** \brief Read point cloud from txt file.
     * \param[in] filepath path to file to read
     * \param[out] point_cloud
     * \return success
     */
    static bool from_txt(const std::string& filepath, PointCloud& point_cloud) {
        std::ifstream file(filepath.c_str());
        std::string line;
        std::stringstream ss;

        std::getline(file, line);
        ss << line;

        int n_points = 0;
        ss >> n_points;

        if (n_points < 0) {
            return false;
        }

        for (int i = 0; i < n_points; i++) {
            std::getline(file, line);

            ss.clear();
            ss.str("");
            ss << line;

            Eigen::Vector3f point(0, 0, 0);
            ss >> point(0);
            ss >> point(1);
            ss >> point(2);

            point_cloud.add_point(point);
        }

        return true;
    }

    /** \brief Add a point to the point cloud.
     * \param[in] point point to add
     */
    void add_point(const Eigen::Vector3f& point) {
        this->points.push_back(point);
    }

    /** \brief Get number of points.
     * \return number of points
     */
    unsigned int num_points() const {
        return this->points.size();
    }

    /** \brief Compute distance to mesh.
      * \param[in] mesh
      * \param[out] distances per point distances
      * \param[out] distance
      * \return success
      */
    bool compute_distance(const Mesh& mesh, float& _distance) {
        _distance = 0;

        if (this->num_points() <= 0) {
            std::cout << "[Error] no points in this point clouds" << std::endl;
            return false;
        }

        if (mesh.num_faces() <= 0) {
            std::cout << "[Error] no faces in given mesh" << std::endl;
            return false;
        }

#pragma omp parallel
        {
#pragma omp for
            for (int i = 0; i < this->points.size(); i++) {

                float min_distance = FLT_MAX;
                for (int f = 0; f < mesh.num_faces(); f++) {
                    Eigen::Vector3f closest_point;
                    Eigen::Vector3f v1 = mesh.vertex(mesh.face(f)(0));
                    Eigen::Vector3f v2 = mesh.vertex(mesh.face(f)(1));
                    Eigen::Vector3f v3 = mesh.vertex(mesh.face(f)(2));

                    triangle_point_distance(this->points[i], v1, v2, v3, closest_point);
                    float distance = (this->points[i] - closest_point).norm();

                    if (distance < min_distance) {
                        min_distance = distance;
                    }
                }

#pragma omp atomic
                _distance += min_distance;
            }
        }

        _distance /= this->num_points();
        return true;
    }

private:
    /** \brief The points of the point cloud. */
    std::vector<Eigen::Vector3f> points;

};

/** \brief Sample points from the mesh
 * \param[in] mesh mesh to sample from
 * \param[in] n batch index in points
 * \param[in] points pre-initialized tensor holding points
 */
bool Mesh::sample(const int N, PointCloud& point_cloud) const {

    // Stores the areas of faces.
    std::vector<float> areas(this->num_faces());
    float sum = 0;

    // Build a probability distribution over faces.
    for (int f = 0; f < this->num_faces(); f++) {
        Eigen::Vector3f a = this->vertices[this->faces[f][0]];
        Eigen::Vector3f b = this->vertices[this->faces[f][1]];
        Eigen::Vector3f c = this->vertices[this->faces[f][2]];

        // Angle between a->b and a->c.
        Eigen::Vector3f ab = b - a;
        Eigen::Vector3f ac = c - a;
        float cos_angle = ab.dot(ac) / (ab.norm() * ac.norm());
        float angle = std::acos(cos_angle);

        // Compute triangle area.
        float area = std::max(0., 0.5 * ab.norm() * ac.norm() * std::sin(angle));
        //std::cout << area << " " << std::pow(area, 1./4.) << " " << angle << " " << ab.norm() << " " << ac.norm() << " " << std::sin(angle) << std::endl;

        // Accumulate.
        //area = std::sqrt(area);
        areas[f] = area;
        sum += area;
        //areas.push_back(1);
        //sum += 1;
    }

    //std::cout << sum << std::endl;
    if (sum < 1e-6) {
        std::cout << "[Error] face area sum of " << sum << std::endl;
        return false;
    }

    for (int f = 0; f < this->num_faces(); f++) {
        //std::cout << areas[f] << " ";
        areas[f] /= sum;
        //std::cout << areas[f] << std::endl;
    }

    std::vector<float> cum_areas(areas.size());
    cum_areas[0] = areas[0];

    for (int f = 1; f < this->num_faces(); f++) {
        cum_areas[f] = areas[f] + cum_areas[f - 1];
    }

    for (int f = 0; f < this->num_faces(); f++) {
        int n = std::max(static_cast<int>(areas[f] * N), 1);

        for (int i = 0; i < n; i++) {
            float r1 = 0;
            float r2 = 0;
            do {
                r1 = static_cast<float>(std::rand()) / static_cast<float>(RAND_MAX);
                r2 = static_cast<float>(std::rand()) / static_cast<float>(RAND_MAX);
            } while (r1 + r2 > 1.f);

            int s = std::rand() % 3;
            //std::cout << face << " " << areas[face] << std::endl;

            Eigen::Vector3f a = this->vertices[this->faces[f](s)];
            Eigen::Vector3f b = this->vertices[this->faces[f]((s + 1) % 3)];
            Eigen::Vector3f c = this->vertices[this->faces[f]((s + 2) % 3)];

            Eigen::Vector3f ab = b - a;
            Eigen::Vector3f ac = c - a;

            Eigen::Vector3f point = a + r1 * ab + r2 * ac;
            point_cloud.add_point(point);
        }
    }

    return true;
}

/** \brief Read all files in a directory matching the given extension.
 * \param[in] directory path to directory
 * \param[out] files read file paths
 * \param[in] extension extension to filter for
 */
void read_directory(const fs::path directory, std::map<std::string, fs::path>& files, const std::vector<std::string>& extensions) {

    files.clear();
    fs::directory_iterator end;

    for (fs::directory_iterator it(directory); it != end; ++it) {
        bool filtered = true;
        for (unsigned int i = 0; i < extensions.size(); i++) {
            if (it->path().extension().string() == extensions[i]) {
                filtered = false;
            }
        }

        if (!filtered) {
            const std::string basename = it->path().filename().stem().string();
            files.insert(std::make_pair(basename, it->path()));
        }
    }
}

/** \brief Main entrance point of the script.
 * Expects one parameter, the path to the corresponding config file in config/.
 */
int main(int argc, char** argv) {
    auto& parser = ArgumentParser::getInstance();
    parser.addArgument("-i", "--input", "", true, "input, either single OFF file or directory containing OFF files where the names correspond to integers (zero padding allowed) and are consecutively numbered starting with zero");
    parser.addArgument("-r", "--reference", "", true, "reference, either single OFF or TXT file or directory containing OFF or TXT files where the names correspond to integers (zero padding allowed) and are consecutively numbered starting with zero (the file names need to correspond to those found in the input directory); for TXT files, accuracy cannot be computed");
    parser.addArgument("-o", "--output", "eval.txt", false, "output file, a TXT file containing accuracy and completeness for each input-reference pair as well as overall averages");
    parser.addArgument("-n", "--n_points", 3000, false, "number points to sample from meshes in order to compute distances");
    parser.addArgument("-s", "--n_skip", 0, false, "skip every n data, if specified");

    if (!parser.parse(argc, argv)) {
        std::cout << parser.helpText() << std::endl;
        std::exit(1);
    }

    fs::path input(parser.getString("input").c_str());
    if (!fs::is_directory(input) && !fs::is_regular_file(input)) {
        std::cout << "Input is neither directory nor file." << std::endl;
        return 1;
    }

    fs::path reference(parser.getString("reference").c_str());
    if (!fs::is_directory(reference) && !fs::is_regular_file(reference)) {
        std::cout << "Reference is neither directory nor file." << std::endl;
        return 1;
    }

    fs::path output(parser.getString("output").c_str());
    if (fs::is_regular_file(output)) {
        std::cout << "Output file already exists; overwriting." << std::endl;
    }

    const int N_points = parser.getInt("n_points");
    std::cout << "Using " << N_points << " points." << std::endl;

    // Traverse file or folder recursively
    std::map<std::string, fs::path> input_files;
    std::map<std::string, fs::path> reference_files;

    if (fs::is_regular_file(input)) {
        if (input.extension().string() != ".off" && input.extension().string() != ".ply") {
            std::cout << "Only OFF and PLY files supported as input." << std::endl;
            return 1;
        }

        input_files.insert(std::make_pair(input.filename().stem().string(), input));
    } else {
        read_directory(input, input_files, { ".off", ".ply" });

        if (input_files.size() <= 0) {
            std::cout << "Could not find any OFF and PLY files in input directory." << std::endl;
            return 1;
        }

        std::cout << "Read " << input_files.size() << " input files." << std::endl;
    }

    if (fs::is_regular_file(reference)) {
        if (reference.extension().string() != ".off" && reference.extension().string() != ".ply" && reference.extension().string() != ".txt") {
            std::cout << "Only OFF, PLY, or TXT files supported as reference." << std::endl;
            return 1;
        }

        reference_files.insert(std::make_pair(reference.filename().stem().string(), reference));
    } else {
        read_directory(reference, reference_files, { ".off", ".ply", ".txt" });

        if (input_files.size() <= 0) {
            std::cout << "Could not find any OFF, PLY, or TXT files in reference directory." << std::endl;
            return 1;
        }

        std::cout << "Read " << reference_files.size() << " reference files." << std::endl;
    }

    // Skip files
    const int n_skip = std::max(1, parser.getInt("n_skip"));
    std::cout << "Skip every " << n_skip << " files." << std::endl;

    std::map<std::string, fs::path> input_files_skip;
    std::map<std::string, fs::path> reference_files_skip;
    int skip_count = 0;
    for (auto it = input_files.begin(); it != input_files.end(); ++it, ++skip_count) {
        if (skip_count % n_skip == 0) {
            const std::string basename = it->first;
            input_files_skip.insert(std::make_pair(basename, input_files[basename]));
            reference_files_skip.insert(std::make_pair(basename, reference_files[basename]));
        }
    }

    input_files = input_files_skip;
    reference_files = reference_files_skip;
    std::cout << "Process " << input_files.size() << " input files." << std::endl;
    std::cout << "Process " << reference_files.size() << " reference files." << std::endl;

    // Compute accuracy and completeness
    std::map<std::string, float> accuracies;
    std::map<std::string, float> completenesses;
    ProgressBar pbar(input_files.size());
    for (auto it = input_files.begin(); it != input_files.end(); ++it) {
        const std::string basename = it->first;
        if (reference_files.find(basename) == reference_files.end()) {
            std::cout << "Could not find the reference file corresponding to " << input_files[basename] << "." << std::endl;
            return 1;
        }

        fs::path input_file = input_files[basename];
        fs::path reference_file = reference_files[basename];

        const std::string input_extension = input_file.extension().string();
        const std::string reference_extension = reference_file.extension().string();

        bool success = false;
        Mesh input_mesh;
        if (input_extension == ".off") {
            success = Mesh::from_off(input_file.string(), input_mesh);
        } else if (input_extension == ".ply") {
            success = Mesh::from_ply(input_file.string(), input_mesh);
        }

        if (!success) {
            std::cout << "Could not read " << input_file << "." << std::endl;
            return 1;
        }

        if (reference_extension == ".off" || reference_extension == ".ply") {
            Mesh reference_mesh;
            if (reference_extension == ".off") {
                success = Mesh::from_off(reference_file.string(), reference_mesh);
            } else if (reference_extension == ".ply") {
                success = Mesh::from_ply(reference_file.string(), reference_mesh);
            }

            if (!success) {
                std::cout << "Could not read " << reference_file << "." << std::endl;
                return 1;
            }

            PointCloud input_point_cloud;
            success = input_mesh.sample(N_points, input_point_cloud);

            if (success) {
                float accuracy = 0;
                success = input_point_cloud.compute_distance(reference_mesh, accuracy);

                if (success) {
                    accuracies[basename] = accuracy;
                    std::cout << "Computed accuracy for " << input_file << ": " << accuracy << std::endl;
                } else {
                    std::cout << "Could not compute accuracy for " << input_file << "." << std::endl;
                }
            } else {
                std::cout << "Could not compute accuracy for " << input_file << "." << std::endl;
            }

            PointCloud reference_point_cloud;
            reference_mesh.sample(N_points, reference_point_cloud);

            if (success) {
                float completeness = 0;
                success = reference_point_cloud.compute_distance(input_mesh, completeness);

                if (success) {
                    completenesses[basename] = completeness;
                    std::cout << "Computed completeness for " << input_file << ": " << completeness << std::endl;
                } else {
                    std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
                }
            } else {
                std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
            }
        } else if (reference_file.extension().string() == ".txt") {
            PointCloud reference_point_cloud;
            success = PointCloud::from_txt(reference_file.string(), reference_point_cloud);

            if (!success) {
                std::cout << "Could not read " << reference_file << "." << std::endl;
                return 1;
            }

            float completeness = 0;
            success = reference_point_cloud.compute_distance(input_mesh, completeness);

            if (success) {
                completenesses[basename] = completeness;
                std::cout << "Computed completeness for " << input_file << "." << std::endl;
            } else {
                std::cout << "Could not compute completeness for " << input_file << "." << std::endl;
            }
        } else {
            std::cout << "Reference file " << reference_file << " has invalid extension." << std::endl;
        }

        pbar.step();
        printf("\n");
    }

    std::ofstream out(output.string().c_str(), std::ios::out);
    if (out.fail()) {
        std::cout << "Could not open " << output << std::endl;
        exit(1);
    }

    float accuracy = 0;
    float completeness = 0;

    for (auto it = input_files.begin(); it != input_files.end(); it++) {
        const std::string basename = it->first;

        out << basename << " ";
        if (accuracies.find(basename) != accuracies.end()) {
            out << accuracies[basename];
            accuracy += accuracies[basename];
        } else {
            out << "-1";
        }

        out << " ";
        if (completenesses.find(basename) != completenesses.end()) {
            out << completenesses[basename];
            completeness += completenesses[basename];
        } else {
            out << "-1";
        }

        out << std::endl;
    }

    if (accuracies.size() > 0) {
        accuracy /= accuracies.size();
        out << accuracy;
        std::cout << "Accuracy (input to reference): " << accuracy << std::endl;
    } else {
        out << "-1";
        std::cout << "Could not compute accuracy." << std::endl;
    }

    out << " ";
    if (completenesses.size() > 0) {
        completeness /= completenesses.size();
        out << completeness;
        std::cout << "Completeness (reference to input): " << completeness << std::endl;
    } else {
        out << "-1";
        std::cout << "Could not compute completeness." << std::endl;
    }

    out.close();
    std::cout << "Wrote " << output << "." << std::endl;

    exit(0);
}


#pragma once
#include <Eigen/Dense>
#include <fstream>
#include <iostream>
#include <list>
#include <string>
#include <iomanip>
#include <limits>
#include <vector>
#include "interpolator.hpp"
#include "system.hpp"
#include "rksolver.hpp"
#include "wkbsolver.hpp"

/** A class to store all information related to a numerical solution run.  */
class Solution
{
    private:
    double t, tf, rtol, atol, h0;
    std::complex<double> x, dx;
    int order;
    const char* fo;
    WKBSolver * wkbsolver;
    WKBSolver1 wkbsolver1;
    WKBSolver2 wkbsolver2;
    WKBSolver3 wkbsolver3;
    /** A \a de_system object to carry information about the ODE. */
    de_system *de_sys_;
    /** These define the event at which integration finishes (currently: when tf
     * is reached. */
    double fend, fnext;
    /** a boolean encoding the direction of integration: 1/True for forward. */
    bool sign;

    public:
    Solution(de_system &de_sys, std::complex<double> x0, std::complex<double>
    dx0, double t_i, double t_f, int o=3, double r_tol=1e-4, double a_tol=0.0,
    double h_0=1, const char* full_output="");

    template<typename X = double> Solution(de_system &de_sys,
    std::complex<double> x0, std::complex<double> dx0, double t_i, double t_f,
    const X &do_times, int o=3, double r_tol=1e-4, double a_tol=0.0,
    double h_0=1, const char* full_output="");

    void solve();

    /** Object to call RK steps */
    RKSolver rksolver;

    /** Successful, total attempted, and successful WKB steps the solver took,
     * respectively  */
    int ssteps,totsteps,wkbsteps;
    /** Lists to contain the solution and its derivative evaluated at internal
     * points taken by the solver (i.e. not dense output) after a run */
    std::list<std::complex<double>> sol, dsol;
    /** List to contain the timepoints at which the solution and derivative are
     * internally evaluated by the solver */
    std::list<double> times;
    /** List to contain the "type" of each step (RK/WKB) taken internally by the
     * solver after a run */
    std::list<bool> wkbs;
    /** Lists to contain the timepoints at which dense output was evaluated */
    std::list<double> dotimes, dotimes_rk;
    /** Lists to contain the dense output of the solution and its derivative */
    std::list<std::complex<double>> dosol, dodsol, dosol_rk, dodsol_rk;
    /** Iterator to iterate over the dense output timepoints, for when these
     * need to be written out to file */
    std::list<double>::iterator dotit;

};

/** Constructor for when dense output was not requested. Sets up solution of the
 * ODE.
 *
 * @param[in] de_sys de_system object carrying information about the ODE being
 * solved
 * @param[in] x0, dx0 initial conditions for the ODE, \f$ x(t) \f$, \f$ \frac{dx}{dt} \f$ evaluated at
 * the start of the integration range
 * @param[in] t_i start of integration range
 * @param[in] t_f end of integration range
 * @param[in] o order of WKB approximation to be used
 * @param[in] r_tol (local) relative tolerance
 * @param[in] a_tol (local) absolute tolerance
 * @param[in] h_0 initial stepsize to use
 * @param[in] full_output file name to write results to
 *
 */
Solution::Solution(de_system &de_sys, std::complex<double> x0,
std::complex<double> dx0, double t_i, double t_f, int o, double r_tol, double
a_tol, double h_0, const char* full_output){

    // Make underlying equation system accessible
    de_sys_ = &de_sys;

    // Set parameters for solver
    x = x0;
    dx = dx0;
    t = t_i;
    tf = t_f;
    order = o;
    rtol = r_tol;
    atol = a_tol;
    h0 = h_0;
    fo = full_output;
    rksolver = RKSolver(*de_sys_);

    // Determine direction of integration, fend>0 and integration ends when
    // it crosses zero
    if((t>=tf) and h0<0){
        // backwards
        fend = t-tf;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 0;
            de_sys_->Ginterp.sign_ = 0;
        }
        else
            sign = 0;

    }
    else if((t<=tf) and h0>0){
        // forward
        fend = tf-t;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 1;
            de_sys_->Ginterp.sign_ = 1;
        }
        else
            sign = 1;
    }
    else{
        throw "Direction of integration in conflict with direction of initial step, terminating. Please check your values for ti, tf, and h. ";
        return;
    }

    // No dense output desired if this constructor was called, so only output
    // answer at t_i and t_f
    dotimes.push_back(t_i);
    dotimes.push_back(t_f);
    dosol.push_back(x0);
    dodsol.push_back(dx0);
    dotit = dotimes.end();

    switch(order){
        case 1: wkbsolver1 = WKBSolver1(*de_sys_, order);
                wkbsolver = &wkbsolver1;
                break;
        case 2: wkbsolver2 = WKBSolver2(*de_sys_, order);
                wkbsolver = &wkbsolver2;
                break;
        case 3: wkbsolver3 = WKBSolver3(*de_sys_, order);
                wkbsolver = &wkbsolver3;
                break;
    };
};

/** Constructor for when dense output was requested. Sets up solution of the
 * ODE.
 *
 * @param[in] de_sys de_system object carrying information about the ODE being
 * solved
 * @param[in] x0, dx0 initial conditions for the ODE, \f$ x(t) \f$, \f$ \frac{dx}{dt} \f$ evaluated at
 * the start of the integration range
 * @param[in] t_i start of integration range
 * @param[in] t_f end of integration range
 * @param[in] do_times timepoints at which dense output is to be produced
 * @param[in] o order of WKB approximation to be used
 * @param[in] r_tol (local) relative tolerance
 * @param[in] a_tol (local) absolute tolerance
 * @param[in] h_0 initial stepsize to use
 * @param[in] full_output file name to write results to
 *
 */
template<typename X> Solution::Solution(de_system &de_sys, std::complex<double> x0,
std::complex<double> dx0, double t_i, double t_f, const X &do_times, int o, double r_tol, double
a_tol, double h_0, const char* full_output){

    // Make underlying equation system accessible
    de_sys_ = &de_sys;
    // Set parameters for solver
    x = x0;
    dx = dx0;
    t = t_i;
    tf = t_f;
    order = o;
    rtol = r_tol;
    atol = a_tol;
    h0 = h_0;
    fo = full_output;
    rksolver = RKSolver(*de_sys_);

    // Determine direction of integration, fend>0 and integration ends when
    // it crosses zero
    if((t>=tf) and h0<0){
        // backwards
        fend = t-tf;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 0;
            de_sys_->Ginterp.sign_ = 0;
        }
        else
            sign = 0;
    }
    else if((t<=tf) and h0>0){
        // forward
        fend = tf-t;
        fnext = fend;
        if(de_sys_->is_interpolated == 1){
            de_sys_->Winterp.sign_ = 1;
            de_sys_->Ginterp.sign_ = 1;
        }
        else
            sign = 1;
    }
    else{
        throw "Direction of integration in conflict with direction of initial step, terminating. Please check your values for ti, tf, and h. ";
        return;
    }

    // Dense output checks:
    int dosize = do_times.size();
    dotimes.resize(dosize);
    dosol.resize(dosize);
    dodsol.resize(dosize);
    int docount = 0;
    auto doit = do_times.begin();
    if(de_sys_->Winterp.sign_ == 1){
        for(auto it=dotimes.begin(); it!=dotimes.end(); it++){
            *it = *doit;
            docount++; doit++;
        }
    }
    else{
         for(auto it=dotimes.rbegin(); it!=dotimes.rend(); ++it){
            *it = *doit;
            docount++; ++doit;
        }
    }
    dotit = dotimes.begin();
    switch(order){
        case 1: wkbsolver1 = WKBSolver1(*de_sys_, order);
                wkbsolver = &wkbsolver1;
                break;
        case 2: wkbsolver2 = WKBSolver2(*de_sys_, order);
                wkbsolver = &wkbsolver2;
                break;
        case 3: wkbsolver3 = WKBSolver3(*de_sys_, order);
                wkbsolver = &wkbsolver3;
                break;
    };
};

/** \brief Function to solve the ODE \f$ \ddot{x} + 2\gamma(t)\dot{x} +
 * \omega^2(t)x = 0 \f$ for \f$ x(t), \frac{dx}{dt} \f$.
 *
 * While solving the ODE, this function will populate the \a Solution object
 * with the following results:
 *
 */
void Solution::solve(){

    int nrk, nwkb1, nwkb2;
    // Settings for MS
    nrk = 5;
    nwkb1 = 2;
    nwkb2 = 4;
    Eigen::Matrix<std::complex<double>,2,2> rkstep;
    Eigen::Matrix<std::complex<double>,3,2> wkbstep;
    Eigen::Matrix<std::complex<double>,1,2> rkx, wkbx;
    Eigen::Matrix<std::complex<double>,1,2> rkerr, wkberr, truncerr;
    Eigen::Matrix<double,1,2> errmeasure_rk;
    Eigen::Matrix<double,1,4> errmeasure_wkb;
    double tnext, hnext, h, hrk, hwkb;
    double wkbdelta, rkdelta;
    std::complex<double> xnext, dxnext;
    bool wkb = false;
    Eigen::Index maxindex_wkb, maxindex_rk;
    h = h0;
    tnext = t+h;
    // Initialise stats
    sol.push_back(x);
    dsol.push_back(dx);
    times.push_back(t);
    wkbs.push_back(false);
    ssteps = 0;
    totsteps = 0;
    wkbsteps = 0;
    // Dense output
    std::list<double> inner_dotimes;
    std::list<std::complex<double>> inner_dosols, inner_dodsols;
    auto it_dosol = dosol.begin();
    auto it_dodsol = dodsol.begin();
    Eigen::Matrix<std::complex<double>,1,2> y_dense_rk;
    std::complex<double> x_dense_rk, dx_dense_rk;

    while(fend > 0){
        // Check if we are reaching the end of integration
        if(fnext < 0){
            h = tf - t;
            tnext = tf;
        };

        // Keep updating stepsize until step is accepted
        while(true){
            // RK step
            rkstep = rksolver.step(x, dx, t, h);
            rkx << rkstep(0,0), rkstep(0,1);
            rkerr << rkstep(1,0), rkstep(1,1);
            // WKB step
            wkbstep = wkbsolver->step(x, dx, t, h, rksolver.ws, rksolver.gs, rksolver.ws5, rksolver.gs5);
            wkbx = wkbstep.row(0);
            wkberr = wkbstep.row(2);
            truncerr = wkbstep.row(1);
            // Safety feature for when all wkb steps are 0 (truncer=0), but not
            // necessarily in good WKB regime:
            truncerr(0) = std::max(1e-10,abs(truncerr(0)));
            truncerr(1) = std::max(1e-10,abs(truncerr(1)));
            // dominant error calculation
            // Error scale measures
            errmeasure_rk << std::abs(rkerr(0))/(std::abs(rkx(0))*rtol+atol), std::abs(rkerr(1))/(std::abs(rkx(1))*rtol+atol);
            errmeasure_wkb << std::abs(truncerr(0))/(std::abs(wkbx(0))*rtol+atol),
            std::abs(truncerr(1))/(std::abs(wkbx(1))*rtol+atol),
            std::abs(wkberr(0))/(std::abs(wkbx(0))*rtol+atol),
            std::abs(wkberr(1))/(std::abs(wkbx(1))*rtol+atol);
            rkdelta = std::max(1e-10, errmeasure_rk.maxCoeff(&maxindex_rk));
            if(std::isnan(errmeasure_wkb.maxCoeff())==false &&
               std::isinf(std::real(wkbx(0)))==false &&
               std::isinf(std::imag(wkbx(0)))==false &&
               std::isinf(std::real(wkbx(1)))==false &&
               std::isinf(std::imag(wkbx(1)))==false &&
               std::isnan(std::real(wkbx(0)))==false &&
               std::isnan(std::imag(wkbx(0)))==false &&
               std::isnan(std::real(wkbx(1)))==false &&
               std::isnan(std::imag(wkbx(1)))==false){
                wkbdelta = std::max(1e-10, errmeasure_wkb.maxCoeff(&maxindex_wkb));
            }
            else{
                wkbdelta = std::numeric_limits<double>::infinity();
            }

            // predict next stepsize
            hrk = h*std::pow((1.0/rkdelta),1.0/nrk);
            if(maxindex_wkb<=1)
                hwkb = h*std::pow(1.0/wkbdelta,1.0/nwkb1);
            else
                hwkb = h*std::pow(1.0/wkbdelta,1.0/nwkb2);
            // choose step with larger predicted stepsize
            if(std::abs(hwkb) >= std::abs(hrk)){
                wkb = true;
            }
            else{
                wkb = false;
            }
            if(wkb){
                xnext = wkbx(0);
                dxnext = wkbx(1);
                // if wkb step chosen, ignore truncation error in
                // stepsize-increase
                wkbdelta = std::max(1e-10, errmeasure_wkb.tail(2).maxCoeff());
                hnext = h*std::pow(1.0/wkbdelta,1.0/nwkb2);
            }
            else{
                xnext = rkx(0);
                dxnext = rkx(1);
                hnext = hrk;
            };
            totsteps += 1;
            // Checking for too many steps and low acceptance ratio:
            if(totsteps % 5000 == 0){
                std::cerr << "Warning: the solver took " << totsteps << " steps, and may take a while to converge." << std::endl;
                if(ssteps/totsteps < 0.05){
                    std::cerr << "Warning: the step acceptance ratio is below 5%, the solver may take a while to converge." << std::endl;
                }
            }

            // check if chosen step was successful
            if(std::abs(hnext)>=std::abs(h)){
                if(dotit!=dotimes.end()){
                    while((*dotit-t>=0 && tnext-*dotit>=0) or (*dotit-t<=0 && tnext-*dotit<=0)){
                        inner_dotimes.push_back(*dotit);
                        dotit++;
                    }
                    if(inner_dotimes.size() > 0){
                        inner_dosols.resize(inner_dotimes.size());
                        inner_dodsols.resize(inner_dotimes.size());
                        if(wkb){
                            // Dense output after successful WKB step
                            wkbsolver->dense_step(t,inner_dotimes,inner_dosols,inner_dodsols);
                        }
                        else{
                            // Dense output after successful RK step
                            for(auto it=inner_dotimes.begin(); it!=inner_dotimes.end(); it++)
                                rksolver.dense_step(t,h,x,dx,inner_dotimes,inner_dosols,inner_dodsols);
                        }
                    }
                }
                auto inner_it=inner_dosols.begin();
                auto inner_dit=inner_dodsols.begin();
                while(inner_it!=inner_dosols.end() && it_dosol!=dosol.end() && inner_dit!=inner_dodsols.end() && it_dodsol!=dodsol.end()){
                    *it_dosol = *inner_it;
                    *it_dodsol = *inner_dit;
                    it_dodsol++;
                    it_dosol++;
                    inner_it++;
                    inner_dit++;
                }
                inner_dotimes.resize(0);
                inner_dosols.resize(0);
                inner_dodsols.resize(0);

                // record type of step
                if(wkb){
                    wkbsteps +=1;
                    wkbs.push_back(true);
                }
                else{
                    wkbs.push_back(false);
                }
                sol.push_back(xnext);
                dsol.push_back(dxnext);
                times.push_back(tnext);
                tnext += hnext;
                x = xnext;
                dx = dxnext;
                t += h;
                h = hnext;
                if(h>0){
                    fend=tf-t;
                    fnext=tf-tnext;
                }
                else{
                    fend=t-tf;
                    fnext=tnext-tf;
                };
                ssteps +=1;
                // Update interpolation bounds
                if(de_sys_->is_interpolated == 1){
                    de_sys_->Winterp.update_interp_bounds();
                    de_sys_->Ginterp.update_interp_bounds();
                }

                break;
            }
            else{
                if(wkb){
                    if(maxindex_wkb<=1){
                        if(nwkb1 > 1)
                            hnext = h*std::pow(1.0/wkbdelta,1.0/(nwkb1-1));
                        else
                            hnext = 0.95*h*1.0/wkbdelta;
                    }
                    else
                        hnext = h*std::pow(1.0/wkbdelta,1.0/(nwkb2-1));
                }
                else
                    hnext = h*std::pow(1.0/rkdelta,1.0/(nrk-1));
                h = hnext;
                tnext = t + hnext;
                if(h>0){
                    fnext=tf-tnext;
                }
                else{
                    fnext=tnext-tf;
                };
            };
        };
    };

    // If integrating backwards, reverse dense output (because it will have been
    // reversed at the start)
    if(de_sys_->is_interpolated == 1){
        if(de_sys_->Winterp.sign_ == 0){
            dosol.reverse();
            dodsol.reverse();
        }
    }
    else{
        if(sign == 0){
            dosol.reverse();
            dodsol.reverse();
        }
    }

    // Write output to file if prompted
    if(not (*fo==0)){
        std::string output(fo);
        std::ofstream f;
        f.open(output);
        f << "# Summary:\n# total steps taken: " + std::to_string(totsteps) +
        "\n# of which successful: " + std::to_string(ssteps) + "\n# of which"+
        +" wkb: " + std::to_string(wkbsteps) + "\n# time, sol, dsol, wkb? (type)\n";
        auto it_t = times.begin();
        auto it_w = wkbs.begin();
        auto it_x = sol.begin();
        auto it_dx = dsol.begin();
        for(int i=0; i<=ssteps; i++){
            f << std::setprecision(15) << *it_t << ";" <<
            std::setprecision(15) << *it_x << ";" << std::setprecision(15) <<
            *it_dx << ";" << *it_w << "\n";
            ++it_t;
            ++it_x;
            ++it_dx;
            ++it_w;
        }
        // print all dense output to file
        int dosize = dosol.size();
        auto it_dosol = dosol.begin();
        auto it_dotimes = dotimes.begin();
        auto it_dodsol = dodsol.begin();
        for(int i=0; i<dosize; i++){
            f << std::setprecision(20) << *it_dotimes << ";" << std::setprecision(20) << *it_dosol << ";" << *it_dodsol << ";\n";
            ++it_dosol;
            ++it_dodsol;
            ++it_dotimes;
        }

        f.close();
    }

};


/*
 * Copyright (c) 2016 Abhishek Agrawal (abhishek.agrawal@protonmail.com)
 * Distributed under the MIT License.
 * See accompanying file LICENSE.md or copy at http://opensource.org/licenses/MIT
 */

// This code will convert multiple cartesian state vectors
// into TLE and check where exactly the Atom is failing. The cartesian
// vectors are generated by converting randomly generated keplerian elements.
// The conversion is achieved through the pykep library of ESA.

#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>
#include <fstream>
#include <exception>
#include <cstdlib>
#include <execinfo.h>

#include <boost/exception/info.hpp>
#include <libsgp4/Globals.h>
#include <SML/sml.hpp>
#include <SML/constants.hpp>
#include <SML/basicFunctions.hpp>

#include "CppProject/randomKepElem.hpp"
#include "CppProject/KepToCart.hpp"
#include "CppProject/KepToCartToTLE.hpp"
#include "CppProject/randomGen.hpp"

typedef double Real;
typedef std::vector< Real > Vector6;
typedef std::vector< Real > Vector3;
typedef std::vector< Real > Vector2;
typedef std::vector < std::vector < Real > > Vector2D;

int main(void)
{

    // some constants values are defined here
    const double km2m = 1000; // conversion from km to m
    // earth radius
    const double EarthRadius = kXKMPER * km2m; // unit m
    const double EarthDiam = 2 * EarthRadius;
    // grav. parameter 'mu' of earth
    // const double muEarth = kMU*( pow( 10, 9 ) ); // unit m^3/s^2
    const int bypass = true; // make this false to execute code with random orbital elements

    if(bypass == false){
        // initialize input parameters for the function generating random orbital elements. Description can be
        // found in randomKepElem.hpp for each of the following parameters.
        const Vector2 range_a      = { (EarthDiam+100000), (EarthDiam+1000000) };
        const Vector2 range_e      = { 0, 1 };
        const Vector2 range_i      = { 0, sml::convertDegreesToRadians( 180.0 ) };
        const Vector2 range_raan   = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const Vector2 range_w      = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const Vector2 range_E      = { 0, sml::convertDegreesToRadians( 360.0 ) };
        const int limit            = 100;
        Vector2D randKep( limit, std::vector< Real >( 6 ) );

        // call the function to generate random keplerian orbital elements. Values are stored in randKepElem in a 2D
        // vector format. A single row represents one set of orbital elements, arranged in the same order as the
        // input argument order of the elements.
        randomKepElem::randomKepElem( range_a, range_e, range_i, range_raan, range_w, range_E, limit, randKep );

        // remove orbital elements where the radius of perigee is inside earth
        Vector2D randKepElem( limit, std::vector< Real >( 6 ) );
        int newLimit = limit; // whenever a row of cartesian (keplerian) elements is removed, the limit will have to be changed.
        Real radiusPerigee = 0; // variable storing the radius of Perigee for an orbit. this is used in our checking condition
        std::vector< int > rowsToDelete; // stores the row numbers which have to be deleted
        int insideCounter = 0; // to count the number of times radius of perigee is inside the Earth.
        int outsideCounter = 0; // to count the number of times the radius of perigee is beyond a cetain upper limit
        int newIndex = 0; // index counter for the second 2D vector
        for(int j = 0; j < newLimit; j++)
        {
            radiusPerigee = randKep[ j ][ 0 ] * (1 - randKep[ j ][ 1 ]);
            if(radiusPerigee <= EarthRadius )
            {
                insideCounter = insideCounter + 1; // counter increments everytime the condition is true
            }
            if(radiusPerigee >= (EarthRadius+2000000))
            {
                outsideCounter = outsideCounter + 1;
            }
            if(radiusPerigee > EarthRadius && radiusPerigee < (EarthRadius+2000000)) // this is obv not the most efficient way
            {
                randKepElem[ newIndex ] = randKep[ j ];
                newIndex++;
            }
        }
        randKepElem.erase( randKepElem.begin() + newIndex, randKepElem.end() ); // delete the left over rows in the final random keplerian element vector
        std::vector< std::vector < Real > > ().swap(randKep); // create an empty vector with no memory allocated to it ...
        // ... and swap it with the vector which you want to delete and deallocate the memory
        std::cout << "Inside counter value = " << insideCounter << std::endl;
        std::cout << "Outside counter value = " << outsideCounter << std::endl;
        std::cout << "Usefull sets left = " << limit - (insideCounter + outsideCounter) << std::endl;
        newLimit = randKepElem.size();
        std::cout << "Number of final rows in randKepElem 2D vector = " << newLimit << std::endl;
        KepToCartToTLE::KepToCartToTLE( newLimit, randKepElem );
    }
    else{
            const int newLimit = 2;
            Vector2D randKepElem( newLimit, std::vector < Real >( 6 ) );
            int indexer = 0;

            Vector2 range_a = { (EarthRadius + 100 * km2m), (EarthRadius + 1000 * km2m) }; // range for semi major axis
            std::vector< Real > semiAxis (newLimit, 0); // initialize vector of size newLimit with all zeros as elements
            randomGen::randomGen( range_a, newLimit, semiAxis); // random generator
            for(int i = 0; i < newLimit; i++)
            {
                randKepElem[ i ][ 0 ] = semiAxis[ i ]; // semi major axis
                randKepElem[ i ][ 1 ] = 0; // eccentricity
                randKepElem[ i ][ 2 ] = sml::convertDegreesToRadians( 0 ); // inclination
                randKepElem[ i ][ 3 ] = sml::convertDegreesToRadians( 0 ); // RAAN
                randKepElem[ i ][ 4 ] = sml::convertDegreesToRadians( 0 ); //AOP
                randKepElem[ i ][ 5 ] = sml::convertDegreesToRadians( 0 ); //EA
            }
            while( indexer < newLimit)
            {
                try
                {
                    indexer++;
                    KepToCartToTLE::KepToCartToTLE( 1, randKepElem );
                    std::cout << "indexer = " << indexer << std::endl;

                }
                catch(const std::exception& err)
                {
                    std::cout << "Error Caught = ";
                    std::cout << err.what() << std::endl;
                }
            }
        }
   return EXIT_SUCCESS;
}


#ifndef SLIDE_POINT_HPP_
#define SLIDE_POINT_HPP_

#include <array>
#include <cstdlib>
#include <iostream>

#include <boost/assert.hpp>

#include "Direction.hpp"
#include "util/abs.hpp"
#include "util/define.hpp"

#ifdef __gcc__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
#endif

namespace slide
{

class Point
{
public:
	int y;
	int x;

	Point() = default;

	constexpr Point(int y, int x) : y(y), x(x) {}

	constexpr explicit Point(uchar v, int w = MAX_DIVISION_NUM) : Point(v/w, v%w) {}

	constexpr uchar toInt(int w = MAX_DIVISION_NUM) const {
		return y * w + x;
	}

	constexpr int l1norm() const {
		return util::abs(y) + util::abs(x);
	}

	constexpr bool isIn(int height, int width) const {
		return isIn(0, 0, height, width);
	}

	constexpr bool isIn(int py, int px, int height, int width) const {
		return py <= y && y < py+height && px <= x && x < px+width;
	}

	constexpr Point operator+ (const Point& p) const {
		return Point(y+p.y, x+p.x);
	}

	constexpr Point operator- (const Point& p) const {
		return Point(y-p.y, x-p.x);
	}

	constexpr Point operator+ () const {
		return *this;
	}

	constexpr Point operator- () const {
		return Point(-y, -x);
	}

	Point& operator+=(const Point& p){
		y += p.y;
		x += p.x;
		return *this;
	}

	Point& operator-=(const Point& p){
		y -= p.y;
		x -= p.x;
		return *this;
	}

	constexpr Point operator* (const int v) const {
		return Point(y*v, x*v);
	}

	Point& operator*=(const int v) {
		y *= v;
		x *= v;
		return *this;
	}

	constexpr bool operator==(const Point& p) const {
		return y == p.y && x == p.x;
	}

	constexpr bool operator!=(const Point& p) const {
		return y != p.y || x != p.x;
	}

	bool operator< (const Point& p) const {
		return (x != p.x ? x < p.x : y < p.y);
	}

	static constexpr Point delta(Direction dir) {
		/*
		// I wanna use C++14!
		switch(dir){
			case Direction::Up:    return Point(-1,  0);
			case Direction::Right: return Point( 0,  1);
			case Direction::Down:  return Point( 1,  0);
			case Direction::Left:  return Point( 0, -1);
		}
		*/
		return
		dir == Direction::Up ?    Point(-1,  0) : (
		dir == Direction::Right ? Point( 0,  1) : (
		dir == Direction::Down ?  Point( 1,  0) :
								  Point( 0, -1)
		));
	}

	friend std::ostream& operator<<(std::ostream& out, const Point& p) {
		return out << '(' << p.x << ',' << p.y << ')';
	}

    /**************************************************************************
     * Rotation and flip
     *************************************************************************/

	constexpr Point rotateCW(int H) const {
		return Point(x, H-y-1);
	}

	constexpr Point rotateCCW(int W) const {
		return Point(W-x-1, y);
	}

	constexpr Point opposite(int H, int W) const {
		return Point(H-y-1, W-x-1);
	}

	constexpr Point flipY(int H) const {
		return Point(H-y-1, x);
	}

	constexpr Point flipX(int W) const {
		return Point(y, W-x-1);
	}

	Point rotateFlip(Direction topSide, bool yflip, bool xflip, int H, int W) const {
		Point ret = *this;
		if(yflip) ret = ret.flipY(H);
		if(xflip) ret = ret.flipX(W);
		if(topSide == Direction::Up)    return ret;
		if(topSide == Direction::Right) return rotateCCW(W);
		if(topSide == Direction::Down)  return opposite(H, W);
		return rotateCW(H);
	}
};

} // end of namespace slide

#ifdef __gcc__
#pragma GCC diagnostic pop
#endif

#endif


// Copyright 2004, 2005 The Trustees of Indiana University.

// Use, modification and distribution is subject to the Boost Software
// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP
#define BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP

#include <cmath>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/named_function_params.hpp>
#include <vector>
#include <list>
#include <algorithm> // for std::min, std::max, std::accumulate
#include <boost/graph/point_traits.hpp>
#include <functional>
#include <numeric>

#include <stdlib.h> // for drand48

namespace boost {

using boost::graph::point_traits;

struct square_distance_attractive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::edge_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return d * d / k;
  }
};

struct square_distance_repulsive_force {
  template<typename Graph, typename T>
  T
  operator()(typename graph_traits<Graph>::vertex_descriptor,
             typename graph_traits<Graph>::vertex_descriptor,
             T k,
             T d,
             const Graph&) const
  {
    return k * k / d;
  }
};

template<typename T>
struct linear_cooling {
  typedef T result_type;

  linear_cooling(std::size_t iterations)
    : temp(T(iterations) / T(10)), step(0.1) { }

  linear_cooling(std::size_t iterations, T temp)
    : temp(temp), step(temp / T(iterations)) { }

  T operator()()
  {
    T old_temp = temp;
    temp -= step;
    if (temp < T(0)) temp = T(0);
    return old_temp;
  }

 private:
  T temp;
  T step;
};

struct all_force_pairs
{
  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    vertex_iterator v, end;
    for (tie(v, end) = vertices(g); v != end; ++v) {
      vertex_iterator u = v;
      for (++u; u != end; ++u) {
        apply_force(*u, *v);
        apply_force(*v, *u);
      }
    }
  }
};

namespace detail {
  template<typename Point>
  Point point_difference(const Point& p1, const Point& p2)
  {
    Point result;
    std::size_t dims = point_traits<Point>::dimensions(p1);
    for (std::size_t i = 0; i < dims; ++i)
      result[i] = p1[i] - p2[i];
    return result;
  }

  template<typename Point>
  typename point_traits<Point>::component_type
  point_norm(const Point& p)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif
    typename point_traits<Point>::component_type result(0);
    std::size_t dims = point_traits<Point>::dimensions(p);
    for (std::size_t i = 0; i < dims; ++i)
      result += p[i] * p[i];
    return sqrt(result);
  }

  template<typename Point>
  void
  maybe_jitter_point(Point& p1, const Point& p2, Point origin, Point extent)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
    using std::fabs;
    using std::rand;
#endif // BOOST_NO_STDC_NAMESPACE
    typedef typename point_traits<Point>::component_type Dim;
    std::size_t dims = point_traits<Point>::dimensions(p1);
    for (std::size_t i = 0; i < dims; ++i) {
      Dim too_close = extent[i] / Dim(10000);
      if (fabs(p1[i] - p2[i]) < too_close) {
        Dim dist_to_move = sqrt(extent[i]) / Dim(200);
        if (p1[i] - origin[i] < origin[i] + extent[i] - p1[i])
          p1[i] += dist_to_move * Dim(rand() % 100) / Dim(100);
        else
          p1[i] -= dist_to_move * Dim(rand() % 100) / Dim(100);
      }
    }
  }

  template<typename PositionMap, typename DisplacementMap,
           typename RepulsiveForce, typename Graph>
  struct fr_apply_force
  {
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef typename property_traits<PositionMap>::value_type Point;
    typedef typename point_traits<Point>::component_type Dim;

    fr_apply_force(const PositionMap& position,
                   const DisplacementMap& displacement,
                   Point origin, Point extent,
                   RepulsiveForce repulsive_force, Dim k, const Graph& g)
      : position(position), displacement(displacement), origin(origin),
        extent(extent), repulsive_force(repulsive_force), k(k), g(g)
    {
      dims = point_traits<Point>::dimensions(origin);
    }

    void operator()(vertex_descriptor u, vertex_descriptor v)
    {
#ifndef BOOST_NO_STDC_NAMESPACE
      using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
      if (u != v) {
        // When the vertices land on top of each other, move the
        // first vertex away from the boundaries.
        maybe_jitter_point(position[u], position[v], origin, extent);

        // DPG TBD: Can we use the Topology concept's
        // distance/move_position_toward to handle this?
        Point delta = detail::point_difference(position[v], position[u]);
        Dim dist = detail::point_norm(delta);

        if (dist == Dim(0)) {
          for (std::size_t i = 0; i < dims; ++i)
            displacement[v][i] += 0.01;
        } else {
          Dim fr = repulsive_force(u, v, k, dist, g);

          for (std::size_t i = 0; i < dims; ++i)
            displacement[v][i] += delta[i] / dist * fr;
        }
      }
    }

  private:
    PositionMap position;
    DisplacementMap displacement;
    Point origin;
    Point extent;
    RepulsiveForce repulsive_force;
    Dim k;
    const Graph& g;
    std::size_t dims;
  };

} // end namespace detail

template<typename PositionMap>
struct grid_force_pairs
{
  typedef typename property_traits<PositionMap>::value_type Point;
  typedef typename point_traits<Point>::component_type Dim;

  template<typename Graph>
  explicit
  grid_force_pairs(const Point& origin, const Point& extent,
                   PositionMap position, const Graph& g)
    : origin(origin), extent(extent), position(position)
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
    std::size_t dims = point_traits<Point>::dimensions(origin);
    two_k = Dim(2) * sqrt(std::accumulate(&extent[0], &extent[0] + dims,
                                          Dim(1), std::multiplies<Dim>()));

    num_buckets.resize(dims);
    for (std::size_t i = 0; i < dims; ++i)
      num_buckets[i] = std::size_t(extent[i] / two_k + Dim(1));
  }

  template<typename Graph, typename ApplyForce >
  void operator()(const Graph& g, ApplyForce apply_force)
  {
    typedef typename graph_traits<Graph>::vertex_iterator vertex_iterator;
    typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
    typedef std::list<vertex_descriptor> bucket_t;
    typedef std::vector<bucket_t> buckets_t;

#ifndef BOOST_NO_STDC_NAMESPACE
    using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE
    std::size_t dims = point_traits<Point>::dimensions(origin);

    buckets_t buckets(std::accumulate(&num_buckets[0], &num_buckets[0] + dims,
                                      std::size_t(1),
                                      std::multiplies<std::size_t>()));
    vertex_iterator v, v_end;
    for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
      std::vector<std::size_t> bucket(dims);
      for (std::size_t i = 0; i < dims; ++i) {
        bucket[i] = std::size_t((position[*v][i] + origin[i]) / two_k);
        if (bucket[i] >= num_buckets[i]) bucket[i] = num_buckets[i] - 1;
      }
      buckets[bucket_to_index(bucket)].push_back(*v);
    }

    std::vector<std::size_t> cell(2);
    do {
      bucket_t& bucket = buckets[bucket_to_index(cell)];

      std::vector<std::size_t> start_cell(dims);
      std::vector<std::size_t> end_cell(dims);

      for (std::size_t i = 0; i < dims; ++i) {
        start_cell[i] = cell[i] == 0? 0 : cell[i] - 1;
        end_cell[i] = cell[i] == num_buckets[i] - 1? cell[i] : cell[i] + 1;
      }

      // Repulse vertices in this bucket
      typedef typename bucket_t::iterator bucket_iterator;

      std::vector<std::size_t> adj_cell = start_cell;
      do {
        for (bucket_iterator u = bucket.begin(); u != bucket.end(); ++u) {
          // Repulse vertices in this bucket
          bucket_t& other_bucket = buckets[bucket_to_index(adj_cell)];
          for (bucket_iterator v = other_bucket.begin(); v != other_bucket.end(); ++v) {
            Point delta = detail::point_difference(position[*u],
                                                   position[*v]);
            Dim dist = detail::point_norm(delta);
            if (dist < two_k) apply_force(*u, *v);
          }
        }
      } while (next_bucket_in_subgrid(adj_cell, start_cell, end_cell));
    } while (next_bucket(cell));
  }

private:
  bool next_bucket_in_subgrid(std::vector<std::size_t>& bucket,
                              const std::vector<std::size_t>& start,
                              const std::vector<std::size_t>& end)
  {
    // Find the next bucket
    std::size_t index = bucket.size() - 1;
    do {
      if (bucket[index]++ >= end[index]) {
        bucket[index] = start[index];

        if (index == 0)
          return false;
        --index;
      } else {
        return true;
      }
    } while (true);
  }

  bool next_bucket(std::vector<std::size_t>& bucket)
  {
    // Find the next bucket
    std::size_t index = bucket.size() - 1;
    do {
      if (++bucket[index] >= num_buckets[index]) {
        bucket[index] = 0;

        if (index == 0)
          return false;
        --index;
      } else {
        return true;
      }
    } while (true);
  }

  std::size_t bucket_to_index(const std::vector<std::size_t>& bucket)
  {
    std::size_t multiplier = 1;
    std::size_t result = 0;

    std::size_t dims = num_buckets.size();
    for (std::size_t i = 0; i < dims; ++i) {
      result += bucket[i] * multiplier;
      multiplier *= num_buckets[i];
    }
    return result;
  }

  Point origin;
  Point extent;
  PositionMap position;
  Dim two_k;
  std::vector<std::size_t> num_buckets;
};

template<typename PositionMap, typename Graph>
inline grid_force_pairs<PositionMap>
make_grid_force_pairs
  (typename property_traits<PositionMap>::value_type const& origin,
   typename property_traits<PositionMap>::value_type const& extent,
   const PositionMap& position, const Graph& g)
{ return grid_force_pairs<PositionMap>(origin, extent, position, g); }

template<typename Graph, typename PositionMap,
         typename AttractiveForce, typename RepulsiveForce,
         typename ForcePairs, typename Cooling, typename DisplacementMap>
void
fruchterman_reingold_force_directed_layout
 (const Graph&    g,
  PositionMap     position,
  typename property_traits<PositionMap>::value_type const& origin,
  typename property_traits<PositionMap>::value_type const& extent,
  AttractiveForce attractive_force,
  RepulsiveForce  repulsive_force,
  ForcePairs      force_pairs,
  Cooling         cool,
  DisplacementMap displacement)
{
  typedef typename property_traits<PositionMap>::value_type Point;
  typedef typename point_traits<Point>::component_type Dim;
  typedef typename graph_traits<Graph>::vertex_iterator   vertex_iterator;
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_descriptor;
  typedef typename graph_traits<Graph>::edge_iterator     edge_iterator;

#ifndef BOOST_NO_STDC_NAMESPACE
  using std::sqrt;
#endif // BOOST_NO_STDC_NAMESPACE

  std::size_t num_dimensions = point_traits<Point>::dimensions(origin);
  Dim volume = std::accumulate(&extent[0], &extent[0] + num_dimensions,
                               Dim(1), std::multiplies<Dim>());

  // assume positions are initialized randomly
  Dim k = sqrt(volume / num_vertices(g));

  detail::fr_apply_force<PositionMap, DisplacementMap,
                         RepulsiveForce, Graph>
    apply_force(position, displacement, origin, extent, repulsive_force, k, g);

  do {
    // Calculate repulsive forces
    vertex_iterator v, v_end;
    for (tie(v, v_end) = vertices(g); v != v_end; ++v)
      displacement[*v] = Point();
    force_pairs(g, apply_force);

    // Calculate attractive forces
    edge_iterator e, e_end;
    for (tie(e, e_end) = edges(g); e != e_end; ++e) {
      vertex_descriptor v = source(*e, g);
      vertex_descriptor u = target(*e, g);

      if (u != v) {
        // When the vertices land on top of each other, move the
        // first vertex away from the boundaries.
        ::boost::detail::maybe_jitter_point(position[u], position[v],
                                            origin, extent);

        // DPG TBD: Can we use the Topology concept's
        // distance/move_position_toward to handle this?
        Point delta = detail::point_difference(position[v], position[u]);
        Dim dist = detail::point_norm(delta);
        Dim fa = attractive_force(*e, k, dist, g);

        for (std::size_t dim = 0; dim < num_dimensions; ++dim) {
          displacement[v][dim] -= delta[dim] / dist * fa;
          displacement[u][dim] += delta[dim] / dist * fa;
        }
      }
    }

    if (Dim temp = cool()) {
      // Update positions
      for (tie(v, v_end) = vertices(g); v != v_end; ++v) {
        BOOST_USING_STD_MIN();
        BOOST_USING_STD_MAX();
        Dim disp_size = detail::point_norm(displacement[*v]);

        for (std::size_t dim = 0; dim < num_dimensions; ++dim) {
          position[*v][dim] += displacement[*v][dim] / disp_size
                             * (min)(disp_size, temp);
          position[*v][dim] = (min)(origin[dim] + extent[dim],
                                    (max)(origin[dim], position[*v][dim]));
        }
      }
    } else {
      break;
    }
  } while (true);
}

namespace detail {
  template<typename DisplacementMap>
  struct fr_force_directed_layout
  {
    template<typename Graph, typename PositionMap,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        typename property_traits<PositionMap>::value_type const& origin,
        typename property_traits<PositionMap>::value_type const& extent,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        DisplacementMap displacement,
        const bgl_named_params<Param, Tag, Rest>&)
    {
      fruchterman_reingold_force_directed_layout
        (g, position, origin, extent, attractive_force, repulsive_force,
         force_pairs, cool, displacement);
    }
  };

  template<>
  struct fr_force_directed_layout<error_property_not_found>
  {
    template<typename Graph, typename PositionMap,
             typename AttractiveForce, typename RepulsiveForce,
             typename ForcePairs, typename Cooling,
             typename Param, typename Tag, typename Rest>
    static void
    run(const Graph&    g,
        PositionMap     position,
        typename property_traits<PositionMap>::value_type const& origin,
        typename property_traits<PositionMap>::value_type const& extent,
        AttractiveForce attractive_force,
        RepulsiveForce  repulsive_force,
        ForcePairs      force_pairs,
        Cooling         cool,
        error_property_not_found,
        const bgl_named_params<Param, Tag, Rest>& params)
    {
      typedef typename property_traits<PositionMap>::value_type Point;
      std::vector<Point> displacements(num_vertices(g));
      fruchterman_reingold_force_directed_layout
        (g, position, origin, extent, attractive_force, repulsive_force,
         force_pairs, cool,
         make_iterator_property_map
         (displacements.begin(),
          choose_const_pmap(get_param(params, vertex_index), g,
                            vertex_index),
          Point()));
    }
  };

} // end namespace detail

template<typename Graph, typename PositionMap, typename Param,
         typename Tag, typename Rest>
void
fruchterman_reingold_force_directed_layout
  (const Graph&    g,
   PositionMap     position,
   typename property_traits<PositionMap>::value_type const& origin,
   typename property_traits<PositionMap>::value_type const& extent,
   const bgl_named_params<Param, Tag, Rest>& params)
{
  typedef typename property_value<bgl_named_params<Param,Tag,Rest>,
                                  vertex_displacement_t>::type D;

  detail::fr_force_directed_layout<D>::run
    (g, position, origin, extent,
     choose_param(get_param(params, attractive_force_t()),
                  square_distance_attractive_force()),
     choose_param(get_param(params, repulsive_force_t()),
                  square_distance_repulsive_force()),
     choose_param(get_param(params, force_pairs_t()),
                  make_grid_force_pairs(origin, extent, position, g)),
     choose_param(get_param(params, cooling_t()),
                  linear_cooling<double>(100)),
     get_param(params, vertex_displacement_t()),
     params);
}

template<typename Graph, typename PositionMap>
void
fruchterman_reingold_force_directed_layout
  (const Graph&    g,
   PositionMap     position,
   typename property_traits<PositionMap>::value_type const& origin,
   typename property_traits<PositionMap>::value_type const& extent)
{
  fruchterman_reingold_force_directed_layout
    (g, position, origin, extent,
     attractive_force(square_distance_attractive_force()));
}

} // end namespace boost

#ifdef BOOST_GRAPH_IS_PARALLEL
#  include <boost/graph/distributed/fruchterman_reingold.hpp>
#endif // BOOST_GRAPH_IS_PARALLEL

#endif // BOOST_GRAPH_FRUCHTERMAN_REINGOLD_FORCE_DIRECTED_LAYOUT_HPP


#include <unsupported/Eigen/MatrixFunctions>
#include <Eigen/Eigenvalues>
#include "BlockMatrix.h"

BlockMatrix::BlockMatrix(int L) {
	init(L);
}

BlockMatrix::BlockMatrix(const BlockMatrix& bm) {
	init(bm._L);
	for (int i = 0; i < bm.size(); i++) {
		for (int j = 0; j < bm.size(); j++) {
			_mats[i][j] = bm._mats[i][j];
		}
	}
}

BlockMatrix::BlockMatrix(int L, const Array& diag) {
	init(L);
	std::vector<int> index = buildIndex(_L);
	for (int i = 0; i < size(); i++) {
		_mats[i][i] = diag.block(index[i], 0, StateCollection::Inst()->StateNumber(_L, i), 1).matrix().asDiagonal();
	}
}

BlockMatrix::~BlockMatrix() {
/*	for (size_t i = 0; i < _mats.size(); i++) {
		for (size_t j = 0; j < _mats.size(); j++) {
			if (_mats[i][j] != NULL) {
				delete _mats[i][j];
			}
		}
	}*/
}

void BlockMatrix::init(int L) {
	this->_L = L;
	_mats.resize((size_t)L + 1, std::vector<Matrix>(L + 1));
}

std::vector<int> BlockMatrix::buildIndex(int L) {
	StateCollection* inst = StateCollection::Inst();
	std::vector<int> index(L + 1);
	index[0] = 0;
	for (int i = 1; i <= L; i++) {
		index[i] = inst->StateNumber(L, i - 1) + index[i - 1];
	}

	return index;
}

bool BlockMatrix::isLadder(int* ladderSize) const {
	int diff = 0;
	bool first = true;
	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (empty(i, j)) continue;
			if (first) {
				diff = i - j;
				first = false;
			}
			else {
				if (i - j != diff) {
					return false;
				}
			}
		}
	}

	*ladderSize = diff;
	return true;
}

void BlockMatrix::fromMixing(const Array& left, const BlockMatrix& center, const Array& right) {
	init(center._L);

	std::vector<int> index = buildIndex(_L);
	for (int i = 0; i < center.size(); i++) {
		for (int j = 0; j < center.size(); j++) {
			if (center.empty(i, j)) continue;
			_mats[i][j].resize(center._mats[i][j].rows(), center._mats[i][j].cols());
			_mats[i][j].fill(0.0);

			for (int x = 0; x < _mats[i][j].rows(); x++) {
				for (int y = 0; y < _mats[i][j].cols(); y++) {
					_mats[i][j](x, y) = left(x + index[i]) * center._mats[i][j](x, y) * right(y + index[j]);
				}
//				_mats[i][j].row(x) = center._mats[i][j].row(x) * left(x + index[i]);
			}
//			for (int y = 0; y < _mats[i][j].cols(); y++) {
//				_mats[i][j].col(y) *= right(y + index[j]);
//			}
		}
	}
}

void BlockMatrix::mixWith(const Array& left, const Array& right) {
	std::vector<int> index = buildIndex(_L);
	for (int i = 0; i < this->size(); i++) {
		for (int j = 0; j < this->size(); j++) {
			if (empty(i, j)) continue;

			for (int x = 0; x < _mats[i][j].rows(); x++) {
				_mats[i][j].row(x) *= left(x + index[i]);
			}
			for (int y = 0; y < _mats[i][j].cols(); y++) {
				_mats[i][j].col(y) *= right(y + index[j]);
			}
		}
	}
}

void BlockMatrix::mixWith(const BlockMatrix& left, const BlockMatrix& right) {
	for (int i = 0; i < this->size(); i++) {
		for (int j = 0; j < this->size(); j++) {
			if (empty(i, j)) continue;
			_mats[i][j] = left._mats[i][i] * _mats[i][j] * right._mats[j][j];
		}
	}
}

void BlockMatrix::fromMixing(const BlockMatrix& left, const Array& diag, const BlockMatrix& right) {
	if (left._L != right._L) {
		throw OtocException("Dimension of left and right matrices are different.");
	}

	init(left._L);
	std::vector<int> index = buildIndex(_L);
	for (int i = 0; i < left.size(); i++) {
		for (int k = 0; k < left.size(); k++) {
			if (left.empty(i, k)) continue;
			Matrix tmp = Matrix::Zero(left._mats[i][k].rows(), left._mats[i][k].cols());
			for (int h = 0; h < tmp.cols(); h++) {
				tmp.col(h) = left._mats[i][k].col(h) * diag(h + index[k]);
			}

			for (int j = 0; j < right.size(); j++) {
				if (right.empty(k, j)) continue;
				if (this->empty(i, j)) {
					this->fill(i, j);
				}
				this->_mats[i][j].noalias() += tmp * right._mats[k][j];
			}
		}
	}
}

void BlockMatrix::setZeroAt(const std::vector<int>& zeroPos) {
	std::vector<int> index = buildIndex(_L);
	StateCollection* inst = StateCollection::Inst();
	int x = 0;
	for (int i = 0; i < zeroPos.size(); i++) {
		if (zeroPos[i] >= index[x] + inst->StateNumber(_L, x)) x++;
//		std::cout << "i=" << i << ", x=" << x << ", state#=" << inst->StateNumber(_L, x) << ", index[x]=" << index[x] << std::endl;
		int y = 0;
		for (int j = 0; j < zeroPos.size(); j++) {
			if (zeroPos[j] >= index[y] + inst->StateNumber(_L, y)) y++;
//			std::cout << "j=" << j << ", y=" << y << ", state#=" << inst->StateNumber(_L, y) << ", index[y]=" << index[y] << std::endl;
			if (empty(x, y)) {
				continue;
			}
			_mats[x][y](zeroPos[i] - index[x], zeroPos[j] - index[y]) = 0;
		}
	}
}

void BlockMatrix::diagonalize(Array& eigenValues, BlockMatrix& eigenVectors, BlockMatrix& eigenVectorsInv) const {
	eigenValues.resize(StateCollection::Inst()->StateNumber(_L));
	eigenVectors.init(_L);
	eigenVectorsInv.init(_L);

	std::vector<int> index = buildIndex(_L);
	Eigen::ComplexEigenSolver<Matrix> solver;
	for (int i = 0; i < size(); i++) {
		int n = _mats[i][i].rows();
		solver.compute(_mats[i][i], true);
		eigenValues.block(index[i], 0, n, 1) = solver.eigenvalues();
		eigenVectors._mats[i][i] = solver.eigenvectors();
		eigenVectorsInv._mats[i][i] = eigenVectors._mats[i][i].inverse();
	}
}

Matrix BlockMatrix::matrix() const {
	std::vector<int> index = buildIndex(_L);
	int size = StateCollection::Inst()->StateNumber(_L);
	Matrix ret = Matrix::Zero(size, size);
	for (int i = 0; i < this->size(); i++) {
		for (int j = 0; j < this->size(); j++) {
			if (this->empty(i, j)) {
				continue;
			}

			ret.block(index[i], index[j], this->_mats[i][j].rows(), this->_mats[i][j].cols()) = this->_mats[i][j];
		}
	}

	return ret;
}

void BlockMatrix::fill(int i, int j) {
	StateCollection* inst = StateCollection::Inst();
	_mats[i][j] = Matrix::Zero(inst->StateNumber(_L, i), inst->StateNumber(_L, j));
//	_mats[i][j]->fill(0.0);
}

Matrix& BlockMatrix::block(int i, int j) {
	return _mats[i][j];
}

BlockMatrix& BlockMatrix::operator += (const BlockMatrix& other) {
	if (this->_L != other._L) {
		throw OtocException("Matrix with different dimensions cannot add.");
	}

	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (other.empty(i, j)) {
				continue;
			}
			if (this->empty(i, j)) {
				_mats[i][j] = other._mats[i][j];
			}
			else {
				_mats[i][j] += other._mats[i][j];
			}
		}
	}

	return *this;
}

BlockMatrix& BlockMatrix::operator -= (const BlockMatrix& other) {
	if (this->_L != other._L) {
		throw OtocException("Matrix with different dimensions cannot add.");
	}

	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (other.empty(i, j)) {
				continue;
			}
			if (this->empty(i, j)) {
				_mats[i][j] = -other._mats[i][j];
			}
			else {
				_mats[i][j] -= other._mats[i][j];
			}
		}
	}

	return *this;
}

BlockMatrix BlockMatrix::operator - (const BlockMatrix& other) const {
	if (this->_L != other._L) {
		throw OtocException("Matrix with different dimensions cannot multiply.");
	}

	BlockMatrix res(this->size() - 1);

	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (this->empty(i, j) && other.empty(i, j)) {
				continue;
			}
			if (other.empty(i, j)) {
				res._mats[i][j] = this->_mats[i][j];
			}
			else if (this->empty(i, j)) {
				res._mats[i][j] = other._mats[i][j];
			}
			else {
				res._mats[i][j] = this->_mats[i][j] - other._mats[i][j];
			}
		}
	}

	return res;
}

BlockMatrix BlockMatrix::operator * (const BlockMatrix& other) const {
	if (this->_L != other._L) {
		throw OtocException("Matrix with different dimensions cannot multiply.");
	}

	BlockMatrix res(this->_L);

	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			for (int k = 0; k < size(); k++) {
				if (this->empty(i, k) || other.empty(k, j)) {
					continue;
				}
				if (res.empty(i, j)) {
					res.fill(i, j);
				}

				res._mats[i][j].noalias() += this->_mats[i][k] * other._mats[k][j];
			}
		}
	}

	return res;
}

void BlockMatrix::multiplyByLadder(const BlockMatrix& ladder, int ladderSize) {
	for (int i = 0; i < size(); i++) {
		if (ladderSize >= 0) {
			for (int j = 0; j < size(); j++) {
				int k = j + ladderSize;
				if (k >= size()) {
					_mats[i][j] = Matrix();
				}
				else {
					_mats[i][j] = _mats[i][k] * ladder._mats[k][j];
				}
			}
		}
		else {
			for (int j = size() - 1; j >= 0; j--) {
				int k = j + ladderSize;
				if (k < 0) {
					_mats[i][j] = Matrix();
				}
				else {
					_mats[i][j] = _mats[i][k] * ladder._mats[k][j];
				}
			}
		}
	}
}

BlockMatrix BlockMatrix::operator * (const var_t& val) const {
	BlockMatrix res(this->size() - 1);

	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (this->empty(i, j)) continue;
			res._mats[i][j] = _mats[i][j] * val;
		}
	}

	return res;
}

BlockMatrix& BlockMatrix::operator *= (const var_t& val) {
	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (this->empty(i, j)) continue;
			_mats[i][j] *= val;
		}
	}

	return *this;
}

BlockMatrix& BlockMatrix::operator *= (const Array& diag) {
	std::vector<int> index = buildIndex(_L);
	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (empty(i, j)) continue;
			for (int x = 0; x < _mats[i][j].cols(); x++) {
				_mats[i][j].col(x) *= diag(index[j] + x);
			}
		}
	}

	return *this;
}

BlockMatrix BlockMatrix::operator * (const Array& diag) {
	BlockMatrix ret(_L);
	std::vector<int> index = buildIndex(_L);
	for (int i = 0; i < size(); i++) {
		for (int j = 0; j < size(); j++) {
			if (empty(i, j)) continue;
			ret.fill(i, j);
			for (int x = 0; x < _mats[i][j].cols(); x++) {
				ret._mats[i][j].col(x) = _mats[i][j].col(x) * diag(index[j] + x);
			}
		}
	}

	return ret;
}

var_t BlockMatrix::trace() const {
	var_t ret(0.0);
	for (int i = 0; i < size(); i++) {
		if (!empty(i, i)) {
			ret += _mats[i][i].trace();
		}
	}

	return ret;
}

BlockMatrix BlockMatrix::exp() const {
	int ladderSize;
	if (!isLadder(&ladderSize)) {
		throw OtocException("Only ladder-like matrix can be exponentiated.");
	}

	BlockMatrix res(this->size() - 1);
	StateCollection* inst = StateCollection::Inst();
	if (ladderSize == 0) {
		for (int i = 0; i < res.size(); i++) {
			if (this->empty(i, i)) {
				res._mats[i][i] = Matrix::Identity(inst->StateNumber(this->_L, i), inst->StateNumber(this->_L, i));
			}
			else {
				res._mats[i][i] = _mats[i][i].exp();
			}
		}
	}
	else {
		Array id = Array::Constant(inst->StateNumber(_L), 1);
		BlockMatrix mat(_L, id);
		res += mat;
		for (int i = 1; i * std::abs(ladderSize) <= _L; i++) {
			mat.multiplyByLadder(*this, ladderSize);
			mat *= 1 / (f_type)i;
			res += mat;
		}
	}

	return res;
}

BlockMatrix BlockMatrix::conjugate() const {
	BlockMatrix res(this->size() - 1);
	for (int i = 0; i < res.size(); i++) {
		for (int j = 0; j < res.size(); j++) {
			if (this->empty(i, j)) continue;
			res._mats[i][j] = _mats[i][j].conjugate();
		}
	}

	return res;
}

BlockMatrix BlockMatrix::adjoint() const {
	BlockMatrix res(this->size() - 1);
	for (int i = 0; i < res.size(); i++) {
		for (int j = 0; j < res.size(); j++) {
			if (this->empty(i, j)) continue;
			res._mats[j][i] = _mats[i][j].adjoint();
		}
	}

	return res;
}

BlockMatrix& BlockMatrix::adjointInPlace() {
	for (int i = 0; i < size(); i++) {
		for (int j = i; j < size(); j++) {
			if (this->empty(i, j) && this->empty(j, i)) continue;
			if (i == j) {
				_mats[i][j].adjointInPlace();
			} else if (this->empty(i, j)) {
				_mats[i][j] = _mats[j][i].adjoint();
				_mats[j][i] = Matrix();
			}
			else if (this->empty(j, i)){
				_mats[j][i] = _mats[i][j].adjoint();
				_mats[i][j] = Matrix();
			}
			else {
				Matrix tmp = _mats[i][j];
				_mats[i][j] = _mats[j][i].adjoint();
				_mats[j][i] = tmp.adjoint();
/*				for (int x = 0; x < _mats[i][j].rows(); x++) {
					for (int y = 0; y < _mats[i][j].cols(); y++) {
						var_t tmp = _mats[i][j](x, y);
						_mats[i][j](x, y) = std::conj(_mats[j][i](y, x));
						_mats[j][i](y, x) = std::conj(tmp);
					}
				}*/
			}
		}
	}

	return *this;
}

BlockMatrix BlockMatrix::inverse() const {
	BlockMatrix res(this->size() - 1);
	for (int i = 0; i < res.size(); i++) {
		res._mats[i][i] = _mats[i][i].inverse();
	}

	return res;
}

BlockMatrix& BlockMatrix::inverseInPlace() {
	for (int i = 0; i < size(); i++) {
		_mats[i][i] = _mats[i][i].inverse();
	}

	return *this;
}

Array BlockMatrix::diagonal() const {
	std::vector<int> index = buildIndex(_L);
	StateCollection* inst = StateCollection::Inst();
	Array ret = Array::Zero(inst->StateNumber(this->_L));
	for (int m = 0; m <= this->_L; m++) {
		if (empty(m, m)) continue;
		ret.block(index[m], 0, _mats[m][m].rows(), 1) = _mats[m][m].diagonal().array();
	}

	return ret;
}

ostream& operator << (ostream& os, const BlockMatrix& bm) {
	for (int i = 0; i < bm.size(); i++) {
		for (int j = 0; j < bm.size(); j++) {
			if (bm.empty(i, j)) continue;
			os << "block(" << i << ", " << j << ")=" << std::endl;
			os << bm._mats[i][j] << std::endl;
		}
	}

	return os;
}