/* Copyright (C) 2010-2019, The Regents of The University of Michigan.
 All rights reserved.

 This software was developed as part of the The Vulcan project in the Intelligent Robotics Lab
 under the direction of Benjamin Kuipers, kuipers@umich.edu. Use of this code is governed by an
 MIT-style License that can be found at "https://github.com/h2ssh/Vulcan".
*/

/**
 * \file     laser_object.cpp
 * \author   Collin Johnson
 *
 * Definition of LaserObject.
 */

#include "tracker/laser_object.h"
#include "math/clustering.h"
#include "math/geometry/shape_fitting.h"
#include "math/geometry/shape_intersection_area.h"
#include "math/statistics.h"
#include "utils/algorithm_ext.h"
#include <array>
#include <boost/range/iterator_range.hpp>

// #define DEBUG_UNCERTAINTY

namespace vulcan
{
namespace tracker
{

const float kMaxCircleRadius = 0.25f;
const float kMaxLegRadius = 0.15f;

using DistIndex = std::pair<double, int>;
using Outliers = std::vector<DistIndex>;

Circle fit_circle_to_points(ConstPointIter begin, ConstPointIter end, Position laserPosition, double maxRadius);
Outliers find_outliers(ConstPointIter begin, ConstPointIter end);
boost::optional<EstimatedShape<TwoArcs>> fit_two_arcs_to_points(ConstPointIter begin,
                                                                ConstPointIter end,
                                                                Position laserPosition,
                                                                double laserVariance,
                                                                const std::vector<int>& splitIndices);
EstimatedShape<TwoRects> fit_two_rects_to_points(ConstPointIter begin,
                                                 ConstPointIter end,
                                                 Position laserPosition,
                                                 double laserVariance,
                                                 const std::vector<int>& splitIndices);
Arc circle_to_arc(Circle circle, ConstPointIter begin, ConstPointIter end);
double circle_fit_error(const Circle& circle, ConstPointIter begin, ConstPointIter end, float maxRadius);
Matrix arc_fit_uncertainty(const Arc& arc, ConstPointIter begin, ConstPointIter end, double laserVariance);
double rect_fit_error(const Rectangle& rect, Position laserPosition, ConstPointIter begin, ConstPointIter end);
Matrix rect_fit_uncertainty(const Rectangle& rect, ConstPointIter begin, ConstPointIter end, double laserVariance);

inline double two_circles_area(const TwoCircles& circles)
{
    return circles[0].area() + circles[1].area();
}

LaserObject::LaserObject(void)
{
}

LaserObject::LaserObject(int64_t timestamp,
                         ConstPointIter pointsBegin,
                         ConstPointIter pointsEnd,
                         const Position& laserPosition,
                         double variance)
: timestamp_(timestamp)
, points_(pointsBegin, pointsEnd)
, laserVariance_(variance)
, fitTwoArcs_(boost::none)
, fitTwoRects_(boost::none)
, laserPosition_(laserPosition)
{
    assert(points_.size() > 2);
    assert(variance > 0.0);
    assert(std::distance(pointsBegin, pointsEnd) > 2);

    fitShapesToPoints(true);   // always use outliers rather than clustering
    calculateUncertainty(BoundaryType::best);
    calculateCircleApprox();

#ifdef DEBUG_UNCERTAINTY
    std::cout << "DEBUG: LaserObject: Uncertainties for object at " << center() << ":\n"
              << centerDistribution_.getCovariance();
#endif
}

LaserObject::LaserObject(const std::vector<const LaserObject*>& objects)
: laserVariance_(objects.front()->laserVariance_)
{
    assert(!objects.empty());

    for (auto& o : objects) {
        points_.insert(points_.end(), o->points_.begin(), o->points_.end());
    }

    timestamp_ = objects.front()->timestamp_;
    laserPosition_ = objects.front()->laserPosition_;

    // If the lasers aren't the same, then need to perform a sort on the points
    bool diffLaser = utils::contains_if(objects, [this](const auto& obj) {
        return obj->laserPosition_ != laserPosition_;
    });

    fitShapesToPoints(!diffLaser);
    calculateUncertainty(BoundaryType::best);
    calculateCircleApprox();
}

LaserObject LaserObject::generateShadowedCircle(void) const
{
    // Copy all existing state
    LaserObject shadow = *this;

    // Replace the existing two-arc with a single arc plus a fabricated arc. The circle sits on the line running
    // between the detected circle and laser. Extend this line to extent two circle radii further and then it should
    // be good to go.
    auto distance = distance_between_points(laserPosition_, fitArc_.shape.center());
    if (distance > 0.0) {
        auto radiusToDist = fitArc_.shape.radius() / distance;
        auto newDistRatio = 1.0 + (2.0 * radiusToDist);
        auto newCenter =
          Point<float>(laserPosition_.x + ((fitArc_.shape.center().x - laserPosition_.x) * newDistRatio),
                       laserPosition_.y + ((fitArc_.shape.center().y - laserPosition_.y) * newDistRatio));

        EstimatedShape<TwoArcs> twoArcs;
        twoArcs.shape[0] = fitArc_.shape;
        twoArcs.shape[1] = math::Arc<float>(fitArc_.shape.radius(), newCenter, math::angle_range_t());
        twoArcs.error = fitArc_.error / 2.0;
        twoArcs.uncertainty = fitArc_.uncertainty * 1.5;
        shadow.fitTwoArcs_ = twoArcs;

        std::cout << "Created a shadow for " << fitArc_.shape.center() << ',' << fitArc_.shape.radius() << " at "
                  << twoArcs.shape[1].center() << ',' << twoArcs.shape[1].radius() << '\n';
    }

    return shadow;
}

void LaserObject::setBoundaryType(BoundaryType type)
{
    calculateUncertainty(type);
}

ObjectBoundary LaserObject::minErrorBoundary(void) const
{
    return minErrorBoundaryAndUncertainty().first;
}

ObjectBoundary LaserObject::boundaryWithType(BoundaryType type) const
{
    switch (type) {
    case BoundaryType::rectangle:
        return ObjectBoundary(fitRect_.shape);
    case BoundaryType::one_circle:
        return ObjectBoundary(fitArc_.shape.toCircle());
    case BoundaryType::two_circles:
        if (fitTwoArcs_) {
            TwoCircles circles = {{fitTwoArcs_->shape[0].toCircle(), fitTwoArcs_->shape[1].toCircle()}};
            return ObjectBoundary(circles);
        } else {
            return minErrorBoundary();
        }
    case BoundaryType::two_rects:
        if (fitTwoRects_) {
            return ObjectBoundary(fitTwoRects_->shape);
        } else {
            return minErrorBoundary();
        }
    case BoundaryType::circle_rect:
        std::cerr << "ERROR: LaserObject: circle-rect not currently estimated.\n";
        return minErrorBoundary();
    case BoundaryType::best:
        return minErrorBoundary();
    case BoundaryType::unknown:
        return minErrorBoundary();
    default:
        return minErrorBoundary();
    }

    return minErrorBoundary();
}

void LaserObject::fitShapesToPoints(bool searchForOutliers)
{
    fitRect_.shape = math::minimum_geometric_error_bounding_rectangle(points_.begin(), points_.end());
    fitRect_.error = rect_fit_error(fitRect_.shape, laserPosition_, points_.begin(), points_.end());
    fitRect_.uncertainty = rect_fit_uncertainty(fitRect_.shape, points_.begin(), points_.end(), laserVariance_);

    auto fitCircle = fit_circle_to_points(points_.begin(), points_.end(), laserPosition_, kMaxCircleRadius);
    fitArc_.error = circle_fit_error(fitCircle, points_.begin(), points_.end(), kMaxCircleRadius);
    fitArc_.shape = circle_to_arc(fitCircle, points_.begin(), points_.end());
    fitArc_.uncertainty = arc_fit_uncertainty(fitArc_.shape, points_.begin(), points_.end(), laserVariance_);

    if (std::distance(points_.begin(), points_.end()) >= 6) {
        std::vector<int> splitIndices;
        if (searchForOutliers) {
            Outliers outliers = find_outliers(points_.begin(), points_.end());
            for (auto o : outliers) {
                splitIndices.push_back(o.second);
            }
        }

        // If there aren't any split indices from outliers, attempt to cluster the points
        if (splitIndices.empty()) {
            auto clusters = math::kmeans_2d_fixed(points_.begin(), points_.end(), 2);
            assert(clusters.assignedCluster.size() == points_.size());
            // Convert the clusters into a new set of points to be used
            decltype(points_) clusteredPoints;
            for (int c = 0; c < clusters.numClusters; ++c) {
                for (std::size_t n = 0; n < points_.size(); ++n) {
                    if (clusters.assignedCluster[n] == c) {
                        clusteredPoints.push_back(points_[n]);
                    }
                }
            }

            // Split only at the cluster boundaries
            int splitPoint = 0;
            for (int size : clusters.clusterSizes) {
                splitPoint += size;
                splitIndices.push_back(splitPoint + 1);
            }
            std::swap(points_, clusteredPoints);
        }

        fitTwoArcs_ =
          fit_two_arcs_to_points(points_.begin(), points_.end(), laserPosition_, laserVariance_, splitIndices);
        //             fitTwoRects_ = fit_two_rects_to_points(points_.begin(), points_.end(), laserVariance_,
        //             splitIndices);
    }
}

void LaserObject::calculateUncertainty(BoundaryType type)
{
    std::pair<ObjectBoundary, Matrix> boundaryAndUncertainty;

    switch (type) {
    case BoundaryType::rectangle:
        boundaryAndUncertainty.first.assign(fitRect_.shape, BoundaryType::rectangle);
        boundaryAndUncertainty.second = fitRect_.uncertainty;
        break;
    case BoundaryType::one_circle:
        boundaryAndUncertainty.first.assign(fitArc_.shape.toCircle(), BoundaryType::one_circle);
        boundaryAndUncertainty.second = fitArc_.uncertainty;
        break;
    case BoundaryType::two_circles:
        if (fitTwoArcs_) {
            TwoCircles fitCircles = {fitTwoArcs_->shape[0].toCircle(), fitTwoArcs_->shape[1].toCircle()};
            boundaryAndUncertainty.first.assign(fitCircles, BoundaryType::two_circles);
            boundaryAndUncertainty.second = fitTwoArcs_->uncertainty;
        } else {
            boundaryAndUncertainty.first.assign(fitArc_.shape.toCircle(), BoundaryType::one_circle);
            boundaryAndUncertainty.second = fitArc_.uncertainty;
        }
        break;
    case BoundaryType::two_rects:
        std::cerr << "WARNING: LaserObject: two_rects not currently estimated. Using best.\n";
        boundaryAndUncertainty = minErrorBoundaryAndUncertainty();
        break;
    case BoundaryType::circle_rect:
        std::cerr << "WARNING: LaserObject: circle_rect not currently estimated. Using best.\n";
        boundaryAndUncertainty = minErrorBoundaryAndUncertainty();
        break;
    case BoundaryType::best:
    case BoundaryType::unknown:   // intentional fall-through
    default:
        boundaryAndUncertainty = minErrorBoundaryAndUncertainty();
        break;
    }

    Vector centerMean(2);
    Matrix centerCov(2, 2);

    centerMean(0) = boundaryAndUncertainty.first.position().x;
    centerMean(1) = boundaryAndUncertainty.first.position().y;

    centerCov = boundaryAndUncertainty.second.submat(0, 0, 1, 1);
    centerDistribution_.setDistributionStatistics(centerMean, centerCov);
}

void LaserObject::calculateCircleApprox(void)
{
    float maxDistToCenter = 0.0f;
    for (auto& point : points_) {
        float dist = distance_between_points(center(), point);
        if (dist > maxDistToCenter) {
            maxDistToCenter = dist;
        }
    }
    circleApproximation_ = Circle(maxDistToCenter, center());
}

std::pair<ObjectBoundary, Matrix> LaserObject::minErrorBoundaryAndUncertainty(void) const
{
    ObjectBoundary bestBoundary;
    Matrix uncertainty;
    bestBoundary.assignApproximation(circleApproximation_);

    if ((fitArc_.error < fitRect_.error) && (!fitTwoArcs_ || (fitArc_.error < fitTwoArcs_->error))
        && (!fitTwoRects_ || (fitArc_.error < fitTwoRects_->error))) {
        bestBoundary.assign(fitArc_.shape.toCircle(), BoundaryType::one_circle);
        uncertainty = fitArc_.uncertainty;
    } else if ((!fitTwoArcs_ || (fitRect_.error < fitTwoArcs_->error))
               && (!fitTwoRects_ || (fitRect_.error < fitTwoRects_->error))) {
        bestBoundary.assign(fitRect_.shape, BoundaryType::rectangle);
        uncertainty = fitRect_.uncertainty;
    } else if (fitTwoArcs_ && (!fitTwoRects_ || (fitTwoArcs_->error < fitTwoRects_->error))) {
        TwoCircles fitCircles = {fitTwoArcs_->shape[0].toCircle(), fitTwoArcs_->shape[1].toCircle()};
        bestBoundary.assign(fitCircles, BoundaryType::two_circles);
        uncertainty = fitTwoArcs_->uncertainty;
    } else   // two rects fit the best
    {
        bestBoundary.assign(fitTwoRects_->shape, BoundaryType::two_rects);
        uncertainty = fitTwoRects_->uncertainty;
    }

    return std::make_pair(bestBoundary, uncertainty);
}

Circle fit_circle_to_points(ConstPointIter begin, ConstPointIter end, Position laserPosition, const double maxRadius)
{
    auto fitCircle = math::minimum_geometric_error_circle(begin, end, 0.01, maxRadius);

    auto minDistIt = std::min_element(begin, end, [laserPosition](const auto& lhs, const auto& rhs) {
        return distance_between_points(lhs, laserPosition) < distance_between_points(rhs, laserPosition);
    });

    double closestLaserDist = distance_between_points(laserPosition, *minDistIt);

    // A fit circle can't have a distance to center closer than the closest laser point. It doesn't make physical sense.
    // because the laser is hitting the outside of the circle. The center being closer than the closest laser means
    // the laser must be seeing through the object, which doesn't make sense.
    // Don't need to worry about the rectangle because it is a minimum error bounding rectangle and the size won't
    // grow arbitrarily. It can't be much larger than the convex hull of the points
    // If the min geometric error circle doesn't make physical sense, then just use the mean circle, which isn't
    // a good fit, but will be physically valid
    if (distance_between_points(fitCircle.center(), laserPosition) < closestLaserDist) {
        fitCircle = math::mean_position_and_radius_circle(begin, end);
    }

    return fitCircle;
}

Outliers find_outliers(ConstPointIter begin, ConstPointIter end)
{
    int numPoints = std::distance(begin, end);
    int firstSplit = 3;

    int firstQuartile = std::max(numPoints / 4, firstSplit);
    int thirdQuartile = std::min(numPoints * 3 / 4, numPoints - firstSplit);

    Outliers distances;
    distances.reserve(numPoints - 1);
    for (int n = 1; n < numPoints; ++n) {
        distances.push_back(std::make_pair(distance_between_points(*(begin + n), *(begin + n - 1)), n));
    }

    std::sort(distances.begin(), distances.end(), [](const DistIndex& lhs, const DistIndex& rhs) {
        return lhs.first < rhs.first;
    });

    auto first = distances[distances.size() / 4];
    auto third = distances[distances.size() * 3 / 4];
    double iqr = (third.first - first.first) * 3;

    std::vector<DistIndex> outliers;
    for (auto& d : distances) {
        if ((d.second >= firstQuartile) && (d.second <= thirdQuartile) && (d.first > third.first + iqr)) {
            outliers.push_back(d);
        }
    }

    outliers.push_back(std::make_pair(0.0, numPoints / 2));

    return outliers;
}

boost::optional<EstimatedShape<TwoArcs>> fit_two_arcs_to_points(ConstPointIter begin,
                                                                ConstPointIter end,
                                                                Position laserPosition,
                                                                double laserVariance,
                                                                const std::vector<int>& splitIndices)
{
    TwoCircles fitCircles;
    std::array<double, 2> circleErrors;
    boost::optional<EstimatedShape<TwoArcs>> estimatedArcs;

    for (int splitIndex : splitIndices) {
        // Ignore any splits that don't leave enough room for a proper circle fit
        if ((std::distance(begin, begin + splitIndex) < 3) || (std::distance(begin + splitIndex, end) < 3)) {
            continue;
        }

        fitCircles[0] = fit_circle_to_points(begin, begin + splitIndex, laserPosition, kMaxLegRadius);
        fitCircles[1] = fit_circle_to_points(begin + splitIndex, end, laserPosition, kMaxLegRadius);

        // The two fit circle must not be concentric. If they are, then they don't  make sense, as they should be two
        // unique circles. Some overlap of boundaries is allowed because sensor error is expected.
        // If they are concentric, just assign them to their mean circles, which won't be a good fit, but won't
        // be concentric either
        if (fitCircles[0].contains(fitCircles[1].center()) || fitCircles[1].contains(fitCircles[0].center())) {
            fitCircles[0] = math::mean_position_and_radius_circle(begin, begin + splitIndex);
            fitCircles[1] = math::mean_position_and_radius_circle(begin + splitIndex, end);
        }

        circleErrors[0] = circle_fit_error(fitCircles[0], begin, begin + splitIndex, kMaxLegRadius);
        circleErrors[1] = circle_fit_error(fitCircles[1], begin + splitIndex, end, kMaxLegRadius);

        if (!estimatedArcs || (circleErrors[0] + circleErrors[1] < estimatedArcs->error)) {
            EstimatedShape<TwoArcs> arcs;
            arcs.error = circleErrors[0] + circleErrors[1];
            arcs.shape[0] = circle_to_arc(fitCircles[0], begin, begin + splitIndex);
            arcs.shape[1] = circle_to_arc(fitCircles[1], begin + splitIndex, end);

            arcs.uncertainty = arc_fit_uncertainty(arcs.shape[0], begin, begin + splitIndex, laserVariance)
              + arc_fit_uncertainty(arcs.shape[1], begin + splitIndex, end, laserVariance);
            estimatedArcs = arcs;
        }
    }

    return estimatedArcs;
}

EstimatedShape<TwoRects> fit_two_rects_to_points(ConstPointIter begin,
                                                 ConstPointIter end,
                                                 Position laserPosition,
                                                 double laserVariance,
                                                 const std::vector<int>& splitIndices)
{
    TwoRects fitRects;
    std::array<double, 2> rectErrors;
    EstimatedShape<TwoRects> estimatedRects;
    estimatedRects.error = HUGE_VAL;

    for (int splitIndex : splitIndices) {
        fitRects[0] = math::minimum_geometric_error_bounding_rectangle(begin, begin + splitIndex);
        fitRects[1] = math::minimum_geometric_error_bounding_rectangle(begin + splitIndex, end);

        rectErrors[0] = rect_fit_error(fitRects[0], laserPosition, begin, begin + splitIndex);
        rectErrors[1] = rect_fit_error(fitRects[1], laserPosition, begin + splitIndex, end);

        if (rectErrors[0] + rectErrors[1] < estimatedRects.error) {
            estimatedRects.error = rectErrors[0] + rectErrors[1];
            estimatedRects.shape = fitRects;
            estimatedRects.uncertainty = rect_fit_uncertainty(fitRects[0], begin, begin + splitIndex, laserVariance)
              + rect_fit_uncertainty(fitRects[1], begin + splitIndex, end, laserVariance);
        }
    }

    return estimatedRects;
}

Arc circle_to_arc(Circle circle, ConstPointIter begin, ConstPointIter end)
{
    // The range needs to contain both the first and last point fit to the circle. Create it containing the first
    // angle. Then expand it to contain the second angle. Ensures that the range invariants are correctly maintained.
    math::angle_range_t range(angle_to_point(circle.center(), *begin));
    range.expand(angle_to_point(circle.center(), *(end - 1)));

    return Arc(circle.radius(), circle.center(), range);
}

double circle_fit_error(const Circle& circle, ConstPointIter begin, ConstPointIter end, float maxRadius)
{
    double totalError = 0.0;

    for (; begin != end; ++begin) {
        totalError += std::abs(distance_between_points(circle.center(), *begin) - circle.radius());
    }

    if (circle.radius() > maxRadius) {
        totalError += 5.0;
    }

    return totalError;
}

Matrix arc_fit_uncertainty(const Arc& arc, ConstPointIter begin, ConstPointIter end, double laserVariance)
{
    int numPoints = std::distance(begin, end);
    Matrix W(numPoints, 3);
    for (int n = 0; n < numPoints; ++n) {
        W(n, 0) = ((begin + n)->x - arc.center().x) / arc.radius();
        W(n, 1) = ((begin + n)->y - arc.center().y) / arc.radius();
        W(n, 2) = 1.0;
    }

    Matrix wPseudo = arma::trans(W) * W;
    wPseudo.diag() += 1e-8;

    Matrix cov = laserVariance * arma::inv(wPseudo);
    return cov;
}

double rect_fit_error(const Rectangle& rect, Position laserPosition, ConstPointIter begin, ConstPointIter end)
{
    // Find the visible edge(s) given the laser position. The visible edges will include the closest edge and possibly
    // one other edge. If the angle subtended by the closest edge is includes the angle subtended by an adjacent edge,
    // then that edge isn't visible (draw the figure and it is easy to see this)
    std::array<Line<float>, 4> edges;
    edges[0] = Line<float>(rect.bottomLeft, rect.bottomRight);
    edges[1] = Line<float>(rect.bottomRight, rect.topRight);
    edges[2] = Line<float>(rect.topRight, rect.topLeft);
    edges[3] = Line<float>(rect.topLeft, rect.bottomLeft);

    // Find the closest edge
    auto minIt = std::min_element(edges.begin(), edges.end(), [laserPosition](const auto& lhs, const auto& rhs) {
        return distance_to_line_segment(laserPosition, lhs) < distance_to_line_segment(laserPosition, rhs);
    });

    // Compute the angle subtend by this edge
    int closestIdx = std::distance(edges.begin(), minIt);
    math::angle_range_t edgeRange(angle_to_point(laserPosition, edges[closestIdx].a));
    edgeRange.expand(angle_to_point(laserPosition, edges[closestIdx].b));

    double initialExtent = edgeRange.extent;

    // Search the neighbors to see if they cause the extent to grow
    int visibleIdx = -1;
    std::array<int, 2> neighborIdx;
    neighborIdx[0] = (closestIdx == 0) ? 3 : closestIdx - 1;
    neighborIdx[1] = (closestIdx == 3) ? 0 : closestIdx + 1;
    for (auto idx : neighborIdx) {
        edgeRange.expand(angle_to_point(laserPosition, edges[idx].a));
        edgeRange.expand(angle_to_point(laserPosition, edges[idx].b));

        if (edgeRange.extent > initialExtent) {
            visibleIdx = idx;
            initialExtent = edgeRange.extent;
        }
    }

    // Set the visible edges for use in the distance computation
    std::array<Line<float>, 2> visibleEdges;
    visibleEdges[0] = edges[closestIdx];
    if (visibleIdx != -1) {
        visibleEdges[1] = edges[visibleIdx];
    }
    // Reuse the closest index for ease of implementation, though not efficiency
    else {
        visibleEdges[1] = edges[closestIdx];
    }

    // Compute the distance to these lines for each of the points
    return std::accumulate(begin, end, 0.0, [&visibleEdges](double total, const Position& p) {
        return total + std::min(distance_to_line(p, visibleEdges[0]), distance_to_line(p, visibleEdges[1]));
    });
}

Matrix rect_fit_uncertainty(const Rectangle& rect, ConstPointIter begin, ConstPointIter end, double laserVariance)
{
    int numPoints = std::distance(begin, end);
    Matrix W(numPoints, 3);
    for (int n = 0; n < numPoints; ++n) {
        auto boundaryPoint = rect.closestPointOnBoundary(*(begin + n));
        auto radius = distance_between_points(rect.center(), boundaryPoint.first);
        W(n, 0) = ((begin + n)->x - rect.center().x) / radius;
        W(n, 1) = ((begin + n)->y - rect.center().y) / radius;
        W(n, 2) = 1.0;
    }

    Matrix wPseudo = arma::trans(W) * W;
    wPseudo.diag() += 1e-8;

    Matrix cov = laserVariance * arma::inv(wPseudo);
    return cov;
}

}   // namespace tracker
}   // namespace vulcan


// ======================================================================
/*!
 * \brief frontier::BezierModel
 *
 *		  Converted from Esa's java code
 */
// ======================================================================

#include "BezierModel.h"
#include "Vector2Dee.h"
#include <boost/foreach.hpp>
#include <math.h>

namespace frontier
{
BezierModel::BezierModel(const std::vector<DirectPosition>& theCurvePositions,
                         boolean isClosedCurve,
                         double theTightness)
    : tightness(theTightness)
{
  init(theCurvePositions, isClosedCurve);
}

void BezierModel::init(const std::vector<DirectPosition>& theCurvePositions, boolean isClosedCurve)
{
  // Creates list of Bezier segments *from the scratch*.

  curvePositions = theCurvePositions;
  //	this.curvePositions = null;
  //	this.curvePositions = new Arraystd::vector<DirectPosition>();
  //	for(DirectPosition pos : curvePositions)
  //	{
  //		this.curvePositions.add(pos);
  //	}

  bIsClosedCurve = isClosedCurve;
  orientation = POS;
  totalCurveLength = 0.;

  int lastIndex = curvePositions.size() - 1;
  int secondLastIndex = curvePositions.size() - 2;

  for (int i = 0; i < (int)curvePositions.size(); i++)
  {
    BezSeg bs(curvePositions, i, isClosedCurve, tightness);
    bs.isSecondLastSegment(false);  // Just an initialization
    bs.isLastSegment(false);        // Just an initialization

    if (i == lastIndex)
    {
      bs.isLastSegment(true);
    }
    else if (i == secondLastIndex)
    {
      bs.isSecondLastSegment(true);
    }

    bezierSegments.push_back(bs);
    totalCurveLength += bs.getSegmentLength();
    cumulatedCurveLength.push_back(totalCurveLength);
  }

  //	this.curveDecorator = new BezierCurve(this);
}

void BezierModel::addCurvePosition(DirectPosition curvePosition, boolean isClosedCurve)
{
  curvePositions.push_back(curvePosition);

  init(curvePositions, isClosedCurve);
}

double BezierModel::getCumulatedCurveLength(int i)
{
  std::list<double>::iterator iter = cumulatedCurveLength.begin();
  advance(iter, i);

  return *iter;
}

const std::vector<DirectPosition>& BezierModel::getCurvePositions() { return curvePositions; }

const std::list<BezSeg>& BezierModel::getBezierSegments() { return bezierSegments; }

int BezierModel::getBezierSegmentCount() { return bezierSegments.size(); }

DirectPosition BezierModel::getStartPointOfLastBezierSegment()
{
  if (curvePositions.size() <= 2) return curvePositions[0];

  return bezierSegments.back().getStartPoint();
}

DirectPosition BezierModel::getEndPointOfLastBezierSegment()
{
  return bezierSegments.back().getEndPoint();
}

DirectPosition BezierModel::getStartPointOfFirstBezierSegment()
{
  return bezierSegments.front().getStartPoint();
}

DirectPosition BezierModel::getEndPointOfFirstBezierSegment()
{
  return bezierSegments.front().getEndPoint();
}

DirectPosition BezierModel::getFirstControlPointOfLastBezierSegment()
{
  return bezierSegments.back().getFirstControlPoint();
}

DirectPosition BezierModel::getSecondControlPointOfFirstBezierSegment()
{
  return bezierSegments.front().getSecondControlPoint();
}

DirectPosition BezierModel::getFirstControlPointOfFirstBezierSegment()
{
  return bezierSegments.front().getFirstControlPoint();
}

DirectPosition BezierModel::getSecondControlPointOfLastBezierSegment()
{
  return bezierSegments.back().getSecondControlPoint();
}

double BezierModel::getTotalLengthOfAllSegments() { return totalCurveLength; }

void BezierModel::setOrientation(Orientation theOrientation) { orientation = theOrientation; }

Orientation BezierModel::getOrientation() { return orientation; }

// public BezierCurve getBezierCurve() {
//	return this.curveDecorator;
//}

// public Shape getCurveLinePath() {
//	return this.curveDecorator.getCurveLinePath();
//}

// public Rectangle getBoundingCurveLineRect() {
//	return this.getCurveLinePath().getBounds();
//}

// public Shape getDecorationLinePath() {
//	return this.curveDecorator.getDecorationLinePath();
//}

// public void setCurveDecorator(BezierCurve curveDecorator) {
//	this.curveDecorator = curveDecorator;
//}

BezSeg BezierModel::getLastBezierSegment() { return bezierSegments.back(); }

boolean BezierModel::isEmpty() { return bezierSegments.empty(); }

// public std::vector<Integer> getEvaluatedCurvePositionSegmentIndices()
//{
//    return this.curveDecorator.getEvaluatedCurvePositionSegmentIndices();
//}

// public DirectPosition getEvaluatedCurvePosition(double cumulatedPathLength) {
//    return this.getBezierCurve().getEvaluatedCurvePosition(cumulatedPathLength);
//}

// public std::vector<DirectPosition> getEvaluatedCurvePositions(double pathLengthIncrement)
//{
//    std::vector<DirectPosition> evaluatedPositions = new Arraystd::vector<DirectPosition>();
//    for(double cumulatedPathLength = 0; cumulatedPathLength < this.getTotalLengthOfAllSegments();
//    cumulatedPathLength += pathLengthIncrement)
//    {
//        DirectPosition evaluatedPos = this.getEvaluatedCurvePosition(cumulatedPathLength);
//        evaluatedPositions.add(evaluatedPos);
//    }
//    return evaluatedPositions;
//}

boolean BezierModel::isClosedCurve() { return bIsClosedCurve; }

void BezierModel::setTightness(double theTightness)
{
  // All Bezier segments in model are assumed to be of same tightness value
  for (std::list<BezSeg>::iterator bezSeg = bezierSegments.begin();
       (bezSeg != bezierSegments.end());
       bezSeg++)
  {
    bezSeg->setTightness(theTightness);
  }
}

double BezierModel::getTightness()
{
  // All Bezier segments in model are assumed to be of same tightness value.
  // (At least one segment should be found)
  return bezierSegments.front().getTightness();
}

// ======================================================================
/*!
 * \brief getSteppedCurvePoints
 *
 * 		  Extracts bezier curve points with given step
 */
// ======================================================================

int BezierModel::getSteppedCurvePoints(
    unsigned int baseStep,   // Base distance between curve points along the line
    unsigned int maxRand,    // Max random distance added to the base
    unsigned int maxRepeat,  // Max number of subsequent points having equal distance
    std::list<DirectPosition>& curvePoints)  // Output points
{
  const unsigned int minBaseStep = 3;

  if (baseStep <= minBaseStep) baseStep = minBaseStep;

  int curveLength = (int)(floor(getTotalLengthOfAllSegments()) + 0.1);  // Total curve length
  double lss = 0.0;  // Cumulative curve length at the start of current segment
  double t = 0.0;    // Relative position within current segment

  unsigned int step;          // Next step
  unsigned int prevStep = 0;  // Previous step
  unsigned int repStep = 0;   // Counter for subsequent points having equal distance/step
  int curPos = 0;             // Current curve position

  // Segment iterator
  std::list<BezSeg>::iterator itb = bezierSegments.begin();

  // Cumulative segment length iterator
  std::list<double>::iterator itl = cumulatedCurveLength.begin(),
                              itlEnd = cumulatedCurveLength.end();

  // Number of points
  int nPoints = 0;

  srand(time(nullptr));

  curvePoints.clear();

  while (true)
  {
    if (nPoints > 0)
    {
      // Step
      step = baseStep;

      if (maxRand > 0)
      {
        step += (int)((maxRand + 1) * (rand() / (RAND_MAX + 1.0)));

        if (step == prevStep)
        {
          if (repStep >= maxRepeat)
          {
            step += ((step == (baseStep + maxRand)) ? -1 : 1);
            repStep = 0;
          }
          else
            repStep++;
        }

        prevStep = step;
      }

      curPos += step;
      if ((curPos + (0.5 * baseStep)) >= curveLength) curPos = curveLength;

      while ((itl != itlEnd) && (*itl < curPos))
      {
        lss = *itl;

        itl++;
        itb++;
      }

      if (itl == itlEnd)
        //
        // Never
        break;

      t = ((curPos < curveLength) ? ((curPos - lss) / (*itl - lss)) : 1.0);
    }

    curvePoints.push_back(DirectPosition(itb->getPosition(t)));

    nPoints++;

    if (curPos >= curveLength) break;
  }

  return nPoints;
}

// ======================================================================
/*!
 * \brief decorateCurve
 *
 * 		  Generates curve decorator points (for cloud border).
 * 		  Converted from Esa's java code.
 */
// ======================================================================

void BezierModel::decorateCurve(
    std::list<DirectPosition>& curvePoints,  // Bezier curve points
    bool negative,                           // Decorator point direction
    int scaleHeightMin,     // Base curve distance (normal length) for the decorator points
    int scaleHeightRandom,  // Max random distance added to the base
    int controlMin,         // Base offset for the decorator points
    int controlRandom,      // Max random offset added to the base
    std::list<DoubleArr>& decoratorPoints)  // Output; decorator points
{
  std::list<DirectPosition>::iterator litcp = curvePoints.begin(), cpend = curvePoints.end(), ritcp;

  ritcp = litcp;
  if (ritcp != cpend) ritcp++;

  srand(time(nullptr));

  decoratorPoints.clear();

  for (; (ritcp != cpend); litcp++, ritcp++)
  {
    DoubleArr leftPosition(litcp->getX(), litcp->getY());
    DoubleArr rightPosition(ritcp->getX(), ritcp->getY());

    DoubleArr basePosition(leftPosition[0] + ((rightPosition[0] - leftPosition[0]) / 2.0) + 0.001,
                           leftPosition[1] + ((rightPosition[1] - leftPosition[1]) / 2.0) + 0.001);

    DoubleArr normalScale = Vector2Dee::getScaledNormal(
        leftPosition,
        basePosition,
        rightPosition,
        negative ? NEG : POS,
        scaleHeightMin + ((scaleHeightRandom + 1) * (rand() / (RAND_MAX + 1.0))));

    DoubleArr control(basePosition[0] + normalScale[0] + controlMin +
                          ((controlRandom + 1) * (rand() / (RAND_MAX + 1.0))),
                      basePosition[1] + normalScale[1] + controlMin +
                          ((controlRandom + 1) * (rand() / (RAND_MAX + 1.0))));

    decoratorPoints.push_back(control);
  }
}

}  // namespace frontier


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

using namespace std;
using namespace boost;
int main()
{
	cout << format("%s:%d+%d=%d\n") % "sum" % 1 % 2 % (1 + 2);

	format fmt("(%1% + %2%) * %2% = %3%\n");
	fmt % 2 % 5;
	fmt % ((2 + 5) * 5);
	cout << fmt.str();

	system("pause");
	return 0;
}

Boost/Notes on Boost learning/5/2-format/format.cpp


/*
 * NOTE: this is the same code as Part 1, only the file circuit.txt has changed
 * -> Line 55
 */
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include <boost/algorithm/string.hpp>

#include "flat_hash_map.hpp"

using Wire = std::string;
using Signal = std::uint16_t;

using Command = Signal (*) (Signal, Signal);
struct Operation {
	Command cmd;
	std::string operand_lhs;
	std::string operand_rhs;
};

template<typename T>
T AND(const T lhs, const T rhs) { return lhs & rhs; }

template<typename T>
T OR(const T lhs, const T rhs) { return lhs | rhs; }

template<typename T>
T LSHIFT(const T lhs, const T rhs) { return lhs << rhs; }

template<typename T>
T RSHIFT(const T lhs, const T rhs) { return lhs >> rhs; }

template<typename T>
T NOT(const T lhs, const T) { return ~lhs; }

template<typename T>
T SIGNAL(const T lhs, const T) { return lhs; }

int main() {
	std::ios_base::sync_with_stdio(false);

	auto filename = std::string{"circuit.txt"};
	auto file = std::fstream{filename};

	if(file.is_open()) {
		auto signal = std::string{};
		auto commands = ska::flat_hash_map<std::string, Command>{
			{"AND", AND},
			{"OR", OR},
			{"LSHIFT", LSHIFT},
			{"RSHIFT", RSHIFT},
			{"NOT", NOT},
			{signal, SIGNAL}
		};

		auto operations = std::unordered_map<Wire, Operation>{};
		auto wires = std::unordered_map<Wire, Signal>{};

		/*
		 * map each line to a set of operations on a single wire
		 */
		const auto max_tokens = 5UL;
		auto tokens = std::vector<std::string>{max_tokens};

		auto line = std::string{};
		while(std::getline(file, line)) {
			boost::split(tokens, line, [](char c) { return (c == ' '); });

			// 4 tokens means a binary operation, 3 tokens is an unary operation,
			// and 2 tokens is a simple assignment
			auto& wire = tokens[tokens.size()-1];
			if(tokens.size() == 4) {
				operations.emplace(wire, Operation{commands[tokens[1]], tokens[0], tokens[2]});
			} else if(tokens.size() == 3) {
				operations.emplace(wire, Operation{commands[tokens[0]], tokens[1], {}});
			} else {
				operations.emplace(wire, Operation{commands[signal], tokens[0], {}});
			}
		}

		/*
		 * execute operations
		 */
		auto get_signal = [] (auto is_operand_signal, auto& operand, auto is_wire_mapped, auto& wire) {
			return (is_operand_signal ? std::stoul(operand) : (is_wire_mapped ? wire->second : 0));
		};

		for(auto operation = operations.begin(); !operations.empty();) {
			auto& strlhs = operation->second.operand_lhs;
			auto& strrhs = operation->second.operand_rhs;

			auto command = operation->second.cmd;

			auto is_lhs_signal = std::isdigit(strlhs.front());
			auto is_rhs_signal = std::isdigit(strrhs.front());

			auto wirelhs = wires.find(strlhs);
			auto wirerhs = wires.find(strrhs);

			auto is_wirelhs_mapped = (wirelhs != wires.end());
			auto is_wirerhs_mapped = (wirerhs != wires.end());

			auto is_lhs_operand_ok = (is_lhs_signal || is_wirelhs_mapped);
			auto is_rhs_operand_ok = (is_rhs_signal || strrhs.empty() || is_wirerhs_mapped);

			// operation has either signals, mapped wires or a mix of both
			if(is_lhs_operand_ok && is_rhs_operand_ok) {
				auto signal_lhs = get_signal(is_lhs_signal, strlhs, is_wirelhs_mapped, wirelhs);
				auto signal_rhs = get_signal(is_rhs_signal, strrhs, is_wirerhs_mapped, wirerhs);

				wires[operation->first] = command(signal_lhs, signal_rhs);

				operation = operations.erase(operation);
			} else {
				// no operation possible right now, so move on
				++operation;
			}

			// if there are still operations left at the end
			// of the map, restart the whole process
			if(operation == operations.end() && !operations.empty()) {
				operation = operations.begin();
			}
		}

		std::cout << wires["a"] << std::endl;
	} else {
		std::cerr << "Error! Could not open \"" << filename << "\"!" << std::endl;
	}

	return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SQRS_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SCALAR_SQRS_HPP_INCLUDED

#include <boost/simd/arithmetic/functions/sqrs.hpp>
#include <boost/simd/include/functions/scalar/sqr.hpp>
#include <boost/simd/include/functions/scalar/abss.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/sqrtvalmax.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( sqrs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< int_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return abss(a0) > boost::simd::Sqrtvalmax<result_type>() ? boost::simd::Valmax<result_type>() : sqr(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< uint_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return a0 > boost::simd::Sqrtvalmax<result_type>() ? boost::simd::Valmax<result_type>() : sqr(a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( sqrs_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      return sqr(a0);
    }
  };
} } }

#endif


#ifndef __CONTROL_HH__
#define __CONTROL_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the CONTROL Module On Board)
LIBRARY DEPENDENCY:
      ((../src/Control.cpp))
PROGRAMMERS:
      (((Chung-Fan Yang) () () () ))
*******************************************************************************/
#include <armadillo>
#include "aux.hh"
#include "matrix/utility.hh"
#include "math_utility.hh"

class Control {
    TRICK_INTERFACE(Control);

 public:
        Control();
        Control(const Control& other);

        Control& operator=(const Control& other);

        void initialize();

        void control(double int_step);
        // double control_normal_accel(double ancomx, double int_step);
        // double control_yaw_accel(double alcomx, double int_step);
        // double control_pitch_rate(double qqdx);
        // double control_gamma(double thtvdcomx);
        void set_thtvdcomx(double in);
        // void set_maut(double in);
        void set_delimx(double in);
        void set_drlimx(double in);
        void set_pgam(double in);
        void set_wgam(double in);
        void set_zgam(double in);
        void pitch_down(double pitchcmd, double int_step);
        void roll_control(double rollcmd, double int_step);
        void yaw_control(double yawcmd, double int_step);
        void AOA_control(double aoacmd, double int_step);
        void S2_B_pseudo_G(arma::vec3 cmd, double int_step);
        void S3_B_pseudo_G(arma::vec3 cmd, double int_step);

        void set_IBBB0(double in1, double in2, double in3);
        void set_IBBB1(double in1, double in2, double in3);
        void set_controller_var(double in1, double in2, double in3, double in4, double in5, double in6);
        void set_S2_PITCH_DOWN_I();
        void set_S2_PITCH_DOWN_II();
        void set_S3_PITCH_DOWN();
        void set_S2_ROLL_CONTROL();
        void set_NO_CONTROL();
        void set_S2_AOA();
        void set_S3_AOA();
        void set_kpp(double in);
        void set_kpi(double in);
        void set_kppp(double in);
        void set_krp(double in);
        void set_kri(double in);
        void set_krpp(double in);
        void set_kyp(double in);
        void set_kyi(double in);
        void set_kypp(double in);
        void set_attcmd(double in1, double in2, double in3);
        void set_kaoap(double in);
        void set_kaoai(double in);
        void set_kaoapp(double in);
        void set_kaoad(double in);
        void set_aoacmd(double in);

        void get_control_gain(double in1, double in2, double in3, double in4, double in5, double in6, double in7, double in8, double in9, double in10, double in11, double in12,
                                double in13, double in14, double in15, double in16, double in17, double in18, double in19, double in20);

        double get_theta_a_cmd();
        double get_theta_b_cmd();
        double get_theta_c_cmd();
        double get_theta_d_cmd();
        void set_ierror_zero();
        void set_reference_point(double in);
        void set_engine_d(double in);

        void pitch_down_test(double pitchcmd, double int_step);

        arma::vec3 euler_angle(arma::mat33 TBD);

        enum CONTROL_TYPE {
            NO_CONTROL = 0,
            S2_PITCH_DOWN_I,
            S2_PITCH_DOWN_II,
            S2_ROLL_CONTROL,
            S3_PITCH_DOWN,
            S2_AOA,
            S3_AOA
        };
        ///////////////////////////////////////////////////////////////////////////////
        // Definition of control module-variables
        // Member function of class 'Hyper'
        // Module-variable locations are assigned to hyper[500-599]
        //
        // maut = |mauty|mautp|
        //
        // mauty = 0 no control, fixed control surfaces
        //       = 5 yaw acceleration control for yaw-to-turn
        //
        // mautp = 0 no control, fixed control surfaces
        //       = 3 pitch acceleration control
        //
        // 030520 Created by Peter H Zipfel
        // 091214 Modified for ROCKET6, PZi
        ///////////////////////////////////////////////////////////////////////////////

        // std::function<int()>    grab_mprop;

        // std::function<double()> grab_dvbe;

        // std::function<double()> grab_gymax;
        // std::function<double()> grab_dyb;
        // std::function<double()> grab_dnb;
        // std::function<double()> grab_dnr;
        // std::function<double()> grab_dndr;
        // std::function<double()> grab_dla;
        // std::function<double()> grab_dma;
        // std::function<double()> grab_dmq;
        // std::function<double()> grab_dmde;
        // std::function<double()> grab_dnd;
        // std::function<double()> grab_dlde;

        // std::function<double()> grab_pdynmc;

        std::function<double()> grab_dvbec;
        std::function<double()> grab_thtvdcx;
        std::function<double()> grab_thtbdcx;
        std::function<double()> grab_phibdcx;
        std::function<double()> grab_psibdcx;
        std::function<double()> grab_alphacx;
        std::function<double()> grab_altc;

        std::function<double()> grab_qqcx;
        std::function<double()> grab_rrcx;

        std::function<arma::vec3()> grab_FSPCB;

        std::function<arma::vec3()> grab_computed_WBIB;
        std::function<arma::vec4()> grab_TBDQ;
        std::function<arma::mat33()> grab_TBD;
        std::function<arma::mat33()> grab_TBICI;
        std::function<arma::mat33()> grab_TBIC;

        double  get_delecx();
        double  get_delrcx();

        arma::mat33 build_321_rotation_matrix(arma::vec3 angle);
        void Quaternion_cmd(double int_step);
        void calculate_xcg_thrust(double int_step);

 private:
        void default_data();

        double  delecx;         /* *io (d)      Pitch command deflection */  // n
        double  delrcx;         /* *io (d)      Yaw command deflection */    // n

        enum CONTROL_TYPE maut;           /* *io (--)     maut=|mauty|mautp| see table */
        int     mfreeze;        /* *io (--)     =0:Unfreeze; =1:Freeze; increment for more */
        double  waclp;          /* *io (r/s)    Nat freq of accel close loop complex pole */
        double  zaclp;          /* *io (--)     Damping of accel close loop complex pole */
        double  paclp;          /* *io (--)     Close loop real pole */
        double  delimx;         /* *io (d)      Pitch command limiter */
        double  drlimx;         /* *io (d)      Yaw command limiter */
        double  yyd;            /* *io (m/s2)   Yaw feed-forward derivative variable */
        double  yy;             /* *io (m/s)    Yaw feed-forward integration variable */
        double  zzd;            /* *io (m/s2)   Pitch feed-forward derivative variable */
        double  zz;             /* *io (m/s)    Pitch feed-forward integration variable */
        double  alcomx_actual;  /* *io (--)     Later accel com limited by 'betalimx' */
        double  ancomx_actual;  /* *io (--)     Normal accel com limited by 'alplimx' */
        arma::vec GAINFP;       /* *io (--)     Feedback gains of pitch accel controller */
        double   _GAINFP[3];    /* *io (--)     Feedback gains of pitch accel controller */
        double  gainp;          /* *io (s2/m)   Proportional gain in pitch acceleration loop */
        double  gainl;          /* *io (--)     Gain in lateral acceleration loop */
        double  gkp;            /* *io (s)      Gain of roll rate feedback */
        double  gkphi;          /* *io (--)     Gain of roll angle feedback */
        double  isetc2;         /* *io (--)     Flag to print freeze variables */
        double  wacly;          /* *io (r/s)    Nat freq of accel close loop pole, yaw */
        double  zacly;          /* *io (--)     Damping of accel close loop pole, yaw */
        double  pacly;          /* *io (--)     Close loop real pole, yaw */
        double  gainy;          /* *io (--)     Gain in lateral acceleration loop */
        arma::vec GAINFY;       /* *io (--)     Feedback gains of yaw accel controller */
        double   _GAINFY[3];    /* *io (--)     Feedback gains of yaw accel controller */
        double  factwaclp;      /* *io (--)     Factor to mod 'waclp': waclp*(1+factwacl) */
        double  factwacly;      /* *io (--)     Factor to mod 'wacly': wacly*(1+factwacl) */
        double  alcomx;         /* *io (--)     Lateral (horizontal) acceleration command */
        double  ancomx;         /* *io (--)     Pitch (normal) acceleration command */
        double  qqdx;           /* *io (--)     Desired pitch rate", "control */
        double  grate;          /* *io (--)     grate */
        double gainff;          /* *io  (--)    gain */
        double thtvdcomx;        /* *io (d)      Flight path angle command */
        arma::mat GAINGAM;      /* *io  (--)    Gain */
        double _GAINGAM[3][1];  /* *io  (--)    Gain */
        double pgam;            /*  *io  (--)    Gain */
        double wgam;            /*  *io  (--)    Gain */
        double zgam;            /*  *io  (--)    Gain */

        double fmasse;
        double mdot;
        double fmass0;
        double xcg_0;
        double xcg_1;
        double isp;

        arma::vec IBBB0;
        double _IBBB0[3];

        arma::vec IBBB1;
        double _IBBB1[3];

        arma::vec IBBB2;
        double _IBBB2[3];

        arma::vec CONTROLCMD;
        double _CONTROLCMD[3];

        arma::vec CMDQ;
        double _CMDQ[4];

        arma::vec TCMDQ;
        double _TCMDQ[4];

        // pitch
        double thterror;
        double perrori;
        double perrorp;
        double perror_old;
        double pitchiout_old;
        double pN;
        double pdout_old;
        double kpp;
        double kpi;
        double kpd;
        double kppp;
        // /////////test////////////
        double thterrort;
        double perrorit;
        double perrorpt;
        double perror_oldt;
        double pitchiout_oldt;
        double pdout_oldt;

        // roll
        double rollerror;
        double rerrori;
        double rerrorp;
        double rerror_old;
        double rolliout_old;
        double rN;
        double rdout_old;
        double krp;
        double kri;
        double krd;
        double krpp;

        // yaw
        double yawerror;
        double yerrori;
        double yerrorp;
        double yerror_old;
        double yawiout_old;
        double yN;
        double ydout_old;
        double kyp;
        double kyi;
        double kyd;
        double kypp;

        // aoa
        double aoaerror;
        double aoaerrori;
        double aoaerrorp;
        double aoaerror_old;
        double aoaiout_old;
        double aoaN;
        double aoadout_old;
        double kaoap;
        double kaoai;
        double kaoad;
        double kaoapp;
        double aoaerror_smooth_old;
        double aoaerror_new;
        double aoaerror_out_old;

        double theta_a_cmd;
        double theta_b_cmd;
        double theta_c_cmd;
        double theta_d_cmd;
        double lx;

        double rollcmd;
        double pitchcmd;
        double yawcmd;
        double aoacmd;

        double pitchcmd_new;
        double pitchcmd_old;
        double pitchcmd_out_old;

        arma::vec delta_euler;
        double _delta_euler[3];

        arma::vec euler;
        double _euler[3];

        double xcg;
        double thrust;
        double mass_ratio;

        arma::vec WBICBT;
        double _WBICBT[3];

        arma::vec CMDG;
        double _CMDG[3];

        double reference_point;
        double d;
};

#endif  // __CONTROL_HH__


#ifndef INCLUDE_SWIFT_VIO_PARALLAX_ANGLE_POINT_HPP_
#define INCLUDE_SWIFT_VIO_PARALLAX_ANGLE_POINT_HPP_

#include <iostream>
#include <random>
#include <Eigen/Geometry>
#include <okvis/kinematics/Transformation.hpp> // for optimizing a PAP point.
#include <okvis/ceres/LocalParamizationAdditionalInterfaces.hpp>

namespace swift_vio {
typedef Eigen::Quaterniond QPD;
typedef Eigen::MatrixXd MXD;

/*!
 * \brief Gets a skew-symmetric matrix from a (column) vector
 * \param   vec 3x1-matrix (column vector)
 * \return skew   3x3-matrix
 */
template <typename PrimType_>
inline static Eigen::Matrix<PrimType_, 3, 3> skewVector(
    const Eigen::Matrix<PrimType_, 3, 1>& vec) {
  Eigen::Matrix<PrimType_, 3, 3> mat;
  mat << 0, -vec(2), vec(1), vec(2), 0, -vec(0), -vec(1), vec(0), 0;
  return mat;
}

template <typename DERIVED, typename GET, unsigned int D, unsigned int E = D>
class ElementBase {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  ElementBase(){};
  virtual ~ElementBase(){};
  static const unsigned int D_ = D;
  static const unsigned int E_ = E;
  typedef Eigen::Matrix<double, D_, 1> mtDifVec;
  typedef GET mtGet;
  std::string name_;
  virtual void boxPlus(const mtDifVec& vecIn, DERIVED& stateOut) const = 0;
  virtual void boxMinus(const DERIVED& stateIn, mtDifVec& vecOut) const = 0;
  virtual void boxMinusJac(const DERIVED& stateIn, MXD& matOut) const = 0;
  virtual void print() const = 0;
  virtual void setIdentity() = 0;
  virtual void setRandom(unsigned int& s) = 0;
  virtual void fix() = 0;
  static DERIVED Identity() {
    DERIVED identity;
    identity.setIdentity();
    return identity;
  }
  DERIVED& operator=(DERIVED other) {
    other.swap(*this);
    return *this;
  }
  virtual mtGet& get(unsigned int i) = 0;
  virtual const mtGet& get(unsigned int i) const = 0;
};

class NormalVectorElement
    : public ElementBase<NormalVectorElement, NormalVectorElement, 2> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  Eigen::Quaterniond q_;
  const Eigen::Vector3d e_x;
  const Eigen::Vector3d e_y;
  const Eigen::Vector3d e_z;
  NormalVectorElement() : e_x(1, 0, 0), e_y(0, 1, 0), e_z(0, 0, 1) {}
  NormalVectorElement(const NormalVectorElement& other)
      : e_x(1, 0, 0), e_y(0, 1, 0), e_z(0, 0, 1) {
    q_ = other.q_;
  }
  NormalVectorElement(const Eigen::Vector3d& vec)
      : e_x(1, 0, 0), e_y(0, 1, 0), e_z(0, 0, 1) {
    setFromVector(vec);
  }
  NormalVectorElement(const Eigen::Quaterniond& q)
      : e_x(1, 0, 0), e_y(0, 1, 0), e_z(0, 0, 1) {
    q_ = q;
  }
  NormalVectorElement(double w, double x, double y, double z)
      : q_(w, x, y, z), e_x(1, 0, 0), e_y(0, 1, 0), e_z(0, 0, 1) {}
  virtual ~NormalVectorElement(){};
  Eigen::Vector3d getVec() const { return q_ * e_z; }
  Eigen::Vector3d getPerp1() const { return q_ * e_x; }
  Eigen::Vector3d getPerp2() const { return q_ * e_y; }
  NormalVectorElement& operator=(const NormalVectorElement& other) {
    q_ = other.q_;
    return *this;
  }
  static Eigen::Vector3d getRotationFromTwoNormals(
      const Eigen::Vector3d& a, const Eigen::Vector3d& b,
      const Eigen::Vector3d& a_perp) {
    const Eigen::Vector3d cross = a.cross(b);
    const double crossNorm = cross.norm();
    const double c = a.dot(b);
    const double angle = std::acos(c);
    if (crossNorm < 1e-6) {
      if (c > 0) {
        return cross;
      } else {
        return a_perp * M_PI;
      }
    } else {
      return cross * (angle / crossNorm);
    }
  }
  static Eigen::Vector3d getRotationFromTwoNormals(
      const NormalVectorElement& a, const NormalVectorElement& b) {
    return getRotationFromTwoNormals(a.getVec(), b.getVec(), a.getPerp1());
  }
  static Eigen::Matrix3d getRotationFromTwoNormalsJac(
      const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
    const Eigen::Vector3d cross = a.cross(b);
    const double crossNorm = cross.norm();
    Eigen::Vector3d crossNormalized = cross / crossNorm;
    Eigen::Matrix3d crossNormalizedSqew = skewVector(crossNormalized);
    const double c = a.dot(b);
    const double angle = std::acos(c);
    if (crossNorm < 1e-6) {
      if (c > 0) {
        return -skewVector(b);
      } else {
        return Eigen::Matrix3d::Zero();
      }
    } else {
      return -1 / crossNorm *
             (crossNormalized * b.transpose() -
              (crossNormalizedSqew * crossNormalizedSqew * skewVector(b) *
               angle));
    }
  }
  static Eigen::Matrix3d getRotationFromTwoNormalsJac(
      const NormalVectorElement& a, const NormalVectorElement& b) {
    return getRotationFromTwoNormalsJac(a.getVec(), b.getVec());
  }
  void setFromVector(Eigen::Vector3d vec) {
    const double d = vec.norm();
    if (d > 1e-6) {
      vec = vec / d;
      Eigen::Vector3d rv = getRotationFromTwoNormals(e_z, vec, e_x);
      Eigen::AngleAxisd aa(rv.norm(), rv.normalized());
      q_ = Eigen::Quaterniond(aa);
    } else {
      q_.setIdentity();
    }
  }
  NormalVectorElement rotated(const QPD& q) const {
    return NormalVectorElement(q * q_);
  }
  NormalVectorElement inverted() const {
    Eigen::Quaterniond q(Eigen::AngleAxisd(M_PI, getPerp1()));
    return NormalVectorElement(q * q_);
  }
  void boxPlus(const mtDifVec& vecIn, NormalVectorElement& stateOut) const {
    Eigen::Vector3d Nu = vecIn(0) * getPerp1() + vecIn(1) * getPerp2();
    Eigen::Quaterniond q(Eigen::AngleAxisd(Nu.norm(), Nu.normalized()));
    stateOut.q_ = q * q_;
  }
  void boxMinus(const NormalVectorElement& stateIn, mtDifVec& vecOut) const {
    vecOut =
        stateIn.getN().transpose() * getRotationFromTwoNormals(stateIn, *this);
  }
  void boxMinusJac(const NormalVectorElement& stateIn, MXD& matOut) const {
    matOut = -stateIn.getN().transpose() *
             getRotationFromTwoNormalsJac(*this, stateIn) * this->getM();
  }
  static mtDifVec boxMinus(const Eigen::Vector3d &l, const Eigen::Vector3d &r) {
    mtDifVec delta;
    NormalVectorElement(l).boxMinus(NormalVectorElement(r), delta);
    return delta;
  }
  static Eigen::Vector3d boxPlus(const Eigen::Vector3d &l, const mtDifVec &r) {
    NormalVectorElement s;
    NormalVectorElement(l).boxPlus(r, s);
    return s.getVec();
  }
  void print() const { std::cout << getVec().transpose() << std::endl; }
  void setIdentity() { q_.setIdentity(); }
  void setRandom(unsigned int& s) {
    std::default_random_engine generator(s);
    std::normal_distribution<double> distribution(0.0, 1.0);
    q_.w() = distribution(generator);
    q_.x() = distribution(generator);
    q_.y() = distribution(generator);
    q_.z() = distribution(generator);
    q_.normalize();
    s++;
  }
  void fix() { q_.normalize(); }
  mtGet& get(unsigned int /*i*/ = 0) {
    return *this;
  }
  const mtGet& get(unsigned int /*i*/ = 0) const {
    return *this;
  }
  // return dVec/dmininalRep
  Eigen::Matrix<double, 3, 2> getM() const {
    Eigen::Matrix<double, 3, 2> M;
    M.col(0) = -getPerp2();
    M.col(1) = getPerp1();
    return M;
  }
  Eigen::Matrix<double, 3, 2> getN() const {
    Eigen::Matrix<double, 3, 2> M;
    M.col(0) = getPerp1();
    M.col(1) = getPerp2();
    return M;
  }
  const double* data() const { return q_.coeffs().data(); }
};

class NormalVectorParameterization
    : public okvis::ceres::LocalParamizationAdditionalInterfaces {
public:
  static const int kGlobalDim = 3;
  static const int kLocalDim = 2;
  // Generalization of the addition operation,
  //
  //   x_plus_delta = Plus(x, delta)
  //
  // with the condition that Plus(x, 0) = x.
  bool Plus(const double *x, const double *delta,
            double *x_plus_delta) const final {
    return plus(x, delta, x_plus_delta);
  }

  // The jacobian of Plus(x, delta) w.r.t delta at delta = 0.
  //
  // jacobian is a row-major GlobalSize() x LocalSize() matrix.
  bool ComputeJacobian(const double *x, double *jacobian) const final {
    return plusJacobian(x, jacobian);
  }

  // Size of x.
  int GlobalSize() const final { return kGlobalDim; }

  // Size of delta.
  int LocalSize() const final { return kLocalDim; }

  /// \brief Trivial destructor.
  ~NormalVectorParameterization() final {}

  /// \brief Computes the minimal difference between a variable x and a
  /// perturbed variable x_plus_delta
  /// @param[in] x Variable.
  /// @param[in] x_plus_delta Perturbed variable.
  /// @param[out] delta minimal difference.
  /// \return True on success.
  bool Minus(const double *x, const double *x_plus_delta,
             double *delta) const final {
    return minus(x, x_plus_delta, delta);
  }

  /// \brief Computes the Jacobian from minimal space to naively
  /// overparameterised space as used by ceres.
  /// @param[in] x Variable.
  /// @param[out] jacobian the Jacobian (dimension minDim x dim).
  /// \return True on success.
  bool ComputeLiftJacobian(const double *x, double *jacobian) const final {
    return liftJacobian(x, jacobian);
  }

  static bool liftJacobian(const double *x0, double *jacobian) {
    Eigen::Map<const Eigen::Vector3d> vx(x0);
    swift_vio::NormalVectorElement nve(vx);
    Eigen::Map<Eigen::Matrix<double, 2, 3, Eigen::RowMajor>> j(jacobian);
    j = nve.getM().transpose(); // This is the pinv of M.
    return true;
  }

  static bool minus(const double *x, const double *x_plus_delta,
                    double *delta) {
    Eigen::Map<const Eigen::Vector3d> vxplus(x_plus_delta);
    Eigen::Map<const Eigen::Vector3d> vx(x);
    Eigen::Map<Eigen::Vector2d> d(delta);
    d = swift_vio::NormalVectorElement::boxMinus(vxplus, vx);
    return true;
  }

  static bool plus(const double *x, const double *delta, double *x_plus_delta) {
    Eigen::Map<Eigen::Vector3d> xplus(x_plus_delta);
    xplus = swift_vio::NormalVectorElement::boxPlus(
        Eigen::Map<const Eigen::Vector3d>(x),
        Eigen::Map<const Eigen::Vector2d>(delta));
    return true;
  }

  /**
   * @brief plusJacobian
   * @param x0
   * @param jacobian  is a row-major 3 x 2 matrix.
   * @return
   */
  static bool plusJacobian(const double *x0, double *jacobian) {
    Eigen::Map<const Eigen::Vector3d> vx0(x0);
    swift_vio::NormalVectorElement nve0(vx0);
    Eigen::Map<Eigen::Matrix<double, 3, 2, Eigen::RowMajor>> j(jacobian);
    j = nve0.getM();
    return true;
  }
};

// Angle range [0, \pi]
class AngleElement : ElementBase<AngleElement, AngleElement, 1> {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  void setFromCosine(double cosTheta) {
    cs_[0] = cosTheta;
    cs_[1] = std::sqrt(1 - cosTheta * cosTheta);
  }

  void set(const double* parameters) {
    cs_[0] = parameters[0];
    cs_[1] = parameters[1];
  }

  AngleElement() {}

  AngleElement(double cosTheta) { setFromCosine(cosTheta); }
  AngleElement(double ct, double st) : cs_(ct, st) {}
  virtual void boxPlus(const mtDifVec& vecIn, AngleElement& stateOut) const {
    double cd = std::cos(vecIn[0]);
    double sd = std::sin(vecIn[0]);
    double cSum = cs_[0] * cd - cs_[1] * sd;
    double sSum = cs_[1] * cd + sd * cs_[0];
    stateOut.cs_[0] = cSum;
    stateOut.cs_[1] = sSum;
  }

  virtual void boxMinus(const AngleElement& stateIn, mtDifVec& vecOut) const {
    double cSum = cs_[0] * stateIn.data()[0] + cs_[1] * stateIn.data()[1];
    double sSum = cs_[1] * stateIn.data()[0] - cs_[0] * stateIn.data()[1];

    vecOut[0] = std::atan2(sSum, cSum);
  }

  virtual void boxMinusJac(const AngleElement& /*stateIn*/, MXD& matOut) const {
    matOut = Eigen::Matrix<double, 1, 1>(1);
  }

  virtual void print() const {
    std::cout << getAngle() << std::endl;
  }
  virtual void setIdentity() {
    cs_[0] = 1;
    cs_[1] = 0;
  }
  virtual void setRandom(unsigned int& s) {
    std::default_random_engine generator(s);
    std::normal_distribution<double> distribution(0.0, 1.0);
    cs_[0] = distribution(generator);
    cs_[1] = distribution(generator);
    cs_.normalize();
    s++;
  }
  virtual void fix() { cs_.normalize(); }

  virtual mtGet& get(unsigned int /*i*/ = 0) { return *this; }

  virtual const mtGet& get(unsigned int /*i*/ = 0) const { return *this; }
  const double* data() const { return cs_.data(); }
  double getAngle() const { return std::acos(cs_[0]); }

 private:
  Eigen::Vector2d cs_;  // cos(\theta) sin(\theta). \theta is in the range of [0, \pi).
};

class ParallaxAnglePoint {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  typedef Eigen::Matrix<double, 3, 1> mtDifVec;
  ParallaxAnglePoint() {}
  virtual ~ParallaxAnglePoint() {}
  /**
   * @brief ParallaxAnglePoint
   * @param bearing bearing vector in main anchor frame, not necessarily has
   * unit norm.
   * @param cosTheta cos(\theta)
   */
  ParallaxAnglePoint(const Eigen::Vector3d& bearing, double cosTheta)
      : theta_(cosTheta) {
    n_.setFromVector(bearing);
  }

  ParallaxAnglePoint(const NormalVectorElement& n, const AngleElement& theta)
      : n_(n), theta_(theta) {}

  ParallaxAnglePoint(double w, double x, double y, double z, double ct, double st)
      : n_(w, x, y, z), theta_(ct, st) {}

  void getParameters(std::vector<double>* parameters) const {
    parameters->resize(6);
    memcpy(parameters->data(), n_.data(), sizeof(double) * 4);
    memcpy(parameters->data() + 4, theta_.data(), sizeof(double) * 2);
  }

  void getParameters(Eigen::Matrix<double, 6, 1>* parameters) const {
    memcpy(parameters->data(), n_.data(), sizeof(double) * 4);
    memcpy(parameters->data() + 4, theta_.data(), sizeof(double) * 2);
  }

  void getParameters(double* parameters) const {
    memcpy(parameters, n_.data(), sizeof(double) * 4);
    memcpy(parameters + 4, theta_.data(), sizeof(double) * 2);
  }

  void set(const double* parameters) {
    n_ = NormalVectorElement(Eigen::Map<const Eigen::Quaterniond>(parameters));
    theta_.set(parameters + 4);
  }

  virtual void setRandom(unsigned int& s) {
    n_.setRandom(s);
    theta_.setRandom(s);
  }

  Eigen::Vector3d getVec() const { return n_.getVec(); }
  double getAngle() const { return theta_.getAngle(); }
  double cosTheta() const { return theta_.data()[0]; }
  double sinTheta() const { return theta_.data()[1]; }

  virtual void boxPlus(const mtDifVec& vecIn, ParallaxAnglePoint& stateOut) const {
    n_.boxPlus(vecIn.head<2>(), stateOut.n_);
    theta_.boxPlus(vecIn.tail<1>(), stateOut.theta_);
  }

  bool initializePosition(
      const std::vector<Eigen::Vector3d,
                        Eigen::aligned_allocator<Eigen::Vector3d>>&
          observationsxy1,
      const std::vector<
          okvis::kinematics::Transformation,
          Eigen::aligned_allocator<okvis::kinematics::Transformation>>& T_WC_list,
      const std::vector<size_t>& anchorIndices);

  bool optimizePosition(
      const std::vector<Eigen::Vector3d,
                        Eigen::aligned_allocator<Eigen::Vector3d>>&
          observationsxy1,
      const std::vector<
          okvis::kinematics::Transformation,
          Eigen::aligned_allocator<okvis::kinematics::Transformation>>& T_WC_list,
      const std::vector<size_t>& anchorIndices);

  NormalVectorElement n_;
  AngleElement theta_;
};
}  // namespace swift_vio
#endif  // INCLUDE_SWIFT_VIO_PARALLAX_ANGLE_POINT_HPP_


#include <algorithm>
#include <iostream>
#include "tile_data.h"

#include <ciso646>
#include <boost/sort/sort.hpp>

using namespace std;

typedef std::pair<OutputObjectsConstIt,OutputObjectsConstIt> OutputObjectsConstItPair;

void TileDataSource::MergeTileCoordsAtZoom(uint zoom, uint baseZoom, const TileIndex &srcTiles, TileCoordinatesSet &dstCoords) {
	if (zoom==baseZoom) {
		// at z14, we can just use tileIndex
		for (auto it = srcTiles.begin(); it!= srcTiles.end(); ++it) {
			TileCoordinates index = it->first;
			dstCoords.insert(index);
		}
	} else {
		// otherwise, we need to run through the z14 list, and assign each way
		// to a tile at our zoom level
		for (auto it = srcTiles.begin(); it!= srcTiles.end(); ++it) {
			TileCoordinates index = it->first;
			TileCoordinate tilex = index.x / pow(2, baseZoom-zoom);
			TileCoordinate tiley = index.y / pow(2, baseZoom-zoom);
			TileCoordinates newIndex(tilex, tiley);
			dstCoords.insert(newIndex);
		}
	}
}

void TileDataSource::MergeSingleTileDataAtZoom(TileCoordinates dstIndex, uint zoom, uint baseZoom, const TileIndex &srcTiles, std::vector<OutputObjectRef> &dstTile) {
	if (zoom==baseZoom) {
		// at z14, we can just use tileIndex
		auto oosetIt = srcTiles.find(dstIndex);
		if(oosetIt == srcTiles.end()) return;
		dstTile.insert(dstTile.end(), oosetIt->second.begin(), oosetIt->second.end());
	} else {
		// otherwise, we need to run through the z14 list, and assign each way
		// to a tile at our zoom level
		int scale = pow(2, baseZoom-zoom);
		TileCoordinates srcIndex1(dstIndex.x*scale, dstIndex.y*scale);
		TileCoordinates srcIndex2((dstIndex.x+1)*scale, (dstIndex.y+1)*scale);

		for(int x=srcIndex1.x; x<srcIndex2.x; x++)
		{
			for(int y=srcIndex1.y; y<srcIndex2.y; y++)
			{
				TileCoordinates srcIndex(x, y);
				auto oosetIt = srcTiles.find(srcIndex);
				if(oosetIt == srcTiles.end()) continue;
				for (auto it = oosetIt->second.begin(); it != oosetIt->second.end(); ++it) {
					OutputObjectRef oo = *it;
					if (oo->minZoom > zoom) continue;
					dstTile.insert(dstTile.end(), oo);
				}
			}
		}
	}
}

TileCoordinatesSet GetTileCoordinates(std::vector<class TileDataSource *> const &sources, unsigned int zoom) {
	TileCoordinatesSet tileCoordinates;

	// Create list of tiles
	tileCoordinates.clear();
	for(size_t i=0; i<sources.size(); i++)
		sources[i]->MergeTileCoordsAtZoom(zoom, tileCoordinates);

	return tileCoordinates;
}

std::vector<OutputObjectRef> GetTileData(std::vector<class TileDataSource *> const &sources, TileCoordinates coordinates, unsigned int zoom)
{
	std::vector<OutputObjectRef> data;
	for(size_t i=0; i<sources.size(); i++)
		sources[i]->MergeSingleTileDataAtZoom(coordinates, zoom, data);

	boost::sort::pdqsort(data.begin(), data.end());
	data.erase(unique(data.begin(), data.end()), data.end());
	return data;
}

OutputObjectsConstItPair GetObjectsAtSubLayer(std::vector<OutputObjectRef> const &data, uint_least8_t layerNum) {
    struct layerComp
    {
        bool operator() ( const OutputObjectRef &x, uint_least8_t layer ) const { return x->layer < layer; }
        bool operator() ( uint_least8_t layer, const OutputObjectRef &x ) const { return layer < x->layer; }
    };

	// compare only by `layer`
	// We get the range within ooList, where the layer of each object is `layerNum`.
	// Note that ooList is sorted by a lexicographic order, `layer` being the most significant.
	return equal_range(data.begin(), data.end(), layerNum, layerComp());
}


#ifndef SOLVERTOOLS_H
#define SOLVERTOOLS_H

#include <opengv/types.hpp>
#include <Eigen/Dense>
#include <opengv/optimization_tools/objective_function_tools/ObjectiveFunctionInfo.hpp>

class ObjectiveFunctionInfo;

class SolverTools{
 public:

  virtual Eigen::Matrix3d exp_R( Eigen::Matrix3d & X ) = 0;

  virtual opengv::rotation_t rotation_solver(opengv::rotation_t & state_rotation, const opengv::translation_t & translation,
					     double &tol, ObjectiveFunctionInfo * info_function, int & k) = 0;

  virtual opengv::translation_t translation_solver(const opengv::rotation_t & rotation,
						   opengv::translation_t & translation, double &tol, ObjectiveFunctionInfo * info_function, double &step, int & k) = 0;

};

#endif


///////////////////////////////////////////////////////////////////////////////
// rolling_moment.hpp
// Copyright 2005 Eric Niebler.
// Copyright (C) 2014 Pieter Bastiaan Ober (Integricom).
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_ROLLING_MOMENT_HPP_EAN_27_11_2005
#define BOOST_ACCUMULATORS_STATISTICS_ROLLING_MOMENT_HPP_EAN_27_11_2005

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/mpl/int.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <boost/accumulators/statistics/rolling_count.hpp>

namespace boost { namespace accumulators
{
namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // rolling_moment_impl
    template<typename N, typename Sample>
    struct rolling_moment_impl
      : accumulator_base
    {
        BOOST_MPL_ASSERT_RELATION(N::value, >, 0);
        // for boost::result_of
        typedef typename numeric::functional::fdiv<Sample, std::size_t,void,void>::result_type result_type;

        template<typename Args>
        rolling_moment_impl(Args const &args)
          : sum_(args[sample | Sample()])
        {
        }

        template<typename Args>
        void operator ()(Args const &args)
        {
            if(is_rolling_window_plus1_full(args))
            {
                this->sum_ -= numeric::pow(rolling_window_plus1(args).front(), N());
            }
            this->sum_ += numeric::pow(args[sample], N());
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            return numeric::fdiv(this->sum_, rolling_count(args));
        }

        // make this accumulator serializeable
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version)
        {
            ar & sum_;
        }

    private:
        result_type sum_;
    };
} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::rolling_moment
//
namespace tag
{
    template<int N>
    struct rolling_moment
      : depends_on< rolling_window_plus1, rolling_count>
    {
        /// INTERNAL ONLY
        ///
        typedef accumulators::impl::rolling_moment_impl<mpl::int_<N>, mpl::_1> impl;

        #ifdef BOOST_ACCUMULATORS_DOXYGEN_INVOKED
        /// tag::rolling_window::window_size named parameter
        static boost::parameter::keyword<tag::rolling_window_size> const window_size;
        #endif
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::rolling_moment
//
namespace extract
{
    BOOST_ACCUMULATORS_DEFINE_EXTRACTOR(tag, rolling_moment, (int))
}

using extract::rolling_moment;

// There is no weighted_rolling_moment (yet)...
//
//// So that rolling_moment<N> can be automatically substituted with
//// weighted_rolling_moment<N> when the weight parameter is non-void
//template<int N>
//struct as_weighted_feature<tag::rolling_moment<N> >
//{
//    typedef tag::weighted_rolling_moment<N> type;
//};
//
//template<int N>
//struct feature_of<tag::weighted_rolling_moment<N> >
//  : feature_of<tag::rolling_moment<N> >
//{
//};
}} // namespace boost::accumulators

#endif


#include <test/test-models/good/variational/std_normal.hpp>
#include <stan/variational/advi.hpp>
#include <stan/callbacks/stream_logger.hpp>
#include <gtest/gtest.h>
#include <test/unit/util.hpp>
#include <vector>
#include <string>
#include <boost/random/additive_combine.hpp>  // L'Ecuyer RNG

typedef boost::ecuyer1988 rng_t;
typedef std_normal_model_namespace::std_normal_model Model;

TEST(rvi_test, std_normal_meanfield) {
  // Create mock data_var_context
  static const std::string DATA = "";
  std::stringstream data_stream(DATA);
  stan::io::dump dummy_context(data_stream);

  // Instantiate model
  Model my_model(dummy_context);

  // RNG
  rng_t base_rng(0);

  // Other params
  stan::callbacks::stream_logger logger(std::cout, std::cout, std::cout,
                                        std::cout, std::cout);
  stan::callbacks::stream_writer stdout_writer(std::cout);

  // Dummy input
  Eigen::VectorXd cont_params = Eigen::VectorXd::Zero(1);
  cont_params(0) = -0.75;

  // ADVI
  int n_monte_carlo_grad = 10;
  int n_monte_carlo_elbo = 100;
  int eval_elbo = 10;
  int n_posterior_samples = 100;
  stan::variational::advi<Model, stan::variational::normal_meanfield, rng_t>
    test_advi(my_model, cont_params, base_rng, n_monte_carlo_grad,
	      n_monte_carlo_elbo, n_posterior_samples);

  int eval_window = 100;
  double window_size = 0.5;
  test_advi.run(1.0, true, 250, 1000,
		eval_window, window_size, 1.1, 0.2, 20, 4, logger,
		stdout_writer, stdout_writer);
}


//
// Created by Sylvain Renevey on 5/11/18.
//

#include "EqMotion.h"
#include <cmath>
#include <cstddef>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/legendre.hpp>
#include "transformations.h"
#include "SatOps/EnvironmentModel.h"
#include "SatOps/Quaternion.h"
#include "SatOps/Spacecraft.h"
#include "SatOps/StateVector.h"
extern "C" {
#include "SpiceUsr.h"
};

EqMotion::EqMotion(const Spacecraft &sc, const EnvironmentModel &env_model, double initial_et): m_env_model(env_model), m_initial_et(initial_et), m_spacecraft(sc) {};

void EqMotion::operator()(StateVector &state, StateVector &state_derivative, double t) {

    // Compute the current ephemeris time
    double et = m_initial_et + t;
    state.setTime(et);
    state.normalizeQuaternion();

    // Gravitational acceleration
    Vector3<dimension::Acceleration> a_grav(state.frame());
    Vector3d t_gg(ReferenceFrame::BODY);
    if (m_env_model.isGravityField()) {
        geopotential(a_grav, t_gg, state, et);
    } else {
        double factor = -m_env_model.centralBody().mu() / pow(state.position().norm(), 3);
        a_grav = factor * state.position();
    }

    // Third-body effect
    Vector3<dimension::Acceleration> a_third_body(state.frame());
    if (!m_env_model.thirdBodies().empty()) {
        thirdBodyEffect(a_third_body, state, et);
    }

    // Aerodynamic perturbations
    Vector3<dimension::Acceleration> a_aero(state.frame());
    Vector3d t_aero(ReferenceFrame::BODY);
    if (m_env_model.isDrag() && m_spacecraft.mass() != 0.0) {
        drag(a_aero, t_aero, state, et, t);
    }

    // Solar radiation pressure
    Vector3<dimension::Acceleration> a_srp(state.frame());
    Vector3d t_srp(ReferenceFrame::BODY);
    if (m_env_model.isSrp()) {
        solarRadiationPressure(a_srp, t_srp, state, et);
    }

    // Magnetic perturbations
    Vector3d t_mag(ReferenceFrame::BODY);
    if (m_env_model.isMagField()) {
        magneticPerturbations(t_mag, state, et);
    }

    Vector3<dimension::Acceleration> a_total = a_grav + a_third_body + a_aero + a_srp;
    Vector3d torques = t_gg + t_aero + t_srp + t_mag;

    state_derivative.setPositionDerivative(state.velocity());
    state_derivative.setVelocityDerivative(a_total);

    // Attitude kinematics
    Quaternion q = state.orientation();
    q.normalize();
    Quaternion dq(0.5 * q.xi() * state.angVelocity());
    state_derivative.setOrientation(dq);

    // Attitude dynamics
    Vector3<dimension::AngularAcceleration> ang_acceleration(m_spacecraft.inertiaMatrix().inverse() * (torques -
            state.angVelocity().cross(m_spacecraft.inertiaMatrix() * state.angVelocity())));
    state_derivative.setAngVelocityDerivative(ang_acceleration);
}

void EqMotion::geopotential(Vector3<dimension::Acceleration> &acc, Vector3d &torque, const StateVector &state, double et) const {
    // Compute acceleration in the state's reference frame.
    acc = m_env_model.computeGravAcc(state);

    // Compute torque
    // Nadir-pointing vector in the body frame
    Vector3<dimension::Distance> nadir = state.position() / state.position().norm();
    Vector3<dimension::Distance> nadir_bff = transformations::rotateToFrame(nadir, ReferenceFrame::BODY, et,
                                                                            state.orientation());
    torque = 3.0 * constants::MU_EARTH / pow(state.position().norm(), 3) * nadir_bff.cross(
            m_spacecraft.inertiaMatrix() * nadir_bff);
}

void EqMotion::drag(Vector3<dimension::Acceleration> &acc, Vector3d &torque, const StateVector &state, double et, double elapsed_time) const {

    // Retrieves atmospheric density
    double density = m_env_model.getAtmDensity(state, elapsed_time);

    // Computes the velocity relative to Earth's atmosphere
    Vector3<dimension::AngularVelocity> earth_ang_vel(ReferenceFrame::J2000, {0., 0., constants::EARTH_ANGULAR_VELOCITY});
    Vector3<dimension::Velocity> earth_velocity(earth_ang_vel.cross(state.position()));
    Vector3<dimension::Velocity> v_rel = state.velocity() - earth_velocity;
    Vector3<dimension::Velocity> v_rel_bff = transformations::rotateToFrame(v_rel, ReferenceFrame::BODY, et,
                                                                            state.orientation());

    // Force and torque
    Vector3<dimension::Acceleration> acc_bff(ReferenceFrame::BODY);
    for (std::size_t i = 0; i < m_spacecraft.faceNormals().size(); ++i) {
        double cos_theta = m_spacecraft.faceNormals()[i].dot(v_rel_bff) / (v_rel.norm() * v_rel.norm());
        Vector3d force_bff(-0.5 * density * m_spacecraft.dragCoeff() * v_rel.norm() * v_rel_bff *
                                   m_spacecraft.faceAreas()[i] * std::max(cos_theta, 0.));
        acc_bff += force_bff/m_spacecraft.mass();
        torque += m_spacecraft.faceCopPositions()[i].cross(force_bff);
    }
    acc = transformations::rotateToFrame(acc_bff, state.frame(), et, state.orientation());
}

void EqMotion::solarRadiationPressure(Vector3<dimension::Acceleration>& acc, Vector3d& torque, const StateVector& state, double et) const {
    Vector3<dimension::Distance> r_sun2sc = m_env_model.sunSpacecraftVector(state);
    double nu = m_env_model.isInShadow(state);
    double p_sr = (constants::SOLAR_PRESSURE * 1E-3) * constants::AU_TO_KM / r_sun2sc.norm(); // kg / (km s^2)

    Vector3<dimension::Distance> r_sc2sun_bff = transformations::rotateToFrame((-r_sun2sc) / r_sun2sc.norm(),
                                                                               ReferenceFrame::BODY, et,
                                                                               state.orientation());
    Vector3<dimension::Acceleration> acc_bff(ReferenceFrame::BODY);
    for (std::size_t i = 0; i < m_spacecraft.faceNormals().size(); ++i) {
        double cos_theta = m_spacecraft.faceNormals()[i].dot(r_sc2sun_bff);
        Vector3d force_bff(-nu * p_sr * m_spacecraft.faceAreas()[i] * (2.0 * (m_spacecraft.diffuseReflectionCoeff()[i] / 3.0 + m_spacecraft.specularReflectionCoeff()[i] * cos_theta) * m_spacecraft.faceNormals()[i] + (1.0 - m_spacecraft.specularReflectionCoeff()[i]) * r_sc2sun_bff) * std::max(cos_theta, 0.));
        acc_bff += force_bff/m_spacecraft.mass();
        torque += m_spacecraft.faceCopPositions()[i].cross(force_bff);
    }
    acc = transformations::rotateToFrame(acc_bff, state.frame(), et);
}

void EqMotion::magneticPerturbations(Vector3d& torque, const StateVector& state, double et) const {

    Vector3d B = m_env_model.computeMagneticField(state);
    Vector3d B_bff = transformations::rotateToFrame(B, ReferenceFrame::BODY, et, state.orientation());

    torque = m_spacecraft.residualDipole().cross(B_bff);
}

void EqMotion::thirdBodyEffect(Vector3<dimension::Acceleration>& acc, const StateVector& state, double et) const {
    auto bodies = m_env_model.thirdBodies();

    for (auto body: bodies) {
        Vector3<dimension::Distance> r_cb2tb = m_env_model.getBodyPosition(body, et, state.frame()); // central body to third body
        Vector3<dimension::Distance> r_cb2sc = state.position(); // central body to spacecraft
        Vector3<dimension::Distance> r_sc2tb = r_cb2tb - r_cb2sc; // spacecraft to third body

        double numerator = r_cb2sc.norm() * r_cb2sc.norm() + r_cb2tb.norm() * r_cb2tb.norm() -
                           ((r_cb2tb - r_cb2sc).norm()) * ((r_cb2tb - r_cb2sc).norm());
        double denominator = 2 * r_cb2sc.norm() * r_cb2tb.norm();
        double cos_zeta = numerator / denominator;
        double h = r_cb2sc.norm() / r_cb2tb.norm();
        double B = 0.0;
        double err = 1.0;
        int i = 1;

        while (std::abs(err) > 1E-8 && i < 100) {
            double B_new = B + boost::math::legendre_p(i, cos_zeta) * pow(h, i);
            err = B_new - B;
            B = B_new;
            ++i;
        }

        double beta = 3.0 * B + 3.0 * B * B + B * B * B;
        acc += -body.mu()/pow(r_cb2tb.norm(), 3) * (r_cb2sc - beta * (r_cb2tb - r_cb2sc));
    }
}


// General ROS functionality
#include <ros/ros.h>

// PI
#include "pi/PIController.h"
#include "pi/global.h"
#include <boost/property_tree/xml_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/foreach.hpp>
// END PI

// Quadrotor services
#include <hal_quadrotor/State.h>

// Quadrotor controllers
#include <hal_quadrotor/control/Takeoff.h>
#include <hal_quadrotor/control/VelocityHeight.h>

#include <algorithm>

#define EPS_POS			5e-1	// epsilon position
#define EPS_VEL			1e-3	// epsilon position

typedef enum { IDLE, TAKEOFF, VELHEIGHT, HOVER, WAYPOINT, OTHER } FSM;

using namespace std;

vector<ros::ServiceClient> srvTakeOff;
vector<ros::ServiceClient> srvVelocityHeight;

vector<FSM> state_FSM; // Finite State Machine state (TODO: get the controller from the msg)
vector<double> jstate;

// PI
PIController pi;
// END PI

vector<double> heights;
vector<string> idunits;
int units;

// Callback for quadrotor state
void StateCallback(int unit, const hal_quadrotor::State::ConstPtr& msg)
{
	if (msg->controller == (std::string) "Hover")
	{
		state_FSM[unit]  = HOVER;
		jstate[unit*4]   = msg->x;
		jstate[unit*4+1] = msg->y;
		jstate[unit*4+2] = msg->u;
		jstate[unit*4+3] = msg->v;
	}
}

// This is called at a fixed rate of 50Hz
void PILoop(const ros::TimerEvent& event)
{
	// Check that units are ready to receive VelocityHeight commands
	bool ready = true;
	for (int i=0; i<units; i++) {
		ready = ready && (state_FSM[i] == HOVER);
	}
	if (ready) {

		// Path Integral controller (based on VelocityHeight)
		vec X = jstate;
		vec action = pi.computeControl(X);
		vec Xp = pi.predictState(X, action);

		// Distribute commands
		for (int i=0; i<units; i++) {

			// convert pi control to VelocityHeight control
			hal_quadrotor::VelocityHeight msgVelocityHeight;
			msgVelocityHeight.request.dx = Xp[i*4 + 2];
			msgVelocityHeight.request.dy = Xp[i*4 + 3];
			msgVelocityHeight.request.z = heights[i];
			msgVelocityHeight.request.yaw = 0.;

			if(!srvVelocityHeight[i].call(msgVelocityHeight)){
				ROS_FATAL("NO VELHEIGHT!!");
			}
		}
	}

	if (event.profile.last_duration.toSec() > 0.1)
		ROS_WARN("PI loop duration should be < 0.1s, found to be %f",event.profile.last_duration.toSec());
}

// Main entry point of application
int main(int argc, char **argv)
{
	if (argc!=2) {
	  ROS_FATAL("requires xml_file as argument");
	  return 1;
	}

	try {
		ifstream is(argv[1]);
		// read parameters from xml file
		using boost::property_tree::ptree;
		ptree pt;
		if (!is.is_open()) {
		  ROS_FATAL("xml config not found");
		  return 1;
		}
		read_xml(is, pt);

		// get vector of desired heights
		BOOST_FOREACH(ptree::value_type &v,	pt.get_child("takeoffz"))
			heights.push_back(v.second.get<double>(""));

		// get vector of ids
		BOOST_FOREACH(ptree::value_type &v,	pt.get_child("idunits"))
			idunits.push_back(v.second.get<string>(""));

		// initialize Joint State variable
		units = pt.get<int>("units");
		jstate.resize( units*pt.get<int>("dimUAVx") );

		// Initialize PI controller
		pi.Init(pt);

		double T = pt.get<double>("T");

	} catch (int e) {
		cout << "Exception" << e << endl;
	}

	// Initialise the ROS client
	string nodestr = string("main_muav");
	ros::init(argc, argv, nodestr.c_str());

	// Create a node handle, which this C code will use to bind to the ROS server
	ros::NodeHandle n;

	// Try and get a list of topics currently being braodcast
	ros::master::V_TopicInfo topics;
	int units_found = 0;
	vector<string> names;
	vector<ros::Subscriber> top_state;

	// Iterate over the topics
	if (ros::master::getTopics(topics)) {
		for (ros::master::V_TopicInfo::iterator i = topics.begin(); i != topics.end(); i++) {

			// Check if type corresponds to a hal_quadrotor/State
			//if (!strcmp("hal_quadrotor/State", i->datatype.c_str())) {
			if (i->name.find("Estimate")!=string::npos) {

				// Unit found
				ROS_INFO("%s -> %s", i->name.c_str(), i->datatype.c_str());
				names.push_back(i->name);
				units_found++;

				// Extract id unit and sanity check of id
				int pos1 = i->name.find("/hal/")+5;
				int pos2 = i->name.find("Estimate")-1;
				string id = i->name.substr(pos1,pos2-pos1);
				if (find(idunits.begin(), idunits.end(), id.c_str()) == idunits.end()) {
					ROS_FATAL("id unit %s not specified in xml", id.c_str());
				  return 1;
				}
			}
		}
	}

	// check if number of units found corresponds to the expected units (defined in the xml)
	if ( units_found != units ) {
	  ROS_FATAL("Units found %d does not match units defined in the config %d: ", units_found, units);
	  return 1;
	}

	// sort the names (currently required, since the joint state is indexed by unit)
	sort(names.begin(), names.end());

	// subscribe and add controllers for each unit
	for (int i=0; i<units; i++) {

		// init Finite State Machine
		state_FSM.push_back(IDLE);

		// Subscribe to the states of the quadrotors (requires explicit)
		top_state.push_back( n.subscribe<hal_quadrotor::State>(
				names[i].c_str(),
				1000,
				boost::bind(StateCallback, i, _1)
				)
		);
		ROS_INFO("%s subscribed!", names[i].c_str());

		// Add TakeOff controller
		string takeoff_cmd = string("hal/") + idunits[i] + string("/controller/Takeoff");
		srvTakeOff.push_back(n.serviceClient<hal_quadrotor::Takeoff>(takeoff_cmd.c_str()) );

		// Add VelocityHeight controller
		string velheight_cmd = string("hal/") + idunits[i] + string("/controller/VelocityHeight");
		srvVelocityHeight.push_back( n.serviceClient<hal_quadrotor::VelocityHeight>(velheight_cmd.c_str()) );
	}

	// Call Takeoff on each unit
	for (int i=0; i<units; i++) {
		hal_quadrotor::Takeoff msgTakeoff;
		msgTakeoff.request.altitude = heights[i];
		if(!srvTakeOff[i].call(msgTakeoff)){
			ROS_FATAL("NO TAKE OFF!!");
		} else {
			ROS_INFO("Taking OFF!!");
		}
		state_FSM[i] = TAKEOFF;
	}

    // Immediately start control loop
    ros::Timer timer = n.createTimer(
        ros::Duration(0.1),
        PILoop
    );

    // Wait here
    ros::spin();

	// Success!
	return 0;
}