#include "sbs/common/primitive.h"

#include <Eigen/Geometry>

namespace sbs {
namespace common {

line_segment_t::line_segment_t(point_t const& p, point_t const& q) : p(p), q(q) {}

triangle_t::triangle_t(point_t const& a, point_t const& b, point_t const& c) : p_{a, b, c} {}

normal_t triangle_t::normal() const
{
    Eigen::Vector3d const ab = b() - a();
    Eigen::Vector3d const ac = c() - a();
    return ab.cross(ac).normalized();
}

double triangle_t::area() const
{
    return 0.5 * (b() - a()).cross(c() - a()).norm();
}

std::array<line_segment_t, 3u> triangle_t::edges() const
{
    return std::array<line_segment_t, 3u>{
        line_segment_t{p1(), p2()},
        line_segment_t{p2(), p3()},
        line_segment_t{p3(), p1()}};
}

std::array<point_t, 3u> const& triangle_t::nodes() const
{
    return p_;
}

std::array<point_t, 3u>& triangle_t::nodes()
{
    return p_;
}

point_t const& triangle_t::p1() const
{
    return p_[0];
}

point_t const& triangle_t::p2() const
{
    return p_[1];
}

point_t const& triangle_t::p3() const
{
    return p_[2];
}

point_t& triangle_t::p1()
{
    return p_[0];
}

point_t& triangle_t::p2()
{
    return p_[1];
}

point_t& triangle_t::p3()
{
    return p_[2];
}

point_t const& triangle_t::a() const
{
    return p1();
}

point_t const& triangle_t::b() const
{
    return p2();
}

point_t const& triangle_t::c() const
{
    return p3();
}

point_t& triangle_t::a()
{
    return p1();
}

point_t& triangle_t::b()
{
    return p2();
}

point_t& triangle_t::c()
{
    return p3();
}

ray_t::ray_t(point_t const& p, direction_t const& v) : p(p), v(v) {}

sphere_t::sphere_t(point_t const& center, double radius) : center(center), radius(radius) {}

sphere_t sphere_t::from(tetrahedron_t const& t)
{
    common::point_t const approx_barycenter =
        0.25 * t.p1() + 0.25 * t.p2() + 0.25 * t.p3() + 0.25 * t.p4();
    auto const d1     = (approx_barycenter - t.p1()).norm();
    auto const d2     = (approx_barycenter - t.p2()).norm();
    auto const d3     = (approx_barycenter - t.p3()).norm();
    auto const d4     = (approx_barycenter - t.p4()).norm();
    auto const radius = std::max({d1, d2, d3, d4});
    sphere_t const sphere{approx_barycenter, radius};
    return sphere;
}

sphere_t sphere_t::from(triangle_t const& t)
{
    common::point_t const approx_barycenter = 0.33 * t.p1() + 0.33 * t.p2() + 0.34 * t.p3();
    auto const d1                           = (approx_barycenter - t.p1()).norm();
    auto const d2                           = (approx_barycenter - t.p2()).norm();
    auto const d3                           = (approx_barycenter - t.p3()).norm();
    auto const radius                       = std::max({d1, d2, d3});
    sphere_t const sphere{approx_barycenter, radius};
    return sphere;
}

bool operator==(line_segment_t const& l1, line_segment_t const& l2)
{
    return l1.p.isApprox(l2.p) && l1.q.isApprox(l2.q);
}

bool operator!=(line_segment_t const& l1, line_segment_t const& l2)
{
    return !(l1 == l2);
}

line_segment_t operator+(line_segment_t const& l, vector3d_t const& t)
{
    return line_segment_t(l.p + t, l.q + t);
}

line_segment_t operator+(vector3d_t const& t, line_segment_t const& l)
{
    return l + t;
}

line_segment_t operator*(Eigen::Matrix3d const& R, line_segment_t const& l)
{
    return line_segment_t(R * l.p, R * l.q);
}

std::tuple<double, double, double>
barycentric_coordinates(point_t const& A, point_t const& B, point_t const& C, point_t const& p)
{
    Eigen::Vector3d const v0 = B - A;
    Eigen::Vector3d const v1 = C - A;
    Eigen::Vector3d const v2 = p - A;

    Eigen::Vector3d const AB = B - A;
    Eigen::Vector3d const AC = C - A;
    Eigen::Vector3d const AP = p - A;

    double const d00   = AB.dot(AB);
    double const d01   = AB.dot(AC);
    double const d11   = AC.dot(AC);
    double const d20   = AP.dot(AB);
    double const d21   = AP.dot(AC);
    double const denom = d00 * d11 - d01 * d01;
    double const v     = (d11 * d20 - d01 * d21) / denom;
    double const w     = (d00 * d21 - d01 * d20) / denom;
    double const u     = 1.0 - v - w;

    return std::make_tuple(u, v, w);
}

bool intersects(tetrahedron_t const& t1, tetrahedron_t const& t2)
{
    std::array<common::normal_t, 44u> separating_axis{};
    auto const& t1_triangles = t1.faces();
    auto const& t2_triangles = t2.faces();

    auto const face_axis_transform = [](common::triangle_t const& f) {
        return f.normal();
    };

    auto separating_axis_it = std::transform(
        t1_triangles.begin(),
        t1_triangles.end(),
        separating_axis.begin(),
        face_axis_transform);
    separating_axis_it = std::transform(
        t2_triangles.begin(),
        t2_triangles.end(),
        separating_axis_it,
        face_axis_transform);

    auto const t1_edges = t1.edges();
    auto const t2_edges = t2.edges();

    for (auto const& edge : t1_edges)
    {
        auto const edge_edge_separating_axis_transform_op =
            [&edge](common::line_segment_t const& e) {
                Eigen::Vector3d const d1 = edge.q - edge.p;
                Eigen::Vector3d d2       = e.q - e.p;

                auto axis = d1.cross(d2);

                double constexpr eps = std::numeric_limits<double>::epsilon();
                // Check if edges are parallel up to numerical precision eps
                if (axis.isZero(eps))
                {
                    d2   = e.p - edge.p;
                    axis = d1.cross(d2);
                }
                // Check if edges are on the same line up to numerical precision eps
                if (axis.isZero(eps))
                {
                    // Cancel this axis as a potential separating axis. When projecting
                    // nodes onto this axis, everything will be zeroed out, and thus no
                    // separating interval can be found. The separating axis test for
                    // this axis will fail.
                    axis.setZero();
                }

                return axis;
            };
        separating_axis_it = std::transform(
            t2_edges.begin(),
            t2_edges.end(),
            separating_axis_it,
            edge_edge_separating_axis_transform_op);
    }

    auto const is_separating_axis = [&t1, &t2](common::normal_t const& axis) {
        if (axis.isZero())
            return false;

        auto const project = [axis](common::point_t const& p) {
            return p.dot(axis);
        };

        std::array<double, 4u> const projection1{
            project(t1.p1()),
            project(t1.p2()),
            project(t1.p3()),
            project(t1.p4())};

        std::array<double, 4u> const projection2{
            project(t2.p1()),
            project(t2.p2()),
            project(t2.p3()),
            project(t2.p4())};

        auto const [min_it1, max_it1] = std::minmax_element(projection1.begin(), projection1.end());
        auto const [min_it2, max_it2] = std::minmax_element(projection2.begin(), projection2.end());

        bool const has_separating_interval = (*max_it1 < *min_it2) || (*max_it2 < *min_it1);
        return has_separating_interval;
    };

    return std::none_of(separating_axis.begin(), separating_axis.end(), is_separating_axis);
}

bool intersects(triangle_t const& triangle, tetrahedron_t const& tetrahedron)
{
    std::array<common::normal_t, 23u> separating_axis{};
    auto const& tet_triangles = tetrahedron.faces();

    auto const face_axis_transform = [](common::triangle_t const& f) {
        return f.normal();
    };

    separating_axis.front() = triangle.normal();

    auto separating_axis_it = separating_axis.begin() + 1u;
    separating_axis_it      = std::transform(
        tet_triangles.begin(),
        tet_triangles.end(),
        separating_axis_it,
        face_axis_transform);

    auto const t1_edges = triangle.edges();
    auto const t2_edges = tetrahedron.edges();

    for (auto const& edge : t1_edges)
    {
        auto const edge_edge_separating_axis_transform_op =
            [&edge](common::line_segment_t const& e) {
                Eigen::Vector3d const d1 = edge.q - edge.p;
                Eigen::Vector3d d2       = e.q - e.p;

                auto axis = d1.cross(d2);

                double constexpr eps = std::numeric_limits<double>::epsilon();
                // Check if edges are parallel up to numerical precision eps
                if (axis.isZero(eps))
                {
                    d2   = e.p - edge.p;
                    axis = d1.cross(d2);
                }
                // Check if edges are on the same line up to numerical precision eps
                if (axis.isZero(eps))
                {
                    // Cancel this axis as a potential separating axis. When projecting
                    // nodes onto this axis, everything will be zeroed out, and thus no
                    // separating interval can be found. The separating axis test for
                    // this axis will fail.
                    axis.setZero();
                }

                return axis;
            };
        separating_axis_it = std::transform(
            t2_edges.begin(),
            t2_edges.end(),
            separating_axis_it,
            edge_edge_separating_axis_transform_op);
    }

    auto const is_separating_axis = [&triangle, &tetrahedron](common::normal_t const& axis) {
        if (axis.isZero())
            return false;

        auto const project = [axis](common::point_t const& p) {
            return p.dot(axis);
        };

        std::array<double, 3u> const projection1{
            project(triangle.p1()),
            project(triangle.p2()),
            project(triangle.p3())};

        std::array<double, 4u> const projection2{
            project(tetrahedron.p1()),
            project(tetrahedron.p2()),
            project(tetrahedron.p3()),
            project(tetrahedron.p4())};

        auto const [min_it1, max_it1] = std::minmax_element(projection1.begin(), projection1.end());
        auto const [min_it2, max_it2] = std::minmax_element(projection2.begin(), projection2.end());

        bool const has_separating_interval = (*max_it1 < *min_it2) || (*max_it2 < *min_it1);
        return has_separating_interval;
    };

    return std::none_of(separating_axis.begin(), separating_axis.end(), is_separating_axis);
}

bool intersects(point_t const& point, tetrahedron_t const& tetrahedron)
{
    auto const project = [](common::point_t const& p, common::triangle_t const& triangle) {
        auto const n = triangle.normal();
        auto const d = p - triangle.p1();
        return n.dot(d);
    };
    std::array<common::triangle_t, 4u> const faces = tetrahedron.faces();
    std::array<double, 4u> const projections{
        project(point, faces[0]),
        project(point, faces[1]),
        project(point, faces[2]),
        project(point, faces[3])};

    return std::none_of(projections.begin(), projections.end(), [](double const s) {
        return s > 0.;
    });
}

std::optional<point_t> intersect(line_segment_t const& segment, triangle_t const& triangle)
{
    Eigen::Vector3d const ab = triangle.b() - triangle.a();
    Eigen::Vector3d const ac = triangle.c() - triangle.a();
    Eigen::Vector3d const qp = segment.p - segment.q;

    Eigen::Vector3d const n = ab.cross(ac);

    double const d = qp.dot(n);
    if (d <= 0.)
        return {};

    Eigen::Vector3d const ap = segment.p - triangle.a();
    double const t           = ap.dot(n);
    if (t < 0.)
        return {};
    if (t > d)
        return {};

    Eigen::Vector3d const e = qp.cross(ap);
    double v                = ac.dot(e);
    if (v < 0. || v > d)
        return {};

    double w = -ab.dot(e);
    if (w < 0. || (v + w) > d)
        return {};

    double const ood = 1. / d;
    v *= ood;
    w *= ood;
    double const u             = 1. - v - w;
    point_t const intersection = u * triangle.a() + v * triangle.b() + w * triangle.c();
    return intersection;
}

std::optional<point_t> intersect(ray_t const& ray, triangle_t const& triangle)
{
    Eigen::Vector3d const& p = ray.p;
    Eigen::Vector3d const& q = ray.p + 1. * ray.v;
    Eigen::Vector3d const ab = triangle.b() - triangle.a();
    Eigen::Vector3d const ac = triangle.c() - triangle.a();
    Eigen::Vector3d const qp = p - q;

    Eigen::Vector3d const n = ab.cross(ac);

    double const d = qp.dot(n);
    if (d <= 0.)
        return {};

    Eigen::Vector3d const ap = p - triangle.a();
    double const t           = ap.dot(n);
    if (t < 0.)
        return {};

    Eigen::Vector3d const e = qp.cross(ap);
    double v                = ac.dot(e);
    if (v < 0. || v > d)
        return {};

    double w = -ab.dot(e);
    if (w < 0. || (v + w) > d)
        return {};

    double const ood = 1. / d;
    // t *= ood;
    v *= ood;
    w *= ood;
    double const u             = 1. - v - w;
    point_t const intersection = u * triangle.a() + v * triangle.b() + w * triangle.c();
    return intersection;
}

std::optional<point_t> intersect_twoway(line_segment_t const& segment, triangle_t const& triangle)
{
    auto const intersection = intersect(segment, triangle);
    if (intersection.has_value())
        return intersection;

    line_segment_t const flipped_segment{segment.q, segment.p};
    return intersect(flipped_segment, triangle);
}

std::optional<point_t> intersect_twoway(ray_t const& ray, triangle_t const& triangle)
{
    auto const intersection = intersect(ray, triangle);
    if (intersection.has_value())
        return intersection;

    triangle_t const flipped_triangle{triangle.a(), triangle.c(), triangle.b()};
    return intersect(ray, flipped_triangle);
}

std::optional<point_t> intersect(line_segment_t const& segment, plane_t const& plane)
{
    double const d           = plane.p.dot(plane.n);
    Eigen::Vector3d const pq = segment.q - segment.p;
    double const t           = (d - plane.n.dot(segment.p)) / (plane.n.dot(pq));

    if (t >= 0.0 && t <= 1.0)
    {
        Eigen::Vector3d const intersection = segment.p + t * pq;
        return intersection;
    }
    return {};
}

/**
 * @brief
 * Implementation of closest point on triangle to a point P from Christer Ericson's Real-Time
 * Collision Detection
 * @param p Point off triangle
 * @param t Triangle on which we wish to find the closest point to p
 * @return The closest point q on triangle t to point p
 */
point_t closest_point(point_t const& p, triangle_t const& t)
{
    auto const& a = t.a();
    auto const& b = t.b();
    auto const& c = t.c();

    // Check if P in vertex region outside A
    common::vector3d_t const ab = b - a;
    common::vector3d_t const ac = c - a;
    common::vector3d_t const ap = p - a;
    double const d1             = ab.dot(ap);
    double const d2             = ac.dot(ap);
    if (d1 <= 0.0 && d2 <= 0.0)
        return a; // barycentric coordinates (1,0,0)

    // Check if P in vertex region outside B
    common::vector3d_t const bp = p - b;
    double const d3             = ab.dot(bp);
    double const d4             = ac.dot(bp);
    if (d3 >= 0.0 && d4 <= d3)
        return b; // barycentric coordinates (0,1,0)

    // Check if P in edge region of AB, if so return projection of P onto AB
    double const vc = d1 * d4 - d3 * d2;
    if (vc <= 0.0 && d1 >= 0.0 && d3 <= 0.0)
    {
        double const v = d1 / (d1 - d3);
        return a + v * ab; // barycentric coordinates (1-v, v,0)
    }

    // Check if P in vertex region outside C
    common::vector3d_t const cp = p - c;
    double const d5             = ab.dot(cp);
    double const d6             = ac.dot(cp);
    if (d6 >= 0.0 && d5 <= d6)
        return c; // barycentric coordinates (0,0,1)

    // Check if P in edge region of AC, if so return projection of P onto AC
    double const vb = d5 * d2 - d1 * d6;
    if (vb <= 0.0 && d2 >= 0.0 && d6 <= 0.0)
    {
        double const w = d2 / (d2 - d6);
        return a + w * ac; // barycentric coordinates (1-w, 0, w)
    }

    // Check if P in edge region of BC, if so return projection of P onto BC
    double const va = d3 * d6 - d5 * d4;
    if (va <= 0.0 && (d4 - d3) >= 0.0 && (d5 - d6) >= 0.0)
    {
        double const w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
        return b + w * (c - b); // barycentric coordinates (0,1-w, w)
    }

    // P inside face region. Compute Q through its barycentric coordinates (u, v, w)
    double const denom = 1.0 / (va + vb + vc);
    double const v     = vb * denom;
    double const w     = vc * denom;
    return a + ab * v + ac * w; //=u*a+v*b+w*c,u=va* denom=1.0f−v−w
}

tetrahedron_t::tetrahedron_t(
    point_t const& p1,
    point_t const& p2,
    point_t const& p3,
    point_t const& p4)
    : p_{p1, p2, p3, p4}
{
}

double tetrahedron_t::unsigned_volume() const
{
    return std::abs(signed_volume());
}

double tetrahedron_t::signed_volume() const
{
    vector3d_t const p21 = p2() - p1();
    vector3d_t const p31 = p3() - p1();
    vector3d_t const p41 = p4() - p1();
    return p21.cross(p31).dot(p41);
}

std::array<triangle_t, 4u> tetrahedron_t::faces() const
{
    return std::array<triangle_t, 4u>{
        triangle_t{p1(), p2(), p4()},
        triangle_t{p2(), p3(), p4()},
        triangle_t{p3(), p1(), p4()},
        triangle_t{p1(), p3(), p2()}};
}

std::array<line_segment_t, 6u> tetrahedron_t::edges() const
{
    return std::array<line_segment_t, 6u>{
        line_segment_t{p1(), p2()},
        line_segment_t{p2(), p3()},
        line_segment_t{p3(), p1()},
        line_segment_t{p1(), p4()},
        line_segment_t{p2(), p4()},
        line_segment_t{p3(), p4()},
    };
}

std::array<point_t, 4u> const& tetrahedron_t::nodes() const
{
    return p_;
}

std::array<point_t, 4u>& tetrahedron_t::nodes()
{
    return p_;
}

point_t const& tetrahedron_t::p1() const
{
    return p_[0];
}

point_t const& tetrahedron_t::p2() const
{
    return p_[1];
}

point_t const& tetrahedron_t::p3() const
{
    return p_[2];
}

point_t const& tetrahedron_t::p4() const
{
    return p_[3];
}

point_t& tetrahedron_t::p1()
{
    return p_[0];
}

point_t& tetrahedron_t::p2()
{
    return p_[1];
}

point_t& tetrahedron_t::p3()
{
    return p_[2];
}

point_t& tetrahedron_t::p4()
{
    return p_[3];
}

aabb_t::aabb_t(point_t const& min, point_t const& max) : min(min), max(max) {}

bool aabb_t::contains(point_t const& p) const
{
    return (p.x() >= min.x() && p.x() <= max.x()) && (p.y() >= min.y() && p.y() <= max.y()) &&
           (p.z() >= min.z() && p.z() <= max.z());
}

aabb_t aabb_t::from(tetrahedron_t const& t)
{
    constexpr double inf = std::numeric_limits<double>::infinity();
    point_t min{inf, inf, inf}, max{-inf, -inf, -inf};
    for (auto const& p : t.nodes())
    {
        if (p.x() > max.x())
            max.x() = p.x();
        if (p.y() > max.y())
            max.y() = p.y();
        if (p.z() > max.z())
            max.z() = p.z();

        if (p.x() < min.x())
            min.x() = p.x();
        if (p.y() < min.y())
            min.y() = p.y();
        if (p.z() < min.z())
            min.z() = p.z();
    }
    return aabb_t{min, max};
}

aabb_t aabb_t::from(triangle_t const& t)
{
    constexpr double inf = std::numeric_limits<double>::infinity();
    point_t min{inf, inf, inf}, max{-inf, -inf, -inf};
    for (auto const& p : t.nodes())
    {
        if (p.x() > max.x())
            max.x() = p.x();
        if (p.y() > max.y())
            max.y() = p.y();
        if (p.z() > max.z())
            max.z() = p.z();

        if (p.x() < min.x())
            min.x() = p.x();
        if (p.y() < min.y())
            min.y() = p.y();
        if (p.z() < min.z())
            min.z() = p.z();
    }
    return aabb_t{min, max};
}

plane_t::plane_t(point_t const& p, normal_t const& n) : p(p), n(n) {}

plane_t::plane_t(triangle_t const& t) : p(t.a()), n()
{
    n = (t.b() - t.a()).cross(t.c() - t.a()).normalized();
}

double plane_t::signed_distance(point_t const& q) const
{
    return (q - p).dot(n);
}

} // namespace common
} // namespace sbs


/*
 *
 * Copyright (c) Toon Knapen, Karl Meerbergen & Kresimir Fresl 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_HBEV_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_HBEV_HPP

#include <boost/numeric/bindings/traits/type.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/lapack/workspace.hpp>
#include <boost/numeric/bindings/traits/detail/array.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits.hpp>
#endif

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // Eigendecomposition of a banded Hermitian matrix.
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * hbev() computes the eigenvalues and optionally the associated
     * eigenvectors of a banded Hermitian matrix A. A matrix is Hermitian
     * when herm( A ) == A. When A is real, a Hermitian matrix is also
     * called symmetric.
     *
     * The eigen decomposition is A = U S * herm(U)  where  U  is a
     * unitary matrix and S is a diagonal matrix. The eigenvalues of A
     * are on the main diagonal of S. The eigenvalues are real.
     *
     * Workspace is organized following the arguments in the calling sequence.
     *  optimal_workspace() : for optimizing use of blas 3 kernels
     *  minimal_workspace() : minimum size of workarrays, but does not allow for optimization
     *                        of blas 3 kernels
     *  workspace( work ) for real matrices where work is a real array with
     *                    vector_size( work ) >= 3*matrix_size1( a ) - 2
     *  workspace( work, rwork ) for complex matrices where work is a complex
     *                           array with vector_size( work ) >= matrix_size1( a )
     *                           and rwork is a real array with
     *                           vector_size( rwork ) >= 3 * matrix_size1( a ) - 2.
     */

    /*
     * If uplo=='L' only the lower triangular part is stored.
     * If uplo=='U' only the upper triangular part is stored.
     *
     * The matrix is assumed to be stored in LAPACK band format, i.e.
     * matrices are stored columnwise, in a compressed format so that when e.g. uplo=='U'
     * the (i,j) element with j>=i is in position  (i-j) + j * (KD+1) + KD  where KD is the
     * half bandwidth of the matrix. For a triadiagonal matrix, KD=1, for a diagonal matrix
     * KD=0.
     * When uplo=='L', the (i,j) element with j>=i is in position  (i-j) + j * (KD+1).
     *
     * The matrix A is thus a rectangular matrix with KD+1 rows and N columns.
     */

    namespace detail {
      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 float* ab, int const ldab, float* w, float* z, int const ldz,
                 float* work, int& info)
      {
	      //for (int i=0; i<n*kd; ++i) std::cout << *(ab+i) << " " ;
	      //std::cout << "\n" ;
        LAPACK_SSBEV (&jobz, &uplo, &n, &kd, ab, &ldab, w, z, &ldz,
                      work, &info);
      }

      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 double* ab, int const ldab, double* w, double* z, int const ldz,
                 double* work, int& info)
      {
        LAPACK_DSBEV (&jobz, &uplo, &n, &kd, ab, &ldab, w, z, &ldz,
                      work, &info);
      }

      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 traits::complex_f* ab, int const ldab, float* w,
                 traits::complex_f* z, int const ldz,
                 traits::complex_f* work, float* rwork, int& info)
      {
        LAPACK_CHBEV (&jobz, &uplo, &n, &kd, traits::complex_ptr(ab), &ldab,
                      w, traits::complex_ptr(z), &ldz,
                      traits::complex_ptr(work), rwork, &info);
      }

      inline
      void hbev (char const jobz, char const uplo, int const n, int const kd,
                 traits::complex_d* ab, int const ldab, double* w,
                 traits::complex_d* z, int const ldz,
                 traits::complex_d* work, double* rwork, int& info)
      {
        LAPACK_ZHBEV (&jobz, &uplo, &n, &kd, traits::complex_ptr(ab), &ldab,
                      w, traits::complex_ptr(z), &ldz,
                      traits::complex_ptr(work), rwork, &info);
      }
    }

    namespace detail {
       template <int N>
       struct Hbev{};

       /// Handling of workspace in the case of one workarray.
       template <>
       struct Hbev< 1 > {
          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, minimal_workspace , int& info ) const {
             traits::detail::array<T> work( 3*n-2 );
             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   info );
          }

          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, optimal_workspace , int& info ) const {
             traits::detail::array<T> work( 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   info );
          }

          template <typename T, typename R, typename W>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, detail::workspace1<W> work,
                           int& info ) const {
             assert( traits::vector_size( work.w_ ) >= 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work.w_ ),
                   info );
          }
       }; // Hbev< 1 >

       /// Handling of workspace in the case of two workarrays.
       template <>
       struct Hbev< 2 > {
          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, minimal_workspace , int& info ) const {
             traits::detail::array<T> work( n );
             traits::detail::array<R> rwork( 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   traits::vector_storage( rwork ),
                   info );
          }

          template <typename T, typename R>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, optimal_workspace , int& info ) const {
             traits::detail::array<T> work( n );
             traits::detail::array<R> rwork( 3*n-2 );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work ),
                   traits::vector_storage( rwork ),
                   info );
          }

          template <typename T, typename R, typename W, typename RW>
          void operator() (char const jobz, char const uplo, int const n,
                           int const kd, T* ab, int const ldab, R* w, T* z,
                           int const ldz, detail::workspace2<W,RW> work,
                           int& info ) const {
             assert( traits::vector_size( work.wr_ ) >= 3*n-2 );
             assert( traits::vector_size( work.w_ ) >= n );

             hbev( jobz, uplo, n, kd, ab, ldab, w, z, ldz,
                   traits::vector_storage( work.w_ ),
                   traits::vector_storage( work.wr_ ),
                   info );
          }
       }; // Hbev< 2 >

       /// Compute eigendecomposition of the banded Hermitian matrix ab.
       /// if jobz=='N' only the eigenvalues are computed.
       /// if jobz=='V' compute the eigenvalues a and the eigenvectors.
       ///
       /// Workspace is organized following the arguments in the calling sequence.
       ///  optimal_workspace() : for optimizing use of blas 3 kernels
       ///  minimal_workspace() : minimum size of workarrays, but does not allow for optimization
       ///                       of blas 3 kernels
       ///  workspace( work ) for real matrices where work is a real array with
       ///                    vector_size( work ) >= 3*matrix_size1( a )-2
       ///  workspace( work, rwork ) for complex matrices where work is a complex
       ///                           array with vector_size( work ) >= matrix_size1( a )
       ///                           and rwork is a real array with
       ///                           vector_size( rwork ) >= 3*matrix_size1( a )-2.
       template <typename AB, typename Z, typename W, typename Work>
       int hbev( char const jobz, AB& ab, W& w, Z& z, Work work ) {
#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<AB>::matrix_structure,
           traits::hermitian_t
         >::value));
#endif

         typedef typename AB::value_type                            value_type ;

         int const n = traits::matrix_size2 (ab);
         assert (n == traits::matrix_size1 (z));
         assert (n == traits::vector_size (w));
         assert ( jobz=='N' || jobz=='V' );

         int info ;
         detail::Hbev< n_workspace_args<value_type>::value >() (jobz,
                       traits::matrix_uplo_tag( ab ), n,
                       traits::matrix_upper_bandwidth(ab),
                       traits::matrix_storage (ab),
                       traits::leading_dimension (ab),
                       traits::vector_storage (w),
                       traits::matrix_storage (z),
                       traits::leading_dimension (z),
                       work, info);
	 return info ;
       } // hbev()

       } // namespace detail

       /// Compute eigendecomposition without eigenvectors
       template <typename AB, typename W, typename Work>
       inline
       int hbev (AB& ab, W& w, Work work) {
          return detail::hbev( 'N', ab, w, ab, work );
       } // hbev()

       /// Compute eigendecomposition with eigenvectors
       template <typename AB, typename W, typename Z, typename Work>
       inline
       int hbev (AB& ab, W& w, Z& z, Work work) {
         BOOST_STATIC_ASSERT((boost::is_same<
           typename traits::matrix_traits<Z>::matrix_structure,
           traits::general_t
         >::value));
         int const n = traits::matrix_size2 (ab);
          assert (n == traits::matrix_size1 (z));
          assert (n == traits::matrix_size2 (z));
          return detail::hbev( 'V', ab, w, z, work );
       } // hbev()

  }

}}}

#endif


#pragma once

#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <queue>
#include <sstream>
#include <stack>
#include <string>
#include <vector>

namespace calculator {
using std::string;
using std::vector;

static int _calc_expr_without_paretheses(const string& input) {
  vector<string> parts;
  boost::split(parts, input, [](char c) { return c == ' '; });
  parts.erase(std::remove_if(parts.begin(), parts.end(),
                             [](const string& part) { return part.empty(); }),
              parts.end());
  vector<string> phase1;
  int i = 0;
  while (i < parts.size()) {
    const auto& op = parts[i];
    if (op == "*" || op == "/") {
      const auto& lhs = phase1.back();
      const auto& rhs = parts[i + 1];
      i += 2;
      const int lval = atoi(lhs.c_str());
      const int rval = atoi(rhs.c_str());
      const int val = op == "*" ? lval * rval : lval / rval;
      phase1.pop_back();
      phase1.push_back(std::to_string(val));
    } else {
      phase1.push_back(op);
      i += 1;
    }
  }
  int result = atoi(phase1[0].c_str());
  for (int i = 1; i < phase1.size() - 1; i++) {
    auto& op = phase1[i];
    int rhs = atoi(phase1[i + 1].c_str());
    if (op == "+") {
      result += rhs;
    } else {
      result -= rhs;
    }
  }
  return result;
}

int calc(const string input) {
  const string source(input);
  const int N = source.size();
  int pos = 0;
  vector<string> exprs;
  string expr;
  for (char ch : source) {
    if (ch == '(') {
      exprs.push_back(expr);
      expr.clear();
    } else if (ch == ')') {
      int result = _calc_expr_without_paretheses(expr);
      expr = exprs.back();
      exprs.pop_back();
      expr += " " + std::to_string(result) + " ";
    } else {
      expr += ch;
    }
  }
  int result = _calc_expr_without_paretheses(expr);
  return result;
}

struct Token {
  // // 字母 D 表示数值类型
  char kind;
  double value;
  Token() : kind('D'), value(0) {}
};

static inline bool is_operator(char ch) {
  return ch == '+' || ch == '-' || ch == '*' || ch == '/';
}

static vector<Token> tokenize(string input) {
  std::stringstream cin(input);
  vector<Token> tokens;
  bool leading = true;
  while (cin) {
    Token token;
    char ch = cin.peek();
    if (isdigit(ch)) {
      cin >> token.value;
      leading = false;
    } else if (leading && (ch == '+' || ch == '-')) {
      cin >> token.value;
      leading = false;
    } else {
      cin >> ch;
      if (isspace(ch)) {  // skip
        continue;
      }
      token.kind = ch;
      leading = (ch == '(' || is_operator(ch));
    }
    tokens.push_back(token);
  }
  return tokens;
}

static inline int op_priority(char ch) {
  switch (ch) {
    case '*':
    case '/':
      return 30;
    case '+':
    case '-':
      return 20;
    default:
      return 0;
  }
}
static bool is_integer(double num) {
  return isfinite(num) && floor(num) == num;
}

static string fmt_double(double d) {
  if (is_integer(d)) {
    long num = static_cast<long>(d);
    return std::to_string(num);
  } else {
    string str = std::to_string(d);
    str.erase(str.find_last_not_of('0') + 1, string::npos);
    return str;
  }
}

string infix_to_postfix(string input) {
  std::vector<string> output;
  std::stack<char> operators;

  vector<Token> tokens = tokenize(input);

  for (Token& token : tokens) {
    const char ch = token.kind;
    if (is_operator(ch)) {
      const int op_pri = op_priority(ch);
      while (!operators.empty()) {
        char prev_op = operators.top();
        const int pre_op_pri = op_priority(prev_op);
        if (pre_op_pri >= op_pri) {
          output.push_back(string(1, prev_op));
          operators.pop();
        } else {
          break;
        }
      }
      operators.push(ch);
    } else if (token.kind == 'D') {
      output.push_back(fmt_double(token.value));
    } else if (ch == '(') {
      operators.push(ch);
    } else if (ch == ')') {
      char prev_op = operators.top();
      while (prev_op != '(') {
        output.push_back(string(1, prev_op));
        operators.pop();
        prev_op = operators.top();
      }
      operators.pop();
    } else {
      // skip
    }
  }

  while (!operators.empty()) {
    char ch = operators.top();
    output.push_back(string(1, ch));
    operators.pop();
  }

  return boost::join(output, " ");
}

double calc_v2(string input) {
  return 0;
}
}  // namespace calculator


#ifndef __DIRICHLET_H__
#define __DIRICHLET_H__

#include <armadillo>

#include <cmath>
#include <numeric>
#include <vector>

#include <besiq/config.h>
#ifndef HAVE_TR1_RANDOM
#include <random>
typedef std::mt19937 prg_type;
#else
#include <tr1/random>
typedef std::tr1::mt19937 prg_type;
#endif

/**
 * Computes the dirichlet multinomial probability of a vector
 * x with prior parameter alpha.
 *
 * @param x The observations.
 * @param alpha The prior parameters of the dirichlet density.
 *
 * @return The posterior probability of x.
 */
double dirmult(const arma::vec &x, const arma::vec &alpha);

/**
 * Computes the dirichlet multinomial log probability of a vector
 * x with prior parameter alpha.
 *
 * @param x The observations.
 * @param alpha The prior parameters of the dirichlet density.
 *
 * @return The log posterior probability of x.
 */
double ldirmult(const arma::vec &x, const arma::vec &alpha);

/**
 * Computes the log of the binomial coefficient (n choose k).
 *
 * @param n Number of elements to draw from.
 * @param k Number of elements to draw.
 *
 * @return The log of the binomial coefficient.
 */
double lbinomial(double n, double k);

/**
 * This class is responsible for generating samples from a
 * dirichlet distribution. It does so by using the fact that
 * gamma(a_i, b) / sum_i gamma( a_i, b ) is dirichlet distributed
 * with parameter a_i.
 */
class dir_generator
{
public:
    /**
     * Constructor.
     *
     * Initializes the random generator with the given seed.
     *
     * @param seed The seed given to the random generator.
     */
    dir_generator(unsigned long seed);

    /**
     * Generates a random sample from the dirichlet distribution
     * with the given parameters.
     *
     * @param x The parameters of the dirichlet density.
     *
     * @return A sample from the dirichlet density.
     */
    arma::vec sample(const arma::vec &alpha);

private:
    /**
     * Mersenne twister random generator.
     */
    prg_type m_generator;
};

#endif /* End of __DIRICHLET_H__ */


/* Copyright (c) 2018, Skolkovo Institute of Science and Technology (Skoltech)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * factor_graph_solve.cpp
 *
 *  Created on: Mar 23, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech
 */

#include "mrob/factor_graph_solve.hpp"
//#include "mrob/CustomCholesky.hpp"

#include <iostream>
#include <Eigen/SparseCore>
#include <Eigen/SparseLU>
#include <Eigen/SparseCholesky>
#include <Eigen/SparseQR>

using namespace mrob;
using namespace std;
using namespace Eigen;

FGraphSolve::FGraphSolve(matrixMethod method, optimMethod optim):
	FGraph(), matrixMethod_(method), optimMethod_(optim), N_(0), M_(0),
	lambda_(1e-6), solutionTolerance_(1e-2)
{

}

FGraphSolve::~FGraphSolve() = default;

void FGraphSolve::solve(optimMethod method, uint_t maxIters)
{
    /**
     * 2800 2D nodes on M3500
     * Time profile :13.902 % build Adjacency matrix,
     *               34.344 % build Information,
     *               48.0506 % build Cholesky,
     *               2.3075 % solve forward and back substitution,
     *               1.3959 % update values,
     *
     */
    optimMethod_ = method; // updates the optimization method
    time_profiles_.reset();

    // Optimization
    switch(optimMethod_)
    {
      case GN:
        this->optimize_gauss_newton();// false => lambda = 0
        this->update_nodes();
        break;
      case LM:
        this->optimize_levenberg_marquardt(maxIters);
        break;
      default:
        assert(0 && "FGraphSolve:: optimization method unknown");
    }

    if (0)
        time_profiles_.print();
}

void FGraphSolve::build_problem(bool useLambda)
{

    // 1) Adjacency matrix A, it has to
    //    linearize and calculate the Jacobians and required matrices
    time_profiles_.start();
    this->build_adjacency();
    time_profiles_.stop("Adjacency");

    // 1.2) builds specifically the information
    switch(matrixMethod_)
    {
      case ADJ:
        time_profiles_.start();
        this->build_info_adjacency();
        time_profiles_.stop("Info Adjacency");
        break;
      case SCHUR:
      default:
        assert(0 && "FGraphSolve: method not implemented");
    }

    // Structure for LM and dampening GN-based methods
    if (useLambda)
    {
        diagL_ = L_.diagonal();
    }
}

void FGraphSolve::optimize_gauss_newton(bool useLambda)
{
    // requires a Column-storage matrix
    SimplicialLDLT<SMatCol,Lower, AMDOrdering<SMatCol::StorageIndex>> cholesky;

    this->build_problem(useLambda);

    // compute cholesky solution
    time_profiles_.start();
    if (useLambda)
    {
        for (uint_t n = 0 ; n < N_; ++n)
            L_.coeffRef(n,n) = lambda_ + diagL_(n);//Circunference =>  diagL_(n) + lambda_
            //L_.coeffRef(n,n) = (1.0 + lambda_)*diagL_(n);//Elipsoid, for circunference =>  diagL_(n) + lambda.
    }
    cholesky.compute(L_);
    time_profiles_.stop("Gauss Newton create Cholesky");
    time_profiles_.start();
    dx_ = cholesky.solve(b_);
    time_profiles_.stop("Gauss Newton solve Cholesky");

}

uint_t FGraphSolve::optimize_levenberg_marquardt(uint_t maxIters)
{
    //SimplicialLDLT<SMatCol,Lower, AMDOrdering<SMatCol::StorageIndex>> cholesky;

    // LM trust region as described in Bertsekas (p.105)

    // 0) parameter initialization
    lambda_ = 1e-5;
    // sigma reference to the fidelity of the model at the proposed solution \in [0,1]
    matData_t sigma1(0.25), sigma2(0.8);// 0 < sigma1 < sigma2 < 1
    matData_t beta1(2.0), beta2(0.25); // lambda updates multiplier values, beta1 > 1 > beta2 >0
    //matData_t lambdaMax, lambdaMin; // XXX lower bound unnecessary

    matData_t currentChi2, deltaChi2, modelFidelity;
    uint_t iter = 0;

    do{
        iter++;
        // 1) solve subproblem and current error
        this->optimize_gauss_newton(true);// Test if solved anything? no nans
        currentChi2 = this->chi2(false);// TODO residuals don't need to be calculated again (see optimizer.cpp)
        this->synchronize_nodes_auxiliary_state();// book-keeps states to undo updates
        this->update_nodes();

        // 1.2) Check for convergence, needs update and re-evaluaiton of errors
        deltaChi2 = currentChi2 - this->chi2(true);
        std::cout << "\nFGraphSolve::optimize_levenberg_marquardt: iteration "
                  << iter << " lambda = " << lambda_ << ", error " << currentChi2
                  << ", and delta = " << deltaChi2
                  << std::endl;
        if (deltaChi2 < 0)
        {
            // proposed dx did not improve, repeat 1) and reduce area of optimization = increase lambda
            lambda_ *= beta1;
            this->synchronize_nodes_state();
            continue;
        }

        // 1.3) check for convergence
        if (deltaChi2 < solutionTolerance_)
            return iter;

        // 2) Fidelity of the quadratized model vs non-linear chi2 evaluation.
        // f = chi2(x_k) - chi2(x_k + dx)
        //     chi2(x_k) - m_k(dx)
        // where m_k is the quadratized model = ||r||^2 - dx'*J' r + 0.5 dx'(J'J + lambda*D2)dx
        modelFidelity = deltaChi2 / (dx_.dot(b_) - 0.5*dx_.dot(L_* dx_));
        std::cout << "model fidelity = " << modelFidelity << " and m_k = " << dx_.dot(b_) << std::endl;

        //3) update lambda
        if (modelFidelity < sigma1)
            lambda_ *= beta1;
        if (modelFidelity > sigma2)
            lambda_ *= beta2;

    } while (iter < maxIters);

    // output
    std::cout << "FGraphSolve::optimize_levenberg_marquardt: failed to converge after "
              << iter << " iterations and error " << currentChi2
              << ", and delta = " << deltaChi2
              << std::endl;
    return 0; //

}

void FGraphSolve::build_adjacency()
{
    // 0) resize properly matrices (if needed)
    r_.resize(obsDim_,1);//dense vector TODO is it better to reserve and push_back??
    A_.resize(obsDim_, stateDim_);//Sparse matrix clear data
    W_.resize(obsDim_, obsDim_);//TODO should we reinitialize this all the time? an incremental should be fairly easy

    // 1) create the vector's structures
    std::deque<std::shared_ptr<Factor> >* factors;
    std::deque<std::shared_ptr<Node> >* nodes;
    // TODO: optimizing subgraph is not an option now, but we maintain generality
    factors = &factors_;
    nodes = &nodes_;

    // 2) vector structure to bookkeep the starting Nodes indices inside A

    // 2.2) Node indexes bookeept
    std::vector<uint_t> indNodesMatrix;
    indNodesMatrix.reserve(nodes->size());

    N_ = 0;
    for (id_t i = 0; i < nodes->size(); ++i)
    {
        // calculate the indices to access
        uint_t dim = (*nodes)[i]->get_dim();
        indNodesMatrix.push_back(N_);
        N_ += dim;

    }
    assert(N_ == stateDim_ && "FGraphSolve::buildAdjacency: State Dimensions are not coincident\n");

    // 3) Evaluate every factor given the current state and bookeeping of Factor indices
    std::vector<uint_t> reservationA;
    reservationA.reserve( obsDim_ );
    std::vector<uint_t> reservationW;
    reservationW.reserve( obsDim_ );
    std::vector<uint_t> indFactorsMatrix;
    indFactorsMatrix.reserve(factors->size());
    M_ = 0;
    for (uint_t i = 0; i < factors->size(); ++i)
    {
        auto f = (*factors)[i];
        f->evaluate_residuals();
        f->evaluate_jacobians();
        f->evaluate_chi2();

        // calculate dimensions for reservation and bookeping vector
        uint_t dim = f->get_dim();
        uint_t allDim = f->get_all_nodes_dim();
        for (uint_t j = 0; j < dim; ++j)
        {
            reservationA.push_back(allDim);
            reservationW.push_back(dim-j);
        }
        indFactorsMatrix.push_back(M_);
        M_ += dim;
    }
    assert(M_ == obsDim_ && "FGraphSolve::buildAdjacency: Observation dimensions are not coincident\n");
    A_.reserve(reservationA); //Exact allocation for elements.
    W_.reserve(reservationW); //same

    // XXX This could be subject to parallelization, maybe on two steps: eval + build
    for (uint_t i = 0; i < factors->size(); ++i)
    {
        auto f = (*factors)[i];

        // 4) Get the calculated residual
        r_.block(indFactorsMatrix[i], 0, f->get_dim(), 1) <<  f->get_residual();

        // 5) build Adjacency matrix as a composition of rows
        // 5.1) Get the number of nodes involved. It is a vector of nodes
        auto neighNodes = f->get_neighbour_nodes();
        // Iterates over the Jacobian row
        for (uint_t l=0; l < f->get_dim() ; ++l)
        {
            uint_t totalK = 0;
            // Iterates over the number of neighbour Nodes (ordered by construction)
            for (uint_t j=0; j < neighNodes->size(); ++j)
            {
                uint_t indNode = (*neighNodes)[j]->get_id();
                uint_t dimNode = (*neighNodes)[j]->get_dim();
                for(uint_t k = 0; k < dimNode; ++k)
                {
                    // order according to the permutation vector
                    uint_t iRow = indFactorsMatrix[i] + l;
                    uint_t iCol = indNodesMatrix[indNode] + k;
                    // This is an ordered insertion
                    A_.insert(iRow,iCol) = f->get_jacobian()(l, k + totalK);
                }
                totalK += dimNode;
            }
        }

        // 5) Get information matrix for every factor
        for (uint_t l = 0; l < f->get_dim(); ++l)
        {
            // only iterates over the upper triangular part
            for (uint_t k = l; k < f->get_dim(); ++k)
            {
                uint_t iRow = indFactorsMatrix[i] + l;
                uint_t iCol = indFactorsMatrix[i] + k;
                W_.insert(iRow,iCol) = f->get_information_matrix()(l,k);
                // If QR, then we need the following, but we dont suppoort QR anyway
                //W_.insert(iRow,iCol) = f->get_trans_sqrt_information_matrix()(l,k);
            }
        }
    } //end factors loop

}

void FGraphSolve::build_info_adjacency()
{
    /**
     * L_ dx = b_ corresponds to the normal equation A'*W*A dx = A'*W*r
     * only store the lower part of the information matrix (symmetric)
     *
     * XXX: In terms of speed, using the selfadjointview does not improve,
     * Eigen stores a temporary object and then copy only the upper part.
     *
     */
    L_ = (A_.transpose() * W_.selfadjointView<Eigen::Upper>() * A_);
    b_ = A_.transpose() * W_.selfadjointView<Eigen::Upper>() * r_;
}

matData_t FGraphSolve::chi2(bool evaluateResidualsFlag)
{
    matData_t totalChi2 = 0.0;
    for (uint_t i = 0; i < factors_.size(); ++i)
    {
        auto f = factors_[i];
        if (evaluateResidualsFlag)
        {
            f->evaluate_residuals();
            f->evaluate_chi2();
        }
        totalChi2 += f->get_chi2();
    }
    return totalChi2;
}

void FGraphSolve::update_nodes()
{
    int acc_start = 0;
    for (uint_t i = 0; i < nodes_.size(); i++)
    {
        // node update is the negative of dx just calculated.
        // x = x - alpha * H^(-1) * Grad = x - dx
        // Depending on the optimization, it is already taking care of the step alpha, so we assume alpha = 1
        auto node_update = -dx_.block(acc_start, 0, nodes_[i]->get_dim(), 1);
        nodes_[i]->update(node_update);

        acc_start += nodes_[i]->get_dim();
    }
}

void FGraphSolve::synchronize_nodes_auxiliary_state()
{
    for (auto n : nodes_)
        n->set_auxiliary_state(n->get_state());
}

void FGraphSolve::synchronize_nodes_state()
{
    for (auto n : nodes_)
        n->set_state(n->get_auxiliary_state());
}

// method to output (to python) or other programs the current state of the system.
std::vector<MatX> FGraphSolve::get_estimated_state()
{
    vector<MatX> results;
    results.reserve(nodes_.size());

    for (uint_t i = 0; i < nodes_.size(); i++)
    {
        //nodes_[i]->print();
        MatX updated_pos = nodes_[i]->get_state();
        results.emplace_back(updated_pos);
    }

    return results;
}

MatX1 FGraphSolve::get_chi2_array()
{
    MatX1 results(factors_.size());

    for (uint_t i = 0; i < factors_.size(); ++i)
    {
        auto f = factors_[i];
        results(i) = f->get_chi2();
    }

    return results;
}


#include "quadric_error_metric.h"
#include <Eigen/Geometry>
#include <iostream>

namespace {
	const double eps = 1e-7;
	const double uv_weight = 1e6;
}
void quadric_error_metric(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F,
	std::vector< Eigen::MatrixXd >& Q)
{
	using namespace std;
	using namespace Eigen;

	// array of 4x4 matrices
	Q.resize( V.rows() );
	for( int i=0; i<Q.size(); i++ )
		Q[i] = Matrix4d::Zero();

	const auto & face_from_three_points = [](const Vector3d& v1, const Vector3d& v2, const Vector3d& v3)
	{
		Vector3d n = (v2-v1).cross(v3-v1);
		n.normalize();
		double d = -n.dot(v1);

		Vector4d res;
		res << n(0), n(1), n(2), d;

		return res;
	};

	// the metric at each vertex equals to the sum of metric of its attached faces
	for( int i=0; i<F.rows(); i++ ) {
		Vector3d v1 = V.row( F(i,0) );
		Vector3d v2 = V.row( F(i,1) );
		Vector3d v3 = V.row( F(i,2) );
		Vector4d p = face_from_three_points(v1, v2, v3);
		Matrix4d metric = p*p.transpose();

		Q[ F(i,0) ] += metric;
		Q[ F(i,1) ] += metric;
		Q[ F(i,2) ] += metric;
	}

	// the cost v.T*Q*v should equal to zero
	for( int i=0; i<V.rows(); i++ ) {
		// cout << fabs(v * Q[i] * v.transpose()) << endl;
		assert( fabs(V.row(i).homogeneous() * Q[i] * V.row(i).homogeneous().transpose()) <= eps );
	}
}

void qslim_5d(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F,
	const Eigen::MatrixXd& TC,
	const Eigen::MatrixXi& FT,
	std::vector< Eigen::MatrixXd >& Q)
{
	using namespace std;
	using namespace Eigen;

	assert( F.rows() == FT.rows() );
	const int nF = F.rows();
	// array of 6x6 matrices
	Q.resize( V.rows() );
	for( int i=0; i<Q.size(); i++ ) {
		MatrixXd metric(6,6);
		metric.setZero();
		Q[i] = metric;
	}

	for(int i=0; i<nF; i++) {
		VectorXd p1(5),p2(5),p3(5);
		p1.head(3) = V.row( F(i,0) );
		p2.head(3) = V.row( F(i,1) );
		p3.head(3) = V.row( F(i,2) );
		p1.tail(2) = TC.row( FT(i,0) );
		p2.tail(2) = TC.row( FT(i,1) );
		p3.tail(2) = TC.row( FT(i,2) );
		// Paper Section 5.1
		VectorXd e1 = (p2-p1)/(p2-p1).norm();
		VectorXd e2 = p3-p1-(e1.dot(p3-p1))*e1;
		e2 /= e2.norm();
		const double eps = 1e-7;
		assert( fabs(e1.norm() - 1) <= eps );
		assert( fabs(e2.norm() - 1) <= eps );

		MatrixXd A(5,5);
		A.setIdentity();
		A = A - e1*e1.transpose() - e2*e2.transpose();
		VectorXd b = p1.dot(e1)*e1 + p1.dot(e2)*e2 - p1;
		double c = p1.dot(p1) - p1.dot(e1)*p1.dot(e1) - p1.dot(e2)*p1.dot(e2);

		// Paper Section 3.4
		MatrixXd metric(6,6);
		metric.block(0,0,5,5) = A;
		metric.block(0,5,5,1) = b;
		metric.block(5,0,1,5) = b.transpose();
		metric(5,5) = c;

		// add metric to each vertex
		Q[ F(i,0) ] += metric;
		Q[ F(i,1) ] += metric;
		Q[ F(i,2) ] += metric;
	}
}

void half_edge_qslim_5d(
	const Eigen::MatrixXd& V,
	const Eigen::MatrixXi& F,
	const Eigen::MatrixXd& TC,
	const Eigen::MatrixXi& FT,
	MapV5d & hash_Q)
{
	using namespace std;
	using namespace Eigen;

	// initialize 5d vertex map, key is (vi,ti), value is zero metric
	assert( F.rows() == FT.rows() );
	const int nF = F.rows();
	for(int i=0; i<nF; i++) {

		/// A. compute metric for each face
		VectorXd p1(5),p2(5),p3(5);
		p1.head(3) = V.row( F(i,0) );
		p2.head(3) = V.row( F(i,1) );
		p3.head(3) = V.row( F(i,2) );
		p1.tail(2) = TC.row( FT(i,0) );
		p2.tail(2) = TC.row( FT(i,1) );
		p3.tail(2) = TC.row( FT(i,2) );
		// Paper Section 5.1
		VectorXd e1 = (p2-p1)/(p2-p1).norm();
		VectorXd e2 = p3-p1-(e1.dot(p3-p1))*e1;
		e2 /= e2.norm();
		const double eps = 1e-7;
		assert( fabs(e1.norm() - 1) <= eps );
		assert( fabs(e2.norm() - 1) <= eps );

		MatrixXd A(5,5);
		A.setIdentity();
		A = A - e1*e1.transpose() - e2*e2.transpose();
		VectorXd b = p1.dot(e1)*e1 + p1.dot(e2)*e2 - p1;
		double c = p1.dot(p1) - p1.dot(e1)*p1.dot(e1) - p1.dot(e2)*p1.dot(e2);

		// Paper Section 3.4
		MatrixXd metric(6,6);
		metric.block(0,0,5,5) = A;
		metric.block(0,5,5,1) = b;
		metric.block(5,0,1,5) = b.transpose();
		metric(5,5) = c;

		/// B. assign the face metric to each 5d vertex, if it hasn't appeared, initialize
		/// it with the metric, otherwise, add the metric to its original metric.
		for(int j=0; j<3; j++) {
			int vi = F(i,j);
			int ti = FT(i,j);
			if( hash_Q[vi].count(ti) == 0 ) {
				hash_Q[vi][ti] = metric;
			}
			else {
				hash_Q[vi][ti] += metric;
			}
		}
	}

}


#include <algorithm>
#include <iostream>
#include <sstream>
#include <fstream>
#include <math.h>
#include <vector>
#include "hybrid_astar.h"
#include <boost/heap/binomial_heap.hpp>

using namespace std;

/**
 * Initializes HAS
 */
HAS::HAS() {

}

HAS::~HAS() {}

//heap optimization method
struct Compare_cost {

  bool operator()(const HAS::Node3D & lhs, const HAS::Node3D & rhs) const {
    return lhs.f > rhs.f;
  }

};
typedef boost::heap::binomial_heap< HAS::Node3D,
                                    boost::heap::compare<Compare_cost>> SortedQueue;

double HAS::heuristic(double x, double y,
                      vector<int> goal,
                      string heuristic_method){

      double dx  = fabs(y - goal[0]);
      double dy  = fabs(x - goal[1]);
      double tie = (1.0 + 1.0/10);

      //http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html#speed-or-accuracy
      if (heuristic_method == "Manhattan")       return dx + dy;
      if (heuristic_method == "Chebyshev")       return (dx + dy) -  min(dx, dy);
      if (heuristic_method == "Octile")          return 2*(dx + dy) - (3-2*2)*min(dx, dy);
      if (heuristic_method == "Euclidean")       return sqrt(dx * dx + dy * dy);
      if (heuristic_method == "Octile_breaktie") return tie*(2*(dx + dy) - (3-2*2)*min(dx, dy));
      //

}

/* HAS::theta_to_stack_number(double theta)
Takes an angle (in radians) and returns which "stack" in the 3D configuration space
this angle corresponds to. Angles near 0 go in the lower stacks while angles near
2 * pi go in the higher stacks.
*/
int HAS::theta_to_stack_number(double theta){

  double new_theta = fmod((theta + 2 * M_PI),(2 * M_PI));
  int stack_number = (int)(round(new_theta * NUM_THETA_CELLS / (2*M_PI))) % NUM_THETA_CELLS;
  return stack_number;
}

/*
Returns the index into the grid for continuous position. So if x is 3.621, then this
would return 3 to indicate that 3.621 corresponds to array index 3.
*/
int HAS::idx(double float_num) {

  return int(floor(float_num));
}

double HAS::turning_cost(double next_turn_angle,
                         double max_turnable,
                         double turning_weight){

    return turning_weight * fabs(next_turn_angle)/max_turnable;

}

vector<HAS::Node3D> HAS::expand(HAS::Node3D state,
                                vector<int> goal,
                                string heuristic_method) {

  int    g     = state.g;
  double x     = state.x;
  double y     = state.y;
  double theta = state.theta;

  int g2 = g + 1 ;
  vector<HAS::Node3D> next_states;

  for(double delta_i = -max_turnable;
             delta_i < (max_turnable + turning_res);
             delta_i += turning_res) {

    // Update next state
    double delta  = M_PI / 180.0 * delta_i;
    double omega  = SPEED / LENGTH * tan(delta);

    double theta2 = theta + omega;
    theta2        = fmod(theta2, 2*M_PI);
    if(theta2 < 0) { theta2 += 2*M_PI;}

    double x2 = x + SPEED * cos(theta);
    double y2 = y + SPEED * sin(theta);

    // Update next state cost
    int f2    = g2
                + turning_cost(delta_i, max_turnable, 1)
                + heuristic(x2, y2, goal, heuristic_method);

    // Create a new State object with all of the "next" values.
    HAS::Node3D state2 {g2, f2, x2, y2, theta2,};
    next_states.emplace_back(state2);

  }
  return next_states;

}

HAS::grid_path HAS::search_heap(vector<vector<int>> grid,
                                vector<double> start,
                                vector<int> goal,
                                string heuristic_method) {

  vector<vector<vector<int> > >    closed(NUM_THETA_CELLS,
                                          vector<vector<int>>(grid[0].size(), vector<int>(grid.size())));
  vector<vector<vector<Node3D> > > came_from(NUM_THETA_CELLS,
                                             vector<vector<Node3D>>(grid[0].size(), vector<Node3D>(grid.size())));
  double theta = start[2];
  int stack    = theta_to_stack_number(theta);
  int g        = 0;
  int f        = g + heuristic(start[0], start[1], goal, heuristic_method);

  // Create new state object to start the search with.
  Node3D state {g, f, start[0], start[1], theta};

  closed[stack][idx(state.x)][idx(state.y)]    = 1;
  came_from[stack][idx(state.x)][idx(state.y)] = state;

  int total_closed = 1;

  // Heap Method
  SortedQueue opened_heap;
  opened_heap.push(state);

  bool finished = false;

  while(!opened_heap.empty()) {

    // Heap Method
    Node3D current = opened_heap.top();// get smallest value
    opened_heap.pop();// delete

    int x = current.x;
    int y = current.y;

    // Check if reach the goal
    if(idx(x) == goal[0] && idx(y) == goal[1]){
      cout << " found path to goal in " << total_closed << " expansions" << endl;
      grid_path path {closed, came_from, current,};

      return path;
    }

    // Otherwise, expand the current state to get
    // a list of possible next states.
    vector<Node3D> next_state = expand(current, goal, heuristic_method);

    for(int i = 0; i < next_state.size(); i++) {
      int g2        = next_state[i].g;
      double x2     = next_state[i].x;
      double y2     = next_state[i].y;
      double theta2 = next_state[i].theta;

      // If we have expanded outside the grid, skip this next_state.
      if((x2 < 0 || x2 >= grid.size()) || (y2 < 0 || y2 >= grid[0].size())) {
        //invalid cell
        continue;
      }

      int stack2 = theta_to_stack_number(theta2);

      //Otherwise, check that we haven't already visited this cell and
      //that there is not an obstacle in the grid there.
      if(closed[stack2][idx(x2)][idx(y2)] == 0 && grid[idx(x2)][idx(y2)] == 0) {

        // The state can be added to the opened stack.
        opened_heap.push(next_state[i]);

        //The stack_number, idx(next_state.x), idx(next_state.y) tuple
        //has now been visited, so it can be closed.
        closed[stack2][idx(x2)][idx(y2)] = 1;

        //The next_state came from the current state, and that is recorded.
        came_from[stack2][idx(x2)][idx(y2)] = current;

        total_closed += 1;
      }

    }

  }
  cout << "no valid path." << endl;
  HAS::grid_path path {closed, came_from, state,};

  return path;

}

vector<HAS::Node3D> HAS::retrace_path(vector< vector< vector<HAS::Node3D> > > came_from,
                                      vector<double> start,
                                      HAS::Node3D final){

	vector<Node3D> path = {final};
	Node3D current = came_from[theta_to_stack_number(final.theta)]
                            [idx(final.x)][idx(final.y)];

  while( current.x !=  start[0] || current.y != start[1] || current.theta != start[2]){
    // add each node from final node
		path.emplace_back(current);
    current = came_from[theta_to_stack_number(current.theta)]
                       [idx(current.x)][idx(current.y)];
	}
  path.emplace_back(current); //add start node
  //reverse path from start
  std::reverse(path.begin(),path.end());

	return path;

}

vector<HAS::Node3D> HAS::smooth_path(vector<Node3D> path,
                                     double weight, double smooth, double tolerance){

  vector<Node3D> newpath;
  // make a copy of old path into newpath
  newpath = path;

  double change = tolerance;
  while (change >= tolerance){

    change = 0.0;
    for (auto node = path.begin()+1; node != path.end()-1; ++node){
      auto index = std::distance(path.begin(), node);

      double aux = newpath[index].x;
      newpath[index].x = newpath[index].x
                         + weight * (path[index].x - newpath[index].x);
      newpath[index].x = newpath[index].x
                         + smooth * (newpath[index-1].x + newpath[index+1].x
                                     - 2.0* newpath[index].x);
      change += abs(aux - newpath[index].x);

      aux = newpath[index].y;
      newpath[index].y = newpath[index].y
                         + weight * (path[index].y - newpath[index].y);
      newpath[index].y = newpath[index].y
                         + smooth * (newpath[index-1].y + newpath[index+1].y
                                     - 2.0* newpath[index].y);
      change += abs(aux - newpath[index].y);

    }
  }

  return newpath;

}


#include "Util.h"

#include <Eigen/SVD>

#include <iostream>

using namespace std;

bool Ubpa::PDIPM_QP::CheckSettingsValid() const {
	//float gamma{ 1.05f }; // t multiplier
	//float epsilon_feas{ 0.001f }; // residule error
	//float epsilon{ 0.001f }; // gap error
	//float beta{ 0.99f }; // alpha shrink multiplier
	//float tau{ 0.02f }; // backtracking

	if (gamma <= 1.f)
		return false;

	if (epsilon_feas <= 0 || epsilon <= 0)
		return false;

	if (beta <= 0 || beta >= 1)
		return false;

	if (tau <= 0 || tau >= 1)
		return false;
#ifndef NDEBUG
	else {
		if (tau < 0.01 || tau > 0.1)
			assert("tau is typically chosen in the range 0.01 to 0.1");
	}
#endif // !NDEBUG

	return true;
}

bool Ubpa::PDIPM_QP::SetProblem(
	Eigen::MatrixXf P, // n x n
	Eigen::VectorXf q, // n x 1
	float           r,

	Eigen::MatrixXf G, // m x n
	Eigen::VectorXf h, // m x 1

	Eigen::MatrixXf A, // p x n
	Eigen::VectorXf b  // p x 1
) {
	size_t n = P.rows();
	size_t m = G.rows();
	size_t p = A.rows();

	if (P.cols() != n
		|| q.size() != n
		|| G.cols() != n
		|| h.size() != m
		|| A.cols() != n
		|| b.size() != p)
	{
		return false;
	}

	this->P = std::move(P);
	this->q = std::move(q);
	this->r = r;

	this->G = std::move(G);
	this->h = std::move(h);

	this->A = std::move(A);
	this->b = std::move(b);

	this->n = n;
	this->m = m;
	this->p = p;

	return true;
}

void Ubpa::PDIPM_QP::Init() {
	// f(x) < 0
	// => Gx - h = -1
	// => x = pinv(G) * (h - 1)

	// presudo inverse of G
	Eigen::JacobiSVD<Eigen::MatrixXf> G_svd(G, Eigen::ComputeFullU | Eigen::ComputeFullV);
	const auto& SV = G_svd.singularValues();
	Eigen::MatrixXf ISV;
	ISV.setZero(G.cols(), G.rows());
	for (size_t i = 0; i < SV.size(); i++) {
		if (SV(i) > 0.000001f)
			ISV(i, i) = 1.f / SV(i);
	}
	Eigen::MatrixXf pinv_G = G_svd.matrixV() * ISV * G_svd.matrixU().transpose();

	x = pinv_G * (h - Eigen::VectorXf::Ones(m));

	lambda.setOnes(m); // > 0
	nu.setZero(p);

	Eigen::VectorXf f_x = -Eigen::VectorXf::Ones(m); // G * x - h

	eta = m; // -f_x.dot(lambda);
	float t = gamma; /* *m / eta */;

	Eigen::MatrixXf diag_lambda;
	diag_lambda.setZero(m, m);
	for (size_t i = 0; i < m; i++)
		diag_lambda(i, i) = lambda(i);

	r_dual = P * x + q + G.transpose() * lambda; /* + A.transpose() * nu */
	r_cent = -diag_lambda * f_x - (1.f / t) * Eigen::VectorXf::Ones(m);
	r_pri = A * x - b;
}

void Ubpa::PDIPM_QP::Iterate() {
	// [[ 1. compute Newton step ]]

	Eigen::MatrixXf M(n + m + p, n + m + p);
	M.setZero();

	Eigen::VectorXf f_x = G * x - h;

	// (0, 0) - (n-1, n-1)
	// P
	for (size_t i = 0; i < n; i++) {
		for (size_t j = 0; j < n; j++)
			M(i, j) = P(i, j);
	}

	// (0, n) - (n-1, n+m-1)
	// G^T
	for (size_t i = 0; i < n; i++) {
		for (size_t j = 0; j < m; j++)
			M(i, j + n) = G(j, i);
	}

	// (0, n+m) - (n-1, n+m+p-1)
	// A^T
	for (size_t i = 0; i < n; i++) {
		for (size_t j = 0; j < p; j++)
			M(i, j + n + m) = A(j, i);
	}

	// (n, 0) - (n+m-1, n-1)
	// - diag(lambda)G

	Eigen::MatrixXf diag_lambda;
	diag_lambda.setZero(m, m);
	for (size_t i = 0; i < m; i++)
		diag_lambda(i, i) = lambda(i);
	Eigen::MatrixXf diag_lambda_G = diag_lambda * G;
	for (size_t i = 0; i < m; i++) {
		for (size_t j = 0; j < n; j++)
			M(i + n, j) = -diag_lambda_G(i, j);
	}

	// (n, n) - (n+m-1, n+m-1)
	// - diag(f(x))
	for (size_t i = 0; i < m; i++)
		M(i + n, i + n) = -f_x(i);

	// (n+m, 0) - (n+m+p-1, n-1)
	// A
	for (size_t i = 0; i < p; i++) {
		for (size_t j = 0; j < n; j++)
			M(i + n + m, j) = A(i, j);
	}

	Eigen::VectorXf r(n + m + p);
	for (size_t i = 0; i < n; i++)
		r(i, 0) = r_dual(i);
	for (size_t i = 0; i < m; i++)
		r(i + n, 0) = r_cent(i);
	for (size_t i = 0; i < p; i++)
		r(i + n + m, 0) = r_pri(i);

	// y = (x, lambda, nu)
	// (n + m + p) x 1
	Eigen::VectorXf delta_y = M.colPivHouseholderQr().solve(-r);

	Eigen::VectorXf delta_x(n);
	Eigen::VectorXf delta_lambda(m);
	Eigen::VectorXf delta_nu(p);

	for (size_t i = 0; i < n; i++)
		delta_x(i, 0) = delta_y(i);
	for (size_t i = 0; i < m; i++)
		delta_lambda(i, 0) = delta_y(i + n);
	for (size_t i = 0; i < p; i++)
		delta_nu(i, 0) = delta_y(i + n + m);

	// [[ 2. backtracking line search ]]

	float alpha_max = 1.f;
	for (size_t i = 0; i < m; i++) {
		if (delta_lambda(i) < 0) {
			float cur = -lambda(i) / delta_lambda(i);
			if (cur < alpha_max)
				alpha_max = cur;
		}
	}

	float alpha = 0.99f * alpha_max;
	bool flag1 = false;
	bool flag2 = false;

	while (!flag1) {
		Eigen::VectorXf x_plus = x + alpha * delta_x;
		Eigen::VectorXf f_x_plus = G * x_plus - h;
		for (size_t i = 0; i < m; i++) {
			if (f_x_plus(i) >= 0) {
				alpha *= beta;
				break;
			}
			if (i == m - 1)
				flag1 = true;
		}
	}

	float rt_norm = Get_residule();

	while (!flag2) {
		Eigen::VectorXf x_plus = x + alpha * delta_x;
		Eigen::VectorXf lambda_plus = lambda + alpha * delta_lambda;
		Eigen::VectorXf nu_plus = nu + alpha * delta_nu;

		Eigen::MatrixXf diag_lambda_plus;
		diag_lambda_plus.setZero(m, m);
		for (size_t i = 0; i < m; i++)
			diag_lambda_plus(i, i) = lambda_plus(i);

		Eigen::VectorXf f_x_plus = G * x_plus - h;

		eta = -f_x_plus.dot(lambda_plus);
		float t = gamma * m / eta;

		r_dual = P * x_plus + q + G.transpose() * lambda_plus + A.transpose() * nu_plus;
		r_cent = -diag_lambda_plus * f_x_plus - (1.f / t) * Eigen::VectorXf::Ones(m);
		r_pri = A * x_plus - b;

		float rt_norm_plus = Get_residule();

		if (rt_norm_plus <= (1 - tau * alpha) * rt_norm)
			flag2 = true;
		else
			alpha *= beta;
	}

	// [[ 3. update ]]
	// x, lambda, nu

	x += alpha * delta_x;
	lambda += alpha * delta_lambda;
	nu += alpha * delta_nu;
}

bool Ubpa::PDIPM_QP::IsStoppable() {
	if (r_dual.norm() > epsilon_feas
		|| r_pri.norm() > epsilon_feas
		|| eta > epsilon)
	{
		return false;
	}

	return true;
}

float Ubpa::PDIPM_QP::Get_residule() const {
	return std::sqrt(
		r_dual.squaredNorm()
		+ r_cent.squaredNorm()
		+ r_pri.squaredNorm());
}


#ifndef CANNON_PHYSICS_SYSTEMS_DYNAMIC_CAR_H
#define CANNON_PHYSICS_SYSTEMS_DYNAMIC_CAR_H

#include <random>
#include <cassert>

#include <ompl/control/ODESolver.h>
#include <ompl/control/spaces/RealVectorControlSpace.h>
#include <ompl/base/spaces/SO2StateSpace.h>
#include <ompl/base/spaces/SE2StateSpace.h>

#include <Eigen/Dense>

#include <cannon/physics/rk4_integrator.hpp>
#include <cannon/physics/systems/system.hpp>
#include <cannon/graphics/geometry/plane.hpp>
#include <cannon/log/registry.hpp>

using namespace Eigen;

using namespace cannon::log;

namespace oc = ompl::control;
namespace ob = ompl::base;

namespace cannon {
  namespace physics {
    namespace systems {

      struct DynamicCarSystem : System {
        DynamicCarSystem(double l = 1.0) : l_(l) {}

        virtual void operator()(const VectorXd& s, VectorXd& dsdt, const double /*t*/) override {
          double th = s[2];
          double v = s[3];
          double dth = s[4];
          double ua = s[5];
          double uth = s[6];

          // From http://planning.cs.uiuc.edu/node658.html
          dsdt.resize(7);
          dsdt[0] = v * std::cos(th);
          dsdt[1] = v * std::sin(th);
          dsdt[2] = (v / l_) * std::tan(dth);
          dsdt[3] = ua;
          dsdt[4] = uth;
          dsdt[5] = 0.0;
          dsdt[6] = 0.0;
        }

        virtual void ompl_ode_adaptor(const oc::ODESolver::StateType& q,
            const oc::Control* control, oc::ODESolver::StateType& qdot) override {

          const double ua = control->as<oc::RealVectorControlSpace::ControlType>()->values[0];
          const double uth = control->as<oc::RealVectorControlSpace::ControlType>()->values[1];

          VectorXd s(7);
          s[0] = q[0];
          s[1] = q[1];
          s[2] = q[2];
          s[3] = q[3];
          s[4] = q[4];
          s[5] = ua;
          s[6] = uth;
          VectorXd dsdt(7);

          (*this)(s, dsdt, 0.0);

          qdot.resize(q.size(), 0);
          for (unsigned int i = 0; i < q.size(); i++) {
            qdot[i] = dsdt[i];
          }
        }

        virtual std::tuple<MatrixXd, MatrixXd, VectorXd> get_linearization(const VectorXd& x) override {
          assert(x.size() == 5);

          double th = x[2];
          double v = x[3];
          double dth = x[4];

          // TODO Don't hardcode timestep at some point
          double timestep = 0.01;

          MatrixXd A(5, 5);
          A << 1, 0, -v * std::sin(th) * timestep, std::cos(th) * timestep, 0,
               0, 1, v * std::cos(th) * timestep, std::sin(th) * timestep, 0,
               0, 0, 1, (tan(dth) / l_) * timestep, (v / l_) * (1.0 / (cos(dth) * cos(dth))) * timestep,
               0, 0, 0, 1, 0,
               0, 0, 0, 0, 1;

          MatrixXd B(5, 2);
          B << 0, 0,
               0, 0,
               0, 0,
               timestep, 0,
               0, timestep;

          VectorXd c(5);
          c << x[0] + v * std::cos(th) * timestep,
               x[1] + v * std::cos(th) * timestep,
               th + (v / l_) * tan(dth) * timestep,
               v,
               dth;

          // We linearize around u = 0, so no additional term is subtracted from c
          return std::make_tuple(A, B, c - A * x);
        }

        virtual void
        get_continuous_time_linearization(const oc::ODESolver::StateType &q,
                                          Ref<MatrixXd> A, Ref<MatrixXd> B) override {
          double c = cos(q[2]), s = sin(q[2]);
          A = 1e-3 * Eigen::MatrixXd::Identity(5, 5);
          A(0, 2) = -q[3] * s;
          A(0, 3) = c;
          A(1, 2) = q[3] * c;
          A(1, 3) = s;
          A(2, 3) = (1.0 / l_) * tan(q[4]);
          A(2, 4) = q[3] * (1.0 / l_) / (cos(q[4]) * cos(q[4]));

          B = Eigen::MatrixXd::Zero(5, 2);
          B(3, 0) = B(4, 1) = 1.;
        }

        static void ompl_post_integration(const ob::State* /*state*/, const
            oc::Control* /*control*/, const double /*duration*/, ob::State *result) {

          ob::SO2StateSpace SO2;
          SO2.enforceBounds(result->as<ob::CompoundStateSpace::StateType>()
                                ->as<ob::SE2StateSpace::StateType>(0)
                                ->as<ob::SO2StateSpace::StateType>(1));
        }

        // Parameters
        double l_;
      };

      class DynamicCar {
        public:
          DynamicCar() = delete;

          DynamicCar(VectorXd s, VectorXd g, double l = 1.0)
              : s_(l), e_(s_, 7, time_step), start_(s), goal_(g) {
            std::random_device rd;
            gen_ = std::mt19937(rd());

            xy_dis_ = std::uniform_real_distribution<double>(-1.0, 1.0);
            th_dis_ = std::uniform_real_distribution<double>(-M_PI, M_PI);

            state_ = VectorXd::Zero(7);
            reset();
          }

          std::pair<VectorXd, double> step(double ua, double uth) {
            //double clipped_uth = std::max(-M_PI * 2.0 / 180.0, std::min(uth, M_PI * 2.0 / 180.0));

            //double clipped_uth = std::max(-M_PI, std::min(uth, M_PI));

            state_[5] = ua;
            state_[6] = uth;

            double goal_r = -std::pow((state_.head(2) - goal_.head(2)).norm(), 2.0);
            double control_r = -std::pow((std::abs(uth) + std::abs(ua)), 2.0);
            double reward = goal_r + 0.001*control_r;

            e_.set_state(state_);
            state_ = e_.step();

            return std::make_pair(state_.head(5), reward);
          }

          VectorXd reset() {
            state_[0] = xy_dis_(gen_);
            state_[1] = xy_dis_(gen_);
            state_[2] = th_dis_(gen_);
            state_[3] = 0.0;
            state_[4] = 0.0;

            state_[4] = 0.0;
            state_[5] = 0.0;

            return state_.head(5);
          }

          VectorXd reset(const VectorXd& s) {
            assert(s.size() == 5);

            state_[0] = s[0];
            state_[1] = s[1];
            state_[2] = s[2];
            state_[3] = s[3];
            state_[4] = s[4];

            state_[5] = 0.0;
            state_[6] = 0.0;

            return state_.head(5);
          }

          // In seconds
          const double time_step = 0.01;

          DynamicCarSystem s_;

        private:
          RK4Integrator e_;

          VectorXd state_;

          VectorXd start_;
          VectorXd goal_;

          std::mt19937 gen_;
          std::uniform_real_distribution<double> xy_dis_;
          std::uniform_real_distribution<double> th_dis_;
      };

    } // namespace physics
  } // namespace physics
} // namespace cannon

#endif /* ifndef CANNON_PHYSICS_SYSTEMS_DYNAMIC_CAR_H */


#include <string>

#include <CGAL/boost/graph/properties.h>
#include <Eigen/SparseCore>
#include <Euclid/Geometry/TriMeshGeometry.h>
#include <Euclid/Util/Assert.h>
#include <Spectra/MatOp/SparseSymMatProd.h>
#include <Spectra/MatOp/SparseSymShiftSolve.h>
#include <Spectra/MatOp/SymShiftInvert.h>
#include <Spectra/SymEigsShiftSolver.h>
#include <Spectra/SymGEigsShiftSolver.h>

namespace Euclid
{

namespace _impl
{

template<typename T, typename DerivedA, typename DerivedB>
unsigned sym_solve(const Eigen::SparseMatrix<T>& L,
                   int k,
                   int nv,
                   unsigned max_iter,
                   double tolerance,
                   Eigen::MatrixBase<DerivedA>& lambdas,
                   Eigen::MatrixBase<DerivedB>& phis)
{
    // use shift-invert mode to get the smallest eigenvalues fast
    auto convergence = std::min(2 * k + 1, nv);
    using Operator = Spectra::SparseSymShiftSolve<T>;
    using Solver = Spectra::SymEigsShiftSolver<Operator>;
    Operator op(L);
    Solver eigensolver(op, k, convergence, -1.0);
    eigensolver.init();
    unsigned n = eigensolver.compute(Spectra::SortRule::LargestMagn,
                                     max_iter,
                                     static_cast<T>(tolerance),
                                     Spectra::SortRule::SmallestMagn);
    if (eigensolver.info() != Spectra::CompInfo::Successful) {
        throw std::runtime_error("Eigen decomposition failed.");
    }
    lambdas = eigensolver.eigenvalues();
    phis = eigensolver.eigenvectors();
    return n;
}

template<typename T, typename DerivedA, typename DerivedB>
unsigned gen_solve(const Eigen::SparseMatrix<T>& S,
                   const Eigen::SparseMatrix<T>& D,
                   int k,
                   int nv,
                   unsigned max_iter,
                   double tolerance,
                   Eigen::MatrixBase<DerivedA>& lambdas,
                   Eigen::MatrixBase<DerivedB>& phis)
{
    int convergence = std::min(2 * k + 1, nv);
    using Operator = Spectra::SymShiftInvert<T, Eigen::Sparse, Eigen::Sparse>;
    using BOperator = Spectra::SparseSymMatProd<T>;
    using Solver =
        Spectra::SymGEigsShiftSolver<Operator,
                                     BOperator,
                                     Spectra::GEigsMode::ShiftInvert>;
    Operator op(S, D);
    BOperator bop(D);
    Solver eigensolver(op, bop, k, convergence, -1.0);
    eigensolver.init();
    unsigned n = eigensolver.compute(Spectra::SortRule::LargestMagn,
                                     max_iter,
                                     static_cast<T>(tolerance),
                                     Spectra::SortRule::SmallestMagn);
    if (eigensolver.info() != Spectra::CompInfo::Successful) {
        throw std::runtime_error("Eigen decomposition failed.");
    }
    lambdas = eigensolver.eigenvalues();
    phis = eigensolver.eigenvectors();
    return n;
}

} // namespace _impl

template<typename Mesh, typename DerivedA, typename DerivedB>
unsigned spectrum(const Mesh& mesh,
                  unsigned k,
                  Eigen::MatrixBase<DerivedA>& lambdas,
                  Eigen::MatrixBase<DerivedB>& phis,
                  SpecOp op,
                  unsigned max_iter,
                  double tolerance)
{
    using T = typename CGAL::Kernel_traits<typename boost::property_traits<
        typename boost::property_map<Mesh, boost::vertex_point_t>::type>::
                                               value_type>::Kernel::FT;
    using SpMat = Eigen::SparseMatrix<T>;
    auto nv = num_vertices(mesh);

    if (k > nv) {
        std::string err("You've requested ");
        err.append(std::to_string(k));
        err.append(" eigenvalues but there are only ");
        err.append(std::to_string(nv));
        err.append(" vertices in your mesh.");
        EWARNING(err);
        k = nv;
    }

    unsigned n;
    if (op == SpecOp::mesh_laplacian) {
        SpMat C = Euclid::cotangent_matrix(mesh);
        SpMat D = Euclid::mass_matrix(mesh);
        n = _impl::gen_solve(C, D, k, nv, max_iter, tolerance, lambdas, phis);
    }
    else {
        auto result = Euclid::adjacency_matrix(mesh);
        SpMat A = std::get<0>(result);
        SpMat D = std::get<1>(result);
        SpMat L = D - A;
        n = _impl::sym_solve(L, k, nv, max_iter, tolerance, lambdas, phis);
    }

    if (n < k) {
        auto str = std::to_string(k);
        str.append(" eigenvalues are requested, but only ");
        str.append(std::to_string(n));
        str.append(" values converged in computation.");
        EWARNING(str);
    }
    EASSERT(lambdas.rows() == n);
    EASSERT(phis.cols() == n);
    EASSERT(phis.rows() == nv);

    return n;
}

} // namespace Euclid


#ifndef __Linear_regression__
#define __Linear_regression__
#include<iostream>
#include<Eigen/Dense>
#include<utility>
#include<string>
#include"generate_data.hpp"
#include<random>
#include <Eigen/Cholesky>
#include<cmath>

Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> shuffling_data(size_t n)
{
    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> shuffled(n);
    shuffled.setIdentity();
    std::random_shuffle(shuffled.indices().data(), shuffled.indices().data() + shuffled.indices().size());
    return shuffled;
}

float compute_mse(const Eigen::VectorXf& pred, const Eigen::VectorXf& y)
{
    float value = (1.0f/pred.rows()) * ((y - pred).transpose() * (y-pred)).sum();
    return value;

}
Eigen::VectorXf gradient_descent(const Eigen::MatrixXf& X, const Eigen::MatrixXf& y, float learning_rate, size_t num_rows)
{
    //I already assume that X is padded with 1's
    Eigen::VectorXf B = Eigen::VectorXf::Random(X.cols());
    B(0) = 0.0f;
    size_t blocks_size = (size_t) (X.rows()/num_rows);
    //in case if the num_rows was larger than the number of rows of X
    if(blocks_size == 0)
    {
        blocks_size = 1;
        num_rows = X.rows();
    }
    int counter = 0;
    Eigen::MatrixXf block_of_X = Eigen::MatrixXf::Zero(num_rows, X.cols());
    Eigen::VectorXf block_of_y = Eigen::VectorXf::Zero(num_rows);
    for(int iter = 0; iter < 100; iter++)
    {
        counter = 0;
        block_of_X = Eigen::MatrixXf(num_rows, X.cols());
        block_of_y = Eigen::VectorXf(num_rows);

        for(int block=0; block < blocks_size; block++)
        {
            //taking a slice from X, it appears that I don't have Eigen::seq
            for(int row=0; row < num_rows; row++)
            {
                block_of_X.row(row) = X.row(row + counter);
                block_of_y.row(row) = y.row(row + counter);
            }
            //updating B
            //std::cout << block_of_y << std::endl;
            // std::cout << "x\t" << block_of_X <<"\n" << std::endl;
            // std::cout << "xT\t" << block_of_X.transpose() <<"\n" << std::endl;

            B = B - learning_rate * 1.0f/(block_of_X.rows()) * block_of_X.transpose() * (block_of_X * B - block_of_y);
            Eigen::VectorXf pred = Eigen::VectorXf(block_of_X.rows());
            pred = block_of_X * B;
            float error = compute_mse(pred, block_of_y);
            std::cout << "Step: " << block + iter * blocks_size  << " error: " << error << std::endl;

            counter += num_rows;
        }
        if(counter < X.rows())
        {
            int rest_of_rows = X.rows() - counter;
            block_of_X = Eigen::MatrixXf(rest_of_rows, X.cols());
            block_of_y = Eigen::VectorXf(rest_of_rows);

            for(int row=0; row < rest_of_rows; row++)
            {
                block_of_X.row(row) = X.row(row + counter);
                block_of_y.row(row) = y.row(row + counter);
            }
            //updating B
            //std::cout << "rows: " << block_of_X.rows() << std::endl;
            B = B - learning_rate * block_of_X.transpose() * ( block_of_X * B - block_of_y );
            Eigen::VectorXf pred = Eigen::VectorXf(rest_of_rows);
            pred = block_of_X * B;
            float error = compute_mse(pred, block_of_y);
            std::cout << "Step: " << blocks_size + 1 + + iter * blocks_size << " error: " << error << std::endl;

        }
    }

    //std::cout << "B\n " << B << std::endl;

    return B;
}
float estimate_variance(const Eigen::MatrixXf& X, const Eigen::MatrixXf& y, const Eigen::VectorXf& B)
{
    //I will use the biased estimate in which I willn't divide by n-1
    float sigma = (1.0f/y.rows()) * ((y - X * B).transpose() * (y - X * B)).sum();
    return sigma;
}
void make_prediction(const Eigen::VectorXf& B, const Eigen::MatrixXf& x, float sigma, const float true_y);

//I will only deal with metric/conttinuous data
void Linear_regression_training(const std::string file_name, float lr, int num_rows)
{
    std::pair<Eigen::MatrixXf, Eigen::MatrixXf> data = load_csv_for_LinearR(file_name);
    Eigen::MatrixXf X = data.first;
    Eigen::MatrixXf X_with_pad = Eigen::MatrixXf(X.rows(), X.cols() + 1 );
    //pad X for the intercept term
    Eigen::VectorXf b0 = Eigen::VectorXf::Ones(X.rows());
    //shifting columns
    X_with_pad.col(0) = b0;
    for(int i=1; i < X_with_pad.cols(); i++)
    {
        X_with_pad.col(i) = X.col(i - 1);
    }
    Eigen::MatrixXf y = data.second;
    Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> shuffled_indeces(shuffling_data(X.rows()));
    //shuffling data
    X_with_pad = shuffled_indeces * X_with_pad;
    y = shuffled_indeces * y;
    //standard normalize X
    // float* means = new float[X_with_pad.cols() - 1];
    // float* std_ = new float[X_with_pad.cols() - 1];
    // Eigen::VectorXf tmp = Eigen::VectorXf::Ones(X_with_pad.rows());

    // for(int i=1; i < X_with_pad.cols(); i++)
    // {
    //     means[i - 1] = X_with_pad.col(i).mean();
    //     X_with_pad.col(i) = X_with_pad.col(i) - tmp * means[i -1];
    //     std_[i - 1] = 1/X_with_pad.rows() * (X_with_pad.col(i).transpose() * X_with_pad.col(i)).sum();
    //     tmp = X_with_pad.col(i);
    //     std::cout << X_with_pad << std::endl;
    //     std::cout << "m " << means[i - 1] << " std: " << std_[i -1] << std::endl;
    //     tmp = tmp / std_[i-1];
    //     std::cout << tmp << std::endl;
    //     X_with_pad.col(i) = tmp;
    // }
    // float meany, std_y;
    // //standard normalize y
    // meany = y.mean();
    // y = y - tmp * meany;
    // std_y = 1/y.rows() * (y.transpose() * y).sum();
    // tmp = y;
    // y = tmp/std_y;
    //use meany, std_y, means and std_ for prediction stage.

    //Finding Value of B
    Eigen::VectorXf B = gradient_descent(X_with_pad, y, lr, num_rows);
    Eigen::VectorXf B_ols = (X_with_pad.transpose() * X_with_pad).inverse() * X_with_pad.transpose() * y;

    std::cout << "Mini-batch GD estimat of of B: \n" << B << std::endl;
    std::cout << "Error in prediction GD: " << compute_mse(X_with_pad * B, y) << std::endl;
    std::cout << "Sigma Estimate by GD: " << estimate_variance(X_with_pad, y, B) << std::endl;
    std::cout << "OLS of B: \n" << B_ols << std::endl;
    std::cout << "Error in prediction OLS: " << compute_mse(X_with_pad * B_ols, y) << std::endl;
    // std::cout << X_with_pad << std::endl;
    // std::cout << y << std::endl;
    make_prediction(B, X_with_pad.row(10).transpose(), estimate_variance(X_with_pad, y, B), y.row(10).sum());

}

void make_prediction(const Eigen::VectorXf& B, const Eigen::MatrixXf& x, float sigma, const float true_y)
{
    auto f = [](float pred, float true_v){

        return std::sqrt((pred - true_v) * (pred - true_v));
    };
    std::cout << "B: " << B.transpose().cols() << std::endl;
    std::cout << "x: " << x.rows() << std::endl;
    float y = (B.transpose() * x).sum();
    std::cout << "Prediciton: " << y << " true value: " << true_y << " rms: " << f(y, true_y) << std::endl;

}

#endif /*__Linear_regression__*/


//
//  main.cpp
//  Assignment 2 - Bearings
//
//  Created by - on 2016/09/27.
//  Copyright © 2016 Eddie Of the Ren. All rights reserved.
//
#include <iostream>
//#include "stdafx.h"
using namespace std;
#include <stdio.h>      /* printf */
#include <math.h>
//#include <boost/lexical_cast.hpp>
#include <string>

int cardinalNum1 = 0;
int cardinalNum2 = 0;

//This function converts the cardinal string to a number that can fit in an array
int cardinalStrToNum(string cardinalStr) {
    int cardinalNum = 0;
    if (cardinalStr== "N") {
        cardinalNum = 2;
    } else if (cardinalStr == "E") {
        cardinalNum = 3;
    } else if (cardinalStr == "S") {
        cardinalNum = 4;
    } else if (cardinalStr == "W") {
        cardinalNum = 5;
    }
    return cardinalNum;

}

//This checks if a string is a number that is composed only of "-.0123456789"
bool is_number(const std::string& s)
{
    return( strspn( s.c_str(), "-.0123456789" ) == s.size() );
}

//If the user enters a bearing that is not within 0 to 359, this converts it to within that limit
int getBearing(int x) {
    double customBearing = 0;
    //Turning negative values positive
    while (x < 0) {
        x = x + 360;
    }
    //Turning overweight values smaller
    while (x >= 360) {
        x = x - 360;
    }
    customBearing = x;

    return (customBearing);
}

//This function will accept a bearing direction and turn it into a compass direction
int bearingToCompass(double bearing, string gameChoice) {
    double choiceInt = 0;

    int quadrant = 0;
    bool CW = false;
    string cardinals [6] = {"W", "N", "E", "S", "W", "N"};

    //getBearing will convert the user entered value into a value between 1 and 360, discrimination is currently enforced to favor 0° over 360°

    //These two lines turn the bearing into a value between 0 and 359 and finds the inverse quadrant it resides in. The quadrants are counted clockwise, NOT counter-clockwise
    bearing = getBearing(bearing);
    quadrant = floor(bearing/90);

    //If this if statement is true, that means the degree applies in a clockwise direction relative to the first cardinal. Ex: N32E. the direciton is 32 east (clockwise) FROM north
    if ((bearing - 90*quadrant) <45) {
        CW = false;
        cout << cardinals[(quadrant + 1)] << (bearing - 90*quadrant) << cardinals[(quadrant+2)] <<endl;
        if (gameChoice == "1") {

            cout << "Primary compass direction:             " << cardinals[(quadrant + 1)] << endl;
            cout << "Compass angle relative to direction:   " << (bearing - 90*quadrant) << endl;
            cout << "Secondary compass direction:           " << cardinals[(quadrant+2)]<< endl;
        }

    }
    //If this if statement is true, that means the degree applies in a counter-clockwise direction relative to the first cardinal. Ex: N65E. The direction is 65 north (counterclockwise) FROM east. This is equivalent to E25N which is a better way of representing the direction.
    else if ((bearing - 90*quadrant) >45) {
        CW = true;
        cout << cardinals[(quadrant + 2)] << (90-(bearing - 90*quadrant)) << cardinals[(quadrant+1)] <<endl;

        if (gameChoice == "1") {
            cout << "Primary compass direction:             " << cardinals[(quadrant + 2)] << endl;
            //The line of code below ensures that the angle is between 1° and 44°
            cout << "Compass angle                          " << (90-(bearing - 90*quadrant)) << "°" <<     endl;
            cout << "Secondary compass direction:           "<< cardinals[(quadrant+1)]<< endl;
        }
    }
    //When the angle is 45° or exactly between two cardinals, the bearing is not displayed
    else if ((bearing - 90*quadrant) == 45) {
        cout << cardinals[(quadrant + 1)]  << cardinals[(quadrant+2)] <<endl;

        if (gameChoice == "1") {
            cout << "Primary compass direction:             " << cardinals[(quadrant + 1)] << endl;
            cout << "Compass angle relative to direction:   " << endl;
            cout << "Secondary compass direction:           " << cardinals[(quadrant+2)]<< endl;
        }
    }

    //cout << cardinals[(quadrant+1)] << endl;
    //cout << bearing<< endl;
    return 0;

}

//This functions checks the cardinals and angle the user entered are valid or not.
bool cardinalCheck (string primaryCardinal, string secondaryCardinal, string angleStr) {
    bool redo = false;
    if (primaryCardinal != "N"&& primaryCardinal!= "E" && primaryCardinal != "W" && primaryCardinal != "S") {
        redo = true;
        cout << "Your primary cardinal direciton is not one of N, E, S, W " << endl;
    } else if (secondaryCardinal != "N"&& secondaryCardinal!= "E" && secondaryCardinal != "W" && secondaryCardinal != "S") {
        redo = true;
        cout << "Your secondary cardinal direciton is not one of N, E, S, W " << endl;
    } else if (is_number(angleStr) == 0)  {
        redo = true;
        cout << "Your angle is not a number " << endl;
    } else {
        //following statements ensure that the user has not entered two identical or opposite cardinals, such as N20N or N20S
        cardinalNum1 = cardinalStrToNum(primaryCardinal);
        cardinalNum2 = cardinalStrToNum(secondaryCardinal);
        if (cardinalNum1 - cardinalNum2 >= 0) {
            if ((cardinalNum1-cardinalNum2)%2 == 0) {
                redo = true;
                cout<< "These coordinates are invalid"<< endl;
            }
        } else if (cardinalNum1 - cardinalNum2 < 0) {
            if ((-(cardinalNum1-cardinalNum2))%2 == 0) {
                redo = true;
                cout<< "These coordinates are invalid"<< endl;
            }
        }
    }
    return redo;
}

//This is the main function
int main() {

    //Getting user to choose which game mode to play
    cout << "You have been unfortunately chosen by the OCDSB to mark Eddie's compass project" << endl;
    cout << "" << endl;
    cout << "To begin choose whether you want to play in:" << endl;
    cout << "     ENTER 1 to play in BEARING mode (bearing to compass)" << endl;
    cout << "     ENTER 2 to play in COMPASS mode (compass to bearing, one step)" << endl;
    cout << "     ENTER 3 to play in COMPASS mode (compass to bearing, three step)" << endl;

    string gameChoice = "3";
    bool correctInput = false;
    cin >> gameChoice;

    do  {
        //This do while loop ensures that the user has entered a value between 1 and 3
        if ((gameChoice == "1") || (gameChoice == "2" || gameChoice == "3")) {
            correctInput = true;
        } else {
            cout << "You have not entered a valid input, please try again" << endl;
            cin >> gameChoice;
        }
        cout << correctInput << endl;
    } while (correctInput == false);

    if (gameChoice == "3") {
        //These are the critical variable inputs
        string primaryCardinal = "derp";
        string angleStr = "derp";
        string secondaryCardinal = "derp";

        double angle;
        double originalAngle = 0;
        bool redo = false;

        do {
            //Asks the user for necessary inputs and checks the inputs to see if they are valid
            cout <<"Please enter your compass direction: " << endl;
            cin >> primaryCardinal;
            cout <<"Please enter your angle: " << endl;
            cin >> angleStr;
            cout <<"Please enter your secondary direciton: " << endl;
            cin >> secondaryCardinal;

            redo = cardinalCheck(primaryCardinal, secondaryCardinal, angleStr);

        } while (redo == true);

        //Some necessary calculations
        angle = stod(angleStr);
        angle = getBearing(angle);
        originalAngle = angle;
        cout << angle << endl;
        string cardinals [6] = {"W", "N", "E", "S", "W", "N"};
        int cardinalNumSave = 0;

        //Turns the cardinals into bearing values
        if (secondaryCardinal == cardinals[cardinalNum1]) {
            angle = ((cardinalNum1-2)*90) + angle;
        } else if (secondaryCardinal == cardinals[cardinalNum1-2]) {
            angle = ((cardinalNum1-2)*90) - angle;
        } else {
            cout << "Something went here calculating the bearing angle" << endl;
        }
        //Makes sure the bearing values are between 0° and 359°
        angle = getBearing(angle);
        cout << "Your bearing is "<<angle<< "°C right fron North. Your direction is:  "<< bearingToCompass(angle, gameChoice)<< endl;

    } else if (gameChoice == "2") {

        cout << "You have been unfortunately chosen by the OCDSB to mark Eddie's compass project" << endl;
        bool redo = false;
        string directionStr = "bleBLERPBLERPrp";
        string primaryCardinal = "blerp";
        string secondaryCardinal = "blBLERPerp";
        string angleStr = "0";
        double angle = 0;

        do {
            //Gets input for the direction
            cout << "Please enter your direction in the form of N40E starting with a cardinal, bearing and secondary cardinal." << endl;
            cin >> directionStr;

            //Segments this direction into cardinals and angle
            primaryCardinal = directionStr[0];
            secondaryCardinal = directionStr[directionStr.length()-1];
            angleStr = directionStr.substr(1, directionStr.length()-2);

            //Checks if these are valid entries
            redo = cardinalCheck(primaryCardinal, secondaryCardinal, angleStr);
        } while (redo == true);

        angle = stod(angleStr);
        angle = getBearing(angle);
        double originalAngle = angle;
        cout << angle << endl;
        string cardinals [6] = {"W", "N", "E", "S", "W", "N"};
        int cardinalNumSave = 0;

        //Converts cardinals to proper angle measures which are added to the inputted angle
        if (secondaryCardinal == cardinals[cardinalNum1]) {
            angle = ((cardinalNum1-2)*90) + angle;
        } else if (secondaryCardinal == cardinals[cardinalNum1-2]) {
            angle = ((cardinalNum1-2)*90) - angle;
        } else {
            cout << "Something went here calculating the bearing angle" << endl;
        }
        //Makes sure the bearing is between 0° and 359°
        angle = getBearing(angle);

        //Outputs the bearing and a correct direction
        cout << "Your bearing is "<<angle<< "°C right fron North. Your direction is:  "<< bearingToCompass(angle, gameChoice)<< endl;

    } else if (gameChoice == "1") {
        cout << "Please enter your bearing in degrees"<< endl;
        double bearing = 0;
        string bearingStr = "hi";

        do {
            //Gets user to input a bearing and checks if it is a number
            cin >> bearingStr;
            if (is_number(bearingStr) == 0) {
                cout << "You have not entered a numeric value for the bearing, please try again my friend" << endl;
            }
        } while (is_number(bearingStr) == 0);
        std::string str = "3.14";
        double strVal;

        bearing = stod(bearingStr);
        cout << bearing << endl;
        //Converts bearing to compass direction
        bearingToCompass(bearing, gameChoice);

        //cin >> gameChoice;

        return 0;
    }
}

Assignment 2 - Bearings/PROJECT: BEARING/main.cpp