#include "software/geom/util.h"

#include <algorithm>
#include <boost/geometry/algorithms/intersection.hpp>
#include <boost/geometry/geometries/segment.hpp>
#include <boost/polygon/voronoi.hpp>
#include <cassert>
#include <cmath>
#include <g3log/g3log.hpp>
#include <iostream>
#include <limits>
#include <tuple>

#include "software/geom/rectangle.h"
#include "software/geom/segment.h"
#include "software/geom/voronoi_diagram.h"
#include "software/new_geom/angle.h"

double proj_length(const Segment &first, const Vector &second)
{
    return proj_length(first.toVector(), second - first.getSegStart().toVector());
}

double proj_length(const Vector &first, const Vector &second)
{
    return first.dot(second) / first.length();
}

double dist(const Point &first, const Point &second)
{
    return (first - second).length();
}

double dist(const Segment &first, const Segment &second)
{
    if (intersects(first, second))
    {
        return 0.0;
    }
    return std::sqrt(std::min(
        std::min(distsq(first, second.getSegStart()), distsq(first, second.getEnd())),
        std::min(distsq(second, first.getSegStart()), distsq(second, first.getEnd()))));
}

double dist(const Line &first, const Point &second)
{
    if (isDegenerate(first))
    {
        return dist(first.getFirst(), second);
    }
    return fabs((second - first.getFirst()).cross(first.getSecond() - first.getFirst()) /
                (first.getSecond() - first.getFirst()).length());
}

double dist(const Point &first, const Line &second)
{
    return dist(second, first);
}

double dist(const Point &first, const Segment &second)
{
    return std::sqrt(distsq(first, second));
}

double dist(const Segment &first, const Point &second)
{
    return dist(second, first);
}

double dist(const Point &first, const Polygon &second)
{
    if (second.containsPoint(first))
    {
        return 0;
    }

    double min_dist = DBL_MAX;

    // Calculate the distance from the point to each edge
    for (auto &segment : second.getSegments())
    {
        double current_dist = dist(first, segment);
        if (current_dist < min_dist)
        {
            min_dist = current_dist;
        }
    }
    return min_dist;
}

double dist(const Point &first, const Rectangle &second)
{
    if (second.containsPoint(first))
    {
        return 0;
    }

    // Calculate the distance from the point to each edge of the rectangle
    std::array<double, 4> distances = {
        dist(first, Segment(second.posXPosYCorner(), second.posXNegYCorner())),
        dist(first, Segment(second.posXNegYCorner(), second.negXNegYCorner())),
        dist(first, Segment(second.negXNegYCorner(), second.negXPosYCorner())),
        dist(first, Segment(second.negXPosYCorner(), second.posXPosYCorner()))};
    return *std::min_element(distances.begin(), distances.end());
}

double distsq(const Point &first, const Segment &second)
{
    double seglensq    = lengthSquared(second);
    Vector relsecond_s = first - second.getSegStart();
    Vector relsecond_e = first - second.getEnd();

    Vector s_vec2 = second.toVector();

    if (s_vec2.dot(relsecond_s) > 0 && second.reverse().toVector().dot(relsecond_e) > 0)
    {
        if (isDegenerate(second))
        {
            return relsecond_s.length();
        }
        double cross = relsecond_s.cross(s_vec2);
        return std::fabs(cross * cross / seglensq);
    }

    double lensq_s = distsq(second.getSegStart(), first),
           lensq_e = distsq(second.getEnd(), first);

    return std::min(lensq_s, lensq_e);
}

double distsq(const Segment &first, const Point &second)
{
    return distsq(second, first);
}

double distsq(const Point &first, const Point &second)
{
    return (first - second).lengthSquared();
}

bool isDegenerate(const Segment &segment)
{
    return distsq(segment.getSegStart(), segment.getEnd()) < EPS2;
}

bool isDegenerate(const Line &line)
{
    return distsq(line.getFirst(), line.getSecond()) < EPS2;
}

bool isDegenerate(const Ray &ray)
{
    return distsq(ray.getRayStart(), Point(ray.getDirection())) < EPS2;
}

double length(const Segment &segment)
{
    return dist(segment.getSegStart(), segment.getEnd());
}

double lengthSquared(const Segment &segment)
{
    return distsq(segment.getSegStart(), segment.getEnd());
}

double lengthSquared(const Line &line)
{
    (void)line;  // unused
    return std::numeric_limits<double>::infinity();
}

bool contains(const LegacyTriangle &out, const Point &in)
{
    double angle = 0;
    for (int i = 0, j = 2; i < 3; j = i++)
    {
        if ((in - out[i]).length() < EPS)
        {
            return true;  // SPECIAL CASE
        }
        double a =
            atan2((out[i] - in).cross(out[j] - in), (out[i] - in).dot(out[j] - in));
        angle += a;
    }
    return std::fabs(angle) > 6;
}

bool contains(const Circle &out, const Point &in)
{
    return distsq(out.getOrigin(), in) <= out.getRadius() * out.getRadius();
}

bool contains(const Circle &out, const Segment &in)
{
    return dist(in, out.getOrigin()) < out.getRadius();
}

bool contains(const Segment &out, const Point &in)
{
    if (collinear(in, out.getSegStart(), out.getEnd()))
    {
        // If the segment and point are in a perfect vertical line, we must use Y
        // coordinate centric logic
        if ((std::abs(in.x() - out.getEnd().x()) < EPS) &&
            (std::abs(out.getEnd().x() - out.getSegStart().x()) < EPS))
        {
            // if collinear we only need to check one of the coordinates,
            // in this case we select Y because all X values are equal
            return (in.y() <= out.getSegStart().y() && in.y() >= out.getEnd().y()) ||
                   (in.y() <= out.getEnd().y() && in.y() >= out.getSegStart().y());
        }

        // if collinear we only need to check one of the coordinates,
        // choose x because we know there is variance in these values
        return (in.x() <= out.getSegStart().x() && in.x() >= out.getEnd().x()) ||
               (in.x() <= out.getEnd().x() && in.x() >= out.getSegStart().x());
    }

    return false;
}

bool contains(const Ray &out, const Point &in)
{
    Point point_in_ray_direction = out.getRayStart() + out.getDirection();
    if (collinear(in, out.getRayStart(), point_in_ray_direction) &&
        (((in - out.getRayStart()).normalize() - out.getDirection().normalize())
             .length() < EPS))
    {
        return true;
    }
    return false;
}

bool contains(const Rectangle &out, const Point &in)
{
    return out.containsPoint(in);
}

bool intersects(const LegacyTriangle &first, const Circle &second)
{
    return contains(first, second.getOrigin()) ||
           dist(getSide(first, 0), second.getOrigin()) < second.getRadius() ||
           dist(getSide(first, 1), second.getOrigin()) < second.getRadius() ||
           dist(getSide(first, 2), second.getOrigin()) < second.getRadius();
}
bool intersects(const Circle &first, const LegacyTriangle &second)
{
    return intersects(second, first);
}

bool intersects(const Circle &first, const Circle &second)
{
    return (first.getOrigin() - second.getOrigin()).length() <
           (first.getRadius() + second.getRadius());
}

bool intersects(const Ray &first, const Segment &second)
{
    auto isect =
        lineIntersection(first.getRayStart(), first.getRayStart() + first.getDirection(),
                         second.getSegStart(), second.getEnd());
    // If the infinitely long vectors defined by ray and segment intersect, check that the
    // intersection is within their definitions
    if (isect.has_value())
    {
        return contains(first, isect.value()) && contains(second, isect.value());
    }
    // If there is no intersection, the ray and segment may be parallel, check if they are
    // overlapped
    return contains(second, first.getRayStart());
}
bool intersects(const Segment &first, const Ray &second)
{
    return intersects(second, first);
}

bool intersects(const Segment &first, const Circle &second)
{
    // if the segment is inside the circle AND at least one of the points is
    // outside the circle
    return contains(second, first) && (distsq(first.getSegStart(), second.getOrigin()) >
                                           second.getRadius() * second.getRadius() ||
                                       distsq(first.getEnd(), second.getOrigin()) >
                                           second.getRadius() * second.getRadius());
}
bool intersects(const Circle &first, const Segment &second)
{
    return intersects(second, first);
}

bool intersects(const Segment &first, const Segment &second)
{
    boost::geometry::model::segment<Point> AB(first.getSegStart(), first.getEnd());
    boost::geometry::model::segment<Point> CD(second.getSegStart(),
                                              second.getEnd());  // similar code

    return boost::geometry::intersects(AB, CD);
}

template <size_t N>
Point getVertex(const LegacyPolygon<N> &poly, unsigned int i)
{
    if (i > N)
        throw std::out_of_range("poly does not have that many sides!!!");
    else
        return poly[i];
}

template <size_t N>
void setVertex(LegacyPolygon<N> &poly, unsigned int i, const Vector &v)
{
    if (i > N)
        throw std::out_of_range("poly does not have that many sides!!!");
    else
        poly[i] = v;
}

template <size_t N>
Segment getSide(const LegacyPolygon<N> &poly, unsigned int i)
{
    return Segment(getVertex(poly, i), getVertex(poly, (i + 1) % N));
}

std::vector<Shot> angleSweepCirclesAll(const Point &src, const Point &p1, const Point &p2,
                                       const std::vector<Point> &obstacles,
                                       const double &radius)
{
    Angle p1_angle = (p1 - src).orientation();
    Angle p2_angle = (p2 - src).orientation();

    Angle start_angle = std::min(p1_angle, p2_angle);
    Angle end_angle   = std::max(p1_angle, p2_angle);

    // This handles the special case where the start and end angle straddle the
    // negative y axis, which causes some issues with angles "ticking over" from pi to
    // -pi and vice-versa
    if (end_angle - start_angle > Angle::half())
    {
        Angle start_angle_new = start_angle + (end_angle - start_angle).angleMod();
        end_angle             = start_angle;
        start_angle           = start_angle_new;
    }

    if (collinear(src, p1, p2))
    {
        // return a result that contains the direction of the line and zero angle if not
        // blocked by obstacles
        Segment collinear_seg = Segment(src, p1);
        for (Point p : obstacles)
        {
            if (intersects(collinear_seg, Circle(p, radius)))
            {
                // intersection with obstacle found, we're done here and we return nothing
                return {};
            }
        }

        return {Shot(Point(collinear_seg.toVector()), Angle::zero())};
    }

    // "Sweep" a line from the `src` to the target line segment, and create an "event"
    // whenever the line enters or leaves an obstacle, int value of `-1` to indicate the
    // sweep "leaving" an obstacle, and `+1` to indicate the sweep "entering" another
    // obstacle
    // The angle for each event is measured relative to the start angle
    std::vector<std::pair<Angle, int>> events;
    for (const Point &obstacle : obstacles)
    {
        Vector diff = obstacle - src;
        if (diff.length() < radius)
        {
            // `src` is within `radius` of this obstacle
            return {};
        }

        const Angle cent   = (diff.orientation() - start_angle).angleMod();
        const Angle span   = Angle::asin(radius / diff.length());
        const Angle range1 = cent - span;
        const Angle range2 = cent + span;

        if (range1 < Angle::zero() && range2 > end_angle - start_angle)
        {
            // Obstacle takes up entire angle we are sweeping
            return {};
        }

        if (range1 < -Angle::half() || range2 > Angle::half())
        {
            continue;
        }
        if (range1 > Angle::zero() && range1 < end_angle - start_angle)
        {
            events.push_back(std::make_pair(range1, -1));
        }
        if (range2 > Angle::zero() && range2 < end_angle - start_angle)
        {
            events.push_back(std::make_pair(range2, 1));
        }
    }

    if (events.empty())
    {
        // No obstacles in the way, so just return a range hitting the entire target
        // line segment
        return {
            Shot(Point((p1.toVector() + p2.toVector()) / 2), end_angle - start_angle)};
    }

    // Sort the events by angle
    std::sort(events.begin(), events.end());

    // Collapse all contiguous sections of "+1" and "-1" respectively, as these represent
    // overlapping obstacles (from the perspective of the `src` point to the target line
    // segment)
    std::vector<std::pair<Angle, int>> events_collapsed;
    for (auto &event : events)
    {
        if (events_collapsed.empty() || event.second != events_collapsed.back().second)
        {
            events_collapsed.emplace_back(event);
        }
    }

    if (events_collapsed[0].second == -1)
    {
        events_collapsed.insert(events_collapsed.begin(),
                                std::make_pair(Angle::zero(), 1));
    }
    if (events_collapsed.back().second == 1)
    {
        events_collapsed.emplace_back(std::make_pair(end_angle - start_angle, -1));
    }

    std::vector<Shot> result;
    for (unsigned i = 1; i < events_collapsed.size(); i += 2)
    {
        // Calculate the center of this range on the target line segement
        Angle range_start = events_collapsed[i - 1].first + start_angle;
        Angle range_end   = events_collapsed[i].first + start_angle;
        Angle mid         = (range_end - range_start) / 2 + range_start;
        Vector ray        = Vector::createFromAngle(mid) * 10.0;
        Point inter       = lineIntersection(src, src + ray, p1, p2).value();

        // Offset the final values by the start angle
        result.emplace_back(Shot(inter, range_end - range_start));
    }

    return result;
}

std::optional<Shot> angleSweepCircles(const Point &src, const Point &p1, const Point &p2,
                                      const std::vector<Point> &obstacles,
                                      const double &radius)
{
    // Get all possible shots we could take
    std::vector<Shot> possible_shots =
        angleSweepCirclesAll(src, p1, p2, obstacles, radius);

    // Sort by the interval angle (ie. the open angle the shot is going through)
    std::sort(possible_shots.begin(), possible_shots.end(),
              [](auto s1, auto s2) { return s1.getOpenAngle() > s2.getOpenAngle(); });

    // Return the shot through the largest open interval if there are any
    if (possible_shots.empty())
    {
        return std::nullopt;
    }
    return possible_shots[0];
}

std::vector<Point> circleBoundaries(const Point &centre, double radius, int num_points)
{
    Angle rotate_amount = Angle::full() / num_points;
    std::vector<Point> ans;
    Vector bound(radius, 0.0);
    for (int i = 0; i < num_points; i++)
    {
        Point temp = centre + bound;
        ans.push_back(temp);
        bound = bound.rotate(rotate_amount);
    }
    return ans;
}

bool collinear(const Point &a, const Point &b, const Point &c)
{
    if ((a - b).lengthSquared() < EPS2 || (b - c).lengthSquared() < EPS2 ||
        (a - c).lengthSquared() < EPS2)
    {
        return true;
    }
    return std::fabs((b - a).cross(c - a)) < EPS;
}

Point clipPoint(const Point &p, const Point &bound1, const Point &bound2)
{
    const double minx = std::min(bound1.x(), bound2.x());
    const double miny = std::min(bound1.y(), bound2.y());
    const double maxx = std::max(bound1.x(), bound2.x());
    const double maxy = std::max(bound1.y(), bound2.y());
    Point ret         = p;
    if (p.x() < minx)
    {
        ret.set(minx, ret.y());
    }
    else if (p.x() > maxx)
    {
        ret.set(maxx, ret.y());
    }
    if (p.y() < miny)
    {
        ret.set(ret.x(), miny);
    }
    else if (p.y() > maxy)
    {
        ret.set(ret.x(), maxy);
    }
    return ret;
}

Point clipPoint(const Point &p, const Rectangle &r)
{
    const double minx = r.negXNegYCorner().x();
    const double miny = r.negXNegYCorner().y();
    const double maxx = r.posXPosYCorner().x();
    const double maxy = r.posXPosYCorner().y();
    Point ret         = p;
    if (p.x() < minx)
    {
        ret.set(minx, ret.y());
    }
    else if (p.x() > maxx)
    {
        ret.set(maxx, ret.y());
    }
    if (p.y() < miny)
    {
        ret.set(ret.x(), miny);
    }
    else if (p.y() > maxy)
    {
        ret.set(ret.x(), maxy);
    }
    return ret;
}

std::vector<Point> lineCircleIntersect(const Point &centre, double radius,
                                       const Point &segA, const Point &segB)
{
    std::vector<Point> ans;

    // take care of 0 length segments too much error here
    if ((segB - segA).lengthSquared() < EPS)
    {
        return ans;
    }

    double lenseg = (segB - segA).dot(centre - segA) / (segB - segA).length();
    Point C       = segA + lenseg * (segB - segA).normalize();

    // if C outside circle no intersections
    if ((C - centre).lengthSquared() > radius * radius + EPS)
    {
        return ans;
    }

    // if C on circle perimeter return the only intersection
    if ((C - centre).lengthSquared() < radius * radius + EPS &&
        (C - centre).lengthSquared() > radius * radius - EPS)
    {
        ans.push_back(C);
        return ans;
    }
    // first possible intersection
    double lensegb = radius * radius - (C - centre).lengthSquared();

    ans.push_back(C - (lensegb * (segB - segA).normalize()));
    ans.push_back(C + lensegb * (segB - segA).normalize());

    return ans;
}

std::vector<Point> lineRectIntersect(const Rectangle &r, const Point &segA,
                                     const Point &segB)
{
    std::vector<Point> ans;
    for (unsigned int i = 0; i < 4; i++)
    {
        const Point &a = r[i];
        // to draw a line segment from point 3 to point 0
        const Point &b = r[(i + 1) % 4];
        if (intersects(Segment(a, b), Segment(segA, segB)) &&
            uniqueLineIntersects(a, b, segA, segB))
        {
            ans.push_back(lineIntersection(a, b, segA, segB).value());
        }
    }
    return ans;
}

Point vectorRectIntersect(const Rectangle &r, const Point &pointA, const Point &pointB)
{
    std::vector<Point> points =
        lineRectIntersect(r, pointA, pointA + ((pointB - pointA) * 100));
    for (Point i : points)
    {
        if (contains(Ray(pointA, (pointB - pointA)), i))
        {
            return i;
        }
    }
    return Point(1.0 / 0.0, 1.0 / 0.0);  // no solution found, propagate infinity
}

Point closestPointOnSeg(const Point &p, const Segment &segment)
{
    return closestPointOnSeg(p, segment.getSegStart(), segment.getEnd());
}
Point closestPointOnSeg(const Point &centre, const Point &segA, const Point &segB)
{
    // if one of the end-points is extremely close to the centre point
    // then return 0.0
    if ((segB - centre).lengthSquared() < EPS2)
    {
        return segB;
    }

    if ((segA - centre).lengthSquared() < EPS2)
    {
        return segA;
    }

    // take care of 0 length segments
    if ((segB - segA).lengthSquared() < EPS2)
    {
        return segA;
    }

    // find point C
    // which is the projection onto the line
    double lenseg = (segB - segA).dot(centre - segA) / (segB - segA).length();
    Point C       = segA + lenseg * (segB - segA).normalize();

    // check if C is in the line seg range
    double AC     = (segA - C).lengthSquared();
    double BC     = (segB - C).lengthSquared();
    double AB     = (segA - segB).lengthSquared();
    bool in_range = AC <= AB && BC <= AB;

    // if so return C
    if (in_range)
    {
        return C;
    }
    double lenA = (centre - segA).length();
    double lenB = (centre - segB).length();

    // otherwise return closest end of line-seg
    if (lenA < lenB)
    {
        return segA;
    }
    return segB;
}

Point closestPointOnLine(const Point &p, const Line &line)
{
    return closestPointOnLine(p, line.getFirst(), line.getSecond());
}
Point closestPointOnLine(const Point &centre, const Point &lineA, const Point &lineB)
{
    // find point C, the projection onto the line
    double len_line = (lineB - lineA).dot(centre - lineA) / (lineB - lineA).length();
    Point C         = lineA + len_line * (lineB - lineA).normalize();
    return C;

    // check if C is in the line range
    double AC     = (lineA - C).lengthSquared();
    double BC     = (lineB - C).lengthSquared();
    double AB     = (lineA - lineB).lengthSquared();
    bool in_range = AC <= AB && BC <= AB;

    // if so return C
    if (in_range)
    {
    }

    double lenA = (centre - lineA).length();
    double lenB = (centre - lineB).length();

    // otherwise return closest end of line-seg
    if (lenA < lenB)
    {
        return lineA;
    }
    return lineB;
}

bool uniqueLineIntersects(const Point &a, const Point &b, const Point &c, const Point &d)
{
    return std::abs((d - c).cross(b - a)) > EPS;
}

std::vector<Point> lineIntersection(const Segment &a, const Segment &b)
{
    if (std::fabs((b.getEnd() - b.getSegStart()).cross(a.getEnd() - a.getSegStart())) <
        EPS)
    {
        // parallel line segments, find if they're collinear and return the 2 points
        // on the line they both lay on if they are collinear and intersecting
        // shamelessly copypasted from
        // https://stackoverflow.com/questions/22456517/algorithm-for-finding-the-segment-overlapping-two-collinear-segments
        if (collinear(a.getSegStart(), b.getSegStart(), b.getEnd()) &&
            collinear(a.getEnd(), b.getSegStart(), b.getEnd()))
        {
            double slope = (a.getEnd().y() - a.getSegStart().y()) /
                           (a.getEnd().x() - a.getSegStart().x());
            bool isHorizontal = slope < EPS;
            bool isDescending = slope < 0 && !isHorizontal;
            double invertY    = isDescending || isHorizontal ? -1 : 1;

            Point min1 =
                Point(std::min(a.getSegStart().x(), a.getEnd().x()),
                      std::min(a.getSegStart().y() * invertY, a.getEnd().y() * invertY));
            Point max1 =
                Point(std::max(a.getSegStart().x(), a.getEnd().x()),
                      std::max(a.getSegStart().y() * invertY, a.getEnd().y() * invertY));

            Point min2 =
                Point(std::min(b.getSegStart().x(), b.getEnd().x()),
                      std::min(b.getSegStart().y() * invertY, b.getEnd().y() * invertY));
            Point max2 =
                Point(std::max(b.getSegStart().x(), b.getEnd().x()),
                      std::max(b.getSegStart().y() * invertY, b.getEnd().y() * invertY));

            Point minIntersection;
            if (isDescending)
                minIntersection = Point(std::max(min1.x(), min2.x()),
                                        std::min(min1.y() * invertY, min2.y() * invertY));
            else
                minIntersection = Point(std::max(min1.x(), min2.x()),
                                        std::max(min1.y() * invertY, min2.y() * invertY));

            Point maxIntersection;
            if (isDescending)
                maxIntersection = Point(std::min(max1.x(), max2.x()),
                                        std::max(max1.y() * invertY, max2.y() * invertY));
            else
                maxIntersection = Point(std::min(max1.x(), max2.x()),
                                        std::min(max1.y() * invertY, max2.y() * invertY));

            bool intersect =
                minIntersection.x() <= maxIntersection.x() &&
                ((!isDescending && minIntersection.y() <= maxIntersection.y()) ||
                 (isDescending && minIntersection.y() >= maxIntersection.y()));

            if (intersect)
            {
                return std::vector<Point>{minIntersection, maxIntersection};
            }
            else
                return std::vector<Point>();
        }
        else
            return std::vector<Point>();
    }

    return std::vector<Point>{
        a.getSegStart() +
        (a.getSegStart() - b.getSegStart()).cross(b.getEnd() - b.getSegStart()) /
            (b.getEnd() - b.getSegStart()).cross(a.getEnd() - a.getSegStart()) *
            (a.getEnd() - a.getSegStart())};
}

// shamelessly copy-pasted from RoboJackets
std::optional<Point> lineIntersection(const Point &a, const Point &b, const Point &c,
                                      const Point &d)
{
    Segment line1(a, b), line2(c, d);
    double x1 = line1.getSegStart().x();
    double y1 = line1.getSegStart().y();
    double x2 = line1.getEnd().x();
    double y2 = line1.getEnd().y();
    double x3 = line2.getSegStart().x();
    double y3 = line2.getSegStart().y();
    double x4 = line2.getEnd().x();
    double y4 = line2.getEnd().y();

    double denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
    if (denom == 0)
    {
        // log the parallel lines when we actually implement logging?
        return std::nullopt;
    }

    double deta = x1 * y2 - y1 * x2;
    double detb = x3 * y4 - y3 * x4;

    Point intersection;

    intersection.set((deta * (x3 - x4) - (x1 - x2) * detb) / denom,
                     (deta * (y3 - y4) - (y1 - y2) * detb) / denom);

    return std::make_optional(intersection);
}

std::pair<std::optional<Point>, std::optional<Point>> raySegmentIntersection(
    const Ray &ray, const Segment &segment)
{
    Point ray2 = ray.getRayStart() + ray.getDirection();

    std::optional<Point> intersection = lineIntersection(
        ray.getRayStart(), ray2, segment.getSegStart(), segment.getEnd());

    // If there exists a single intersection, and it exists on the ray and within the
    // segment
    if (intersection.has_value() && contains(ray, intersection.value()) &&
        contains(segment, intersection.value()))
    {
        return std::make_pair(intersection, std::nullopt);
    }
    // The ray and segment are parallel, and collinear
    else if (!intersection.has_value() &&
             collinear(ray.getRayStart(), segment.getSegStart(), segment.getEnd()))
    {
        // Check if ray passes through both segment start and end
        if (ray.getDirection().normalize() ==
                (segment.getSegStart() - ray.getRayStart()).normalize() &&
            ray.getDirection().normalize() ==
                (segment.getEnd() - ray.getRayStart()).normalize())
        {
            return std::make_pair(segment.getSegStart(), segment.getEnd());
        }

        // Since we know the ray and segment are overlapping (with ray origin within the
        // segment), return the ray start position, and the end of the segment that is in
        // the direction of the ray
        ray.getDirection().normalize() ==
                (segment.getEnd() - segment.getSegStart()).normalize()
            ? intersection = std::make_optional(segment.getEnd())
            : intersection = std::make_optional(segment.getSegStart());
        return std::make_pair(ray.getRayStart(), intersection.value());
    }
    // The ray and segment do not intersect at all
    else
    {
        return std::make_pair(std::nullopt, std::nullopt);
    }
}

std::pair<std::optional<Point>, std::optional<Point>> rayRectangleIntersection(
    const Ray &ray, const Rectangle &rectangle)
{
    std::vector<Segment> rectangle_segments = {
        Segment(rectangle.posXPosYCorner(), rectangle.negXPosYCorner()),
        Segment(rectangle.negXPosYCorner(), rectangle.negXNegYCorner()),
        Segment(rectangle.negXNegYCorner(), rectangle.posXNegYCorner()),
        Segment(rectangle.posXNegYCorner(), rectangle.posXPosYCorner()),
    };
    std::pair<std::optional<Point>, std::optional<Point>> result =
        std::make_pair(std::nullopt, std::nullopt);
    for (const auto &seg : rectangle_segments)
    {
        auto intersection = raySegmentIntersection(ray, seg);
        // Always take the result with more non-nullopt values
        if ((intersection.first && !result.first) ||
            (intersection.second && !result.second))
        {
            result = intersection;
        }
    }

    return result;
}

std::optional<Point> getRayIntersection(Ray ray1, Ray ray2)
{
    // Calculate if the intersecion exists along segments of infinite length
    std::optional<Point> intersection =
        lineIntersection(ray1.getRayStart(), ray1.getRayStart() + ray1.getDirection(),
                         ray2.getRayStart(), ray2.getRayStart() + ray2.getDirection());

    // Return if no intersection exists
    if (!intersection.has_value())
    {
        return std::nullopt;
    }

    // Check of the intersection exits along the direction of both rays
    if (((intersection.value() - ray1.getRayStart()).normalize() ==
         ray1.getDirection().normalize()) &&
        (intersection.value() - ray2.getRayStart()).normalize() ==
            ray2.getDirection().normalize())
    {
        return intersection.value();
    }
    else
    {
        return std::nullopt;
    }
}

Vector reflect(const Vector &v, const Vector &n)
{
    if (n.length() < EPS)
    {
        return v;
    }
    Vector normal = n.normalize();
    return v - 2 * v.dot(normal) * normal;
}

Point reflect(const Point &a, const Point &b, const Point &p)
{
    // Make a as origin.
    // Rotate by 90 degrees, does not matter which direction?
    Vector n = (b - a).rotate(Angle::quarter());
    return a + reflect(p - a, n);
}

Point calcBlockCone(const Vector &a, const Vector &b, const double &radius)
{
    if (a.length() < EPS || b.length() < EPS)
    {
    }
    // unit vector and bisector
    Vector au = a / a.length();
    Vector c  = au + b / b.length();
    // use similar triangle
    return Point(c * (radius / std::fabs(au.cross(c))));
}

Point calcBlockCone(const Point &a, const Point &b, const Point &p, const double &radius)
{
    return p + (calcBlockCone(a - p, b - p, radius)).toVector();
}

Vector calcBlockOtherRay(const Point &a, const Point &c, const Point &g)
{
    return reflect(c - a, g - c);  // this, and the next two instances, were
                                   // changed from a - c since reflect() was
                                   // fixed
}

double offsetToLine(Point x0, Point x1, Point p)
{
    Vector n;

    // get normal to line
    n = (x1 - x0).perpendicular().normalize();

    return fabs(n.dot(p - x0));
}

double offsetAlongLine(Point x0, Point x1, Point p)
{
    Vector n, v;

    // get normal to line
    n = x1 - x0;
    n = n.normalize();

    v = p - x0;

    return n.dot(v);
}

Point segmentNearLine(Point a0, Point a1, Point b0, Point b1)
{
    Vector v, n;
    Point p;
    double dn, t;

    v = a1 - a0;
    n = (b1 - b0).normalize();
    n = n.perpendicular();

    dn = v.dot(n);
    if (std::fabs(dn) < EPS)
    {
        return a0;
    }

    t = -(a0 - b0).dot(n) / dn;

    if (t < 0)
    {
        t = 0;
    }
    if (t > 1)
    {
        t = 1;
    }
    p = a0 + v * t;

    return p;
}

Point intersection(Point a1, Point a2, Point b1, Point b2)
{
    Vector a = a2 - a1;

    Vector b1r = (b1 - a1).rotate(-a.orientation());
    Vector b2r = (b2 - a1).rotate(-a.orientation());
    Vector br  = (b1r - b2r);

    return Vector(b2r.x() - b2r.y() * (br.x() / br.y()), 0.0).rotate(a.orientation()) +
           a1;
}

Angle acuteVertexAngle(Vector v1, Vector v2)
{
    return v1.orientation().minDiff(v2.orientation());
}

Angle acuteVertexAngle(Point p1, Point p2, Point p3)
{
    return acuteVertexAngle(p1 - p2, p3 - p2);
}

double closestPointTime(Point x1, Vector v1, Point x2, Vector v2)
{
    Vector v  = v1 - v2;
    double sl = v.lengthSquared();
    double t;

    if (sl < EPS)
    {
        return 0.0;  // parallel tracks, any time is ok.
    }
    t = -v.dot(x1 - x2) / sl;
    if (t < 0.0)
    {
        return 0.0;  // nearest time was in the past, now is closest point from
                     // now on.
    }
    return t;
}

bool pointInFrontVector(Point offset, Vector direction, Point p)
{
    // compare angle different
    Angle a1   = direction.orientation();
    Angle a2   = (p - offset).orientation();
    Angle diff = (a1 - a2).angleMod();
    return diff < Angle::quarter() && diff > -Angle::quarter();
}

std::pair<Point, Point> getCircleTangentPoints(const Point &start, const Circle &circle,
                                               double buffer)
{
    // If the point is already inside the circe arccos won't work so just return
    // the perp points
    if (contains(circle, start))
    {
        double perpDist = std::sqrt(circle.getRadius() * circle.getRadius() -
                                    (circle.getOrigin() - start).lengthSquared());
        Point p1 =
            start +
            (circle.getOrigin() - start).perpendicular().normalize(perpDist + buffer);
        Point p2 =
            start -
            ((circle.getOrigin() - start).perpendicular().normalize(perpDist + buffer));
        return std::make_pair(p1, p2);
    }
    else
    {
        double radiusAngle =
            std::acos(circle.getRadius() / (start - circle.getOrigin()).length());
        Point p1 = circle.getOrigin() + (start - circle.getOrigin())
                                            .rotate(Angle::fromRadians(radiusAngle))
                                            .normalize(circle.getRadius() + buffer);
        Point p2 = circle.getOrigin() + (start - circle.getOrigin())
                                            .rotate(-Angle::fromRadians(radiusAngle))
                                            .normalize(circle.getRadius() + buffer);
        return std::make_pair(p1, p2);
    }
}

std::pair<Ray, Ray> getCircleTangentRays(const Point reference, const Circle circle,
                                         double buffer)
{
    auto [tangent_point1, tangent_point2] =
        getCircleTangentPoints(reference, circle, buffer);

    return std::make_pair(Ray(tangent_point1, (tangent_point1 - reference).normalize()),
                          Ray(tangent_point2, (tangent_point2 - reference).normalize()));
}

bool pointIsRightOfLine(const Segment &line, const Point &point)
{
    return (line.getEnd().x() - line.getSegStart().x()) *
                   (point.y() - line.getSegStart().y()) -
               (line.getEnd().y() - line.getSegStart().y()) *
                   (point.x() - line.getSegStart().x()) <
           0.0;
}

Point getPointsMean(const std::vector<Point> &points)
{
    Point average = Point(0, 0);
    for (unsigned int i = 0; i < points.size(); i++)
    {
        average += points[i].toVector();
    }

    Vector averageVector = average.toVector();

    averageVector /= static_cast<double>(points.size());
    return Point(averageVector);
}

double getPointsVariance(const std::vector<Point> &points)
{
    Point mean = getPointsMean(points);

    double sum = 0.0;
    for (unsigned int i = 0; i < points.size(); i++)
    {
        sum += (points[i] - mean).lengthSquared();
    }

    sum /= static_cast<double>(points.size());
    return sqrt(sum);
}

std::optional<Segment> segmentEnclosedBetweenRays(Segment segment, Ray ray1, Ray ray2)
{
    // Create rays located at the extremes of the segment, that point in the direction
    // outwards are parallel to the segment
    const Ray extremes1 =
        Ray(segment.getEnd(), Vector(segment.getEnd() - segment.getSegStart()));
    const Ray extremes2 =
        Ray(segment.getSegStart(), Vector(segment.getSegStart() - segment.getEnd()));

    const std::optional<Point> extreme_intersect11 = getRayIntersection(extremes1, ray1);
    const std::optional<Point> extreme_intersect12 = getRayIntersection(extremes2, ray1);
    const std::optional<Point> extreme_intersect21 = getRayIntersection(extremes1, ray2);
    const std::optional<Point> extreme_intersect22 = getRayIntersection(extremes2, ray2);

    // Check for the cases that the rays intersect the same segment projection
    if ((extreme_intersect11.has_value() == extreme_intersect21.has_value()) ||
        (extreme_intersect21.has_value() == extreme_intersect22.has_value()))
    {
        return std::nullopt;
    }
    else
    {
        // Since we know that both rays aren't passing through the same side of the
        // segment at this point, then as long as they both only intersect 1 point the
        // segment must be enclosed between them
        if ((extreme_intersect11.has_value() != extreme_intersect12.has_value()) &&
            (extreme_intersect21.has_value() != extreme_intersect22.has_value()))
        {
            return std::make_optional(segment);
        }
        // Covers the case where a single ray passes by both sides of the segment
        else
        {
            return std::nullopt;
        }
    }
}

std::optional<Segment> getIntersectingSegment(Ray ray1, Ray ray2, Segment segment)
{
    // Check if the segment is enclosed between the rays
    if (segmentEnclosedBetweenRays(segment, ray1, ray2))
    {
        return segment;
    }

    // Calculate intersections of each individual ray and the segment
    auto [intersect11, intersect12] = raySegmentIntersection(ray1, segment);
    auto [intersect21, intersect22] = raySegmentIntersection(ray2, segment);

    // Check if there are any real intersections
    if (!intersect11.has_value() && !intersect21.has_value())
    {
        return std::nullopt;
    }
    // Check if one of the rays is overlapping the segment. If this is the case, return
    // the segment (If a ray intersects a ray more than one time it must be overlapping)
    else if ((intersect11.has_value() && intersect12.has_value()) ||
             (intersect21.has_value() && intersect22.has_value()))
    {
        return segment;
    }
    // If there is only one intersection point for each ray combine the intersections into
    // a segment
    else if ((intersect11.has_value() && !intersect12.has_value()) &&
             (intersect21.has_value() && !intersect22.has_value()))
    {
        return std::make_optional(Segment(intersect11.value(), intersect21.value()));
    }
    // If only one ray intersects the segment return the segment between the intersection
    // and the segment extreme (intersection11 is real, intersection22 is not)
    else if (intersect11.has_value() && !intersect21.has_value())
    {
        const Ray extremes1 =
            Ray(segment.getEnd(), Vector(segment.getEnd() - segment.getSegStart()));
        const Ray extremes2 =
            Ray(segment.getSegStart(), Vector(segment.getSegStart() - segment.getEnd()));
        ;

        std::optional<Point> extreme_intersect1 = getRayIntersection(extremes1, ray2);
        std::optional<Point> extreme_intersect2 = getRayIntersection(extremes2, ray2);

        if (extreme_intersect1.has_value())
        {
            return std::make_optional(Segment(intersect11.value(), segment.getEnd()));
        }
        else if (extreme_intersect2.has_value())
        {
            return std::make_optional(
                Segment(intersect11.value(), segment.getSegStart()));
        }
    }
    // If only one ray intersects the segment return the segment between the intersection
    // and the segment extreme (intersection11 is real, intersection22 is not)
    else if (intersect11.has_value() && !intersect21.has_value())
    {
        const Ray extremes1 =
            Ray(segment.getEnd(), Vector(segment.getEnd() - segment.getSegStart()));
        const Ray extremes2 =
            Ray(segment.getSegStart(), Vector(segment.getSegStart() - segment.getEnd()));
        ;

        std::optional<Point> extreme_intersect1 = getRayIntersection(extremes1, ray1);
        std::optional<Point> extreme_intersect2 = getRayIntersection(extremes2, ray1);

        if (extreme_intersect1.has_value())
        {
            return std::make_optional(Segment(intersect21.value(), segment.getEnd()));
        }
        else if (extreme_intersect2.has_value())
        {
            return std::make_optional(
                Segment(intersect21.value(), segment.getSegStart()));
        }
    }
    // All cases have been checked, return std::nullopt
    return std::nullopt;
}

std::optional<Segment> mergeOverlappingParallelSegments(Segment segment1,
                                                        Segment segment2)
{
    std::optional<Segment> redundant_segment =
        mergeFullyOverlappingSegments(segment1, segment2);

    // If the segments are not parallel, then return std::nullopt. (The segments are
    // parallel of all points are collinear)
    if (!collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getSegStart()) &&
        !collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getEnd()))
    {
        return std::nullopt;
    }
    // Check the case where one segment is completely contained in the other
    else if (redundant_segment.has_value())
    {
        return redundant_segment;
    }
    // Check if the beginning of segment2 lays inside segment1
    else if (contains(segment1, segment2.getSegStart()))
    {
        // If segment2.getSegStart() lays in segment1, then the combined segment is
        // segment2,getEnd() and the point furthest from segmen2.getEnd()
        return (segment1.getSegStart() - segment2.getEnd()).lengthSquared() >
                       (segment1.getEnd() - segment2.getEnd()).lengthSquared()
                   ? Segment(segment1.getSegStart(), segment2.getEnd())
                   : Segment(segment1.getEnd(), segment2.getEnd());
    }
    // Now check if the end of segment2 lays inside segment1
    else if (contains(segment1, segment2.getEnd()))
    {
        // If segment2.getSegStart() lays in segment1, then the combined segment is
        // segment2,getEnd() and the point furtherst from segmen2.getEnd()
        return (segment1.getSegStart() - segment2.getSegStart()).lengthSquared() >
                       (segment1.getEnd() - segment2.getSegStart()).lengthSquared()
                   ? Segment(segment1.getSegStart(), segment2.getSegStart())
                   : Segment(segment1.getEnd(), segment2.getSegStart());
    }
    return std::nullopt;
}

std::optional<Segment> mergeFullyOverlappingSegments(Segment segment1, Segment segment2)
{
    // If the segments are not parallel, then return std::nullopt. (The segments are
    // parallel if all points are collinear)
    if (!collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getSegStart()) &&
        !collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getEnd()))
    {
        return std::nullopt;
    }

    Segment largest_segment, smallest_segment;
    // Grab the largest segment
    if (segment1.toVector().lengthSquared() > segment2.toVector().lengthSquared())
    {
        largest_segment  = segment1;
        smallest_segment = segment2;
    }
    else
    {
        largest_segment  = segment2;
        smallest_segment = segment1;
    }

    // The segment is redundant if both points of the smallest segment are contained in
    // the largest segment
    if (contains(largest_segment, smallest_segment.getSegStart()) &&
        contains(largest_segment, smallest_segment.getEnd()))
    {
        return std::make_optional(largest_segment);
    }
    else
    {
        return std::nullopt;
    }
}

int calcBinaryTrespassScore(const Rectangle &rectangle, const Point &point)
{
    if (rectangle.containsPoint(point))
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

std::vector<Circle> findOpenCircles(Rectangle bounding_box, std::vector<Point> points)
{
    // We use a Voronoi Diagram and it's Delaunay triangulation to find the largest
    // open circles in the field
    // Reference: https://www.cs.swarthmore.edu/~adanner/cs97/s08/papers/schuster.pdf
    //
    // You can think of the Delauney triangulation as a way to connect all the points
    // (and the bounding_box corners) such that every point has three edges, and the
    // triangles formed are setup to be as regular as possible (ie. we avoid things
    // like super narrow triangles). The Voronoi diagram is the *dual* of this (scary
    // math words, I know), which just means you can construct it by taking the center
    // of each triangle and connecting it to the center of every adjacent triangle.
    //
    // So we can take each vertex on our voronoi diagram as the center of a open circle
    // on the field, and the size of the circle is the distance to the closest vertex
    // on the triangle that this vertex was created from

    // Filters out points that are outside of the bounding box
    points.erase(std::remove_if(points.begin(), points.end(),
                                [&bounding_box](const Point &p) {
                                    return !bounding_box.containsPoint(p);
                                }),
                 points.end());

    std::vector<Circle> empty_circles;

    // Creating the Voronoi diagram with 2 or less points will produce no edges so we need
    // to handle these cases manually
    if (points.empty())
    {
        // If there are no points, return an empty vector since there are no constraints
        // to the size of the circle.
        return empty_circles;
    }
    if (points.size() == 1)
    {
        // If there is only 1 point, return circles centered at all four corners of the
        // bounding bounding_box.
        for (Point &corner : bounding_box.corners())
        {
            empty_circles.emplace_back(Circle(corner, dist(points.front(), corner)));
        }
        return empty_circles;
    }
    if (points.size() == 2)
    {
        // If there are 2 point, split the points with a vector perpendicular to the
        // vector connecting the two points. Return 2 circles that are centered at the
        // points where the splitting vector intercepts the bounding_box. We should also
        // include circles centered at each of the corners.
        Vector connectedVec           = points[1] - points[0];
        Point halfPoint               = points[0] + (connectedVec * 0.5);
        Vector perpVec                = connectedVec.perpendicular();
        std::vector<Point> intersects = lineRectIntersect(
            bounding_box,
            halfPoint +
                (perpVec * dist(bounding_box.furthestCorner(halfPoint), halfPoint)),
            halfPoint -
                (perpVec * dist(bounding_box.furthestCorner(halfPoint), halfPoint)));
        std::vector<Point> corners = bounding_box.corners();
        intersects.insert(intersects.end(), corners.begin(), corners.end());
        for (const Point &intersect : intersects)
        {
            double radius = dist(findClosestPoint(intersect, points).value(), intersect);
            empty_circles.emplace_back(intersect, radius);
        }
        return empty_circles;
    }

    // Construct the voronoi diagram
    VoronoiDiagram vd(points);

    // The corners of the rectangles are locations for the centre of circles with their
    // radius being the distance to the corner's closest point.
    for (const Point &corner : bounding_box.corners())
    {
        Point closest = findClosestPoint(corner, points).value();
        empty_circles.emplace_back(Circle(corner, dist(corner, closest)));
    }

    std::vector<Point> intersects = vd.findVoronoiEdgeRecIntersects(bounding_box);

    // Radius of the circle will be the distance from the interception point
    // to the nearest input point.
    for (const Point &p : intersects)
    {
        double radius = (points[0] - p).length();
        for (const Point &inputP : points)
        {
            radius = std::min(radius, (inputP - p).length());
        }
        empty_circles.emplace_back(Circle(p, radius));
    }

    std::vector<Circle> calculatedEmptyCircles =
        vd.voronoiVerticesToOpenCircles(bounding_box);
    empty_circles.insert(empty_circles.end(), calculatedEmptyCircles.begin(),
                         calculatedEmptyCircles.end());

    // Sort the circles in descending order of radius
    std::sort(empty_circles.begin(), empty_circles.end(),
              [](auto c1, auto c2) { return c1.getRadius() > c2.getRadius(); });

    return empty_circles;
}

Polygon circleToPolygon(const Circle &circle, size_t num_points)
{
    std::vector<Point> points;
    for (unsigned i = 0; i < num_points; i++)
    {
        Point p = circle.getOrigin() +
                  Vector(circle.getRadius(), 0)
                      .rotate(Angle::fromDegrees((360.0 / num_points) * i));
        points.emplace_back(p);
    }
    return Polygon(points);
}

std::optional<Point> findClosestPoint(const Point &origin_point,
                                      std::vector<Point> test_points)
{
    std::optional<Point> closest_point = std::nullopt;

    if (!test_points.empty())
    {
        closest_point = *std::min_element(
            test_points.begin(), test_points.end(),
            [&](const Point &test_point1, const Point &test_point2) {
                return dist(origin_point, test_point1) < dist(origin_point, test_point2);
            });
    }

    return closest_point;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    cholesky.cpp
 * @brief   Efficient incomplete Cholesky on rank-deficient matrices, todo: constrained Cholesky
 * @author  Richard Roberts
 * @date    Nov 5, 2010
 */

#include <gtsam/base/debug.h>
#include <gtsam/base/cholesky.h>
#include <gtsam/base/timing.h>

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

using namespace std;

namespace gtsam {

  static const double negativePivotThreshold = -1e-1;
  static const double zeroPivotThreshold = 1e-6;
  static const double underconstrainedPrior = 1e-5;
  static const int underconstrainedExponentDifference = 12;

/* ************************************************************************* */
static inline int choleskyStep(Matrix& ATA, size_t k, size_t order) {

  const bool debug = ISDEBUG("choleskyCareful");

  // Get pivot value
  double alpha = ATA(k,k);

  // Correct negative pivots from round-off error
  if(alpha < negativePivotThreshold) {
    if(debug) {
      cout << "pivot = " << alpha << endl;
      print(ATA, "Partially-factorized matrix: ");
    }
    return -1;
  } else if(alpha < 0.0)
    alpha = 0.0;

  const double beta = sqrt(alpha);

  if(beta > zeroPivotThreshold) {
    const double betainv = 1.0 / beta;

    // Update k,k
    ATA(k,k) = beta;

    if(k < (order-1)) {
      // Update A(k,k+1:end) <- A(k,k+1:end) / beta
      typedef Matrix::RowXpr::SegmentReturnType BlockRow;
      BlockRow V = ATA.row(k).segment(k+1, order-(k+1));
      V *= betainv;

      // Update A(k+1:end, k+1:end) <- A(k+1:end, k+1:end) - v*v' / alpha
      ATA.block(k+1, k+1, order-(k+1), order-(k+1)) -= V.transpose() * V;
//      ATA.bottomRightCorner(order-(k+1), order-(k+1)).selfadjointView<Eigen::Upper>()
//          .rankUpdate(V.adjoint(), -1);
    }
    return 1;
  } else {
    // For zero pivots, add the underconstrained variable prior
    ATA(k,k) = underconstrainedPrior;
    for(size_t j=k+1; j<order; ++j)
      ATA(k,j) = 0.0;
    if(debug) cout << "choleskyCareful:  Skipping " << k << endl;
    return 0;
  }
}

/* ************************************************************************* */
pair<size_t,bool> choleskyCareful(Matrix& ATA, int order) {

  const bool debug = ISDEBUG("choleskyCareful");

  // Check that the matrix is square (we do not check for symmetry)
  assert(ATA.rows() == ATA.cols());

  // Number of rows/columns
  const size_t n = ATA.rows();

  // Negative order means factor the entire matrix
  if(order < 0)
    order = int(n);

  assert(size_t(order) <= n);

  // The index of the row after the last non-zero row of the square-root factor
  size_t maxrank = 0;
  bool success = true;

  // Factor row-by-row
  for(size_t k = 0; k < size_t(order); ++k) {
    int stepResult = choleskyStep(ATA, k, size_t(order));
    if(stepResult == 1) {
      if(debug) cout << "choleskyCareful:  Factored through " << k << endl;
      if(debug) print(ATA, "ATA: ");
      maxrank = k+1;
    } else if(stepResult == -1) {
      success = false;
      break;
    } /* else if(stepResult == 0) Found zero pivot */
  }

  return make_pair(maxrank, success);
}

/* ************************************************************************* */
bool choleskyPartial(Matrix& ABC, size_t nFrontal) {

  gttic(choleskyPartial);

  const bool debug = ISDEBUG("choleskyPartial");

  assert(ABC.rows() == ABC.cols());
  assert(ABC.rows() >= 0 && nFrontal <= size_t(ABC.rows()));

  const size_t n = ABC.rows();

  // Compute Cholesky factorization of A, overwrites A.
  gttic(lld);
  Eigen::ComputationInfo lltResult;
  if(nFrontal > 0)
  {
    Eigen::LLT<Matrix, Eigen::Upper> llt = ABC.block(0, 0, nFrontal, nFrontal).selfadjointView<Eigen::Upper>().llt();
    ABC.block(0, 0, nFrontal, nFrontal).triangularView<Eigen::Upper>() = llt.matrixU();
    lltResult = llt.info();
  }
  else
  {
    lltResult = Eigen::Success;
  }
  gttoc(lld);

  if(debug) cout << "R:\n" << Eigen::MatrixXd(ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>()) << endl;

  // Compute S = inv(R') * B
  gttic(compute_S);
  if(n - nFrontal > 0) {
    ABC.topLeftCorner(nFrontal,nFrontal).triangularView<Eigen::Upper>().transpose().solveInPlace(
        ABC.topRightCorner(nFrontal, n-nFrontal));
  }
  if(debug) cout << "S:\n" << ABC.topRightCorner(nFrontal, n-nFrontal) << endl;
  gttoc(compute_S);

  // Compute L = C - S' * S
  gttic(compute_L);
  if(debug) cout << "C:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
  if(n - nFrontal > 0)
    ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>().rankUpdate(
        ABC.topRightCorner(nFrontal, n-nFrontal).transpose(), -1.0);
  if(debug) cout << "L:\n" << Eigen::MatrixXd(ABC.bottomRightCorner(n-nFrontal,n-nFrontal).selfadjointView<Eigen::Upper>()) << endl;
  gttoc(compute_L);

  // Check last diagonal element - Eigen does not check it
  bool ok;
  if(lltResult == Eigen::Success) {
    if(nFrontal >= 2) {
      int exp2, exp1;
      (void)frexp(ABC(nFrontal-2, nFrontal-2), &exp2);
      (void)frexp(ABC(nFrontal-1, nFrontal-1), &exp1);
      ok = (exp2 - exp1 < underconstrainedExponentDifference);
    } else if(nFrontal == 1) {
      int exp1;
      (void)frexp(ABC(0,0), &exp1);
      ok = (exp1 > -underconstrainedExponentDifference);
    } else {
      ok = true;
    }
  } else {
    ok = false;
  }

  return ok;
}

}


/******************************************************************************
Copyright (c) 2017, Farbod Farshidian. All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder nor the names of its
  contributors may be used to endorse or promote products derived from
  this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
******************************************************************************/

#include <algorithm>
#include <cmath>
#include <iostream>
#include <vector>

#include <boost/numeric/odeint.hpp>

#include <ocs2_core/Types.h>
#include <ocs2_core/integration/eigenIntegration.h>

#include "ocs2_ddp/test/bouncingmass/Reference.h"

Reference::Reference(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) {
  Create5thOrdPol(t0, t1, p0, p1);
  polV_ = polyder(polX_);
  polU_ = polyder(polV_);

  t0_ = t0;
  t1_ = t1;
}

void Reference::getInput(scalar_t time, vector_t& input) const {
  input.setZero(1);
  for (int i = 0; i < polU_.size(); i++) {
    input[0] += polU_[i] * std::pow(time, i);
  }
}

vector_t Reference::getInput(scalar_t time) const {
  vector_t input;
  getInput(time, input);
  return input;
}

void Reference::getState(scalar_t time, vector_t& x) const {
  if (time <= t1_ && time >= t0_) {
    x.setZero(3);
    for (int i = 0; i < polU_.size(); i++) {
      x[0] += polX_[i] * std::pow(time, i);
      x[1] += polV_[i] * std::pow(time, i);
    }
  } else {
    interpolate_ext(time, x);
  }
}

void Reference::extendref(scalar_t delta, Reference* refPre, Reference* refPost) {
  delta_ = delta;
  boost::numeric::odeint::runge_kutta_dopri5<vector_t, scalar_t, vector_t, scalar_t, boost::numeric::odeint::vector_space_algebra> stepper;
  // Lambda for general system dynamics, assuming that the reference input is available
  auto model = [](const vector_t& x, vector_t& dxdt, const double t, vector_t uref) {
    matrix_t A(3, 3);
    A << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
    matrix_t B(3, 1);
    B << 0.0, 1.0, 0.0;

    dxdt = A * x + B * uref;
  };
  // pre-part of extension
  if (refPre != nullptr) {
    // Construct Lambda to represent System Dynamics with correct reference input
    auto preModel = [&refPre, &model](const vector_t& x, vector_t& dxdt, const double t) {
      vector_t uref = refPre->getInput(t);
      model(x, dxdt, t, uref);
    };
    // Construct lambda to act as observer, which will store the time and state trajectories
    scalar_array_t* timeStorePtr = &tPre_;
    vector_array_t* stateStorePtr = &xPre_;
    auto preObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) {
      timeStorePtr->push_back(t);
      stateStorePtr->push_back(x);
    };

    vector_t x0;
    getState(t0_, x0);
    scalar_t t0 = t0_;
    scalar_t t1 = t0 - delta;
    scalar_t dt = -1e-3;

    boost::numeric::odeint::integrate_adaptive(stepper, preModel, x0, t0, t1, dt, preObserver);
    std::reverse(std::begin(tPre_), std::end(tPre_));
    std::reverse(std::begin(xPre_), std::end(xPre_));
  }

  // post-part of extension
  if (refPost != nullptr) {
    // Construct Lambda to represent System Dynamics with correct reference input
    auto postModel = [&refPost, &model](const vector_t& x, vector_t& dxdt, const double t) {
      vector_t uref = refPost->getInput(t);
      model(x, dxdt, t, uref);
    };
    // Construct lambda to act as observer, which will store the time and state trajectories
    scalar_array_t* timeStorePtr = &tPost_;
    vector_array_t* stateStorePtr = &xPost_;
    auto postObserver = [&timeStorePtr, &stateStorePtr](vector_t& x, scalar_t& t) {
      timeStorePtr->push_back(t);
      stateStorePtr->push_back(x);
    };

    vector_t x0;
    getState(t1_, x0);
    scalar_t t0 = t1_;
    scalar_t t1 = t0 + delta;
    scalar_t dt = 1e-3;
    boost::numeric::odeint::integrate_adaptive(stepper, postModel, x0, t0, t1, dt, postObserver);
  }
}

void Reference::Create5thOrdPol(scalar_t t0, scalar_t t1, const vector_t& p0, const vector_t& p1) {
  Eigen::Matrix<scalar_t, 6, 6> A;
  Eigen::Matrix<scalar_t, 6, 6> Ainv;

  A << 1, t0, std::pow(t0, 2), std::pow(t0, 3), std::pow(t0, 4), std::pow(t0, 5), 0, 1, 2 * t0, 3 * std::pow(t0, 2), 4 * std::pow(t0, 3),
      5 * std::pow(t0, 4), 0, 0, 2, 6 * t0, 12 * std::pow(t0, 2), 20 * std::pow(t0, 3), 1, t1, std::pow(t1, 2), std::pow(t1, 3),
      std::pow(t1, 4), std::pow(t1, 5), 0, 1, 2 * t1, 3 * std::pow(t1, 2), 4 * std::pow(t1, 3), 5 * std::pow(t1, 4), 0, 0, 2, 6 * t1,
      12 * std::pow(t1, 2), 20 * std::pow(t1, 3);

  Ainv = A.inverse();

  Eigen::Matrix<scalar_t, 6, 1> x;
  x << p0, p1;
  polX_ = Ainv * x;
}

void Reference::interpolate_ext(scalar_t time, vector_t& x) const {
  const scalar_array_t* tVec;
  const vector_array_t* xVec;
  if (time < t0_) {
    tVec = &tPre_;
    xVec = &xPre_;
    x = xPre_.front();
  } else {
    tVec = &tPost_;
    xVec = &xPost_;
    x = xPost_.back();
  }

  int idx;
  for (int i = 0; i < tVec->size() - 1; i++) {
    if (time > tVec->at(i) && time < tVec->at(i + 1)) {
      idx = i;
      scalar_t fac = (time - tVec->at(idx)) / (tVec->at(idx + 1) - tVec->at(idx));
      x = fac * xVec->at(idx) + (1 - fac) * xVec->at(idx + 1);
      return;
    }
  }
}

void Reference::display() {
  std::cerr << "#########################" << std::endl;
  std::cerr << "#Pre-Extended-Trajectory#" << std::endl;
  std::cerr << "#########################" << std::endl;
  for (int i = 0; i < tPre_.size(); i++) {
    std::cerr << tPre_[i] << ";" << xPre_[i][0] << ";" << xPre_[i][1] << std::endl;
  }

  std::cerr << "#########################" << std::endl;
  std::cerr << "####Normal-Trajectory####" << std::endl;
  std::cerr << "#########################" << std::endl;

  scalar_t dt = 0.01;
  for (int i = 0; i < (t1_ - t0_) / dt; i++) {
    scalar_t t = t0_ + dt * i;
    vector_t x;
    getState(t, x);

    std::cerr << t << ";" << x[0] << ";" << x[1] << std::endl;
  }

  std::cerr << "##########################" << std::endl;
  std::cerr << "#Post-Extended-Trajectory#" << std::endl;
  std::cerr << "##########################" << std::endl;

  for (int i = 0; i < tPost_.size(); i++) {
    std::cerr << tPost_[i] << ";" << xPost_[i][0] << ";" << xPost_[i][1] << std::endl;
  }
}

Eigen::Matrix<scalar_t, 6, 1> Reference::polyder(Eigen::Matrix<scalar_t, 6, 1> pol) {
  Eigen::Matrix<scalar_t, 6, 1> polOld = pol;

  for (int i = 0; i < pol.size(); i++) {
    if (i < pol.size() - 1) {
      pol[i] = (i + 1) * polOld[i + 1];
    } else {
      pol[i] = 0;
    }
  }

  return pol;
}


#include "ekf.h"

#include <ros/ros.h>
#include <ros/console.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Range.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include "conversion.h"

using namespace std;
using namespace Eigen;

//X_state: p q v gb ab   with time stamp aligned between imu and img
/*
    EKF model
    prediction:
    xt~ = xt-1 + dt*f(xt-1, ut, 0)
    sigmat~ = Ft*sigmat-1*Ft' + Vt*Qt*Vt'
    Update:
    Kt = sigmat~*Ct'*(Ct*sigmat~*Ct' + Wt*Rt*Wt')^-1
    xt = xt~ + Kt*(zt - g(xt~,0))
    sigmat = sigmat~ - Kt*Ct*sigmat~
*/
/*
   -pi ~ pi crossing problem:
   1. the model prpagation: X_state should be limited to [-pi,pi] after predicting and updating
   2. inovation crossing: (measurement - g(X_state)) should also be limited to [-pi,pi] when getting the inovation.
   z_measurement is normally in [-pi~pi]
*/

// imu frame is imu body frame

//odom: pose px,py pz orientation qw qx qy qz
//imu: acc: x y z gyro: wx wy wz

ros::Publisher odom_pub;
ros::Publisher cam_odom_pub;

//state
geometry_msgs::Pose pose;
Vector3d position, orientation, velocity;

// Now set up the relevant matrices
//states X [p q pdot]  [px,py,pz, wx,wy,wz, vx,vy,vz]
size_t stateSize;                                // x = [p q pdot bg ba]
size_t stateSize_pqv;                                // x = [p q pdot]
size_t measurementSize;                          // z = [p q]
size_t inputSize;                                // u = [w a]
VectorXd X_state(stateSize);                     // x (in most literature)
VectorXd u_input;
VectorXd Z_measurement;                          // z
MatrixXd StateCovariance;                        // sigma
MatrixXd Kt_kalmanGain;                          // Kt
VectorXd X_state_correct(stateSize);                     // x (in most literature)
MatrixXd StateCovariance_correct;                        // sigma
MatrixXd Qt;
MatrixXd Rt;
Vector3d u_gyro;
Vector3d u_acc;
Vector3d gravity(0., 0., -9.8); //need to estimate the bias 9.8099
Vector3d bg_0(0., 0., 0); //need to estimate the bias
Vector3d ba_0(0., 0., 0); //need to estimate the bias  0.1
Vector3d ng(0., 0., 0.);
Vector3d na(0., 0., 0.);
Vector3d nbg(0., 0., 0.);
Vector3d nba(0., 0., 0.);

Vector3d q_last;
Vector3d bg_last;
Vector3d ba_last;

//Qt imu covariance matrix  smaller believe system(imu) more
double gyro_cov = 0.01;
double acc_cov = 0.01;
//Rt visual odomtry covariance smaller believe measurement more
double position_cov = 0.1;
double q_rp_cov = 0.1;
double q_yaw_cov = 0.1;

double dt = 0.005; //second
double t_last, t_now;
bool first_frame_imu = true;
bool first_frame_tag_odom = true;
bool test_odomtag_call = false;
bool odomtag_call = false;

double time_now, time_last;
double time_odom_tag_now;
double diff_time;
//world frame points velocity
vector< pair<VectorXd, sensor_msgs::Imu> > sys_seq;  // keep a sequence of sys imu and X_state(before imu system prediction)
vector<MatrixXd> cov_seq;

//Rotation from the camera frame to the IMU frame
Matrix3d Rc_i;
Vector3d tc_i;  //  cam in imu frame
int cnt = 0;
Vector3d INNOVATION_;
Matrix3d Rr_i;
Vector3d tr_i;  //  rigid body in imu frame

void imu_callback(const sensor_msgs::Imu::ConstPtr &msg)
{
    // first_frame_tag_odom 初始为true，收到vicon后设置为false
    if(!first_frame_tag_odom)
    {
        // 第一次回调：发布初始值
        if(first_frame_imu)
        {
            first_frame_imu = false;
            time_now = msg->header.stamp.toSec();
            time_last = time_now;
            // 发布初始值
            system_pub(msg->header.stamp);
        }
        else
        {
            time_now = msg->header.stamp.toSec();
            // 时间差
            dt = time_now - time_last;

            if(odomtag_call)
            {
                odomtag_call = false;
                diff_time = time_now - time_odom_tag_now;
                if(diff_time<0)
                {
                    cout << "diff time: " << diff_time << endl;  //???!!! exist !!!???
                    cout << "timeimu: " << time_now - 1.60889e9 << " time_odom: " << time_odom_tag_now - 1.60889e9 << endl;
                    // cout << "diff time: " << diff_time << endl;  //about 30ms
                }
            }
            MatrixXd Ft;
            MatrixXd Vt;

            // 将imu存为输入值
            u_gyro(0) = msg->angular_velocity.x;
            u_gyro(1) = msg->angular_velocity.y;
            u_gyro(2) = msg->angular_velocity.z;
            u_acc(0)  = msg->linear_acceleration.x;
            u_acc(1)  = msg->linear_acceleration.y;
            u_acc(2)  = msg->linear_acceleration.z;

            // 上一时刻姿态
            q_last = X_state.segment<3>(3);  // last X2
            bg_last = X_state.segment<3>(9);  //last X4
            ba_last = X_state.segment<3>(12);  //last X5
            Ft = MatrixXd::Identity(stateSize, stateSize) + dt*diff_f_diff_x(q_last, u_gyro, u_acc, bg_last, ba_last);

            Vt = dt*diff_f_diff_n(q_last);

            // 使用输入更新状态
            X_state += dt*F_model(u_gyro, u_acc);
            // 欧拉角限制幅度
            if(X_state(3) > PI)  X_state(3) -= 2*PI;
            if(X_state(3) < -PI) X_state(3) += 2*PI;
            if(X_state(4) > PI)  X_state(4) -= 2*PI;
            if(X_state(4) < -PI) X_state(4) += 2*PI;
            if(X_state(5) > PI)  X_state(5) -= 2*PI;
            if(X_state(5) < -PI) X_state(5) += 2*PI;
            // 更新COV
            StateCovariance = Ft*StateCovariance*Ft.transpose() + Vt*Qt*Vt.transpose();

            time_last = time_now;

            system_pub(msg->header.stamp);
        }
    }

}

VectorXd get_pose_from_mocap(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    Matrix3d Rr_w;    //rigid body in world
    Vector3d tr_w;
    Matrix3d Ri_w;
    Vector3d ti_w;
    Vector3d p_temp;
    // 位置
    p_temp(0) = msg->pose.position.x;
    p_temp(1) = msg->pose.position.y;
    p_temp(2) = msg->pose.position.z;
    //quaternion2euler:  ZYX  roll pitch yaw
    Quaterniond q;
    // 姿态
    q.w() = msg->pose.orientation.w;
    q.x() = msg->pose.orientation.x;
    q.y() = msg->pose.orientation.y;
    q.z() = msg->pose.orientation.z;

    // 刚体的位置和姿态
    Rr_w = q.toRotationMatrix();
    tr_w = p_temp;
    // imu的位置和姿态
    Ri_w = Rr_w * Rr_i.inverse();
    ti_w = tr_w - Ri_w*tr_i;
    Vector3d euler = mat2euler(Ri_w);

    VectorXd pose = VectorXd::Random(6);
    // imu的位置和姿态
    pose.segment<3>(0) = ti_w;
    pose.segment<3>(3) = euler;

    return pose;
}

void mocap_callback(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    // 第一次回调
    if(first_frame_tag_odom)
    {
        first_frame_tag_odom = false;
        time_odom_tag_now = msg->header.stamp.toSec();

        VectorXd odom_pose = get_pose_from_mocap(msg);
        X_state.segment<3>(0) = odom_pose.segment<3>(0);
        X_state.segment<3>(3) = odom_pose.segment<3>(3);
    }
    else
    {
        time_odom_tag_now = msg->header.stamp.toSec();
        MatrixXd Ct;
        MatrixXd Wt;

        // 获取得到imu的位置姿态（imu和质心存在偏差）
        VectorXd odom_pose = get_pose_from_mocap(msg);
        // 更新测量值
        Z_measurement.segment<3>(0) = odom_pose.segment<3>(0);
        Z_measurement.segment<3>(3) = odom_pose.segment<3>(3);
        // 发布测量值（即mocap的原始值）
        cam_system_pub(msg->header.stamp);

        Ct = diff_g_diff_x();
        Wt = diff_g_diff_v();

        // 更新kalman增益
        Kt_kalmanGain = StateCovariance*Ct.transpose() * (Ct*StateCovariance*Ct.transpose() + Wt*Rt*Wt.transpose()).inverse();
        VectorXd gg = g_model();
        VectorXd innovation = Z_measurement - gg;

        //Prevent innovation changing suddenly when euler from -Pi to Pi
        if(innovation(3) > 6)  innovation(3) -= 2*PI;
        if(innovation(3) < -6) innovation(3) += 2*PI;
        if(innovation(4) > 6)  innovation(4) -= 2*PI;
        if(innovation(4) < -6) innovation(4) += 2*PI;
        if(innovation(5) > 6)  innovation(5) -= 2*PI;
        if(innovation(5) < -6) innovation(5) += 2*PI;
        INNOVATION_ = innovation.segment<3>(3);
        // 使用测量值更新状态
        X_state += Kt_kalmanGain*(innovation);
        if(X_state(3) > PI)  X_state(3) -= 2*PI;
        if(X_state(3) < -PI) X_state(3) += 2*PI;
        if(X_state(4) > PI)  X_state(4) -= 2*PI;
        if(X_state(4) < -PI) X_state(4) += 2*PI;
        if(X_state(5) > PI)  X_state(5) -= 2*PI;
        if(X_state(5) < -PI) X_state(5) += 2*PI;
        StateCovariance = StateCovariance - Kt_kalmanGain*Ct*StateCovariance;

        test_odomtag_call = true;
        odomtag_call = true;

        if(cnt == 10||cnt==50||cnt==90)
        {
            // cout << "Ct: \n" << Ct << "\nWt:\n" << Wt << endl;
            // cout << "Kt_kalmanGain: \n" << Kt_kalmanGain << endl;
            // cout << "\ninnovation: \n" << Kt_kalmanGain*innovation  << "\ndt:\n" << dt << endl;
            // cout << "\ninnovation: \n" << Kt_kalmanGain*innovation  << endl;
            // cout << "\ninnovation: \n" << INNOVATION_ << endl;
        }
        cnt++;
        if(cnt>100) cnt=101;
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "mocap_ekf_node");
    ros::NodeHandle n("~");
    // 【订阅】 IMU数据，来自PX4
    ros::Subscriber s1 = n.subscribe("imu", 100, imu_callback, ros::TransportHints().tcpNoDelay());
    // 【订阅】 VICON数据
    ros::Subscriber s2 = n.subscribe("pose", 100, mocap_callback, ros::TransportHints().tcpNoDelay());
    // 【发布】 融合后的odom(发布频率为imu的频率)
    odom_pub = n.advertise<nav_msgs::Odometry>("ekf_odom", 100);
    // 【发布】 相机odom？
    cam_odom_pub = n.advertise<nav_msgs::Odometry>("cam_ekf_odom", 100);

    n.getParam("gyro_cov", gyro_cov);
    n.getParam("acc_cov", acc_cov);
    n.getParam("position_cov", position_cov);
    n.getParam("q_rp_cov", q_rp_cov);
    n.getParam("q_yaw_cov", q_yaw_cov);

    cout << "Q:" << gyro_cov << " " << acc_cov << " R: " << position_cov << " " << q_rp_cov << " " << q_yaw_cov << endl;

    // 初始化
    initsys();
    cout << "initsys" << endl;

    cout << "======================" << endl;
    double r = atan2(1,-100);
    double p = asin(-0.707);
    double y = atan2(-1, -100);
    cout << "r: " << r << " p: " << p << " y: " << y << endl;
    cout << "======================" << endl;

    ros::spin();
}

void system_pub(ros::Time stamp)
{
    nav_msgs::Odometry odom_fusion;
    odom_fusion.header.stamp = stamp;
    odom_fusion.header.frame_id = "world";
    // odom_fusion.header.frame_id = "imu";
    odom_fusion.pose.pose.position.x = X_state(0);
    odom_fusion.pose.pose.position.y = X_state(1);
    odom_fusion.pose.pose.position.z = X_state(2);
    Quaterniond q;
    q = euler2quaternion(X_state.segment<3>(3));
    odom_fusion.pose.pose.orientation.w = q.w();
    odom_fusion.pose.pose.orientation.x = q.x();
    odom_fusion.pose.pose.orientation.y = q.y();
    odom_fusion.pose.pose.orientation.z = q.z();
    odom_fusion.twist.twist.linear.x = X_state(6);
    odom_fusion.twist.twist.linear.y = X_state(7);
    odom_fusion.twist.twist.linear.z = X_state(8);
    odom_pub.publish(odom_fusion);
}

void cam_system_pub(ros::Time stamp)
{
    // 测量值
    nav_msgs::Odometry odom_fusion;
    odom_fusion.header.stamp = stamp;
    odom_fusion.header.frame_id = "world";

    odom_fusion.pose.pose.position.x = Z_measurement(0);
    odom_fusion.pose.pose.position.y = Z_measurement(1);
    odom_fusion.pose.pose.position.z = Z_measurement(2);
    Quaterniond q;
    q = euler2quaternion(Z_measurement.segment<3>(3));
    odom_fusion.pose.pose.orientation.w = q.w();
    odom_fusion.pose.pose.orientation.x = q.x();
    odom_fusion.pose.pose.orientation.y = q.y();
    odom_fusion.pose.pose.orientation.z = q.z();

    odom_fusion.twist.twist.angular.x = diff_time;
    odom_fusion.twist.twist.angular.y = dt;
    cam_odom_pub.publish(odom_fusion);
}

//process model
void initsys()
{
    //  camera position in the IMU frame = (0.05, 0.05, 0)
    // camera orientaion in the IMU frame = Quaternion(0, 1, 0, 0); w x y z, respectively
    //					   RotationMatrix << 1, 0, 0,
    //							             0, -1, 0,
    //                                       0, 0, -1;
    //set the cam2imu params
    Rc_i = Quaterniond(0, 1, 0, 0).toRotationMatrix();
    // cout << "R_cam" << endl << Rc_i << endl;
    tc_i << 0.05, 0.05, 0;

    Rr_i = Quaterniond(1, 0, 0, 0).toRotationMatrix();
    tr_i << 0, 0, 0.055;
    cout << "Rr_i: " << endl << Rr_i << endl;
    cout << "tr_i: " << endl << tr_i << endl;
    //  rigid body position in the IMU frame = (0, 0, 0.04)
    // rigid body orientaion in the IMU frame = Quaternion(1, 0, 0, 0); w x y z, respectively
    //					   RotationMatrix << 1, 0, 0,
    //							             0, 1, 0,
    //                                       0, 0, 1;

    //states X
    // 状态维度 [p q pdot bg ba]  [px,py,pz, wx,wy,wz（欧拉角）, vx,vy,vz bgx,bgy,bgz bax,bay,baz]
    stateSize = 15;                                                                 // x = [p q pdot bg ba]
    stateSize_pqv = 9;                                                              // x = [p q pdot]
    // 测量维度 位置+姿态
    measurementSize = 6;                                                            // z = [p q]
    // 输入维度 w是角速度 a是加速度
    inputSize = 6;                                                                  // u = [w a]
    X_state = VectorXd::Zero(stateSize);                                            // x
    //velocity
    X_state(6) = 0;
    X_state(7) = 0;
    X_state(8) = 0;
    // bias
    X_state.segment<3>(9) = bg_0;
    X_state.segment<3>(12) = ba_0;
    // 输入
    u_input = VectorXd::Zero(inputSize);
    // 测量
    Z_measurement = VectorXd::Zero(measurementSize);                                // z
    // 状态cov
    StateCovariance = MatrixXd::Identity(stateSize, stateSize);                     // sigma
    // kalman增益
    Kt_kalmanGain = MatrixXd::Identity(stateSize, measurementSize);                 // Kt
    // Ct_stateToMeasurement = MatrixXd::Identity(stateSize, measurementSize);         // Ct
    // ？
    X_state_correct = X_state;
    // ？
    StateCovariance_correct = StateCovariance;

    Qt = MatrixXd::Identity(inputSize, inputSize);  //6x6 input [gyro acc]covariance
    Rt = MatrixXd::Identity(measurementSize, measurementSize); //6x6 measurement [p q]covariance

    // You should also tune these parameters
    // Q imu covariance matrix; Rt visual odomtry covariance matrix
    // //Rt visual odomtry covariance smaller believe measurement more
    Qt.topLeftCorner(3, 3) = gyro_cov * Qt.topLeftCorner(3, 3);
    Qt.bottomRightCorner(3, 3) = acc_cov * Qt.bottomRightCorner(3, 3);
    Rt.topLeftCorner(3, 3) = position_cov * Rt.topLeftCorner(3, 3);
    Rt.bottomRightCorner(3, 3) = q_rp_cov * Rt.bottomRightCorner(3, 3);
    Rt.bottomRightCorner(1, 1) = q_yaw_cov * Rt.bottomRightCorner(1, 1);
}

void getState(Vector3d& p, Vector3d& q, Vector3d& v, Vector3d& bg, Vector3d& ba)
{
	p = X_state.segment<3>(0);
	q = X_state.segment<3>(3);
	v = X_state.segment<3>(6);
    bg = X_state.segment<3>(9);
    ba = X_state.segment<3>(12);
}

VectorXd F_model(Vector3d gyro, Vector3d acc)
{
    // IMU is in FLU frame
    // Transform IMU frame into "world" frame whose original point is FLU's original point and the XOY plain is parallel with the ground and z axis is up
    VectorXd f(VectorXd::Zero(stateSize));
    Vector3d p, q, v, bg, ba;
    getState(p, q, v, bg, ba);
    f.segment<3>(0) = v;
    f.segment<3>(3) = w_Body2Euler(q)*(gyro-bg-ng);
    f.segment<3>(6) = gravity + euler2mat(q)*(acc-ba-na);
    f.segment<3>(9) = nbg;
    f.segment<3>(12) = nba;
    return f;
}

VectorXd g_model()
{
    VectorXd g(VectorXd::Zero(measurementSize));

    g.segment<6>(0) = X_state.segment<6>(0);

    return g;
}

//F_model G_model Jocobian
//diff_f()/diff_x (x_t-1  ut  noise=0)   At     Ft = I+dt*At
MatrixXd diff_f_diff_x(Vector3d q_last, Vector3d gyro, Vector3d acc, Vector3d bg_last, Vector3d ba_last)
{
    double cr = cos( q_last(0));
    double sr = sin( q_last(0));
    double cp = cos( q_last(1));
    double sp = sin( q_last(1));
    double cy = cos( q_last(2));
    double sy = sin( q_last(2));

    // ng na = 0 nbg nba = 0
    double Ax = acc(0) - ba_last(0);
    double Ay = acc(1) - ba_last(1);
    double Az = acc(2) - ba_last(2);
    // double Wx = gyro(0) - bg_last(0);
    double Wy = gyro(1) - bg_last(1);
    double Wz = gyro(2) - bg_last(2);

    MatrixXd diff_f_diff_x_jacobian(MatrixXd::Zero(stateSize, stateSize));
    MatrixXd diff_f_diff_x_jacobian_pqv(MatrixXd::Zero(stateSize_pqv, stateSize_pqv));

    diff_f_diff_x_jacobian_pqv <<  0, 0, 0, 0, 0, 0, 1, 0, 0,
      0, 0, 0, 0, 0, 0, 0, 1, 0,
      0, 0, 0, 0, 0, 0, 0, 0, 1,
      0, 0, 0, (sp*(Wy*cr - Wz*sr))/cp, (Wz*cr + Wy*sr)/(cp*cp), 0, 0, 0, 0,
      0, 0, 0, (- Wz*cr - Wy*sr), 0, 0, 0, 0, 0,
      0, 0, 0, (Wy*cr - Wz*sr)/cp, (sp*(Wz*cr + Wy*sr))/(cp*cp), 0, 0, 0, 0,
      0, 0, 0, (Ay*(sr*sy + cr*cy*sp) + Az*(cr*sy - cy*sp*sr)), (Az*cp*cr*cy - Ax*cy*sp + Ay*cp*cy*sr), (Az*(cy*sr - cr*sp*sy) - Ay*(cr*cy + sp*sr*sy) - Ax*cp*sy), 0, 0, 0,
      0, 0, 0, (- Ay*(cy*sr - cr*sp*sy) - Az*(cr*cy + sp*sr*sy)), (Az*cp*cr*sy - Ax*sp*sy + Ay*cp*sr*sy), (Az*(sr*sy + cr*cy*sp) - Ay*(cr*sy - cy*sp*sr) + Ax*cp*cy), 0, 0, 0,
      0, 0, 0, (Ay*cp*cr - Az*cp*sr), (- Ax*cp - Az*cr*sp - Ay*sp*sr), 0, 0, 0, 0 ;

    diff_f_diff_x_jacobian.block<9, 9>(0, 0) = diff_f_diff_x_jacobian_pqv;
    diff_f_diff_x_jacobian.block<3, 3>(3, 9) = -w_Body2Euler(q_last);
    diff_f_diff_x_jacobian.block<3, 3>(6, 12) = -euler2mat(q_last);

    return diff_f_diff_x_jacobian;

    // cp != 0 pitch != 90° !!!!!!!!!
}
//diff_f()/diff_n (x_t-1  ut  noise=0)  Ut    Vt = dt*Ut
MatrixXd diff_f_diff_n(Vector3d q_last)
{
    MatrixXd diff_f_diff_n_jacobian(MatrixXd::Zero(stateSize, inputSize));
    diff_f_diff_n_jacobian.block<3,3>(3,0) = -w_Body2Euler(q_last);
    diff_f_diff_n_jacobian.block<3,3>(6,3) = -euler2mat(q_last);

    return diff_f_diff_n_jacobian;
}
//diff_g()/diff_x  (xt~ noise=0)  Ct
MatrixXd diff_g_diff_x()
{
    MatrixXd diff_g_diff_x_jacobian(MatrixXd::Zero(measurementSize, stateSize));
    diff_g_diff_x_jacobian.block<3,3>(0,0) = MatrixXd::Identity(3,3);
    diff_g_diff_x_jacobian.block<3,3>(3,3) = MatrixXd::Identity(3,3);

    return diff_g_diff_x_jacobian;
}
//diff_g()/diff_v  (xt~ noise=0) Wt
MatrixXd diff_g_diff_v()
{
    MatrixXd diff_g_diff_v_jacobian(MatrixXd::Identity(measurementSize, measurementSize));

    return diff_g_diff_v_jacobian;
}


//  saturating.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was extracted by Vicente J. Botet Escriba from Beman Dawes time2_demo.cpp which
was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.9 Time utilities [time] of the C++ committee's working paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

#define _CRT_SECURE_NO_WARNINGS  // disable VC++ foolishness

#include <boost/chrono/chrono.hpp>
#include <boost/type_traits.hpp>

#include <iostream>

//////////////////////////////////////////////////////////
//////////////////// User2 Example ///////////////////////
//////////////////////////////////////////////////////////

// Demonstrate User2:
// A "saturating" signed integral type  is developed.  This type has +/- infinity and a nan
// (like IEEE floating point) but otherwise obeys signed integral arithmetic.
// This class is subsequently used as the rep in boost::chrono::duration to demonstrate a
// duration class that does not silently ignore overflow.
#include <ostream>
#include <stdexcept>
#include <climits>

namespace User2
{

template <class I>
class saturate
{
public:
    typedef I int_type;

    static const int_type nan = int_type(int_type(1) << (sizeof(int_type) * CHAR_BIT - 1));
    static const int_type neg_inf = nan + 1;
    static const int_type pos_inf = -neg_inf;
private:
    int_type i_;

//     static_assert(std::is_integral<int_type>::value && std::is_signed<int_type>::value,
//                   "saturate only accepts signed integral types");
//     static_assert(nan == -nan && neg_inf < pos_inf,
//                   "saturate assumes two's complement hardware for signed integrals");

public:
    saturate() : i_(nan) {}
    explicit saturate(int_type i) : i_(i) {}
    // explicit
    operator int_type() const;

    saturate& operator+=(saturate x);
    saturate& operator-=(saturate x) {return *this += -x;}
    saturate& operator*=(saturate x);
    saturate& operator/=(saturate x);
    saturate& operator%=(saturate x);

    saturate  operator- () const {return saturate(-i_);}
    saturate& operator++()       {*this += saturate(int_type(1)); return *this;}
    saturate  operator++(int)    {saturate tmp(*this); ++(*this); return tmp;}
    saturate& operator--()       {*this -= saturate(int_type(1)); return *this;}
    saturate  operator--(int)    {saturate tmp(*this); --(*this); return tmp;}

    friend saturate operator+(saturate x, saturate y) {return x += y;}
    friend saturate operator-(saturate x, saturate y) {return x -= y;}
    friend saturate operator*(saturate x, saturate y) {return x *= y;}
    friend saturate operator/(saturate x, saturate y) {return x /= y;}
    friend saturate operator%(saturate x, saturate y) {return x %= y;}

    friend bool operator==(saturate x, saturate y)
    {
        if (x.i_ == nan || y.i_ == nan)
            return false;
        return x.i_ == y.i_;
    }

    friend bool operator!=(saturate x, saturate y) {return !(x == y);}

    friend bool operator<(saturate x, saturate y)
    {
        if (x.i_ == nan || y.i_ == nan)
            return false;
        return x.i_ < y.i_;
    }

    friend bool operator<=(saturate x, saturate y)
    {
        if (x.i_ == nan || y.i_ == nan)
            return false;
        return x.i_ <= y.i_;
    }

    friend bool operator>(saturate x, saturate y)
    {
        if (x.i_ == nan || y.i_ == nan)
            return false;
        return x.i_ > y.i_;
    }

    friend bool operator>=(saturate x, saturate y)
    {
        if (x.i_ == nan || y.i_ == nan)
            return false;
        return x.i_ >= y.i_;
    }

    friend std::ostream& operator<<(std::ostream& os, saturate s)
    {
        switch (s.i_)
        {
        case pos_inf:
            return os << "inf";
        case nan:
            return os << "nan";
        case neg_inf:
            return os << "-inf";
        };
        return os << s.i_;
    }
};

template <class I>
saturate<I>::operator I() const
{
    switch (i_)
    {
    case nan:
    case neg_inf:
    case pos_inf:
        throw std::out_of_range("saturate special value can not convert to int_type");
    }
    return i_;
}

template <class I>
saturate<I>&
saturate<I>::operator+=(saturate x)
{
    switch (i_)
    {
    case pos_inf:
        switch (x.i_)
        {
        case neg_inf:
        case nan:
            i_ = nan;
        }
        return *this;
    case nan:
        return *this;
    case neg_inf:
        switch (x.i_)
        {
        case pos_inf:
        case nan:
            i_ = nan;
        }
        return *this;
    }
    switch (x.i_)
    {
    case pos_inf:
    case neg_inf:
    case nan:
        i_ = x.i_;
        return *this;
    }
    if (x.i_ >= 0)
    {
        if (i_ < pos_inf - x.i_)
            i_ += x.i_;
        else
            i_ = pos_inf;
        return *this;
    }
    if (i_ > neg_inf - x.i_)
        i_ += x.i_;
    else
        i_ = neg_inf;
    return *this;
}

template <class I>
saturate<I>&
saturate<I>::operator*=(saturate x)
{
    switch (i_)
    {
    case 0:
        switch (x.i_)
        {
        case pos_inf:
        case neg_inf:
        case nan:
            i_ = nan;
        }
        return *this;
    case pos_inf:
        switch (x.i_)
        {
        case nan:
        case 0:
            i_ = nan;
            return *this;
        }
        if (x.i_ < 0)
            i_ = neg_inf;
        return *this;
    case nan:
        return *this;
    case neg_inf:
        switch (x.i_)
        {
        case nan:
        case 0:
            i_ = nan;
            return *this;
        }
        if (x.i_ < 0)
            i_ = pos_inf;
        return *this;
    }
    switch (x.i_)
    {
    case 0:
        i_ = 0;
        return *this;
    case nan:
        i_ = nan;
        return *this;
    case pos_inf:
        if (i_ < 0)
            i_ = neg_inf;
        else
            i_ = pos_inf;
        return *this;
    case neg_inf:
        if (i_ < 0)
            i_ = pos_inf;
        else
            i_ = neg_inf;
        return *this;
    }
    int s = (i_ < 0 ? -1 : 1) * (x.i_ < 0 ? -1 : 1);
    i_ = i_ < 0 ? -i_ : i_;
    int_type x_i_ = x.i_ < 0 ? -x.i_ : x.i_;
    if (i_ <= pos_inf / x_i_)
        i_ *= x_i_;
    else
        i_ = pos_inf;
    i_ *= s;
    return *this;
}

template <class I>
saturate<I>&
saturate<I>::operator/=(saturate x)
{
    switch (x.i_)
    {
    case pos_inf:
    case neg_inf:
        switch (i_)
        {
        case pos_inf:
        case neg_inf:
        case nan:
            i_ = nan;
            break;
        default:
            i_ = 0;
            break;
        }
        return *this;
    case nan:
        i_ = nan;
        return *this;
    case 0:
        switch (i_)
        {
        case pos_inf:
        case neg_inf:
        case nan:
            return *this;
        case 0:
            i_ = nan;
            return *this;
        }
        if (i_ > 0)
            i_ = pos_inf;
        else
            i_ = neg_inf;
        return *this;
    }
    switch (i_)
    {
    case 0:
    case nan:
        return *this;
    case pos_inf:
    case neg_inf:
        if (x.i_ < 0)
            i_ = -i_;
        return *this;
    }
    i_ /= x.i_;
    return *this;
}

template <class I>
saturate<I>&
saturate<I>::operator%=(saturate x)
{
//    *this -= *this / x * x;  // definition
    switch (x.i_)
    {
    case nan:
    case neg_inf:
    case 0:
    case pos_inf:
        i_ = nan;
        return *this;
    }
    switch (i_)
    {
    case neg_inf:
    case pos_inf:
        i_ = nan;
    case nan:
        return *this;
    }
    i_ %= x.i_;
    return *this;
}

// Demo overflow-safe integral durations ranging from picoseconds resolution to millennium resolution
typedef boost::chrono::duration<saturate<long long>, boost::pico                 > picoseconds;
typedef boost::chrono::duration<saturate<long long>, boost::nano                 > nanoseconds;
typedef boost::chrono::duration<saturate<long long>, boost::micro                > microseconds;
typedef boost::chrono::duration<saturate<long long>, boost::milli                > milliseconds;
typedef boost::chrono::duration<saturate<long long>                            > seconds;
typedef boost::chrono::duration<saturate<long long>, boost::ratio<         60LL> > minutes;
typedef boost::chrono::duration<saturate<long long>, boost::ratio<       3600LL> > hours;
typedef boost::chrono::duration<saturate<long long>, boost::ratio<      86400LL> > days;
typedef boost::chrono::duration<saturate<long long>, boost::ratio<   31556952LL> > years;
typedef boost::chrono::duration<saturate<long long>, boost::ratio<31556952000LL> > millennium;

}  // User2

// Demonstrate custom promotion rules (needed only if there are no implicit conversions)
namespace User2 { namespace detail {

template <class T1, class T2, bool = boost::is_integral<T1>::value>
struct promote_helper;

template <class T1, class T2>
struct promote_helper<T1, saturate<T2>, true>  // integral
{
    typedef typename boost::common_type<T1, T2>::type rep;
    typedef User2::saturate<rep> type;
};

template <class T1, class T2>
struct promote_helper<T1, saturate<T2>, false>  // floating
{
    typedef T1 type;
};

} }

namespace boost
{

template <class T1, class T2>
struct common_type<User2::saturate<T1>, User2::saturate<T2> >
{
    typedef typename common_type<T1, T2>::type rep;
    typedef User2::saturate<rep> type;
};

template <class T1, class T2>
struct common_type<T1, User2::saturate<T2> >
    : User2::detail::promote_helper<T1, User2::saturate<T2> > {};

template <class T1, class T2>
struct common_type<User2::saturate<T1>, T2>
    : User2::detail::promote_helper<T2, User2::saturate<T1> > {};

// Demonstrate specialization of duration_values:

namespace chrono {

template <class I>
struct duration_values<User2::saturate<I> >
{
    typedef User2::saturate<I> Rep;
public:
    static Rep zero() {return Rep(0);}
    static Rep max BOOST_PREVENT_MACRO_SUBSTITUTION ()  {return Rep(Rep::pos_inf-1);}
    static Rep min BOOST_PREVENT_MACRO_SUBSTITUTION ()  {return -(max)();}
};

}  // namespace chrono

}  // namespace boost

#include <iostream>

void testUser2()
{
    std::cout << "*************\n";
    std::cout << "* testUser2 *\n";
    std::cout << "*************\n";
    using namespace User2;
    typedef seconds::rep sat;
    years yr(sat(100));
    std::cout << "100 years expressed as years = " << yr.count() << '\n';
    nanoseconds ns = yr;
    std::cout << "100 years expressed as nanoseconds = " << ns.count() << '\n';
    ns += yr;
    std::cout << "200 years expressed as nanoseconds = " << ns.count() << '\n';
    ns += yr;
    std::cout << "300 years expressed as nanoseconds = " << ns.count() << '\n';
//    yr = ns;  // does not compile
    std::cout << "yr = ns;  // does not compile\n";
//    picoseconds ps1 = yr;  // does not compile, compile-time overflow in ratio arithmetic
    std::cout << "ps = yr;  // does not compile\n";
    ns = yr;
    picoseconds ps = ns;
    std::cout << "100 years expressed as picoseconds = " << ps.count() << '\n';
    ps = ns / sat(1000);
    std::cout << "0.1 years expressed as picoseconds = " << ps.count() << '\n';
    yr = years(sat(-200000000));
    std::cout << "200 million years ago encoded in years: " << yr.count() << '\n';
    days d = boost::chrono::duration_cast<days>(yr);
    std::cout << "200 million years ago encoded in days: " << d.count() << '\n';
    millennium c = boost::chrono::duration_cast<millennium>(yr);
    std::cout << "200 million years ago encoded in millennium: " << c.count() << '\n';
    std::cout << "Demonstrate \"uninitialized protection\" behavior:\n";
    seconds sec;
    for (++sec; sec < seconds(sat(10)); ++sec)
        ;
    std::cout << sec.count() << '\n';
    std::cout << "\n";
}

void testStdUser()
{
    std::cout << "***************\n";
    std::cout << "* testStdUser *\n";
    std::cout << "***************\n";
    using namespace boost::chrono;
    hours hr = hours(100);
    std::cout << "100 hours expressed as hours = " << hr.count() << '\n';
    nanoseconds ns = hr;
    std::cout << "100 hours expressed as nanoseconds = " << ns.count() << '\n';
    ns += hr;
    std::cout << "200 hours expressed as nanoseconds = " << ns.count() << '\n';
    ns += hr;
    std::cout << "300 hours expressed as nanoseconds = " << ns.count() << '\n';
//    hr = ns;  // does not compile
    std::cout << "hr = ns;  // does not compile\n";
//    hr * ns;  // does not compile
    std::cout << "hr * ns;  // does not compile\n";
    duration<double> fs(2.5);
    std::cout << "duration<double> has count() = " << fs.count() << '\n';
//    seconds sec = fs;  // does not compile
    std::cout << "seconds sec = duration<double> won't compile\n";
    seconds sec = duration_cast<seconds>(fs);
    std::cout << "seconds has count() = " << sec.count() << '\n';
    std::cout << "\n";
}

int main()
{
    testStdUser();
    testUser2();
    return 0;
}


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/test/sqrt.cpp
 *
 * \brief Test suite for the \c sqrt operation.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2011, Marco Guazzone
 *
 * 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)
 */

#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublasx/operation/sqrt.hpp>
#include <cmath>
#include <complex>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"

namespace ublas = ::boost::numeric::ublas;
namespace ublasx = ::boost::numeric::ublasx;

static const double tol = 1.0e-5;

BOOST_UBLASX_TEST_DEF( test_real_vector )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Vector" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<value_type> vector_type;

    const size_type n(4);

    vector_type v(n);

    v(0) = 1;
    v(1) = 2;
    v(2) = 3;
    v(3) = 4;

    vector_type res;
    vector_type expect_res(n);

    res = ublasx::sqrt(v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "sqrt(v) = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        expect_res(i) = ::std::sqrt(v(i));
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_vector )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Vector" );

    typedef std::complex<double> in_value_type;
    typedef in_value_type out_value_type;
    typedef std::size_t size_type;
    typedef ublas::vector<in_value_type> in_vector_type;
    typedef ublas::vector<out_value_type> out_vector_type;

    const size_type n(4);

    in_vector_type v(n);

    v(0) = in_value_type( 1, 2);
    v(1) = in_value_type(-2, 3);
    v(2) = in_value_type(-3,-4);
    v(3) = in_value_type( 4,-5);

    out_vector_type res;
    out_vector_type expect_res(n);

    res = ublasx::sqrt(v);

    BOOST_UBLASX_DEBUG_TRACE( "v = " << v );
    BOOST_UBLASX_DEBUG_TRACE( "sqrt(v) = " << res );

    for (size_type i = 0; i < n; ++i)
    {
        expect_res(i) = ::std::sqrt(v(i));
    }

    BOOST_UBLASX_TEST_CHECK_VECTOR_CLOSE( res, expect_res, n, tol );
}

BOOST_UBLASX_TEST_DEF( test_real_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Real - Matrix" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type> matrix_type;

    const size_type nr(2);
    const size_type nc(3);

    matrix_type A(nr,nc);

    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type R;
    matrix_type expect_R(nr,nc);

    R = ublasx::sqrt(A);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "sqrt(A) = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = ::std::sqrt(A(r,c));
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

BOOST_UBLASX_TEST_DEF( test_complex_matrix )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Complex - Matrix" );

    typedef std::complex<double> in_value_type;
    typedef in_value_type out_value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<in_value_type> in_matrix_type;
    typedef ublas::matrix<out_value_type> out_matrix_type;

    const size_type nr(2);
    const size_type nc(3);

    in_matrix_type A(nr,nc);

    A(0,0) = in_value_type( 1, 2); A(0,1) = in_value_type(-2, 3); A(0,2) = in_value_type(-3,-4);
    A(1,0) = in_value_type(-4,-5); A(1,1) = in_value_type( 5,-6); A(1,2) = in_value_type( 6, 7);

    out_matrix_type R;
    out_matrix_type expect_R(nr,nc);

    R = ublasx::sqrt(A);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "sqrt(A) = " << R );

    for (size_type r = 0; r < nr; ++r)
    {
        for (size_type c = 0; c < nc; ++c)
        {
            expect_R(r,c) = ::std::sqrt(A(r,c));
        }
    }

    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, nr, nc, tol );
}

int main()
{

    BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'sqrt' operation");

    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( test_real_vector );
    BOOST_UBLASX_TEST_DO( test_complex_vector );
    BOOST_UBLASX_TEST_DO( test_real_matrix );
    BOOST_UBLASX_TEST_DO( test_complex_matrix );

    BOOST_UBLASX_TEST_END();
}