#include "software/geom/util.h"

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

#include "software/logger/logger.h"
#include "software/new_geom/angle.h"
#include "software/new_geom/rectangle.h"
#include "software/new_geom/segment.h"
#include "software/new_geom/triangle.h"
#include "software/new_geom/util/collinear.h"
#include "software/new_geom/util/distance.h"
#include "software/new_geom/util/intersection.h"
#include "software/new_geom/util/intersects.h"

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

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

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

bool collinear(const Segment &segment1, const Segment &segment2)
{
    // Two segments are collinear if all Points are collinear
    if (collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getSegStart()) &&
        collinear(segment1.getSegStart(), segment1.getEnd(), segment2.getEnd()))
    {
        return true;
    }
    return false;
}

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;
}

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;
}

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

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();
}

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));
}

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);
}

bool pointInFrontVector(Point offset, Vector direction, Point p)
{
    // compare angle different
    Angle a1   = direction.orientation();
    Angle a2   = (p - offset).orientation();
    Angle diff = (a1 - a2).clamp();
    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 (circle.contains(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> getCircleTangentRaysWithReferenceOrigin(const Point reference,
                                                            const Circle circle)
{
    auto [tangent_point1, tangent_point2] = getCircleTangentPoints(reference, circle, 0);

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

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);
}

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 = intersection(extremes1, ray1);
    const std::optional<Point> extreme_intersect12 = intersection(extremes2, ray1);
    const std::optional<Point> extreme_intersect21 = intersection(extremes1, ray2);
    const std::optional<Point> extreme_intersect22 = intersection(extremes2, ray2);

    // Check for the cases that the rays intersect the same segment projection
    if ((extreme_intersect11.has_value() && extreme_intersect21.has_value()) ||
        (extreme_intersect12.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
    std::vector<Point> intersection1 = intersection(ray1, segment);
    std::vector<Point> intersection2 = intersection(ray2, segment);

    std::optional<Point> intersect11;
    std::optional<Point> intersect12;
    std::optional<Point> intersect21;
    std::optional<Point> intersect22;

    if (!intersection1.empty())
    {
        intersect11 = intersection1[0];

        if (intersection1.size() > 1)
        {
            intersect12 = intersection1[1];
        }
    }

    if (!intersection2.empty())
    {
        intersect21 = intersection2[0];

        if (intersection2.size() > 1)
        {
            intersect22 = intersection2[1];
        }
    }

    // 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 Point (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 = intersection(extremes1, ray2);
        std::optional<Point> extreme_intersect2 = intersection(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 (intersect21.has_value() && !intersect11.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 = intersection(extremes1, ray1);
        std::optional<Point> extreme_intersect2 = intersection(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, segment2))
    {
        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 (segment1.contains(segment2.getSegStart()))
    {
        // If segment2.getSegStart() lays in segment1, then the combined segment is
        // segment2,getEnd() and the point furthest from segment2.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 (segment1.contains(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, segment2))
    {
        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 (largest_segment.contains(smallest_segment.getSegStart()) &&
        largest_segment.contains(smallest_segment.getEnd()))
    {
        return std::make_optional(largest_segment);
    }
    else
    {
        return std::nullopt;
    }
}

std::vector<Segment> getEmptySpaceWithinParentSegment(std::vector<Segment> segments,
                                                      Segment parent_segment)
{
    // Make sure the starting point of all segments is closer to the start of the
    // reference segment to simplify the evaluation
    for (auto &unordered_seg : segments)
    {
        if ((parent_segment.getSegStart() - unordered_seg.getSegStart()).length() >
            (parent_segment.getSegStart() - unordered_seg.getEnd()).length())
        {
            // We need to flip the start/end of the segment
            Segment temp = unordered_seg;
            unordered_seg.setSegStart(temp.getEnd());
            unordered_seg.setEnd(temp.getSegStart());
        }
    }

    // Now we must sort the segments so that we can iterate through them in order to
    // generate open angles sort using a lambda expression
    // We sort the segments based on how close their 'start' point is to the 'start'
    // of the reference Segment
    std::sort(segments.begin(), segments.end(), [parent_segment](Segment &a, Segment &b) {
        return (parent_segment.getSegStart() - a.getSegStart()).length() <
               (parent_segment.getSegStart() - b.getSegStart()).length();
    });

    // Now we need to find the largest open segment/angle
    std::vector<Segment> open_segs;

    // The first Angle is between the reference Segment and the first obstacle Segment
    // After this one, ever open angle is between segment(i).end and
    // segment(i+1).start
    open_segs.push_back(
        Segment(parent_segment.getSegStart(), segments.front().getSegStart()));

    // The 'open' Segment in the space between consecutive 'blocking' Segments
    for (std::vector<Segment>::const_iterator it = segments.begin();
         it != segments.end() - 1; it++)
    {
        open_segs.push_back(Segment(it->getEnd(), (it + 1)->getSegStart()));
    }

    // Lastly, the final open angle is between obstacles.end().getEnd() and
    // reference_segment.getEnd()
    open_segs.push_back(Segment(segments.back().getEnd(), parent_segment.getEnd()));

    // Remove all zero length open Segments
    for (std::vector<Segment>::const_iterator it = open_segs.begin();
         it != open_segs.end();)
    {
        if (it->length() < EPS)
        {
            open_segs.erase(it);
        }
        else
        {
            it++;
        }
    }

    return open_segs;
}

std::vector<Segment> projectCirclesOntoSegment(Segment segment,
                                               std::vector<Circle> circles, Point origin)
{
    // Loop through all obstacles to create their projected Segment
    std::vector<Segment> obstacle_segment_projections = {};

    for (Circle circle : circles)
    {
        // If the reference is inside an obstacle there is no open direction
        if (circle.contains(origin))
        {
            obstacle_segment_projections.push_back(segment);
            return obstacle_segment_projections;
        }

        // Get the tangent rays from the reference point to the obstacle
        auto [ray1, ray2] = getCircleTangentRaysWithReferenceOrigin(origin, circle);

        // Project the tangent Rays to obtain a 'blocked' segment on the reference
        // Segment
        std::optional<Segment> intersect_segment =
            getIntersectingSegment(ray1, ray2, segment);

        if (intersect_segment.has_value())
        {
            obstacle_segment_projections.push_back(intersect_segment.value());
        }
    }
    return obstacle_segment_projections;
}

std::vector<Segment> combineToParallelSegments(std::vector<Segment> segments,
                                               Vector direction)
{
    std::vector<Segment> projected_segments = {};

    // Project all Segments onto the direction Vector
    for (Segment segment : segments)
    {
        // The projection of the Segment without including the original Segment location
        Vector raw_projection = segment.toVector().project(direction);

        // Only count projections that have a non-zero magnitude
        if (raw_projection.lengthSquared() > EPS)
        {
            projected_segments.push_back(
                Segment(segment.getSegStart(), segment.getSegStart() + raw_projection));
        }
    }
    std::vector<Segment> unique_segments;

    unsigned int j = 0;
    // Loop through all segments and combine segments
    // to reduce the vector to the smallest number of independent (not overlapping)
    // segments
    while (projected_segments.size() > 0)
    {
        std::optional<Segment> temp_segment;
        unique_segments.push_back(projected_segments[0]);
        projected_segments.erase(projected_segments.begin());

        for (unsigned int i = 0; i < projected_segments.size(); i++)
        {
            temp_segment = mergeOverlappingParallelSegments(unique_segments[j],
                                                            projected_segments[i]);

            if (temp_segment.has_value())
            {
                unique_segments[j] = temp_segment.value();
                // Remove segments[i] from the list as it is not unique
                projected_segments.erase(projected_segments.begin() + i);
                i--;
            }
        }
        j++;
    }

    return unique_segments;
}


//  (C) Copyright Nick Thompson 2018.
//  (C) Copyright Matt Borland 2020.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_HPP
#define BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_HPP

#include <boost/math/statistics/detail/single_pass.hpp>
#include <boost/math/tools/config.hpp>
#include <boost/math/tools/assert.hpp>
#include <algorithm>
#include <iterator>
#include <tuple>
#include <cmath>
#include <vector>
#include <type_traits>
#include <utility>
#include <numeric>
#include <list>

// Support compilers with P0024R2 implemented without linking TBB
// https://en.cppreference.com/w/cpp/compiler_support
#ifndef BOOST_NO_CXX17_HDR_EXECUTION
#include <execution>

namespace boost::math::statistics {

template<class ExecutionPolicy, class ForwardIterator>
inline auto mean(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            return detail::mean_sequential_impl<double>(first, last);
        }
        else
        {
            return std::reduce(exec, first, last, 0.0) / std::distance(first, last);
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            return detail::mean_sequential_impl<Real>(first, last);
        }
        else
        {
            return std::reduce(exec, first, last, Real(0.0)) / Real(std::distance(first, last));
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto mean(ExecutionPolicy&& exec, Container const & v)
{
    return mean(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto mean(ForwardIterator first, ForwardIterator last)
{
    return mean(std::execution::seq, first, last);
}

template<class Container>
inline auto mean(Container const & v)
{
    return mean(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto variance(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
           return std::get<2>(detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::get<1>(results) / std::get<4>(results);
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            return std::get<2>(detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::get<1>(results) / std::get<4>(results);
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto variance(ExecutionPolicy&& exec, Container const & v)
{
    return variance(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto variance(ForwardIterator first, ForwardIterator last)
{
    return variance(std::execution::seq, first, last);
}

template<class Container>
inline auto variance(Container const & v)
{
    return variance(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto sample_variance(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    const auto n = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(exec, first, last)/(n-1);
}

template<class ExecutionPolicy, class Container>
inline auto sample_variance(ExecutionPolicy&& exec, Container const & v)
{
    return sample_variance(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto sample_variance(ForwardIterator first, ForwardIterator last)
{
    return sample_variance(std::execution::seq, first, last);
}

template<class Container>
inline auto sample_variance(Container const & v)
{
    return sample_variance(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto mean_and_sample_variance(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<2>(results)*std::get<3>(results)/(std::get<3>(results)-1.0));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<1>(results) / (std::get<4>(results)-1.0));
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<2>(results)*std::get<3>(results)/(std::get<3>(results)-Real(1)));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::make_pair(std::get<0>(results), std::get<1>(results) / (std::get<4>(results)-Real(1)));
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto mean_and_sample_variance(ExecutionPolicy&& exec, Container const & v)
{
    return mean_and_sample_variance(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto mean_and_sample_variance(ForwardIterator first, ForwardIterator last)
{
    return mean_and_sample_variance(std::execution::seq, first, last);
}

template<class Container>
inline auto mean_and_sample_variance(Container const & v)
{
    return mean_and_sample_variance(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto first_four_moments(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;

    if constexpr (std::is_integral_v<Real>)
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::first_four_moments_sequential_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                std::get<3>(results) / std::get<4>(results));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<double, double, double, double, double>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                   std::get<3>(results) / std::get<4>(results));
        }
    }
    else
    {
        if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
        {
            const auto results = detail::first_four_moments_sequential_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                   std::get<3>(results) / std::get<4>(results));
        }
        else
        {
            const auto results = detail::first_four_moments_parallel_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
            return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                                   std::get<3>(results) / std::get<4>(results));
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto first_four_moments(ExecutionPolicy&& exec, Container const & v)
{
    return first_four_moments(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto first_four_moments(ForwardIterator first, ForwardIterator last)
{
    return first_four_moments(std::execution::seq, first, last);
}

template<class Container>
inline auto first_four_moments(Container const & v)
{
    return first_four_moments(std::execution::seq, std::cbegin(v), std::cend(v));
}

// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ExecutionPolicy, class ForwardIterator>
inline auto skewness(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    using std::sqrt;

    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            return detail::skewness_sequential_impl<double>(first, last);
        }
        else
        {
            return detail::skewness_sequential_impl<Real>(first, last);
        }
    }
    else
    {
        const auto [M1, M2, M3, M4] = first_four_moments(exec, first, last);
        const auto n = std::distance(first, last);
        const auto var = M2/(n-1);

        if (M2 == 0)
        {
            // The limit is technically undefined, but the interpretation here is clear:
            // A constant dataset has no skewness.
            if constexpr (std::is_integral_v<Real>)
            {
                return double(0);
            }
            else
            {
                return Real(0);
            }
        }
        else
        {
            return M3/(M2*sqrt(var)) / Real(2);
        }
    }
}

template<class ExecutionPolicy, class Container>
inline auto skewness(ExecutionPolicy&& exec, Container & v)
{
    return skewness(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto skewness(ForwardIterator first, ForwardIterator last)
{
    return skewness(std::execution::seq, first, last);
}

template<class Container>
inline auto skewness(Container const & v)
{
    return skewness(std::execution::seq, std::cbegin(v), std::cend(v));
}

// Follows equation 1.6 of:
// https://prod.sandia.gov/techlib-noauth/access-control.cgi/2008/086212.pdf
template<class ExecutionPolicy, class ForwardIterator>
inline auto kurtosis(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    const auto [M1, M2, M3, M4] = first_four_moments(exec, first, last);
    if (M2 == 0)
    {
        return M2;
    }
    return M4/(M2*M2);
}

template<class ExecutionPolicy, class Container>
inline auto kurtosis(ExecutionPolicy&& exec, Container const & v)
{
    return kurtosis(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto kurtosis(ForwardIterator first, ForwardIterator last)
{
    return kurtosis(std::execution::seq, first, last);
}

template<class Container>
inline auto kurtosis(Container const & v)
{
    return kurtosis(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class ForwardIterator>
inline auto excess_kurtosis(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    return kurtosis(exec, first, last) - 3;
}

template<class ExecutionPolicy, class Container>
inline auto excess_kurtosis(ExecutionPolicy&& exec, Container const & v)
{
    return excess_kurtosis(exec, std::cbegin(v), std::cend(v));
}

template<class ForwardIterator>
inline auto excess_kurtosis(ForwardIterator first, ForwardIterator last)
{
    return excess_kurtosis(std::execution::seq, first, last);
}

template<class Container>
inline auto excess_kurtosis(Container const & v)
{
    return excess_kurtosis(std::execution::seq, std::cbegin(v), std::cend(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
auto median(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last)
{
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero length vector is undefined.");
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(exec, first, middle, last);
        return *middle;
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(exec, first, middle, last);
        std::nth_element(exec, middle, middle+1, last);
        return (*middle + *(middle+1))/2;
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto median(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return median(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto median(RandomAccessIterator first, RandomAccessIterator last)
{
    return median(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto median(RandomAccessContainer & v)
{
    return median(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
inline auto gini_coefficient(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last)
{
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;

    if(!std::is_sorted(exec, first, last))
    {
        std::sort(exec, first, last);
    }

    if constexpr (std::is_same_v<std::remove_reference_t<decltype(exec)>, decltype(std::execution::seq)>)
    {
        if constexpr (std::is_integral_v<Real>)
        {
            return detail::gini_coefficient_sequential_impl<double>(first, last);
        }
        else
        {
            return detail::gini_coefficient_sequential_impl<Real>(first, last);
        }
    }

    else if constexpr (std::is_integral_v<Real>)
    {
        return detail::gini_coefficient_parallel_impl<double>(exec, first, last);
    }

    else
    {
        return detail::gini_coefficient_parallel_impl<Real>(exec, first, last);
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto gini_coefficient(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return gini_coefficient(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    return gini_coefficient(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto gini_coefficient(RandomAccessContainer & v)
{
    return gini_coefficient(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
inline auto sample_gini_coefficient(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last)
{
    const auto n = std::distance(first, last);
    return n*gini_coefficient(exec, first, last)/(n-1);
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto sample_gini_coefficient(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return sample_gini_coefficient(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    return sample_gini_coefficient(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto sample_gini_coefficient(RandomAccessContainer & v)
{
    return sample_gini_coefficient(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class RandomAccessIterator>
auto median_absolute_deviation(ExecutionPolicy&& exec, RandomAccessIterator first, RandomAccessIterator last,
    typename std::iterator_traits<RandomAccessIterator>::value_type center=std::numeric_limits<typename std::iterator_traits<RandomAccessIterator>::value_type>::quiet_NaN())
{
    using std::abs;
    using Real = typename std::iterator_traits<RandomAccessIterator>::value_type;
    using std::isnan;
    if (isnan(center))
    {
        center = boost::math::statistics::median(exec, first, last);
    }
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero-length vector is undefined.");
    auto comparator = [&center](Real a, Real b) { return abs(a-center) < abs(b-center);};
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(exec, first, middle, last, comparator);
        return abs(*middle);
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(exec, first, middle, last, comparator);
        std::nth_element(exec, middle, middle+1, last, comparator);
        return (abs(*middle) + abs(*(middle+1)))/abs(static_cast<Real>(2));
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto median_absolute_deviation(ExecutionPolicy&& exec, RandomAccessContainer & v,
    typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(exec, std::begin(v), std::end(v), center);
}

template<class RandomAccessIterator>
inline auto median_absolute_deviation(RandomAccessIterator first, RandomAccessIterator last,
    typename RandomAccessIterator::value_type center=std::numeric_limits<typename RandomAccessIterator::value_type>::quiet_NaN())
{
    return median_absolute_deviation(std::execution::seq, first, last, center);
}

template<class RandomAccessContainer>
inline auto median_absolute_deviation(RandomAccessContainer & v,
    typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(std::execution::seq, std::begin(v), std::end(v), center);
}

template<class ExecutionPolicy, class ForwardIterator>
auto interquartile_range(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    using Real = typename std::iterator_traits<ForwardIterator>::value_type;
    static_assert(!std::is_integral_v<Real>, "Integer values have not yet been implemented.");
    auto m = std::distance(first,last);
    BOOST_MATH_ASSERT_MSG(m >= 3, "At least 3 samples are required to compute the interquartile range.");
    auto k = m/4;
    auto j = m - (4*k);
    // m = 4k+j.
    // If j = 0 or j = 1, then there are an even number of samples below the median, and an even number above the median.
    //    Then we must average adjacent elements to get the quartiles.
    // If j = 2 or j = 3, there are an odd number of samples above and below the median, these elements may be directly extracted to get the quartiles.

    if (j==2 || j==3)
    {
        auto q1 = first + k;
        auto q3 = first + 3*k + j - 1;
        std::nth_element(exec, first, q1, last);
        Real Q1 = *q1;
        std::nth_element(exec, q1, q3, last);
        Real Q3 = *q3;
        return Q3 - Q1;
    } else {
        // j == 0 or j==1:
        auto q1 = first + k - 1;
        auto q3 = first + 3*k - 1 + j;
        std::nth_element(exec, first, q1, last);
        Real a = *q1;
        std::nth_element(exec, q1, q1 + 1, last);
        Real b = *(q1 + 1);
        Real Q1 = (a+b)/2;
        std::nth_element(exec, q1, q3, last);
        a = *q3;
        std::nth_element(exec, q3, q3 + 1, last);
        b = *(q3 + 1);
        Real Q3 = (a+b)/2;
        return Q3 - Q1;
    }
}

template<class ExecutionPolicy, class RandomAccessContainer>
inline auto interquartile_range(ExecutionPolicy&& exec, RandomAccessContainer & v)
{
    return interquartile_range(exec, std::begin(v), std::end(v));
}

template<class RandomAccessIterator>
inline auto interquartile_range(RandomAccessIterator first, RandomAccessIterator last)
{
    return interquartile_range(std::execution::seq, first, last);
}

template<class RandomAccessContainer>
inline auto interquartile_range(RandomAccessContainer & v)
{
    return interquartile_range(std::execution::seq, std::begin(v), std::end(v));
}

template<class ExecutionPolicy, class ForwardIterator, class OutputIterator>
inline OutputIterator mode(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    if(!std::is_sorted(exec, first, last))
    {
        if constexpr (std::is_same_v<typename std::iterator_traits<ForwardIterator>::iterator_category(), std::random_access_iterator_tag>)
        {
            std::sort(exec, first, last);
        }
        else
        {
            BOOST_MATH_ASSERT("Data must be sorted for sequential mode calculation");
        }
    }

    return detail::mode_impl(first, last, output);
}

template<class ExecutionPolicy, class Container, class OutputIterator>
inline OutputIterator mode(ExecutionPolicy&& exec, Container & v, OutputIterator output)
{
    return mode(exec, std::begin(v), std::end(v), output);
}

template<class ForwardIterator, class OutputIterator>
inline OutputIterator mode(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    return mode(std::execution::seq, first, last, output);
}

// Requires enable_if_t to not clash with impl that returns std::list
// Very ugly. std::is_execution_policy_v returns false for the std::execution objects and decltype of the objects (e.g. std::execution::seq)
template<class Container, class OutputIterator, std::enable_if_t<!std::is_convertible_v<std::execution::sequenced_policy, Container> &&
                                                                 !std::is_convertible_v<std::execution::parallel_unsequenced_policy, Container> &&
                                                                 !std::is_convertible_v<std::execution::parallel_policy, Container>
                                                                 #if __cpp_lib_execution > 201900
                                                                 && !std::is_convertible_v<std::execution::unsequenced_policy, Container>
                                                                 #endif
                                                                 , bool> = true>
inline OutputIterator mode(Container & v, OutputIterator output)
{
    return mode(std::execution::seq, std::begin(v), std::end(v), output);
}

// std::list is the return type for the proposed STL stats library

template<class ExecutionPolicy, class ForwardIterator, class Real = typename std::iterator_traits<ForwardIterator>::value_type>
inline auto mode(ExecutionPolicy&& exec, ForwardIterator first, ForwardIterator last)
{
    std::list<Real> modes;
    mode(exec, first, last, std::inserter(modes, modes.begin()));
    return modes;
}

template<class ExecutionPolicy, class Container>
inline auto mode(ExecutionPolicy&& exec, Container & v)
{
    return mode(exec, std::begin(v), std::end(v));
}

template<class ForwardIterator>
inline auto mode(ForwardIterator first, ForwardIterator last)
{
    return mode(std::execution::seq, first, last);
}

template<class Container>
inline auto mode(Container & v)
{
    return mode(std::execution::seq, std::begin(v), std::end(v));
}

} // Namespace boost::math::statistics

#else // Backwards compatible bindings for C++11

namespace boost { namespace math { namespace statistics {

template<bool B, class T = void>
using enable_if_t = typename std::enable_if<B, T>::type;

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double mean(const ForwardIterator first, const ForwardIterator last)
{
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");
    return detail::mean_sequential_impl<double>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double mean(const Container& c)
{
    return mean(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real mean(const ForwardIterator first, const ForwardIterator last)
{
    BOOST_MATH_ASSERT_MSG(first != last, "At least one sample is required to compute the mean.");
    return detail::mean_sequential_impl<Real>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real mean(const Container& c)
{
    return mean(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double variance(const ForwardIterator first, const ForwardIterator last)
{
    return std::get<2>(detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double variance(const Container& c)
{
    return variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real variance(const ForwardIterator first, const ForwardIterator last)
{
    return std::get<2>(detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last));

}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real variance(const Container& c)
{
    return variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto n = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(first, last)/(n-1);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_variance(const Container& c)
{
    return sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto n = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(n > 1, "At least two samples are required to compute the sample variance.");
    return n*variance(first, last)/(n-1);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_variance(const Container& c)
{
    return sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::pair<double, double> mean_and_sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::variance_sequential_impl<std::tuple<double, double, double, double>>(first, last);
    return std::make_pair(std::get<0>(results), std::get<3>(results)*std::get<2>(results)/(std::get<3>(results)-1.0));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::pair<double, double> mean_and_sample_variance(const Container& c)
{
    return mean_and_sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::pair<Real, Real> mean_and_sample_variance(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::variance_sequential_impl<std::tuple<Real, Real, Real, Real>>(first, last);
    return std::make_pair(std::get<0>(results), std::get<3>(results)*std::get<2>(results)/(std::get<3>(results)-Real(1)));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::pair<Real, Real> mean_and_sample_variance(const Container& c)
{
    return mean_and_sample_variance(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::tuple<double, double, double, double> first_four_moments(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::first_four_moments_sequential_impl<std::tuple<double, double, double, double, double>>(first, last);
    return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                           std::get<3>(results) / std::get<4>(results));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline std::tuple<double, double, double, double> first_four_moments(const Container& c)
{
    return first_four_moments(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::tuple<Real, Real, Real, Real> first_four_moments(const ForwardIterator first, const ForwardIterator last)
{
    const auto results = detail::first_four_moments_sequential_impl<std::tuple<Real, Real, Real, Real, Real>>(first, last);
    return std::make_tuple(std::get<0>(results), std::get<1>(results) / std::get<4>(results), std::get<2>(results) / std::get<4>(results),
                           std::get<3>(results) / std::get<4>(results));
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline std::tuple<Real, Real, Real, Real> first_four_moments(const Container& c)
{
    return first_four_moments(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double skewness(const ForwardIterator first, const ForwardIterator last)
{
    return detail::skewness_sequential_impl<double>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double skewness(const Container& c)
{
    return skewness(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real skewness(const ForwardIterator first, const ForwardIterator last)
{
    return detail::skewness_sequential_impl<Real>(first, last);
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real skewness(const Container& c)
{
    return skewness(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    std::tuple<double, double, double, double> M = first_four_moments(first, last);

    if(std::get<1>(M) == 0)
    {
        return std::get<1>(M);
    }
    else
    {
        return std::get<3>(M)/(std::get<1>(M)*std::get<1>(M));
    }
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double kurtosis(const Container& c)
{
    return kurtosis(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    std::tuple<Real, Real, Real, Real> M = first_four_moments(first, last);

    if(std::get<1>(M) == 0)
    {
        return std::get<1>(M);
    }
    else
    {
        return std::get<3>(M)/(std::get<1>(M)*std::get<1>(M));
    }
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real kurtosis(const Container& c)
{
    return kurtosis(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double excess_kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    return kurtosis(first, last) - 3;
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double excess_kurtosis(const Container& c)
{
    return excess_kurtosis(std::begin(c), std::end(c));
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real excess_kurtosis(const ForwardIterator first, const ForwardIterator last)
{
    return kurtosis(first, last) - 3;
}

template<class Container, typename Real = typename Container::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real excess_kurtosis(const Container& c)
{
    return excess_kurtosis(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type>
Real median(RandomAccessIterator first, RandomAccessIterator last)
{
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero length vector is undefined.");
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last);
        return *middle;
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last);
        std::nth_element(middle, middle+1, last);
        return (*middle + *(middle+1))/2;
    }
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type>
inline Real median(RandomAccessContainer& c)
{
    return median(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    if(!std::is_sorted(first, last))
    {
        std::sort(first, last);
    }

    return detail::gini_coefficient_sequential_impl<double>(first, last);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double gini_coefficient(RandomAccessContainer& c)
{
    return gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    if(!std::is_sorted(first, last))
    {
        std::sort(first, last);
    }

    return detail::gini_coefficient_sequential_impl<Real>(first, last);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real gini_coefficient(RandomAccessContainer& c)
{
    return gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    const auto n = std::distance(first, last);
    return n*gini_coefficient(first, last)/(n-1);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<std::is_integral<Real>::value, bool> = true>
inline double sample_gini_coefficient(RandomAccessContainer& c)
{
    return sample_gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_gini_coefficient(RandomAccessIterator first, RandomAccessIterator last)
{
    const auto n = std::distance(first, last);
    return n*gini_coefficient(first, last)/(n-1);
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type,
         enable_if_t<!std::is_integral<Real>::value, bool> = true>
inline Real sample_gini_coefficient(RandomAccessContainer& c)
{
    return sample_gini_coefficient(std::begin(c), std::end(c));
}

template<class RandomAccessIterator, typename Real = typename std::iterator_traits<RandomAccessIterator>::value_type>
Real median_absolute_deviation(RandomAccessIterator first, RandomAccessIterator last,
    typename std::iterator_traits<RandomAccessIterator>::value_type center=std::numeric_limits<typename std::iterator_traits<RandomAccessIterator>::value_type>::quiet_NaN())
{
    using std::abs;
    using std::isnan;
    if (isnan(center))
    {
        center = boost::math::statistics::median(first, last);
    }
    const auto num_elems = std::distance(first, last);
    BOOST_MATH_ASSERT_MSG(num_elems > 0, "The median of a zero-length vector is undefined.");
    auto comparator = [&center](Real a, Real b) { return abs(a-center) < abs(b-center);};
    if (num_elems & 1)
    {
        auto middle = first + (num_elems - 1)/2;
        std::nth_element(first, middle, last, comparator);
        return abs(*middle);
    }
    else
    {
        auto middle = first + num_elems/2 - 1;
        std::nth_element(first, middle, last, comparator);
        std::nth_element(middle, middle+1, last, comparator);
        return (abs(*middle) + abs(*(middle+1)))/abs(static_cast<Real>(2));
    }
}

template<class RandomAccessContainer, typename Real = typename RandomAccessContainer::value_type>
inline Real median_absolute_deviation(RandomAccessContainer& c,
    typename RandomAccessContainer::value_type center=std::numeric_limits<typename RandomAccessContainer::value_type>::quiet_NaN())
{
    return median_absolute_deviation(std::begin(c), std::end(c), center);
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type>
Real interquartile_range(ForwardIterator first, ForwardIterator last)
{
    static_assert(!std::is_integral<Real>::value, "Integer values have not yet been implemented.");
    auto m = std::distance(first,last);
    BOOST_MATH_ASSERT_MSG(m >= 3, "At least 3 samples are required to compute the interquartile range.");
    auto k = m/4;
    auto j = m - (4*k);
    // m = 4k+j.
    // If j = 0 or j = 1, then there are an even number of samples below the median, and an even number above the median.
    //    Then we must average adjacent elements to get the quartiles.
    // If j = 2 or j = 3, there are an odd number of samples above and below the median, these elements may be directly extracted to get the quartiles.

    if (j==2 || j==3)
    {
        auto q1 = first + k;
        auto q3 = first + 3*k + j - 1;
        std::nth_element(first, q1, last);
        Real Q1 = *q1;
        std::nth_element(q1, q3, last);
        Real Q3 = *q3;
        return Q3 - Q1;
    }
    else
    {
        // j == 0 or j==1:
        auto q1 = first + k - 1;
        auto q3 = first + 3*k - 1 + j;
        std::nth_element(first, q1, last);
        Real a = *q1;
        std::nth_element(q1, q1 + 1, last);
        Real b = *(q1 + 1);
        Real Q1 = (a+b)/2;
        std::nth_element(q1, q3, last);
        a = *q3;
        std::nth_element(q3, q3 + 1, last);
        b = *(q3 + 1);
        Real Q3 = (a+b)/2;
        return Q3 - Q1;
    }
}

template<class Container, typename Real = typename Container::value_type>
Real interquartile_range(Container& c)
{
    return interquartile_range(std::begin(c), std::end(c));
}

template<class ForwardIterator, class OutputIterator,
    enable_if_t<std::is_same<typename std::iterator_traits<ForwardIterator>::iterator_category(), std::random_access_iterator_tag>::value, bool> = true>
inline OutputIterator mode(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    if(!std::is_sorted(first, last))
    {
        std::sort(first, last);
    }

    return detail::mode_impl(first, last, output);
}

template<class ForwardIterator, class OutputIterator,
    enable_if_t<!std::is_same<typename std::iterator_traits<ForwardIterator>::iterator_category(), std::random_access_iterator_tag>::value, bool> = true>
inline OutputIterator mode(ForwardIterator first, ForwardIterator last, OutputIterator output)
{
    if(!std::is_sorted(first, last))
    {
        BOOST_MATH_ASSERT("Data must be sorted for mode calculation");
    }

    return detail::mode_impl(first, last, output);
}

template<class Container, class OutputIterator>
inline OutputIterator mode(Container& c, OutputIterator output)
{
    return mode(std::begin(c), std::end(c), output);
}

template<class ForwardIterator, typename Real = typename std::iterator_traits<ForwardIterator>::value_type>
inline std::list<Real> mode(ForwardIterator first, ForwardIterator last)
{
    std::list<Real> modes;
    mode(first, last, std::inserter(modes, modes.begin()));
    return modes;
}

template<class Container, typename Real = typename Container::value_type>
inline std::list<Real> mode(Container& c)
{
    return mode(std::begin(c), std::end(c));
}
}}}
#endif
#endif // BOOST_MATH_STATISTICS_UNIVARIATE_STATISTICS_HPP


#include "ros/ros.h"

#include "real_time_simulator/FSM.h"
#include "real_time_simulator/State.h"
#include "real_time_simulator/Sensor.h"

#include "real_time_simulator/Control.h"

#include "geometry_msgs/Vector3.h"
#include "std_msgs/String.h"

#include <time.h>
#include <sstream>
#include <string>
#include <iostream>
#include <fstream>

#include <boost/numeric/odeint.hpp>
#include <boost/numeric/odeint/stepper/runge_kutta_dopri5.hpp>

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

#include <chrono>
#include <random>

using namespace Eigen;

class Rocket {
public:
    float dry_mass;
    float propellant_mass;
    float Isp;
    float minTorque;
    float maxTorque;

    std::vector<float> maxThrust{0, 0, 0};
    std::vector<float> minThrust{0, 0, 0};

    float dry_CM;
    float propellant_CM;
    float total_CM; // Current Cm of rocket, in real time

    float total_length;

    float initial_speed;

    float h0;

    std::vector<float> target_apogee = {0, 0, 0};
    std::vector<float> Cd = {0, 0, 0};
    std::vector<float> surface = {0, 0, 0};
    std::vector<float> drag_coeff = {0, 0, 0};

    std::vector<float> dry_Inertia{0, 0, 0};
    std::vector<float> total_Inertia{0, 0, 0};

    std::vector<float> J_inv{0, 0, 0};

    // Sensor data
    Vector3d sensor_acc;
    Vector3d sensor_gyro;
    float sensor_baro;

    float acc_noise, acc_bias;
    float gyro_noise, gyro_bias;
    float baro_noise, baro_bias;

    using state = Matrix<double, 14, 1>;
    using control = Matrix<double, 3, 2>;

    void update_CM(float current_prop_mass) {
        total_CM = total_length - (dry_CM * dry_mass + propellant_CM * current_prop_mass) /
                                  (dry_mass + current_prop_mass); // From aft of rocket

        float new_inertia = dry_Inertia[0] + pow(total_CM - (total_length - propellant_CM), 2) * current_prop_mass;

        total_Inertia[0] = new_inertia;
        total_Inertia[1] = new_inertia;

        J_inv[0] = total_CM / total_Inertia[0];
        J_inv[1] = total_CM / total_Inertia[1];
        J_inv[2] = 1 / total_Inertia[2];
    }

    void init(ros::NodeHandle n) {
        n.getParam("/rocket/minTorque", minTorque);
        n.getParam("/rocket/maxTorque", maxTorque);
        n.getParam("/rocket/maxThrust", maxThrust);
        n.getParam("/rocket/minThrust", minThrust);
        n.getParam("/rocket/Isp", Isp);

        n.getParam("/rocket/dry_mass", dry_mass);
        n.getParam("/rocket/propellant_mass", propellant_mass);

        n.getParam("/rocket/Cd", Cd);
        n.getParam("/rocket/dry_I", dry_Inertia);

        n.getParam("/rocket/dry_CM", dry_CM);
        n.getParam("/rocket/propellant_CM", propellant_CM);

        n.getParam("/environment/apogee", target_apogee);

        n.getParam("/rocket/initial_speed", initial_speed);

        std::vector<float> diameter = {0, 0, 0};
        std::vector<float> length = {0, 0, 0};

        int nStage;

        n.getParam("/rocket/diameters", diameter);
        n.getParam("/rocket/stage_z", length);
        n.getParam("/rocket/stages", nStage);

        total_length = length[nStage - 1];

        surface[0] = diameter[1] * total_length;
        surface[1] = surface[0];
        surface[2] = diameter[1] * diameter[1] / 4 * 3.14159;

        float rho_air = 1.225;
        drag_coeff[0] = 0.5 * rho_air * surface[0] * Cd[0];
        drag_coeff[1] = 0.5 * rho_air * surface[1] * Cd[1];
        drag_coeff[2] = 0.5 * rho_air * surface[2] * Cd[2];

        total_Inertia[2] = dry_Inertia[2];

        update_CM(propellant_mass);

        n.getParam("/perturbation/acc_noise", acc_noise);
        n.getParam("/perturbation/acc_bias", acc_bias);

        n.getParam("/perturbation/gyro_noise", gyro_noise);
        n.getParam("/perturbation/gyro_bias", gyro_bias);

        n.getParam("/perturbation/baro_noise", baro_noise);
        n.getParam("/perturbation/baro_bias", baro_bias);

        n.getParam("/environment/ground_altitude", h0);

    }

    void dynamics_flight(const state &x,
                         state &xdot,
                         control &rocket_control,
                         control &aero_control,
                         control &perturbation_control,
                         const double &t) {
        // -------------- Simulation variables -----------------------------
        double g0 = 3.986e14 / pow(6371e3 + h0 + x(2), 2);  // Earth gravity in [m/s^2]

        double mass = dry_mass + x(13);                  // Instantaneous mass of the rocket in [kg]

        // Orientation of the rocket with quaternion
        Quaternion<double> attitude(x(9), x(6), x(7), x(8));
        attitude.normalize();
        Matrix<double, 3, 3> rot_matrix = attitude.toRotationMatrix();
        //std::cout << (180/3.14)*std::acos(x(9)*x(9) - x(6)*x(6) - x(7)*x(7) + x(8)*x(8)) << "\n";

        // Force in inertial frame: gravity
        Matrix<double, 3, 1> gravity;
        gravity << 0, 0, g0 * mass;

        // Total force in inertial frame [N]
        Matrix<double, 3, 1> total_force;
        total_force = rot_matrix * rocket_control.col(0) - gravity + aero_control.col(0) + perturbation_control.col(0);
        //std::cout << total_force.transpose() << "\n";

        // Angular velocity omega in quaternion format to compute quaternion derivative
        Quaternion<double> omega_quat(0.0, x(10), x(11), x(12));
        //std::cout << x.segment(10,3).transpose()*57.29 << "\n\n";

        // Tortal torque in body frame
        Matrix<double, 3, 1> I_inv;
        I_inv << 1 / total_Inertia[0], 1 / total_Inertia[1], 1 / total_Inertia[2];

        Matrix<double, 3, 1> total_torque;
        total_torque =
                rocket_control.col(1) + rot_matrix.transpose() * (aero_control.col(1) + perturbation_control.col(1));

        // -------------- Differential equation ---------------------

        // Position variation is speed
        xdot.head(3) = x.segment(3, 3);

        // Speed variation is Force/mass
        xdot.segment(3, 3) = total_force / mass;

        // Quaternion variation is 0.5*w◦q
        xdot.segment(6, 4) = 0.5 * (omega_quat * attitude).coeffs();

        // Angular speed variation is Torque/Inertia
        xdot.segment(10, 3) = rot_matrix * (total_torque.cwiseProduct(I_inv));

        // Mass variation is proportional to total thrust
        if (Isp != -1) {
            xdot(13) = -rocket_control.col(0).norm() / (Isp * g0);
        } else {
            xdot(13) = 0;
        }

        // Fake sensor data update -----------------
        sensor_acc = rot_matrix.transpose() * (total_force + gravity) / mass;

        sensor_gyro = rot_matrix.transpose() * x.segment(10, 3);

        sensor_baro = x(2);
    }

    void dynamics_rail(const state &x,
                       state &xdot,
                       control &rocket_control,
                       control &aero_control,
                       const double &t) {
        // -------------- Simulation variables -----------------------------
        double g0 = 3.986e14 / pow(6371e3 + x(2), 2);  // Earth gravity in [m/s^2]

        double mass = dry_mass + x(13);     // Instantaneous mass of the rocket in [kg]

        // Orientation of the rocket with quaternion
        Quaternion<double> attitude(x(9), x(6), x(7), x(8));
        attitude.normalize();
        Matrix<double, 3, 3> rot_matrix = attitude.toRotationMatrix();

        // Force in inertial frame: gravity
        Matrix<double, 3, 1> gravity;
        gravity << 0, 0, g0 * mass;

        // Total force in initial body frame [N] (rail frame)
        Matrix<double, 3, 1> total_force;
        total_force = rocket_control.col(0) - rot_matrix.transpose() * (gravity + aero_control.col(0));

        Matrix<double, 3, 1> body_acceleration;

        total_force.head(2) << 0.0, 0.0; // Zero force on axes perpendicular to rail to force rocket to stay on rail
        //std::cout << total_force << "\n";

        // Angular velocity omega in quaternion format to compute quaternion derivative
        Quaternion<double> omega_quat(0.0, x(10), x(11), x(12));

        // -------------- Differential equation ---------------------

        // Position variation is speed
        xdot.head(3) = x.segment(3, 3);

        // Speed variation is Force/mass
        xdot.segment(3, 3) = rot_matrix * total_force / mass;

        // Quaternion variation is zero to keep rail orientation
        xdot.segment(6, 4) << 0.5 * (omega_quat * attitude).coeffs();

        // Angular speed variation is zero to keep rail orientation
        xdot.segment(10, 3) << 0.0, 0.0, 0.0;

        // Mass variation is proportional to total thrust
        xdot.tail(1) << -rocket_control.col(0).norm() / (Isp * g0);

        // Fake sensor data update -----------------
        sensor_acc = (total_force + rot_matrix.transpose() * gravity) / mass;

        sensor_gyro << 0.0, 0.0, 0.0;

        sensor_baro = x(2);
    }
};


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

// TestDatum.h
#include <test/Helpers.h>

#include <vw/Cartography/Datum.h>
#include <vw/Core/Stopwatch.h>
#include <boost/assign/std/vector.hpp>

using namespace vw;
using namespace vw::cartography;
using namespace vw::test;

using namespace boost::assign;

vw::Vector3 vermille_2011_cart_to_geodetic( Datum const& d, Vector3 const& cart ) {
  const double a2 = d.semi_major_axis() * d.semi_major_axis();
  const double b2 = d.semi_minor_axis() * d.semi_minor_axis();
  const double e2 = 1 - b2 / a2;
  const double e4 = e2 * e2;

  double xy_dist = sqrt( cart[0] * cart[0] + cart[1] * cart[1] );
  double p = ( cart[0] * cart[0] + cart[1] * cart[1] ) / a2;
  double q = ( 1 - e2 ) * cart[2] * cart[2] / a2;
  double r = ( p + q - e4 ) / 6.0;
  double r3 = r * r * r;

  Vector3 llh;

  double evolute = 8 * r3 + e4 * p * q;
  double u = std::numeric_limits<double>::quiet_NaN();
  if ( evolute > 0 ) {
    // outside the evolute
    double right_inside_pow = sqrt(e4 * p * q);
    double sqrt_evolute = sqrt( evolute );
    u = r + 0.5 * pow(sqrt_evolute + right_inside_pow,2.0/3.0) +
      0.5 * pow(sqrt_evolute - right_inside_pow,2.0/3.0);
  } else if ( fabs(cart[2]) < std::numeric_limits<double>::epsilon() ) {
    // On the equator plane
    llh[1] = 0;
    llh[2] = norm_2( cart ) - d.semi_major_axis();
  } else if ( evolute < 0 and fabs(q) > std::numeric_limits<double>::epsilon() ) {
    // On or inside the evolute
    double atan_result = atan2( sqrt( e4 * p * q ), sqrt( -evolute ) + sqrt(-8 * r3) );
    u = -4 * r * sin( 2.0 / 3.0 * atan_result ) *
      cos( M_PI / 6.0 + 2.0 / 3.0 * atan_result );
  } else if ( fabs(q) < std::numeric_limits<double>::epsilon() and p <= e4 ) {
    // In the singular disc
    llh[2] = -d.semi_major_axis() * sqrt(1 - e2) * sqrt(e2 - p) / sqrt(e2);
    llh[1] = 2 * atan2( sqrt(e4 - p), sqrt(e2*(e2 - p)) + sqrt(1-e2) * sqrt(p) );
  } else {
    // Near the cusps of the evolute
    double inside_pow = sqrt(evolute) + sqrt(e4 * p * q);
    u = r + 0.5 * pow(inside_pow,2.0/3.0) +
      2 * r * r * pow(inside_pow,-2.0/3.0);
  }

  if (!std::isnan(u) ) {
    double v = sqrt( u * u + e4 * q );
    double u_v = u + v;
    double w = e2 * ( u_v - q ) / ( 2 * v );
    double k = u_v / ( w + sqrt( w * w + u_v ) );
    double D = k * xy_dist / ( k + e2 );
    double dist_2 = D * D + cart[2] * cart[2];
    llh[2] = ( k + e2 - 1 ) * sqrt( dist_2 ) / k;
    llh[1] = 2 * atan2( cart[2], sqrt( dist_2 ) + D );
  }

  if ( xy_dist + cart[0] > ( sqrt(2) - 1 ) * cart[1] ) {
    // Longitude is between -135 and 135
    llh[0] = 360.0 * atan2( cart[1], xy_dist + cart[0] ) / M_PI;
  } else if ( xy_dist + cart[1] < ( sqrt(2) + 1 ) * cart[0] ) {
    // Longitude is between -225 and 45
    llh[0] = - 90.0 + 360.0 * atan2( cart[0], xy_dist - cart[1] ) / M_PI;
  } else {
    // Longitude is between -45 and 225
    llh[0] = 90.0 - 360.0 * atan2( cart[0], xy_dist + cart[1] ) / M_PI;
  }
  llh[1] *= 180.0 / M_PI;

  return llh;
}

#if defined(VW_HAVE_PKG_PROTOBUF) && VW_HAVE_PKG_PROTOBUF==1
TEST( Datum, DatumDesc ) {
  Datum datum("NAD27");

  DatumDesc desc = datum.build_desc();

  Datum datum2(desc);

  EXPECT_EQ(datum.build_desc().DebugString(), datum2.build_desc().DebugString());
}
#endif

TEST( Datum, GeodeticConversion ) {
  Datum datum("WGS84");

  std::vector<double> values;
  values += -8000000.,-7000000.,-6500000.,-6100000.,-5788000.,-500000.,-100000.,-50000.,-10000.,-6000.,-1000.,-200.,-1.,0.,8000000.,7000000.,6500000.,6100000.,5788000.,500000.,100000.,50000.,10000.,6000.,1000.,200.,1;

  // The precision seems to be around a center when the vector is near
  // the center of the earth, which is the worse possible
  // situation. Things are better near the surface.
  for ( size_t ix = 0; ix < values.size(); ix++ ) {
    for ( size_t iy = 0; iy < values.size(); iy++ ) {
      for ( size_t iz = 0; iz < values.size(); iz++ ) {
        Vector3 test_xyz(values[ix],values[iy],values[iz]);
        EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(vermille_2011_cart_to_geodetic(datum,test_xyz)),
                            test_xyz, std::max(1e-12 * norm_2(test_xyz), 1e-2) );
      }
    }
  }

  EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(datum.cartesian_to_geodetic(Vector3(10000,10,-10))),
                      Vector3(10000,10,-10), 1e-6 );
  EXPECT_VECTOR_NEAR( datum.cartesian_to_geodetic(datum.geodetic_to_cartesian(Vector3(30,-10,173740))),
                      Vector3(30,-10,173740), 1e-6 );

  datum.set_well_known_datum("D_MOON"); // This is a spherical datum

  // The precision seems to be around a center when the vector is near
  // the center of the earth, which is the worse possible
  // situation. Things are better near the surface.
  for ( size_t ix = 0; ix < values.size(); ix++ ) {
    for ( size_t iy = 0; iy < values.size(); iy++ ) {
      for ( size_t iz = 0; iz < values.size(); iz++ ) {
        Vector3 test_xyz(values[ix],values[iy],values[iz]);
        EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(vermille_2011_cart_to_geodetic(datum,test_xyz)),
                            test_xyz, std::max(1e-12 * norm_2(test_xyz), 1e-2) );
      }
    }
  }

  EXPECT_VECTOR_NEAR( datum.geodetic_to_cartesian(datum.cartesian_to_geodetic(Vector3(10000,10,-10))),
                      Vector3(10000,10,-10), 1e-6 );
  EXPECT_VECTOR_NEAR( datum.cartesian_to_geodetic(datum.geodetic_to_cartesian(Vector3(30,-10,173740))),
                      Vector3(30,-10,173740), 1e-6 );
}


// Created by Lina Felsner on Thu Aug 18th 2016

#ifndef __model_fdct_calibration_correction_hxx
#define __model_fdct_calibration_correction_hxx

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <LibProjectiveGeometry/ProjectionMatrix.h>
#include "Model.hxx"

#include <Utils/Projtable.hxx>

namespace Geometry {

	struct ModelFDCTCalibrationCorrection : public Model<7>
	{

		double pp_u;
		double pp_v;
		double spacing;
		double sid;
		double sdd;

		static std::vector<std::string> ParameterNames()
		{
			std::vector<std::string> names;
			// Set parameter names
			names.resize(size());
			names[0] = "Translation u";
			names[1] = "Translation v";
			names[2] = "Yaw";
			names[3] = "Pitch";
			names[4] = "Roll";
			names[5] = "Source Isocenter Distance";
			names[6] = "Source Detector Distance";
			return names;
		}

		enum ParameterSet { Identity, DetectorShifts, DetectorRigid2D, DetectorRotations, SDDandSID, All };
		static std::vector<std::string> ParameterSets()
		{
			std::vector<std::string> sets;
			sets[0] = "Identity";
			sets[1] = "DetectorShifts";
			sets[2] = "DetectorRigid2D";
			sets[4] = "DetectorRotations";
			sets[5] = "Source Detector and Iso-Center Distances";
			sets[6] = "All";
			return sets;
		}

		void computeMeanPPandSIDandSDD(const std::vector<Geometry::ProjectionMatrix> Ps)
		{
			std::vector<double> sids, sdds;
			ProjTable::ctCircularTrajectoryToParameters(Ps, spacing, 0x0, 0x0, 0x0, &sids, &sdds);

			// calculate mean
			double sumSID = std::accumulate(sids.begin(), sids.end(), 0.0);
			sid = sumSID / sids.size();
			double sumSDD = std::accumulate(sdds.begin(), sdds.end(), 0.0);
			sdd = sumSDD / sdds.size();

			std::cout << "Mean sid and sdd: " << sid << " " << sdd << std::endl;

			Geometry::RP2Point ppmean(0,0,0);
			for (int i = 0; i < (int)Ps.size(); i++)
			{
				Geometry::RP2Point pp=Ps[i].block<3, 3>(0, 0)*Ps[i].block<1, 3>(2, 0).transpose();
				pp = pp / pp(2);
				ppmean+=pp;
			}
			pp_u = ppmean[0] / ppmean(2);
			pp_v = ppmean[1] / ppmean(2);
			std::cout << "Mean principal point: " << pp_u << " " << pp_v << std::endl;
		}

		ModelFDCTCalibrationCorrection(int _nu, int _nv, double  _spacing, double sid, double sdd, ParameterSet active = Identity)
			: Model<7>(ParameterNames())
			, pp_u(0.5*_nu)
			, pp_v(0.5*_nv)
			, spacing(_spacing)
		{
			// Set parameter names
			names = ModelFDCTCalibrationCorrection::ParameterNames();
			// Set active parameters
			setActiveParameters(active);
		}

		ModelFDCTCalibrationCorrection(double  _spacing, const std::vector<Geometry::ProjectionMatrix>& Ps, ParameterSet active = Identity)
			: Model<7>(ParameterNames())
			, spacing(_spacing)
		{
			// Set parameter names
			names = ModelFDCTCalibrationCorrection::ParameterNames();
			// Set active parameters
			setActiveParameters(active);
			// Analyze geometry
			computeMeanPPandSIDandSDD(Ps);
		}

		/// Several common sets of active parameters
		void setActiveParameters(ParameterSet set)
		{
			for (int i = 0; i<size(); i++) active[i] = (set==All);

			switch (set)
			{
				default:
				case Identity:
				case All:
					break;
				case DetectorShifts:
					active[0] = active[1] = true;
				break;
				case SDDandSID:
					active[5] = active[6] = true;
				break;
				case DetectorRotations:
					active[2]=active[3]=active[4]=true;
				break;
				case DetectorRigid2D:
					active[0] =active[1] = active[4] = true;
				break;
			}
		}

		// Transform the Projection Matrix via: H*H_init*P*T
		void transform(std::vector<Geometry::ProjectionMatrix> &Ps, Eigen::Matrix3d H_init = Eigen::Matrix3d::Identity()) const
		{

			//TODO
			std::pair<Eigen::Matrix3d, Eigen::Matrix4d> HT = getTransforms(Ps);
			Eigen::Matrix3d H = HT.first;
			Eigen::Matrix4d T = HT.second;

			for (int i = 0; i < Ps.size(); i++)
			{
				Ps[i] = H*H_init*Ps[i]*T;
				Geometry::normalizeProjectionMatrix(Ps[i]);
			}
		}

		// Compose a homography from Parametrization
		std::pair<Eigen::Matrix3d, Eigen::Matrix4d> getTransforms(std::vector<Geometry::ProjectionMatrix> Ps) const
		{
			const double *x(param); //parameter vector

			Eigen::Matrix3d H = Eigen::Matrix3d::Identity();
			Eigen::Matrix4d T = Eigen::Matrix4d::Identity();

			Geometry::RP2Point ppmean(pp_u, pp_v, 1.0);

			double sid_scale = (sid + x[5]) / sid;
			double sdd_scale = (sdd + x[6]) / sdd;

//// H = Matrix 3x3
			Eigen::Matrix3d H_roll, H_shift, H_scale;
			H_roll = H_shift = H_scale = H;

			// using the principal point / center of the image
			auto Hpp = H, Hppinv = H;
			Hpp.block<3, 1>(0, 2) = ppmean;
			Hppinv(0, 2) = -ppmean[0];
			Hppinv(1, 2) = -ppmean[1];

			//Roll -> Rotation
			if (x[4] != 0)
			{
				H_roll <<
					+cos(x[4]), -sin(x[4]), 0,
					+sin(x[4]), +cos(x[4]), 0,
					0,			0,			1;
			}

			// Translation u and Yaw
			H_shift(0, 2) += x[0];
			H_shift(0, 2) += tan(x[2])*sdd;

			// Translation v and Pitch
			H_shift(1, 2) += x[1];
			H_shift(1, 2) += tan(x[3])*sdd;

			//SDD
			H_scale.block<2, 2>(0, 0) *= sdd_scale;

			H = H_shift * Hpp * H_roll * H_scale * Hppinv;

//// T = Matrix 4x4
			//SID
			T.block<3, 3>(0, 0) *= sid_scale;

//// HT
			std::pair<Eigen::Matrix3d, Eigen::Matrix4d> HT = { H, T };
			return HT;
		}

	};

}

#endif // __model_fdct_calibration_correction_hxx


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * Permission is hereby granted, free of charge, to any person
 * obtaining a copy of this software and associated documentation
 * files (the "Software"), to deal in the Software without
 * restriction, including without limitation the rights to use,
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following
 * conditions:
 *
 * The above copyright notice and this permission notice shall be
 * included in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 */

#include "arithmetic.hpp"

#include <boost/range/counting_range.hpp>

namespace cirkit
{

/******************************************************************************
 * Types                                                                      *
 ******************************************************************************/

/******************************************************************************
 * Private functions                                                          *
 ******************************************************************************/

/* move? */
bdd ite( const bdd& cond, const bdd& _then, const bdd& _else )
{
  return ( !cond && _else ) || ( cond && _then );
}

std::tuple<bdd, bdd> full_adder( const bdd& x, const bdd& y, const bdd& cin )
{
  const auto sum  = x ^ y ^ cin;
  const auto cout = ( x && y ) || ( x && cin ) || ( y && cin );

  return std::make_tuple( sum, cout );
}

/******************************************************************************
 * Public functions                                                           *
 ******************************************************************************/

std::vector<bdd> bdd_subtract( const std::vector<bdd>& minuend, const std::vector<bdd>& subtrahend )
{
  assert( minuend.size() == subtrahend.size() );
  assert( !minuend.empty() );

  std::vector<bdd> diff( minuend.size() );
  bdd carry = minuend.front().manager->bdd_top();

  for ( const auto& i : boost::counting_range( 0ul, static_cast<unsigned long>( minuend.size() ) ) )
  {
    std::tie( diff[i], carry ) = full_adder( minuend[i], !subtrahend[i], carry );
  }

  return diff;
}

std::vector<bdd> bdd_abs( const std::vector<bdd>& n )
{
  std::vector<bdd> mask( n.size(), n.back() );
  std::vector<bdd> result;

  for ( const auto& i : boost::counting_range( 0ul, static_cast<unsigned long>( n.size() ) ) )
  {
    result.push_back( n[i] ^ mask[i] );
  }

  auto diff = bdd_subtract( result, mask );
  std::copy( diff.begin(), diff.begin() + n.size(), result.begin() );

  return result;
}

std::vector<bdd> zero_extend( const std::vector<bdd>& n, unsigned to )
{
  assert( to >= n.size() );
  auto ze = n;
  ze.resize( to, n.front().manager->bdd_bot() );
  return ze;
}

std::vector<bdd> sign_extend( const std::vector<bdd>& n, unsigned to )
{
  assert( to >= n.size() );
  auto se = n;
  se.resize( to, n.back() );
  return se;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


//==================================================================================================
/*!
  @file

    @copyright 2016 NumScale SAS

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!

    @ingroup group-arithmetic
    This function object computes the greatest integral representable value of
    its parameter type which is less or equal to it.

    @par Header <boost/simd/function/floor.hpp>

    @par Notes

     - @c floor is also used as parameter to pass to @ref div or @ref rem

    @par Decorators

    - std_ for floating entries call std::floor

    @see  ceil, round, nearbyint, trunc, ifloor

    @par Example:

      @snippet floor.cpp floor

    @par Possible output:

      @snippet floor.txt floor

  **/
  Value floor(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/floor.hpp>
#include <boost/simd/function/simd/floor.hpp>

#endif


// Boost.Geometry (aka GGL, Generic Geometry Library)
//
// Copyright (c) 2012 Barend Gehrels, Amsterdam, the Netherlands.
//
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// Projection example 5 (reworked from 4), using small factory

#include <fstream>

#include <boost/foreach.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/multi/geometries/multi_polygon.hpp>

#include <boost/geometry/io/svg/svg_mapper.hpp>
#include <boost/geometry/extensions/gis/latlong/latlong.hpp>

#include <boost/geometry/extensions/gis/projections/parameters.hpp>
#include <boost/geometry/extensions/gis/projections/proj/goode.hpp>
#include <boost/geometry/extensions/gis/projections/proj/moll.hpp>
#include <boost/geometry/extensions/gis/projections/proj/natearth.hpp>
#include <boost/geometry/extensions/gis/projections/proj/robin.hpp>

#include <boost/geometry/extensions/gis/projections/new_projection.hpp>

// Define a specific projection transformer
// (NOTE: this might become part of the library - copied from p04)
template <typename Projection>
struct projection_transformer
{
    Projection const& m_prj;

    inline projection_transformer(Projection const& prj)
        : m_prj(prj)
    {}

    inline bool apply(typename Projection::geographic_point_type const& p1,
                typename Projection::cartesian_point_type& p2) const
    {
        return m_prj.forward(p1, p2);
    }
};

void p05_example(int projection_id,
        std::string const& wkt_filename,
        std::string const& svg_filename)
{
    using namespace boost::geometry;
    using namespace boost::geometry::projections;

    typedef model::ll::point<degree> pll;
    typedef model::d2::point_xy<double> pxy;

    // Idea and headerfile "new_projection" submitted by Krzysztof Czainski:
    // They are useful, when:
    // - you have a small set of types of projections you'll use,
    // - you know the type of projection during it's creation, and later
    //   you want to use it through an abstract base pointer,
    // - you want to avoid the overhead of factory: generating code for
    //   creating a projection of every type you don't use, and selecting
    //   the type from a string.

    projection<pll, pxy>* prj = NULL;

    parameters pars = projections::init("+ellps=WGS84");
    switch(projection_id)
    {
        case 1 : prj = new_projection<robin_spheroid<pll, pxy> >(pars); break;
        case 2 : prj = new_projection<moll_spheroid<pll, pxy> >(pars); break;
        case 3 : prj = new_projection<goode_spheroid<pll, pxy> >(pars); break;
        case 4 : prj = new_projection<natearth_spheroid<pll, pxy> >(pars); break;
        default : return;
    }

    typedef model::multi_polygon<model::polygon<pll> > mp_ll;
    typedef model::multi_polygon<model::polygon<pxy> > mp_xy;

    std::vector<mp_ll> countries_in_ll;

    // Read polygons from WKT
    std::ifstream cpp_file(wkt_filename.c_str());
    if (! cpp_file.is_open())
    {
        throw std::string("File not found: ") + wkt_filename;
    }

    while (! cpp_file.eof() )
    {
        std::string line;
        std::getline(cpp_file, line);
        if (boost::starts_with(line, "MULTIPOLYGON"))
        {
            countries_in_ll.resize(countries_in_ll.size() + 1);
            boost::geometry::read_wkt(line, countries_in_ll.back());
        }
    }

    projection_transformer<projection<pll, pxy> > strategy(*prj);

    // Project the polygons, and at the same time get the bounding box (in xy)
    std::vector<mp_xy> countries_in_xy;
    model::box<pxy> bbox;
    assign_inverse(bbox);
    BOOST_FOREACH(mp_ll const& country_ll, countries_in_ll)
    {
        mp_xy country_xy;
        if (transform(country_ll, country_xy, strategy))
        {
            expand(bbox, return_envelope<model::box<pxy> >(country_xy));
            countries_in_xy.push_back(country_xy);
        }
    }

    // Create an SVG image
    std::ofstream svg(svg_filename.c_str());
    boost::geometry::svg_mapper<pxy> mapper(svg, 1000, 800);
    mapper.add(bbox);

    BOOST_FOREACH(mp_xy const& country, countries_in_xy)
    {
        mapper.map(country, "fill-opacity:0.6;fill:rgb(153,204,0);stroke:rgb(0,128,0);stroke-width:0.2");
    }

    delete prj;
}

int main(int argc, char** argv)
{
    // Note, file location: trunk/libs/geometry/example/data
    // update path below if necessary
    std::string const data = "../../../../example/data/world.wkt";
    try
    {
        p05_example(1, data, "p05_world_1.svg");
        p05_example(2, data, "p05_world_2.svg");
        p05_example(3, data, "p05_world_3.svg");
        p05_example(4, data, "p05_world_4.svg");
    }
    catch(std::exception const& e)
    {
        std::cerr << "Exception: " << e.what() << std::endl;
        return 1;
    }
    catch(std::string const& s)
    {
        std::cerr << "Exception: " << s << std::endl;
        return 1;
    }

    return 0;
}