#include <iostream>

#include <armadillo>

#include <glm/irls.hpp>
#include <glm/models/binomial.hpp>
#include <cpp-argparse/OptionParser.h>

#include <besiq/method/scaleinv_method.hpp>
#include <besiq/method/boxcox_method.hpp>
#include <besiq/method/method.hpp>

#include "common_options.hpp"

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-scaleinv [OPTIONS] pairs genotype_plink_prefix";
const std::string DESCRIPTION = "Scale invariant tests (multiple link function tests).";

int
main(int argc, char *argv[])
{
    OptionParser parser = create_common_options( USAGE, DESCRIPTION, true );

    char const* const model_choices[] = { "binomial", "normal" };
    char const* const factor_choices[] = { "factor", "additive", "tukey" };

    OptionGroup group = OptionGroup( parser, "Options for glm", "These options will change the behavior of the glm model." );
    group.add_option( "-m", "--model" ).choices( &model_choices[ 0 ], &model_choices[ 2 ] ).metavar( "model" ).help( "The model to use for the phenotype, 'binomial' or 'normal', default = 'binomial'." ).set_default( "binomial" );
    group.add_option( "-f", "--factor" ).choices( &factor_choices[ 0 ], &factor_choices[ 3 ] ).help( "Determines how to code the SNPs, in 'factor' no order of the alleles is assumed, in 'additive' the SNPs are coded as the number of minor alleles, in 'tukey' the coding is the same as factor except that a single parameter for the interaction is used." ).set_default( "factor" );
    group.add_option( "-b", "--box-cox" ).action( "store_true" ).help(  "Runs a box-cox:ish methodology using a family of link functions." );
    group.add_option( "--bc-start" ).set_default( -2.0 ).help(  "Start lambda for box-cox (default=-2.0)." );
    group.add_option( "--bc-end" ).set_default( 3.0 ).help(  "End lambda for box-cox (default=3.0)." );
    group.add_option( "--bc-step" ).set_default( 0.5 ).help(  "Step lambda for box-cox (default=0.5)." );
    group.add_option( "--power-odds" ).action( "store_true" ).help(  "Use the power-odds family for continuous data." );
    parser.add_option_group( group );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 2 )
    {
        parser.print_help( );
        exit( 1 );
    }
    shared_ptr<common_options> parsed_data = parse_common_options( options, parser.args( ) );
    parsed_data->data->phenotype.elem( arma::find_nonfinite( parsed_data->data->phenotype ) ).zeros( );

    model_matrix *model_matrix = make_model_matrix( options[ "factor" ], parsed_data->data->covariate_matrix, parsed_data->data->phenotype.n_elem );

    method_type *m = NULL;

    if( !options.is_set( "box_cox" ) )
    {
        if( options[ "model" ] == "binomial" )
        {
            m = new scaleinv_method( parsed_data->data, *model_matrix, false );
        }
        else if( options[ "model" ] == "normal" )
        {
            m = new scaleinv_method( parsed_data->data, *model_matrix, true );
        }
    }
    else
    {
        float lambda_start = (float) options.get( "bc_start" );
        float lambda_end = (float) options.get( "bc_end" );
        float lambda_step = (float) options.get( "bc_step" );

        if( options[ "model" ] == "binomial" )
        {
            m = new boxcox_method( parsed_data->data, *model_matrix, false, lambda_start, lambda_end, lambda_step );
        }
        else if( options[ "model" ] == "normal" )
        {
            m = new boxcox_method( parsed_data->data, *model_matrix, true, lambda_start, lambda_end, lambda_step, options.is_set( "power_odds" ) );
        }
    }

    run_method( *m, parsed_data->genotypes, *parsed_data->pairs, *parsed_data->result_file );

    delete m;
    delete model_matrix;

    return 0;
}


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/xray/structure_factors_simple.h>
#include <boost/python/class.hpp>

namespace cctbx { namespace xray { namespace structure_factors {
namespace boost_python {

namespace {

  struct simple_wrappers
  {
    typedef simple<> w_t;
    typedef w_t::scatterer_type scatterer_type;

    static void
    wrap()
    {
      using namespace boost::python;
      class_<w_t>("structure_factors_simple", no_init)
        .def(init<uctbx::unit_cell const&,
                  sgtbx::space_group const&,
                  af::const_ref<miller::index<> > const&,
                  af::const_ref<scatterer_type> const&,
                  scattering_type_registry const&>())
        .def("f_calc", &w_t::f_calc)
      ;
    }
  };

} // namespace <anoymous>

}} // namespace structure_factors::boost_python

namespace boost_python {

  void wrap_structure_factors_simple()
  {
    structure_factors::boost_python::simple_wrappers::wrap();
  }

}}} // namespace cctbx::xray::boost_python


// Copyright 2021 Apex.AI, Inc.
//
// 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.

// Co-developed by Tier IV, Inc. and Apex.AI, Inc.

#ifndef TRACKING__PROJECTION_HPP_
#define TRACKING__PROJECTION_HPP_

#include <autoware_auto_msgs/msg/shape.hpp>
#include <common/types.hpp>
#include <geometry_msgs/msg/polygon.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry/convex_hull.hpp>
#include <geometry/common_2d.hpp>
#include <Eigen/Geometry>
#include <geometry/interval.hpp>
#include <experimental/optional>
#include <tracking/visibility_control.hpp>
#include <list>
#include <algorithm>
#include <vector>

namespace autoware
{
namespace perception
{
namespace tracking
{
struct TRACKING_PUBLIC Projection
{
  using Point = geometry_msgs::msg::Point32;
  std::list<Point> shape{};
};

/// \brief Camera intrinsic parameters
struct TRACKING_PUBLIC CameraIntrinsics
{
  std::size_t width{0U};
  std::size_t height{0U};
  float32_t fx{0.0F};
  float32_t fy{0.0F};
  float32_t ox{0.0F};
  float32_t oy{0.0F};
  float32_t skew{0.0F};
};

/// \brief This model represents a camera in 3D space and can project 3D shapes into an image
// frame.
class TRACKING_PUBLIC CameraModel
{
public:
  using float32_t = common::types::float32_t;
  using Interval = common::geometry::Interval<float32_t>;
  using Point = geometry_msgs::msg::Point32;
  using EigPoint = Eigen::Vector3f;

  /// \brief Cnstructor
  /// \param intrinsics Camera intrinsics
  explicit CameraModel(
    const CameraIntrinsics & intrinsics
  );

  /// \brief Bring 3D points in ego frame to the camera frame and project onto the image plane.
  /// All 3D points in a shape (vertices on the bottom face and the top face) are projected onto
  // the image of the camera model. The resulting list of points are then outlined using the
  // `convex_hull` algorithm and then the outline of points are returned.
  /// `K * p_3d = p_2d * depth` where K is the projection matrix.
  /// \param points 3D set of points in the camera frame.
  /// \return List of points on the camera frame that outline the given 3D object.
  std::experimental::optional<Projection>
  project(const std::vector<Point> & points) const;

private:
  /// \brief Project a 3D point and return the value if the projection is valid (in fron of the
  // camera)
  std::experimental::optional<EigPoint> project_point(const EigPoint & pt_3d) const;

  Eigen::Matrix3f m_intrinsics;
  Interval m_height_interval;
  Interval m_width_interval;
  std::list<Point> m_corners;
};

}  // namespace tracking
}  // namespace perception
}  // namespace autoware

#endif  // TRACKING__PROJECTION_HPP_


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

  @copyright 2012-2015 NumScale SAS
  @copyright 2015 J.T.Lapreste

  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_SCALAR_DIVIDES_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_DIVIDES_INCLUDED

#include <boost/simd/function/definition/divides.hpp>
#include <boost/simd/arch/common/scalar/function/divides.hpp>
#include <boost/simd/arch/common/generic/function/divides.hpp>
//#include <boost/simd/arch/common/function/simd/divides.hpp>

#endif


#ifndef BOOST_ASTRONOMY_COORDINATE_BASE_ECLIPTIC_FRAME_HPP
#define BOOST_ASTRONOMY_COORDINATE_BASE_ECLIPTIC_FRAME_HPP

#include <type_traits>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/astronomy/coordinate/base_frame.hpp>
#include <boost/astronomy/coordinate/spherical_representation.hpp>
#include <boost/astronomy/coordinate/spherical_coslat_differential.hpp>

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

template
<
    typename Representation, typename Differential
>
struct base_ecliptic_frame : public base_frame<Representation, Differential>
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG((std::is_base_of
            <spherical_representation<typename Representation::type,
            typename Representation::quantity1, typename Representation::quantity2,
            typename Representation::quantity3>, Representation>::value),
            "argument type is expected to be a spherical_representation class");
    BOOST_STATIC_ASSERT_MSG((std::is_base_of
            <spherical_coslat_differential<typename Differential::type,
            typename Differential::quantity1, typename Differential::quantity2,
            typename Differential::quantity3>, Differential>::value),
            "argument type is expected to be a spherical_coslat_differential class");
    ///@endcond

protected:
    boost::posix_time::ptime equinox;

public:
    //default constructor no initialization
    base_ecliptic_frame() {}

    //!constructs object from another representation object
    template <typename OtherRepresentation>
    base_ecliptic_frame(OtherRepresentation const& representation_data)
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_representation, OtherRepresentation>
            ::value), "argument type is expected to be a representation class");

        auto temp = make_spherical_representation
            <
                typename Representation::type,
                typename Representation::quantity1,
                typename Representation::quantity2,
                typename Representation::quantity3,
                typename OtherRepresentation::type,
                typename OtherRepresentation::quantity1,
                typename OtherRepresentation::quantity2,
                typename OtherRepresentation::quantity3
            >(representation_data);
        this->data = temp;
    }

    //!constructs object from provided components of representation
    base_ecliptic_frame
    (
        typename Representation::quantity1 const& lat,
        typename Representation::quantity2 const& lon,
        typename Representation::quantity3 const& distance
    )
    {
        this->data.set_lat_lon_dist(lat, lon, distance);
    }

    //!constructs object from provided components of representation and differential
    base_ecliptic_frame
    (
        typename Representation::quantity1 const& lat,
        typename Representation::quantity2 const& lon,
        typename Representation::quantity3 const& distance,
        typename Differential::quantity1 const& pm_lat,
        typename Differential::quantity2 const& pm_lon_coslat,
        typename Differential::quantity3 const& radial_velocity
    ) : base_ecliptic_frame(lat, lon, distance)
    {
        this->motion.set_dlat_dlon_coslat_ddist(pm_lat, pm_lon_coslat, radial_velocity);
    }

    //!constructs object from other representation and differential
    template <typename OtherRepresentation, typename OtherDifferential>
    base_ecliptic_frame
    (
        OtherRepresentation const& representation_data,
        OtherDifferential const& differential_data
    )
    {
        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_representation, OtherRepresentation>
            ::value), "argument type is expected to be a representation class");

        BOOST_STATIC_ASSERT_MSG((boost::astronomy::detail::is_base_template_of
            <boost::astronomy::coordinate::base_differential, OtherDifferential>
            ::value), "argument type is expected to be a differential class");

        auto rep_temp = make_spherical_representation
            <
                typename Representation::type,
                typename Representation::quantity1,
                typename Representation::quantity2,
                typename Representation::quantity3,
                typename OtherRepresentation::type,
                typename OtherRepresentation::quantity1,
                typename OtherRepresentation::quantity2,
                typename OtherRepresentation::quantity3
            >(representation_data);
        this->data = rep_temp;

        auto dif_temp = make_spherical_coslat_differential
            <
                typename Differential::type,
                typename Differential::quantity1,
                typename Differential::quantity2,
                typename Differential::quantity3,
                typename OtherDifferential::type,
                typename OtherDifferential::quantity1,
                typename OtherDifferential::quantity2,
                typename OtherDifferential::quantity3
            >(differential_data);
        this->motion = dif_temp;
    }

    //!returns latitude component of the coordinate
    typename Representation::quantity1 get_lat() const
    {
        return this->data.get_lat();
    }

    //!returns longitude component of the coordinate
    typename Representation::quantity2 get_lon() const
    {
        return this->data.get_lon();
    }

    //!returns distance component of the coordinate
    typename Representation::quantity3 get_distance() const
    {
        return this->data.get_dist();
    }

    //!returns the (lat, lon, dist) in the form of tuple
    std::tuple
    <
        typename Representation::quantity1,
        typename Representation::quantity2,
        typename Representation::quantity3
    > get_lat_lon_dist() const
    {
        return this->data.get_lat_lon_dist();
    }

    //!returns proper motion in latitude
    typename Differential::quantity1 get_pm_lat() const
    {
        return this->motion.get_dlat();
    }

    //!returns proper motion in longitude including cos(lat)
    typename Differential::quantity2 get_pm_lon_coslat() const
    {
        return this->motion.get_dlon_coslat();
    }

    //!returns radial_velocity
    typename Differential::quantity3 get_radial_velocity() const
    {
        return this->motion.get_ddist();
    }

    //!returns the proper motion in form of tuple
    std::tuple
    <
        typename Differential::quantity1,
        typename Differential::quantity2,
        typename Differential::quantity3
    > get_pm_lat_lon_radial() const
    {
        return this->motion.get_dlat_dlon_coslat_ddist();
    }

    //!sets value of latitude component of the coordinate
    void set_lat(typename Representation::quantity1 const& lat)
    {
        this->data.set_lat(lat);
    }

    //!sets value of longitude component of the coordinate
    void set_lon(typename Representation::quantity2 const& lon)
    {
        this->data.set_lon(lon);
    }

    //!sets value of distance component of the coordinate
    void set_distance(typename Representation::quantity3 const& distance)
    {
        this->data.set_dist(distance);
    }

    //!sets values of all component of the coordinate
    void set_lat_lon_dist
    (
        typename Representation::quantity1 const& lat,
        typename Representation::quantity2 const& lon,
        typename Representation::quantity3 const& dist
    )
    {
        this->data.set_lat_lon_dist(lat, lon, dist);
    }

    //!sets the proper motion in latitude
    void set_pm_lat(typename Differential::quantity1 const& pm_lat)
    {
        this->motion.set_dlat(pm_lat);
    }

    //!sets the proper motion in longitude including cos(lat)
    void set_pm_lon_coslat(typename Differential::quantity2 const& pm_lon_coslat)
    {
        this->motion.set_dlon_coslat(pm_lon_coslat);
    }

    //!sets the radial_velocity
    void set_radial_velocity(typename Differential::quantity3 const& radial_velocity)
    {
        this->motion.set_ddist(radial_velocity);
    }

    //!set value of motion  including cos(b)
    void set_pm_lat_lon_radial
    (
        typename Differential::quantity1 const& pm_lat,
        typename Differential::quantity2 const& pm_lon_coslat,
        typename Differential::quantity3 const& radial_velocity
    )
    {
        this->motion.set_dlat_dlon_coslat_ddist(pm_lat, pm_lon_coslat, radial_velocity);
    }

    //!returns equinox time
    boost::posix_time::ptime get_equinox() const
    {
        return this->equinox;
    }

    //!sets equinox time
    void set_equinox(boost::posix_time::ptime const& time)
    {
        this->equinox = time;
    }
}; //base_ecliptic_frame

}}} //namespace boost::astronomy::coordinate

#endif // !BOOST_ASTRONOMY_COORDINATE_BASE_ECLIPTIC_FRAME_HPP


//头文件
#include <ros/ros.h>

#include <iostream>

//话题头文件

#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/State.h>

#include <Eigen/Eigen>
#include <math.h>
#include <math_utils.h>

using namespace std;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "test");
    ros::NodeHandle nh("~");

    ros::Rate rate(20.0);
    Eigen::Vector3d vector;

    vector       = Eigen::Vector3d(0.0,0.0,0.0);

    //float a;

    Eigen::Vector3d thr_sp;
    float yaw_sp;

//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主  循  环<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {

        cout << "Please input the thrust_x: "<<endl;
        cin >> thr_sp(0);
        cout << "Please input the thrust_y: "<<endl;
        cin >> thr_sp(1);
        cout << "Please input the thrust_z: "<<endl;
        cin >> thr_sp(2);
        cout << "Please input the yaw_sp: "<<endl;
        cin >> yaw_sp;

        //Eigen::Quaterniond q_sp = ThrottleToAttitude(thr_sp, yaw_sp);

        //周期休眠
        rate.sleep();
    }

    return 0;

}


/*
 Copyright (C) 2016 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

#include <boost/make_shared.hpp>
#include <ql/cashflows/fixedratecoupon.hpp>
#include <ql/math/comparison.hpp>
#include <qle/pricingengines/crossccyswapengine.hpp>
#include <qle/termstructures/crossccyfixfloatswaphelper.hpp>

using QuantExt::CrossCcySwapEngine;
using QuantLib::YieldTermStructure;
using QuantLib::FixedRateCoupon;
using QuantLib::close;

namespace QuantExt {

namespace {
void no_deletion(YieldTermStructure*) {}
} // namespace

CrossCcyFixFloatSwapHelper::CrossCcyFixFloatSwapHelper(
    const Handle<Quote>& rate, const Handle<Quote>& spotFx, Natural settlementDays, const Calendar& paymentCalendar,
    BusinessDayConvention paymentConvention, const Period& tenor, const Currency& fixedCurrency,
    Frequency fixedFrequency, BusinessDayConvention fixedConvention, const DayCounter& fixedDayCount,
    const boost::shared_ptr<IborIndex>& index, const Handle<YieldTermStructure>& floatDiscount,
    const Handle<Quote>& spread, bool endOfMonth)
    : RelativeDateRateHelper(rate), spotFx_(spotFx), settlementDays_(settlementDays), paymentCalendar_(paymentCalendar),
      paymentConvention_(paymentConvention), tenor_(tenor), fixedCurrency_(fixedCurrency),
      fixedFrequency_(fixedFrequency), fixedConvention_(fixedConvention), fixedDayCount_(fixedDayCount), index_(index),
      floatDiscount_(floatDiscount), spread_(spread), endOfMonth_(endOfMonth) {

    QL_REQUIRE(!spotFx_.empty(), "Spot FX quote cannot be empty.");
    QL_REQUIRE(fixedCurrency_ != index_->currency(), "Fixed currency should not equal float leg currency.");

    registerWith(spotFx_);
    registerWith(index_);
    registerWith(floatDiscount_);
    registerWith(spread_);

    initializeDates();
}

void CrossCcyFixFloatSwapHelper::update() {
    // Maybe FX spot quote or spread quote changed
    if (!close(spotFx_->value(), swap_->fixedNominal()) ||
        (!spread_.empty() && !close(spread_->value(), swap_->floatSpread()))) {
        initializeDates();
    }

    // Maybe evaluation date changed. RelativeDateRateHelper will take care of this
    // Note: if initializeDates() was called in above if statement, it will not be called
    // again in RelativeDateRateHelper::update() because evaluationDate_ is set in
    // initializeDates(). So, redundant instrument builds are avoided.
    RelativeDateRateHelper::update();
}

Real CrossCcyFixFloatSwapHelper::impliedQuote() const {
    QL_REQUIRE(termStructure_, "Term structure needs to be set");
    swap_->recalculate();
    return swap_->fairFixedRate();
}

void CrossCcyFixFloatSwapHelper::setTermStructure(YieldTermStructure* yts) {
    boost::shared_ptr<YieldTermStructure> temp(yts, no_deletion);
    termStructureHandle_.linkTo(temp, false);
    RelativeDateRateHelper::setTermStructure(yts);
}

void CrossCcyFixFloatSwapHelper::accept(AcyclicVisitor& v) {
    if (Visitor<CrossCcyFixFloatSwapHelper>* v1 = dynamic_cast<Visitor<CrossCcyFixFloatSwapHelper>*>(&v))
        v1->visit(*this);
    else
        RateHelper::accept(v);
}

void CrossCcyFixFloatSwapHelper::initializeDates() {

    // Swap start and end
    Date referenceDate = evaluationDate_ = Settings::instance().evaluationDate();
    referenceDate = paymentCalendar_.adjust(referenceDate);
    Date start = paymentCalendar_.advance(referenceDate, settlementDays_ * Days);
    Date end = start + tenor_;

    // Nominals
    Real floatNominal = 1.0;
    Real fixedNominal = spotFx_->value();

    // Fixed schedule
    Schedule fixedSchedule(start, end, Period(fixedFrequency_), paymentCalendar_, fixedConvention_, fixedConvention_,
                           DateGeneration::Backward, endOfMonth_);

    // Float schedule
    Schedule floatSchedule(start, end, index_->tenor(), paymentCalendar_, paymentConvention_, paymentConvention_,
                           DateGeneration::Backward, endOfMonth_);

    // Create the swap
    Natural paymentLag = 0;
    Spread floatSpread = spread_.empty() ? 0.0 : spread_->value();
    swap_.reset(new CrossCcyFixFloatSwap(CrossCcyFixFloatSwap::Payer, fixedNominal, fixedCurrency_, fixedSchedule, 0.0,
                                         fixedDayCount_, paymentConvention_, paymentLag, paymentCalendar_, floatNominal,
                                         index_->currency(), floatSchedule, index_, floatSpread, paymentConvention_,
                                         paymentLag, paymentCalendar_));

    earliestDate_ = swap_->startDate();
    maturityDate_ = swap_->maturityDate();

    // Swap is Payer => first leg is fixed leg
    latestRelevantDate_ = earliestDate_;
    for (Size i = 0; i < swap_->leg(0).size(); ++i) {
        latestRelevantDate_ = std::max(latestRelevantDate_, swap_->leg(0)[i]->date());
    }
    pillarDate_ = latestDate_ = latestRelevantDate_;

    // Attach engine
    boost::shared_ptr<PricingEngine> engine = boost::make_shared<CrossCcySwapEngine>(
        fixedCurrency_, termStructureHandle_, index_->currency(), floatDiscount_, spotFx_);
    swap_->setPricingEngine(engine);
}

} // namespace QuantExt


// Copyright (c) 2015-2020 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "read_tail_bias.hpp"

#include <algorithm>
#include <iterator>
#include <random>
#include <functional>
#include <cmath>
#include <cassert>

#include <boost/variant.hpp>

#include "io/variant/vcf_record.hpp"
#include "basics/aligned_read.hpp"
#include "core/types/allele.hpp"
#include "utils/maths.hpp"
#include "../facets/samples.hpp"
#include "../facets/alleles.hpp"
#include "../facets/read_assignments.hpp"

namespace octopus { namespace csr {

const std::string ReadTailBias::name_ = "RTB";

std::unique_ptr<Measure> ReadTailBias::do_clone() const
{
    return std::make_unique<ReadTailBias>(*this);
}

Measure::ValueType ReadTailBias::get_value_type() const
{
    return double {};
}

void ReadTailBias::do_set_parameters(std::vector<std::string> params)
{
    if (params.size() != 1) {
        throw BadMeasureParameters {this->name(), "only has one parameter (tail fraction)"};
    }
    try {
        tail_fraction_ = boost::lexical_cast<decltype(tail_fraction_)>(params.front());
    } catch (const boost::bad_lexical_cast&) {
        throw BadMeasureParameters {this->name(), "given parameter \"" + params.front() + "\" cannot be parsed"};
    }
    if (tail_fraction_ < 0 || tail_fraction_ > 1) {
        throw BadMeasureParameters {this->name(), "tail fraction must be between 0 and 1"};
    }
}

namespace {

struct TailDefinition
{
    double tail_fraction = 0.03;
};

struct PositionCounts
{
    unsigned head, tail;
};

bool overlaps_rhs(const Allele& allele, const AlignedRead& read)
{
    using D = GenomicRegion::Distance;
    return overlaps(allele, expand_lhs(mapped_region(read), -static_cast<D>(region_size(read) / 2)));
}

auto tail_region(const AlignedRead& read, const unsigned tail_bases)
{
    if (is_forward_strand(read)) {
        return octopus::tail_region(read, tail_bases);
    } else {
        return head_region(read, tail_bases);
    }
}

bool is_in_tail(const Allele& allele, const AlignedRead& read, const TailDefinition tail_def = TailDefinition {})
{
    assert(contains(read, allele));
    const auto tail_bases = std::max(static_cast<unsigned>(sequence_size(read) * tail_def.tail_fraction), 1u);
    return contains(tail_region(read, tail_bases), allele);
}

auto compute_head_tail_counts(const Allele& allele, const ReadRefSupportSet& support, const TailDefinition tail_def = TailDefinition {})
{
    PositionCounts result {};
    for (const auto& read : support) {
        if (contains(read, allele)) {
            if (is_in_tail(allele, read)) {
                ++result.tail;
            } else {
                ++result.head;
            }
        }
    }
    return result;
}

double calculate_tail_bias(const PositionCounts counts, const double tolerance = 0.5, const double prior = 1.0)
{
    assert(tolerance > 0.0 && tolerance < 1.0);
    return maths::beta_cdf(counts.head + prior, counts.tail + prior, tolerance);
}

double calculate_tail_bias(const Allele& allele, const ReadRefSupportSet& support, const TailDefinition tail_def = TailDefinition {})
{
    const auto counts = compute_head_tail_counts(allele, support);
    return calculate_tail_bias(counts, std::min(3 * tail_def.tail_fraction, 0.5));
}

double calculate_max_tail_bias(const std::vector<Allele>& alleles, const AlleleSupportMap& support, const TailDefinition tail_def = TailDefinition {})
{
    double result {0};
    for (const auto& allele : alleles) {
        const auto support_set_itr = support.find(allele);
        if (support_set_itr != std::cend(support)) {
            auto bias = calculate_tail_bias(allele, support_set_itr->second, tail_def);
            result = std::max(result, bias);
        }
    }
    return result;
}

} // namespace

Measure::ResultType ReadTailBias::do_evaluate(const VcfRecord& call, const FacetMap& facets) const
{
    const auto& samples = get_value<Samples>(facets.at("Samples"));
    const auto& alleles = get_value<Alleles>(facets.at("Alleles"));
    const auto& assignments = get_value<ReadAssignments>(facets.at("ReadAssignments")).alleles;
    Array<ValueType> result {};
    result.reserve(samples.size());
    const TailDefinition tail_def {tail_fraction_};
    for (const auto& sample : samples) {
        result.emplace_back(calculate_max_tail_bias(get_called(alleles, call, sample), assignments.at(sample), tail_def));
    }
    return result;
}

Measure::ResultCardinality ReadTailBias::do_cardinality() const noexcept
{
    return ResultCardinality::samples;
}

const std::string& ReadTailBias::do_name() const
{
    return name_;
}

std::string ReadTailBias::do_describe() const
{
    return "Probability allele occurs in the tail of supporting reads";
}

std::vector<std::string> ReadTailBias::do_requirements() const
{
    return {"Samples", "Alleles", "ReadAssignments"};
}

bool ReadTailBias::is_equal(const Measure& other) const noexcept
{
    return tail_fraction_ == static_cast<const ReadTailBias&>(other).tail_fraction_;
}

} // namespace csr
} // namespace octopus


// Copyright 2020 Oscar Higgott

// 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.

#include <iostream>
#include "lemon_mwpm.h"
#include <lemon/list_graph.h>
#include <lemon/matching.h>
#include <lemon/connectivity.h>
#include <vector>
#include <string>
#include "matching_graph.h"
#include <stdexcept>
#include <set>
#include <pybind11/pybind11.h>
#include <pybind11/numpy.h>
#include <boost/graph/adjacency_list.hpp>

typedef lemon::ListGraph UGraph;
typedef UGraph::EdgeMap<double> LengthMap;
typedef lemon::MaxWeightedPerfectMatching<UGraph,LengthMap> MWPM;

const char * BlossomFailureException::what() const throw() {
    return "The Lemon implementation of the blossom algorithm "
            "(lemon::MaxWeightedPerfectMatching) "
            "was unable to find a solution to the minimum-weight "
            "perfect matching problem.";
}

MatchingResult::MatchingResult() {}

MatchingResult::MatchingResult(py::array_t<std::uint8_t> correction, double weight)
    : correction(correction), weight(weight) {}

std::string arr_repr(py::array_t<std::uint8_t> arr) {
    std::stringstream ss;
    ss << "array([";
    bool first = true;
    for (auto i : arr) {
        if (first){
            first = false;
        } else {
            ss << ", ";
        }
        ss << i;
    }
    ss << "], dtype=uint8)";
    return ss.str();
}

std::string MatchingResult::repr() const {
    std::stringstream ss;
    ss << "pymatching._cpp_mwpm.MatchingResult(correction=";
    ss << arr_repr(correction);
    ss << ", weight=" << weight << ")";
    return ss.str();
}

class DefectGraph {
    public:
        DefectGraph(int num_nodes);
        void AddEdge(int i, int j, double weight);
        UGraph g;
        LengthMap length;
        int num_nodes;
};

DefectGraph::DefectGraph(int num_nodes) : num_nodes(num_nodes),
         length(g)
{
    for (int i=0; i<num_nodes; i++){
        UGraph::Node x;
        x = g.addNode();
    }
}

void DefectGraph::AddEdge(int i, int j, double weight){
    UGraph::Edge e = g.addEdge(g.nodeFromId(i), g.nodeFromId(j));
    length[e] = weight;
}

MatchingResult ExactMatching(
    MatchingGraph& graph,
    const py::array_t<int>& defects,
    bool return_weight
    ){
    MatchingResult matching_result;
    if (!graph.HasComputedAllPairsShortestPaths()){
        graph.ComputeAllPairsShortestPaths();
    }
    int num_nodes = graph.GetNumNodes();

    auto d = defects.unchecked<1>();
    std::set<int> defects_set;
    for (int i=0; i<d.shape(0); i++){
        if (d(i) >= num_nodes){
            throw std::invalid_argument(
            "Defect id must be less than the number of nodes in the matching graph"
            );
        }
        defects_set.insert(d(i));
    }
    graph.FlipBoundaryNodesIfNeeded(defects_set);

    std::vector<int> defects_vec(defects_set.begin(), defects_set.end());

    int num_defects = defects_vec.size();

    DefectGraph defect_graph(num_defects);

    for (py::size_t i = 0; i<num_defects; i++){
        for (py::size_t j=i+1; j<num_defects; j++){
            defect_graph.AddEdge(i, j, -1.0*graph.Distance(
                defects_vec[i], defects_vec[j]
                ));
        }
    };

    MWPM pm(defect_graph.g, defect_graph.length);
    bool success = pm.run();
    if (!success){
        throw BlossomFailureException();
    }

    int N = graph.GetNumQubits();
    auto correction = new std::vector<int>(N, 0);
    std::set<int> qids;
    for (py::size_t i = 0; i<num_defects; i++){
        int j = defect_graph.g.id(pm.mate(defect_graph.g.nodeFromId(i)));
        if (i<j){
            std::vector<int> path = graph.ShortestPath(
                defects_vec[i], defects_vec[j]
                );
            for (std::vector<int>::size_type k=0; k<path.size()-1; k++){
                qids = graph.QubitIDs(path[k], path[k+1]);
                for (auto qid : qids){
                    if ((qid != -1) && (qid >= 0) && (qid < N)){
                        (*correction)[qid] = ((*correction)[qid] + 1) % 2;
                    }
                }
            }
        }
    }

    auto capsule = py::capsule(correction, [](void *correction) { delete reinterpret_cast<std::vector<int>*>(correction); });
    auto corr = py::array_t<int>(correction->size(), correction->data(), capsule);

    if (return_weight) {
        matching_result.weight = -1*pm.matchingWeight();
    } else {
        matching_result.weight = -1.0;
    }

    matching_result.correction = corr;
    return matching_result;
}

MatchingResult LocalMatching(
    MatchingGraph& graph,
    const py::array_t<int>& defects,
    int num_neighbours,
    bool return_weight,
    int max_attempts
    ){
    if (num_neighbours <= 0){
        throw std::invalid_argument("num_neighbours must be greater than zero");
    }
    auto d = defects.unchecked<1>();
    std::set<int> defects_set;
    for (int i=0; i<d.shape(0); i++) {
        defects_set.insert(d(i));
    }
    int num_attempts = 0;
    while (true) {
        try{
            return LemonDecodeMatchNeighbourhood(
                graph,
                defects_set,
                num_neighbours,
                return_weight
            );
        } catch (BlossomFailureException& e) {
            num_attempts++;
            if (num_neighbours >= defects_set.size() || num_attempts >= max_attempts){
                throw;
            } else {
                num_neighbours *= 2;
            }
        }
    }
}

MatchingResult LemonDecodeMatchNeighbourhood(
    MatchingGraph& graph,
    std::set<int>& defects_set,
    int num_neighbours,
    bool return_weight
    ){
    MatchingResult matching_result;

    int num_nodes = graph.GetNumNodes();

    for (auto d : defects_set){
        if (d >= num_nodes){
            throw std::invalid_argument(
            "Defect id must be less than the number of nodes in the matching graph"
            );
        }
    }

    graph.FlipBoundaryNodesIfNeeded(defects_set);

    std::vector<int> defects_vec(defects_set.begin(), defects_set.end());
    int num_defects = defects_vec.size();
    std::vector<int> defect_id(num_nodes, -1);
    for (int i=0; i<num_defects; i++){
        defect_id[defects_vec[i]] = i;
    }
    num_neighbours = std::min(num_neighbours, num_defects-1);

    DefectGraph defect_graph(num_defects);

    std::vector<std::pair<int, double>> neighbours;
    int j;
    bool is_in;
    for (int i=0; i<num_defects; i++){
        neighbours = graph.GetNearestNeighbours(defects_vec[i], num_neighbours, defect_id);
        for (const auto &neighbour : neighbours){
            j = defect_id[neighbour.first];
            UGraph::Edge FoundEdge = lemon::findEdge(
                defect_graph.g,
                defect_graph.g.nodeFromId(i),
                defect_graph.g.nodeFromId(j));
            is_in = FoundEdge != lemon::INVALID;
            if (!is_in && i!=j){
                defect_graph.AddEdge(i, j, -1.0*neighbour.second);
            }
        }
    }

    MWPM pm(defect_graph.g, defect_graph.length);
    bool success = pm.run();
    if (!success){
        throw BlossomFailureException();
    }

    int N = graph.GetNumQubits();
    auto correction = new std::vector<int>(N, 0);

    std::set<int> remaining_defects;
    for (int i=0; i<num_defects; i++){
        remaining_defects.insert(i);
    }

    std::vector<int> path;
    int i;
    std::set<int> qids;
    while (remaining_defects.size() > 0){
        i = *remaining_defects.begin();
        remaining_defects.erase(remaining_defects.begin());
        j = defect_graph.g.id(pm.mate(defect_graph.g.nodeFromId(i)));
        remaining_defects.erase(j);
        path = graph.GetPath(defects_vec[i], defects_vec[j]);
        for (std::vector<int>::size_type k=0; k<path.size()-1; k++){
            qids = graph.QubitIDs(path[k], path[k+1]);
            for (auto qid : qids){
                if ((qid != -1) && (qid >= 0) && (qid < N)){
                    (*correction)[qid] = ((*correction)[qid] + 1) % 2;
                }
            }
        }
    }
    auto capsule = py::capsule(correction, [](void *correction) { delete reinterpret_cast<std::vector<int>*>(correction); });
    auto corr = py::array_t<int>(correction->size(), correction->data(), capsule);

    if (return_weight) {
        matching_result.weight = -1*pm.matchingWeight();
    } else {
        matching_result.weight = -1.0;
    }

    matching_result.correction = corr;
    return matching_result;
}


#ifndef N_BODY_TREE_HPP
#define N_BODY_TREE_HPP

#include "data.hpp"
#include "logging.hpp"
#include "overloaded.hpp"
#include "space.hpp"
#include <array>
#include <boost/mpi.hpp>
#include <boost/optional.hpp>
#include <boost/serialization/access.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/variant.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/variant.hpp>
#include <cmath>
#include <cstddef>
#include <iostream>
#include <memory>
#include <tuple>
#include <vector>

namespace n_body::data::tree {

namespace detail {

constexpr std::size_t children_number(std::size_t dimension) noexcept {
  // if exception occurs, process will be terminated
  return static_cast<std::size_t>(std::pow(2, dimension));
}

} // namespace detail

enum class NodeType {
  Inner = 0,
  Leaf = 1,
};

std::ostream &operator<<(std::ostream &os, NodeType node_type) {
  switch (node_type) {
  case NodeType::Inner:
    return os << "Inner";
  case NodeType::Leaf:
    return os << "Leaf";
  default:
    return os << "Invalid node type";
  }
}

template <typename T, std::size_t Dimension> struct BodyTreeInnerNode {
  inline static constexpr std::size_t CHILDREN_NUMBER =
      detail::children_number(Dimension);

  using space_type = Space<T, Dimension>;

  std::array<boost::optional<std::size_t>, CHILDREN_NUMBER> children;

  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(children);
  }
};

template <typename T, std::size_t Dimension> struct BodyTreeLeafNode {
  using space_type = Space<T, Dimension>;

  std::size_t body;

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(body);
  }
};

template <typename T, std::size_t Dimension> struct BodyTreeNode {
  using space_type = Space<T, Dimension>;
  using inner_node_type = BodyTreeInnerNode<T, Dimension>;
  using leaf_node_type = BodyTreeLeafNode<T, Dimension>;

  Space<T, Dimension> space;
  Scalar<T> mass;
  Vector<T, Dimension> center_of_mass;
  boost::variant<inner_node_type, leaf_node_type> variant_part;

  NodeType node_type() const {
    return static_cast<NodeType>(variant_part.which());
  }

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int /* version */) {
    ar &BOOST_SERIALIZATION_NVP(space);
    ar &BOOST_SERIALIZATION_NVP(mass);
    ar &BOOST_SERIALIZATION_NVP(center_of_mass);
    ar &BOOST_SERIALIZATION_NVP(variant_part);
  }
};

template <typename T, std::size_t Dimension> struct BodyTree {
  using node_type = BodyTreeNode<T, Dimension>;
  using inner_node_type = typename node_type::inner_node_type;
  using leaf_node_type = typename node_type::leaf_node_type;

  using bodies_type = Bodies<T, Dimension>;
  using space_type = Space<T, Dimension>;

  std::vector<node_type> tree;

  void push(const bodies_type &bodies, const space_type &root_space,
            std::size_t body) {

    if (this->tree.empty()) {
      this->push_new_leaf_node(bodies, body, root_space);
    } else {
      // push to the root subtree
      this->push(0, bodies, body);
    }
  }

  void push(std::size_t subtree, const bodies_type &bodies, std::size_t body) {
    switch (this->node(subtree).node_type()) {
    case NodeType::Inner: {
      logging::logger(logging::Level::Trace)
          << "push body " << body << " to "
          << "inner node " << subtree << std::endl;
      data::average_position_by_mass_in_place(
          this->node(subtree).center_of_mass, this->node(subtree).mass,
          bodies[body].position, bodies[body].mass);

      auto body_part = space::part_of_space(this->node(subtree).space,
                                            bodies[body].position);
      if (auto next = this->child_of_node(subtree, body_part)) {
        // just push into the node
        push(*next, bodies, body);
      } else {
        // just use the place
        // root_inner is invalidated by push
        this->child_of_node(subtree, body_part) = this->push_new_leaf_node(
            bodies, body,
            space::subspace(this->node(subtree).space, body_part));
      }
      break;
    }
    case NodeType::Leaf: {
      logging::logger(logging::Level::Trace)
          << "push body " << body << " to "
          << "leaf node " << subtree << std::endl;
      if (this->node(subtree).center_of_mass == bodies[body].position) {
        throw std::runtime_error("two points are at exactly same point");
      }

      // change the leaf to an inner node
      this->expand_leaf_to_inner(subtree);
      // try insert again
      this->push(subtree, bodies, body);
      break;
    }
    }
  }

  void expand_leaf_to_inner(std::size_t leaf) {

    auto part = space::part_of_space(this->node(leaf).space,
                                     this->node(leaf).center_of_mass);
    auto new_leaf_node = tree.size();

    this->tree.push_back(node_type{
        space::subspace(this->node(leaf).space, part),
        this->node(leaf).mass,
        this->node(leaf).center_of_mass,
        this->node(leaf).variant_part,
    }); // node is invalidated
    inner_node_type inner_node;
    inner_node.children[part] = new_leaf_node;
    this->variant_part_of_node(leaf) =
        inner_node; // finally change the leaf into inner

    logging::logger(logging::Level::Trace)
        << "expand leaf " << leaf << " to " << new_leaf_node << std::endl;
  }

  void merge_tree(const BodyTree<T, Dimension> &other,
                  const Bodies<T, Dimension> &bodies) {
    if (this->tree.empty()) {
      this->tree = other.tree;
    } else if (other.tree.empty()) {
      return;
    } else {
      this->merge_tree(0, other, 0, bodies);
    }
  }

  void merge_tree(std::size_t root, const BodyTree<T, Dimension> &other,
                  std::size_t other_root, const Bodies<T, Dimension> &bodies) {
    if (other.tree[other_root].node_type() == NodeType::Leaf) {
      // if merging a leaf node
      // just push the body into the place and return
      this->push(root, bodies, other.body_of_node(other_root));
    } else {
      // if merging a inner node
      if (this->tree[root].node_type() == NodeType::Inner) {
        // if current node type is inner
        data::average_position_by_mass_in_place(
            this->tree[root].center_of_mass, this->tree[root].mass,
            other.tree[other_root].center_of_mass, other.tree[other_root].mass);

        for (std::size_t i = 0; i < inner_node_type::CHILDREN_NUMBER; ++i) {
          if (auto other_child = other.child_of_node(other_root, i)) {
            // if and only if other node's child exists
            if (auto this_child = this->child_of_node(root, i)) {
              this->merge_tree(*this_child, other, *other_child, bodies);
            } else {
              this->child_of_node(root, i) =
                  this->copy_tree(other, *other_child);
            }
          }
        }
      } else {
        // if current node type is leaf
        auto body = this->body_of_node(root);
        copy_tree_in_place(root, other, other_root);
        this->push(root, bodies, body);
      }
    }
  }

  // copy subtree to the back of the tree vector
  std::size_t copy_tree(const BodyTree<T, Dimension> &other,
                        std::size_t other_root) {
    auto place = this->tree.size();
    this->tree.push_back(other.node(other_root)); // do copy
    if (other.node(other_root).node_type() == NodeType::Inner) {
      for (std::size_t i = 0; i < inner_node_type::CHILDREN_NUMBER; ++i) {
        if (auto child = other.child_of_node(other_root, i)) {
          this->child_of_node(place, i) = this->copy_tree(other, *child);
        }
      }
    }
    return place;
  }

  // copy subtree into the place of tree vector
  void copy_tree_in_place(std::size_t place,
                          const BodyTree<T, Dimension> &other,
                          std::size_t other_root) {
    this->node(place) = other.node(other_root); // do copy
    if (other.node(other_root).node_type() == NodeType::Inner) {
      for (std::size_t i = 0; i < inner_node_type::CHILDREN_NUMBER; ++i) {
        if (auto child = other.child_of_node(other_root, i)) {
          this->child_of_node(place, i) = this->copy_tree(other, *child);
        }
      }
    }
  }

  // access methods
  boost::variant<inner_node_type, leaf_node_type> &
  variant_part_of_node(std::size_t node) {
    return this->tree[node].variant_part;
  }

  const boost::variant<inner_node_type, leaf_node_type> &
  variant_part_of_node(std::size_t node) const {
    return this->tree[node].variant_part;
  }

  boost::optional<std::size_t> &child_of_node(std::size_t node,
                                              std::size_t part) {
    return boost::get<inner_node_type>(this->tree[node].variant_part)
        .children[part];
  }

  const boost::optional<std::size_t> &child_of_node(std::size_t node,
                                                    std::size_t part) const {
    return boost::get<inner_node_type>(this->tree[node].variant_part)
        .children[part];
  }

  std::size_t &body_of_node(std::size_t node) {
    return boost::get<leaf_node_type>(this->tree[node].variant_part).body;
  }

  std::size_t body_of_node(std::size_t node) const {
    return boost::get<leaf_node_type>(this->tree[node].variant_part).body;
  }

  node_type &node(std::size_t node) { return this->tree[node]; }

  const node_type &node(std::size_t node) const { return this->tree[node]; }

private:
  std::size_t push_new_leaf_node(const bodies_type &bodies, std::size_t body,
                                 const Space<T, Dimension> &space) {
    auto new_node = tree.size();
    this->tree.push_back(node_type{
        space,                 // space
        bodies[body].mass,     // mass
        bodies[body].position, // center of mass
        leaf_node_type{
            body, // body
        },
    });
    logging::logger(logging::Level::Trace)
        << "create new leaf node " << new_node << " for body " << body
        << std::endl;
    return new_node;
  }

private:
  /* serialization */
  friend class boost::serialization::access;
  template <typename Archive>
  void serialize(Archive &ar, const unsigned int version) {
    ar &BOOST_SERIALIZATION_NVP(tree);
  }
};

// the root space fo t1 and t2 must be same
template <typename T, std::size_t Dimension>
BodyTree<T, Dimension> merge_tree(const BodyTree<T, Dimension> &t1,
                                  const BodyTree<T, Dimension> &t2,
                                  const Bodies<T, Dimension> &bodies) {
  BodyTree<T, Dimension> result;
  if (t1.tree.empty()) {
    result.tree = t2.tree;
  } else if (t2.tree.empty()) {
    result.tree = t1.tree;
  } else {
    result.tree = t1.tree;
    result.merge_tree(0, t2, 0, bodies);
  }
  return result;
}

// the root space fo t1 and t2 must be same
template <typename T, std::size_t Dimension>
BodyTree<T, Dimension> build_tree(const boost::mpi::communicator &comm,
                                  const Space<T, Dimension> &root_space,
                                  const Bodies<T, Dimension> &bodies) {
  communication::Division division(comm, bodies.size());

  BodyTree<T, Dimension> tree;
  for (auto i = division.begin; i < division.end; ++i) {
    tree.push(bodies, root_space, i);
  }

  // merge local trees
  logging::logger(logging::Level::Trace)
      << "start merging local trees" << std::endl;
  boost::mpi::all_reduce(comm, boost::mpi::inplace(tree),
                         [&bodies](const auto &t1, const auto &t2) {
                           logging::logger(logging::Level::Trace)
                               << "merge tree " << &t1 << " and " << &t2
                               << std::endl;
                           return merge_tree(t1, t2, bodies);
                         });
  return tree;
}

} // namespace n_body::data::tree

#endif


#include <boost/geometry/policies/robustness/segment_ratio.hpp>


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2013-2015 Adam Wulkiewicz, Lodz, Poland.

// This file was modified by Oracle on 2015, 2016.
// Modifications copyright (c) 2016, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, 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)

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BOX_IN_BOX_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BOX_IN_BOX_HPP

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategies/covered_by.hpp>
#include <boost/geometry/strategies/within.hpp>
#include <boost/geometry/util/normalize_spheroidal_coordinates.hpp>

namespace boost { namespace geometry { namespace strategy
{

namespace within
{

struct box_within_coord
{
    template <typename BoxContainedValue, typename BoxContainingValue>
    static inline bool apply(BoxContainedValue const& bed_min,
                             BoxContainedValue const& bed_max,
                             BoxContainingValue const& bing_min,
                             BoxContainingValue const& bing_max)
    {
        return bing_min <= bed_min && bed_max <= bing_max // contained in containing
            && bed_min < bed_max;                         // interiors overlap
    }
};

struct box_covered_by_coord
{
    template <typename BoxContainedValue, typename BoxContainingValue>
    static inline bool apply(BoxContainedValue const& bed_min,
                             BoxContainedValue const& bed_max,
                             BoxContainingValue const& bing_min,
                             BoxContainingValue const& bing_max)
    {
        return bed_min >= bing_min && bed_max <= bing_max;
    }
};

template <typename Geometry, std::size_t Dimension, typename CSTag>
struct box_within_range
    : box_within_coord
{};

template <typename Geometry, std::size_t Dimension, typename CSTag>
struct box_covered_by_range
    : box_covered_by_coord
{};

struct box_within_longitude_diff
{
    template <typename CalcT>
    static inline bool apply(CalcT const& diff_ed)
    {
        return diff_ed > CalcT(0);
    }
};

struct box_covered_by_longitude_diff
{
    template <typename CalcT>
    static inline bool apply(CalcT const&)
    {
        return true;
    }
};

template <typename Geometry,
          typename CoordCheck,
          typename InteriorCheck>
struct box_longitude_range
{
    template <typename BoxContainedValue, typename BoxContainingValue>
    static inline bool apply(BoxContainedValue const& bed_min,
                             BoxContainedValue const& bed_max,
                             BoxContainingValue const& bing_min,
                             BoxContainingValue const& bing_max)
    {
        typedef typename select_most_precise
            <
                BoxContainedValue,
                BoxContainingValue
            >::type calc_t;
        typedef typename coordinate_system<Geometry>::type::units units_t;
        typedef math::detail::constants_on_spheroid<calc_t, units_t> constants;

        if (CoordCheck::apply(bed_min, bed_max, bing_min, bing_max))
        {
            return true;
        }

        // min <= max <=> diff >= 0
        calc_t const diff_ed = bed_max - bed_min;
        calc_t const diff_ing = bing_max - bing_min;

        // if containing covers the whole globe it contains all
        if (diff_ing >= constants::period())
        {
            return true;
        }

        // if containing is smaller it cannot contain
        // and check interior (within vs covered_by)
        if (diff_ing < diff_ed || ! InteriorCheck::apply(diff_ed))
        {
            return false;
        }

        // calculate positive longitude translation with bing_min as origin
        calc_t const diff_min = math::longitude_distance_unsigned<units_t>(bing_min, bed_min);

        // max of contained translated into the containing origin must be lesser than max of containing
        return bing_min + diff_min + diff_ed <= bing_max
            /*|| bing_max - diff_min - diff_ed >= bing_min*/;
    }
};

// spherical_equatorial_tag, spherical_polar_tag and geographic_cat are casted to spherical_tag
template <typename Geometry>
struct box_within_range<Geometry, 0, spherical_tag>
    : box_longitude_range<Geometry, box_within_coord, box_within_longitude_diff>
{};

template <typename Geometry>
struct box_covered_by_range<Geometry, 0, spherical_tag>
    : box_longitude_range<Geometry, box_covered_by_coord, box_covered_by_longitude_diff>
{};

template
<
    template <typename, std::size_t, typename> class SubStrategy,
    typename Box1,
    typename Box2,
    std::size_t Dimension,
    std::size_t DimensionCount
>
struct relate_box_box_loop
{
    static inline bool apply(Box1 const& b_contained, Box2 const& b_containing)
    {
        assert_dimension_equal<Box1, Box2>();
        typedef typename tag_cast<typename cs_tag<Box1>::type, spherical_tag>::type cs_tag_t;

        if (! SubStrategy<Box1, Dimension, cs_tag_t>::apply(
                    get<min_corner, Dimension>(b_contained),
                    get<max_corner, Dimension>(b_contained),
                    get<min_corner, Dimension>(b_containing),
                    get<max_corner, Dimension>(b_containing)
                )
            )
        {
            return false;
        }

        return relate_box_box_loop
            <
                SubStrategy,
                Box1, Box2,
                Dimension + 1, DimensionCount
            >::apply(b_contained, b_containing);
    }
};

template
<
    template <typename, std::size_t, typename> class SubStrategy,
    typename Box1,
    typename Box2,
    std::size_t DimensionCount
>
struct relate_box_box_loop<SubStrategy, Box1, Box2, DimensionCount, DimensionCount>
{
    static inline bool apply(Box1 const& , Box2 const& )
    {
        return true;
    }
};

template
<
    typename Box1,
    typename Box2,
    template <typename, std::size_t, typename> class SubStrategy = box_within_range
>
struct box_in_box
{
    static inline bool apply(Box1 const& box1, Box2 const& box2)
    {
        return relate_box_box_loop
            <
                SubStrategy,
                Box1, Box2, 0, dimension<Box1>::type::value
            >::apply(box1, box2);
    }
};

} // namespace within

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace within { namespace services
{

template <typename BoxContained, typename BoxContaining>
struct default_strategy
    <
        box_tag, box_tag,
        box_tag, areal_tag,
        cartesian_tag, cartesian_tag,
        BoxContained, BoxContaining
    >
{
    typedef within::box_in_box<BoxContained, BoxContaining> type;
};

// spherical_equatorial_tag, spherical_polar_tag and geographic_cat are casted to spherical_tag
template <typename BoxContained, typename BoxContaining>
struct default_strategy
    <
        box_tag, box_tag,
        box_tag, areal_tag,
        spherical_tag, spherical_tag,
        BoxContained, BoxContaining
    >
{
    typedef within::box_in_box<BoxContained, BoxContaining> type;
};

}} // namespace within::services

namespace covered_by { namespace services
{

template <typename BoxContained, typename BoxContaining>
struct default_strategy
    <
        box_tag, box_tag,
        box_tag, areal_tag,
        cartesian_tag, cartesian_tag,
        BoxContained, BoxContaining
    >
{
    typedef within::box_in_box
                <
                    BoxContained, BoxContaining,
                    within::box_covered_by_range
                > type;
};

// spherical_equatorial_tag, spherical_polar_tag and geographic_cat are casted to spherical_tag
template <typename BoxContained, typename BoxContaining>
struct default_strategy
    <
        box_tag, box_tag,
        box_tag, areal_tag,
        spherical_tag, spherical_tag,
        BoxContained, BoxContaining
    >
{
    typedef within::box_in_box
                <
                    BoxContained, BoxContaining,
                    within::box_covered_by_range
                > type;
};

}} // namespace covered_by::services

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}}} // namespace boost::geometry::strategy

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_BOX_IN_BOX_HPP


/*

Copyright (c) 2015, Project OSRM contributors
All rights reserved.

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

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

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

*/

#include "../../algorithms/coordinate_calculation.hpp"
#include "../../algorithms/geospatial_query.hpp"
#include "../../data_structures/static_rtree.hpp"
#include "../../data_structures/query_node.hpp"
#include "../../data_structures/edge_based_node.hpp"
#include "../../util/floating_point.hpp"
#include "../../typedefs.h"

#include <boost/functional/hash.hpp>
#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <osrm/coordinate.hpp>

#include <cstdint>
#include <cmath>

#include <algorithm>
#include <memory>
#include <random>
#include <string>
#include <utility>
#include <unordered_set>
#include <vector>

BOOST_AUTO_TEST_SUITE(static_rtree)

constexpr uint32_t TEST_BRANCHING_FACTOR = 8;
constexpr uint32_t TEST_LEAF_NODE_SIZE = 64;

typedef EdgeBasedNode TestData;
using TestStaticRTree = StaticRTree<TestData,
                                    std::vector<FixedPointCoordinate>,
                                    false,
                                    TEST_BRANCHING_FACTOR,
                                    TEST_LEAF_NODE_SIZE>;
using MiniStaticRTree = StaticRTree<TestData, std::vector<FixedPointCoordinate>, false, 2, 3>;

// Choosen by a fair W20 dice roll (this value is completely arbitrary)
constexpr unsigned RANDOM_SEED = 42;
static const int32_t WORLD_MIN_LAT = -90 * COORDINATE_PRECISION;
static const int32_t WORLD_MAX_LAT = 90 * COORDINATE_PRECISION;
static const int32_t WORLD_MIN_LON = -180 * COORDINATE_PRECISION;
static const int32_t WORLD_MAX_LON = 180 * COORDINATE_PRECISION;

template <typename DataT> class LinearSearchNN
{
  public:
    LinearSearchNN(const std::shared_ptr<std::vector<FixedPointCoordinate>> &coords,
                   const std::vector<DataT> &edges)
        : coords(coords), edges(edges)
    {
    }

    std::vector<DataT> Nearest(const FixedPointCoordinate &input_coordinate,
                               const unsigned num_results)
    {
        std::vector<DataT> local_edges(edges);

        std::nth_element(
            local_edges.begin(), local_edges.begin() + num_results, local_edges.end(),
            [this, &input_coordinate](const DataT &lhs, const DataT &rhs)
            {
                float current_ratio = 0.;
                FixedPointCoordinate nearest;
                const float lhs_dist = coordinate_calculation::perpendicular_distance(
                    coords->at(lhs.u), coords->at(lhs.v), input_coordinate, nearest, current_ratio);
                const float rhs_dist = coordinate_calculation::perpendicular_distance(
                    coords->at(rhs.u), coords->at(rhs.v), input_coordinate, nearest, current_ratio);
                return lhs_dist < rhs_dist;
            });
        local_edges.resize(num_results);

        return local_edges;
    }

  private:
    const std::shared_ptr<std::vector<FixedPointCoordinate>> &coords;
    const std::vector<TestData> &edges;
};

template <unsigned NUM_NODES, unsigned NUM_EDGES> struct RandomGraphFixture
{
    struct TupleHash
    {
        typedef std::pair<unsigned, unsigned> argument_type;
        typedef std::size_t result_type;

        result_type operator()(const argument_type &t) const
        {
            std::size_t val{0};
            boost::hash_combine(val, t.first);
            boost::hash_combine(val, t.second);
            return val;
        }
    };

    RandomGraphFixture() : coords(std::make_shared<std::vector<FixedPointCoordinate>>())
    {
        BOOST_TEST_MESSAGE("Constructing " << NUM_NODES << " nodes and " << NUM_EDGES << " edges.");

        std::mt19937 g(RANDOM_SEED);

        std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT);
        std::uniform_int_distribution<> lon_udist(WORLD_MIN_LON, WORLD_MAX_LON);

        for (unsigned i = 0; i < NUM_NODES; i++)
        {
            int lat = lat_udist(g);
            int lon = lon_udist(g);
            nodes.emplace_back(QueryNode(lat, lon, OSMNodeID(i)));
            coords->emplace_back(FixedPointCoordinate(lat, lon));
        }

        std::uniform_int_distribution<> edge_udist(0, nodes.size() - 1);

        std::unordered_set<std::pair<unsigned, unsigned>, TupleHash> used_edges;

        while (edges.size() < NUM_EDGES)
        {
            TestData data;
            data.u = edge_udist(g);
            data.v = edge_udist(g);
            if (used_edges.find(std::pair<unsigned, unsigned>(
                    std::min(data.u, data.v), std::max(data.u, data.v))) == used_edges.end())
            {
                data.component.id = 0;
                edges.emplace_back(data);
                used_edges.emplace(std::min(data.u, data.v), std::max(data.u, data.v));
            }
        }
    }

    std::vector<QueryNode> nodes;
    std::shared_ptr<std::vector<FixedPointCoordinate>> coords;
    std::vector<TestData> edges;
};

struct GraphFixture
{
    GraphFixture(const std::vector<std::pair<float, float>> &input_coords,
                 const std::vector<std::pair<unsigned, unsigned>> &input_edges)
        : coords(std::make_shared<std::vector<FixedPointCoordinate>>())
    {

        for (unsigned i = 0; i < input_coords.size(); i++)
        {
            FixedPointCoordinate c(input_coords[i].first * COORDINATE_PRECISION,
                                   input_coords[i].second * COORDINATE_PRECISION);
            coords->emplace_back(c);
            nodes.emplace_back(QueryNode(c.lat, c.lon, OSMNodeID(i)));
        }

        for (const auto &pair : input_edges)
        {
            TestData d;
            d.u = pair.first;
            d.v = pair.second;
            // We set the forward nodes to the target node-based-node IDs, just
            // so we have something to test against.  Because this isn't a real
            // graph, the actual values aren't important, we just need something
            // to examine during tests.
            d.forward_edge_based_node_id = pair.second;
            d.reverse_edge_based_node_id = pair.first;
            edges.emplace_back(d);
        }
    }

    std::vector<QueryNode> nodes;
    std::shared_ptr<std::vector<FixedPointCoordinate>> coords;
    std::vector<TestData> edges;
};

typedef RandomGraphFixture<TEST_LEAF_NODE_SIZE * 3, TEST_LEAF_NODE_SIZE / 2>
    TestRandomGraphFixture_LeafHalfFull;
typedef RandomGraphFixture<TEST_LEAF_NODE_SIZE * 5, TEST_LEAF_NODE_SIZE>
    TestRandomGraphFixture_LeafFull;
typedef RandomGraphFixture<TEST_LEAF_NODE_SIZE * 10, TEST_LEAF_NODE_SIZE * 2>
    TestRandomGraphFixture_TwoLeaves;
typedef RandomGraphFixture<TEST_LEAF_NODE_SIZE * TEST_BRANCHING_FACTOR * 3,
                           TEST_LEAF_NODE_SIZE * TEST_BRANCHING_FACTOR>
    TestRandomGraphFixture_Branch;
typedef RandomGraphFixture<TEST_LEAF_NODE_SIZE * TEST_BRANCHING_FACTOR * 3,
                           TEST_LEAF_NODE_SIZE * TEST_BRANCHING_FACTOR * 2>
    TestRandomGraphFixture_MultipleLevels;

template <typename RTreeT>
void simple_verify_rtree(RTreeT &rtree,
                         const std::shared_ptr<std::vector<FixedPointCoordinate>> &coords,
                         const std::vector<TestData> &edges)
{
    BOOST_TEST_MESSAGE("Verify end points");
    for (const auto &e : edges)
    {
        const FixedPointCoordinate &pu = coords->at(e.u);
        const FixedPointCoordinate &pv = coords->at(e.v);
        auto result_u = rtree.Nearest(pu, 1);
        auto result_v = rtree.Nearest(pv, 1);
        BOOST_CHECK(result_u.size() == 1 && result_v.size() == 1);
        BOOST_CHECK(result_u.front().u == e.u || result_u.front().v == e.u);
        BOOST_CHECK(result_v.front().u == e.v || result_v.front().v == e.v);
    }
}

template <typename RTreeT>
void sampling_verify_rtree(RTreeT &rtree, LinearSearchNN<TestData> &lsnn, const std::vector<FixedPointCoordinate>& coords, unsigned num_samples)
{
    std::mt19937 g(RANDOM_SEED);
    std::uniform_int_distribution<> lat_udist(WORLD_MIN_LAT, WORLD_MAX_LAT);
    std::uniform_int_distribution<> lon_udist(WORLD_MIN_LON, WORLD_MAX_LON);
    std::vector<FixedPointCoordinate> queries;
    for (unsigned i = 0; i < num_samples; i++)
    {
        queries.emplace_back(FixedPointCoordinate(lat_udist(g), lon_udist(g)));
    }

    BOOST_TEST_MESSAGE("Sampling queries");
    for (const auto &q : queries)
    {
        auto result_rtree = rtree.Nearest(q, 1);
        auto result_lsnn = lsnn.Nearest(q, 1);
        BOOST_CHECK(result_rtree.size() == 1);
        BOOST_CHECK(result_lsnn.size() == 1);
        auto rtree_u = result_rtree.back().u;
        auto rtree_v = result_rtree.back().v;
        auto lsnn_u = result_lsnn.back().u;
        auto lsnn_v = result_lsnn.back().v;

        float current_ratio = 0.;
        FixedPointCoordinate nearest;
        const float rtree_dist = coordinate_calculation::perpendicular_distance(
            coords[rtree_u], coords[rtree_v], q, nearest, current_ratio);
        const float lsnn_dist = coordinate_calculation::perpendicular_distance(
            coords[lsnn_u], coords[lsnn_v], q, nearest, current_ratio);
        BOOST_CHECK_LE(std::abs(rtree_dist - lsnn_dist), std::numeric_limits<float>::epsilon());
    }
}

template <typename FixtureT, typename RTreeT = TestStaticRTree>
void build_rtree(const std::string &prefix,
                 FixtureT *fixture,
                 std::string &leaves_path,
                 std::string &nodes_path)
{
    nodes_path = prefix + ".ramIndex";
    leaves_path = prefix + ".fileIndex";
    const std::string coords_path = prefix + ".nodes";

    boost::filesystem::ofstream node_stream(coords_path, std::ios::binary);
    const auto num_nodes = static_cast<unsigned>(fixture->nodes.size());
    node_stream.write((char *)&num_nodes, sizeof(unsigned));
    node_stream.write((char *)&(fixture->nodes[0]), num_nodes * sizeof(QueryNode));
    node_stream.close();

    RTreeT r(fixture->edges, nodes_path, leaves_path, fixture->nodes);
}

template <typename FixtureT, typename RTreeT = TestStaticRTree>
void construction_test(const std::string &prefix, FixtureT *fixture)
{
    std::string leaves_path;
    std::string nodes_path;
    build_rtree<FixtureT, RTreeT>(prefix, fixture, leaves_path, nodes_path);
    RTreeT rtree(nodes_path, leaves_path, fixture->coords);
    LinearSearchNN<TestData> lsnn(fixture->coords, fixture->edges);

    simple_verify_rtree(rtree, fixture->coords, fixture->edges);
    sampling_verify_rtree(rtree, lsnn, *fixture->coords, 100);
}

BOOST_FIXTURE_TEST_CASE(construct_half_leaf_test, TestRandomGraphFixture_LeafHalfFull)
{
    construction_test("test_1", this);
}

BOOST_FIXTURE_TEST_CASE(construct_full_leaf_test, TestRandomGraphFixture_LeafFull)
{
    construction_test("test_2", this);
}

BOOST_FIXTURE_TEST_CASE(construct_two_leaves_test, TestRandomGraphFixture_TwoLeaves)
{
    construction_test("test_3", this);
}

BOOST_FIXTURE_TEST_CASE(construct_branch_test, TestRandomGraphFixture_Branch)
{
    construction_test("test_4", this);
}

BOOST_FIXTURE_TEST_CASE(construct_multiple_levels_test, TestRandomGraphFixture_MultipleLevels)
{
    construction_test("test_5", this);
}

// Bug: If you querry a point that lies between two BBs that have a gap,
// one BB will be pruned, even if it could contain a nearer match.
BOOST_AUTO_TEST_CASE(regression_test)
{
    using Coord = std::pair<float, float>;
    using Edge = std::pair<unsigned, unsigned>;
    GraphFixture fixture(
        {
            Coord(40.0, 0.0), Coord(35.0, 5.0),

            Coord(5.0, 5.0), Coord(0.0, 10.0),

            Coord(20.0, 10.0), Coord(20.0, 5.0),

            Coord(40.0, 100.0), Coord(35.0, 105.0),

            Coord(5.0, 105.0), Coord(0.0, 110.0),
        },
        {Edge(0, 1), Edge(2, 3), Edge(4, 5), Edge(6, 7), Edge(8, 9)});

    std::string leaves_path;
    std::string nodes_path;
    build_rtree<GraphFixture, MiniStaticRTree>("test_regression", &fixture, leaves_path,
                                               nodes_path);
    MiniStaticRTree rtree(nodes_path, leaves_path, fixture.coords);
    LinearSearchNN<TestData> lsnn(fixture.coords, fixture.edges);

    // query a node just right of the center of the gap
    FixedPointCoordinate input(20.0 * COORDINATE_PRECISION, 55.1 * COORDINATE_PRECISION);
    auto result_rtree = rtree.Nearest(input, 1);
    auto result_ls = lsnn.Nearest(input, 1);

    BOOST_CHECK(result_rtree.size() == 1);
    BOOST_CHECK(result_ls.size() == 1);

    BOOST_CHECK_EQUAL(result_ls.front().u, result_rtree.front().u);
    BOOST_CHECK_EQUAL(result_ls.front().v, result_rtree.front().v);
}

void TestRectangle(double width, double height, double center_lat, double center_lon)
{
    FixedPointCoordinate center(center_lat * COORDINATE_PRECISION,
                                center_lon * COORDINATE_PRECISION);

    TestStaticRTree::Rectangle rect;
    rect.min_lat = center.lat - height / 2.0 * COORDINATE_PRECISION;
    rect.max_lat = center.lat + height / 2.0 * COORDINATE_PRECISION;
    rect.min_lon = center.lon - width / 2.0 * COORDINATE_PRECISION;
    rect.max_lon = center.lon + width / 2.0 * COORDINATE_PRECISION;

    unsigned offset = 5 * COORDINATE_PRECISION;
    FixedPointCoordinate north(rect.max_lat + offset, center.lon);
    FixedPointCoordinate south(rect.min_lat - offset, center.lon);
    FixedPointCoordinate west(center.lat, rect.min_lon - offset);
    FixedPointCoordinate east(center.lat, rect.max_lon + offset);
    FixedPointCoordinate north_east(rect.max_lat + offset, rect.max_lon + offset);
    FixedPointCoordinate north_west(rect.max_lat + offset, rect.min_lon - offset);
    FixedPointCoordinate south_east(rect.min_lat - offset, rect.max_lon + offset);
    FixedPointCoordinate south_west(rect.min_lat - offset, rect.min_lon - offset);

    /* Distance to line segments of rectangle */
    BOOST_CHECK_EQUAL(rect.GetMinDist(north),
                      coordinate_calculation::great_circle_distance(
                          north, FixedPointCoordinate(rect.max_lat, north.lon)));
    BOOST_CHECK_EQUAL(rect.GetMinDist(south),
                      coordinate_calculation::great_circle_distance(
                          south, FixedPointCoordinate(rect.min_lat, south.lon)));
    BOOST_CHECK_EQUAL(rect.GetMinDist(west),
                      coordinate_calculation::great_circle_distance(
                          west, FixedPointCoordinate(west.lat, rect.min_lon)));
    BOOST_CHECK_EQUAL(rect.GetMinDist(east),
                      coordinate_calculation::great_circle_distance(
                          east, FixedPointCoordinate(east.lat, rect.max_lon)));

    /* Distance to corner points */
    BOOST_CHECK_EQUAL(rect.GetMinDist(north_east),
                      coordinate_calculation::great_circle_distance(
                          north_east, FixedPointCoordinate(rect.max_lat, rect.max_lon)));
    BOOST_CHECK_EQUAL(rect.GetMinDist(north_west),
                      coordinate_calculation::great_circle_distance(
                          north_west, FixedPointCoordinate(rect.max_lat, rect.min_lon)));
    BOOST_CHECK_EQUAL(rect.GetMinDist(south_east),
                      coordinate_calculation::great_circle_distance(
                          south_east, FixedPointCoordinate(rect.min_lat, rect.max_lon)));
    BOOST_CHECK_EQUAL(rect.GetMinDist(south_west),
                      coordinate_calculation::great_circle_distance(
                          south_west, FixedPointCoordinate(rect.min_lat, rect.min_lon)));
}

BOOST_AUTO_TEST_CASE(rectangle_test)
{
    TestRectangle(10, 10, 5, 5);
    TestRectangle(10, 10, -5, 5);
    TestRectangle(10, 10, 5, -5);
    TestRectangle(10, 10, -5, -5);
    TestRectangle(10, 10, 0, 0);
}

BOOST_AUTO_TEST_CASE(bearing_tests)
{
    using Coord = std::pair<float, float>;
    using Edge = std::pair<unsigned, unsigned>;
    GraphFixture fixture(
        {
            Coord(0.0, 0.0), Coord(10.0, 10.0),
        },
        {Edge(0, 1), Edge(1, 0)});

    std::string leaves_path;
    std::string nodes_path;
    build_rtree<GraphFixture, MiniStaticRTree>("test_bearing", &fixture, leaves_path, nodes_path);
    MiniStaticRTree rtree(nodes_path, leaves_path, fixture.coords);
    GeospatialQuery<MiniStaticRTree> query(rtree, fixture.coords);

    FixedPointCoordinate input(5.0 * COORDINATE_PRECISION, 5.1 * COORDINATE_PRECISION);

    {
        auto results = query.NearestPhantomNodes(input, 5);
        BOOST_CHECK_EQUAL(results.size(), 2);
        BOOST_CHECK_EQUAL(results.back().phantom_node.forward_node_id, 0);
        BOOST_CHECK_EQUAL(results.back().phantom_node.reverse_node_id, 1);
    }

    {
        auto results = query.NearestPhantomNodes(input, 5, 270, 10);
        BOOST_CHECK_EQUAL(results.size(), 0);
    }

    {
        auto results = query.NearestPhantomNodes(input, 5, 45, 10);
        BOOST_CHECK_EQUAL(results.size(), 2);
        BOOST_CHECK_EQUAL(results[0].phantom_node.forward_node_id, 1);
        BOOST_CHECK_EQUAL(results[0].phantom_node.reverse_node_id, SPECIAL_NODEID);
        BOOST_CHECK_EQUAL(results[1].phantom_node.forward_node_id, SPECIAL_NODEID);
        BOOST_CHECK_EQUAL(results[1].phantom_node.reverse_node_id, 1);
    }

    {
        auto results = query.NearestPhantomNodesInRange(input, 11000);
        BOOST_CHECK_EQUAL(results.size(), 2);
    }

    {
        auto results = query.NearestPhantomNodesInRange(input, 11000, 270, 10);
        BOOST_CHECK_EQUAL(results.size(), 0);
    }

    {
        auto results = query.NearestPhantomNodesInRange(input, 11000, 45, 10);
        BOOST_CHECK_EQUAL(results.size(), 2);
        BOOST_CHECK_EQUAL(results[0].phantom_node.forward_node_id, 1);
        BOOST_CHECK_EQUAL(results[0].phantom_node.reverse_node_id, SPECIAL_NODEID);
        BOOST_CHECK_EQUAL(results[1].phantom_node.forward_node_id, SPECIAL_NODEID);
        BOOST_CHECK_EQUAL(results[1].phantom_node.reverse_node_id, 1);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include <boost/heap/pairing_heap.hpp>


#include <iostream>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>

#include <solver.hpp>
#include <polynome_reader.hpp>
#include <args_parser.hpp>

// FloatType for computations can be specified when configuring the project via CMake
// boost::multiprecision::cpp_dec_float_100 is very accurate but relatively slow in comparison to double
#ifdef WITH_BOOST_MULTIPRECISION
#include <boost/multiprecision/cpp_dec_float.hpp>
using FloatType = boost::multiprecision::cpp_dec_float_100;

//template Sqrt should be instantiated because we don't want to lose accuracy when using std::sqrt(double)
template<>
boost::multiprecision::cpp_dec_float_100 SqrtImpl(const boost::multiprecision::cpp_dec_float_100 &value) {
    return boost::multiprecision::sqrt(value);
}

//this template instantiation should be faster than using intermediate stringstream
template<>
boost::multiprecision::cpp_dec_float_100 ReadInteger(const char *str) {
    return boost::multiprecision::cpp_dec_float_100(str);
}
#else
using FloatType = double;
#endif

// this class holds Producer and Consumer threads and some other entities which is needed for square solving
class SquareSolverService
{
public:
    SquareSolverService(PolynomeReader<FloatType, 3> &reader, bool silent = false, int batchSize = 32):
        reader(reader),
        solver(BuildSolver<FloatType>()),
        kSilent(silent),
        kBatchSize(batchSize)
    {
    }
    virtual ~SquareSolverService() = default;

    void Run() {
        std::thread consumer([&](){ this->ConsumerThread(); });
        ProducerThread();
        StopConsumer();
        consumer.join();
    }

private:
    PolynomeReader<FloatType, 3> &reader;

    const std::unique_ptr<EquationSolver<FloatType> > solver;
    const int kBatchSize = 32;
    const bool kSilent = false;

    std::mutex mtx;
    std::condition_variable cvar;

    //we don't want to lose too much time because of the overhead of inter-thread synchronization
    //that's why we send data in batches, which are not too small and not too big
    using BatchType = std::vector<Polynome<FloatType> >;
    using BatchPtrType = std::unique_ptr<BatchType>;

    std::atomic<bool> batchReady = false;
    BatchPtrType preparedBatch;
    BatchPtrType formingBatch;

    void SendBatch() {
        std::unique_lock<std::mutex> ul(mtx);
        preparedBatch = std::move(formingBatch);
        batchReady = true;
        ul.unlock();
        cvar.notify_one();

        //reservation at some extent increases performance here
        formingBatch = std::make_unique<BatchType>();
        formingBatch->reserve(kBatchSize);

        ul.lock();
        cvar.wait(ul, [&]() { return this->batchReady.load() == false; });
    }

    void StopConsumer() {
        //consumer thread can be stopped via sending empty pointer to batch
        std::unique_lock<std::mutex> ul(mtx);

        batchReady = true;
        ul.unlock();
        cvar.notify_one();
    }

    void ProducerThread() {
        formingBatch = std::make_unique<BatchType>();
        formingBatch->reserve(kBatchSize);

        Polynome<FloatType> poly;
        while (reader >> poly) {
            formingBatch->emplace_back(std::move(poly));

            if (formingBatch->size() >= kBatchSize) {
                SendBatch();
            }
        }

        //don't forget to process remaining equations
        if (!formingBatch->empty()) {
            SendBatch();
        }
    }

    void ConsumerThread() {
        while (true) {
            std::unique_lock<std::mutex> ul(mtx);
            cvar.wait(ul, [&]() { return this->batchReady.load(); });

            BatchPtrType batch = std::move(preparedBatch);
            batchReady = false;

            ul.unlock();
            cvar.notify_one();

            //sending empty batch pointer will stop the consumer
            if (!batch) break;

            for (const Polynome<FloatType> &poly: (*batch)) {
                auto res = solver->Solve(poly);
                //printing results in console can take too much time in comparison to all the algo
                //so it can be disabled
                if (!kSilent) {
                    std::cout << poly << " => " << res << std::endl;
                }
            }
        }
    }
};

int main(int argc, char **argv) {
    std::cout.precision(std::numeric_limits<FloatType>::digits10);

    //argc_offset is used to make clear from which point of arguments interesting data is placed
    int argc_offset;
    const Configuration config = ParseCmdArgs(argc, argv, argc_offset);

    clock_t requestStartTime = clock();

    //interactive mode allows to type coeffs in console in real time
    PolynomeReader<FloatType, 3> reader = config.interactive ? PolynomeReader<FloatType, 3>(std::cin) : PolynomeReader<FloatType, 3>(argc - argc_offset, argv + argc_offset);
    SquareSolverService service(reader, config.silent, config.interactive ? 1 : 32);
    service.Run();

    clock_t requestEndTime = clock();

    if (config.measurePerformance) {
        std::cout.precision(6);
        std::cout << "request processing time: " << (requestEndTime - requestStartTime) / double(CLOCKS_PER_SEC) << std::endl;
    }

    return 0;
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// @project        Open Space Toolkit ▸ Physics
/// @file           OpenSpaceToolkit/Physics/Coordinate/Position.cpp
/// @author         Lucas Brémond <lucas@loftorbital.com>
/// @license        Apache License 2.0

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <OpenSpaceToolkit/Physics/Coordinate/Frame.hpp>
#include <OpenSpaceToolkit/Physics/Coordinate/Position.hpp>

#include <OpenSpaceToolkit/Core/Error.hpp>
#include <OpenSpaceToolkit/Core/Utilities.hpp>

#include <boost/stacktrace.hpp>

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

namespace ostk
{
namespace physics
{
namespace coord
{

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                Position::Position                          (   const   Vector3d&                   aCoordinateSet,
                                                                                const   Position::Unit&             aUnit,
                                                                                const   Shared<const Frame>&        aFrameSPtr                                  )
                                :   coordinates_(aCoordinateSet),
                                    unit_(aUnit),
                                    frameSPtr_(aFrameSPtr)
{

}

                                Position::Position                          (   const   Position&                   aPosition                                   )
                                :   coordinates_(aPosition.coordinates_),
                                    unit_(aPosition.unit_),
                                    frameSPtr_(aPosition.frameSPtr_)
{

}

Position&                       Position::operator =                        (   const   Position&                   aPosition                                   )
{

    if (this != &aPosition)
    {

        coordinates_ = aPosition.coordinates_ ;
        unit_ = aPosition.unit_ ;
        frameSPtr_ = aPosition.frameSPtr_ ;

    }

    return *this ;

}

bool                            Position::operator ==                       (   const   Position&                   aPosition                                   ) const
{

    if ((!this->isDefined()) || (!aPosition.isDefined()))
    {
        return false ;
    }

    return (coordinates_ == aPosition.coordinates_) && (unit_ == aPosition.unit_) && ((*frameSPtr_) == (*aPosition.frameSPtr_)) ;

}

bool                            Position::operator !=                       (   const   Position&                   aPosition                                   ) const
{
    return !((*this) == aPosition) ;
}

std::ostream&                   operator <<                                 (           std::ostream&               anOutputStream,
                                                                                const   Position&                   aPosition                                   )
{

    ostk::core::utils::Print::Header(anOutputStream, "Position") ;

    ostk::core::utils::Print::Line(anOutputStream) << "Coordinates:"         << (aPosition.coordinates_.isDefined() ? aPosition.coordinates_.toString() : "Undefined") ;
    ostk::core::utils::Print::Line(anOutputStream) << "Unit:"                << ((aPosition.unit_ != Position::Unit::Undefined) ? Length::StringFromUnit(aPosition.unit_) : "Undefined") ;
    ostk::core::utils::Print::Line(anOutputStream) << "Frame:"               << (((aPosition.frameSPtr_ != nullptr) && aPosition.frameSPtr_->isDefined()) ? aPosition.frameSPtr_->getName() : "Undefined") ;

    ostk::core::utils::Print::Footer(anOutputStream) ;

    return anOutputStream ;

}

bool                            Position::isDefined                         ( ) const
{
    return coordinates_.isDefined() && (unit_ != Position::Unit::Undefined) && (frameSPtr_ != nullptr) && frameSPtr_->isDefined() ;
}

bool                            Position::isNear                            (   const   Position&                   aPosition,
                                                                                const   Length&                     aLength                                     ) const
{

    if (!aPosition.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    if (!aLength.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Length") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    if (frameSPtr_ != aPosition.frameSPtr_)
    {
        throw ostk::core::error::RuntimeError("Position are expressed in different frames.") ;
    }

    if (unit_ == aPosition.unit_)
    {
        return (coordinates_ - aPosition.coordinates_).norm() <= aLength.in(unit_) ;
    }
    else
    {
        return (coordinates_ - aPosition.inUnit(unit_).coordinates_).norm() <= aLength.in(unit_) ;
    }

    return false ;

}

const Vector3d&                 Position::accessCoordinates                 ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    return coordinates_ ;

}

Shared<const Frame>             Position::accessFrame                       ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    return frameSPtr_ ;

}

Vector3d                        Position::getCoordinates                    ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    return coordinates_ ;

}

Position::Unit                  Position::getUnit                           ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    return unit_ ;

}

Position                        Position::inUnit                            (   const   Position::Unit&             aUnit                                       ) const
{

    if (aUnit == Position::Unit::Undefined)
    {
        throw ostk::core::error::runtime::Undefined("Unit") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    return { coordinates_ * Length(1.0, unit_).in(aUnit), aUnit, frameSPtr_ } ;

}

Position                        Position::inMeters                          ( ) const
{
    return this->inUnit(Position::Unit::Meter) ;
}

Position                        Position::inFrame                           (   const   Shared<const Frame>&        aFrameSPtr,
                                                                                const   Instant&                    anInstant                                   ) const
{

    if ((aFrameSPtr == nullptr) || (!aFrameSPtr->isDefined()))
    {
        throw ostk::core::error::runtime::Undefined("Frame") ;
    }

    if (!this->isDefined())
    {
        // std::cout << boost::stacktrace::stacktrace() ;
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    return { frameSPtr_->getTransformTo(aFrameSPtr, anInstant).applyToPosition(coordinates_), unit_, aFrameSPtr } ;

}

String                          Position::toString                          (   const   Integer&                    aPrecision                                  ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Position") ;
    }

    return String::Format("{} [{}] @ {}", (aPrecision.isDefined() ? coordinates_.toString(aPrecision) : coordinates_.toString()), Length::SymbolFromUnit(unit_), frameSPtr_->getName()) ;

}

Position                        Position::Undefined                         ( )
{
    return { Vector3d::Undefined(), Position::Unit::Undefined, nullptr } ;
}

Position                        Position::Meters                            (   const   Vector3d&                   aCoordinateSet,
                                                                                const   Shared<const Frame>&        aFrameSPtr                                  )
{
    return { aCoordinateSet, Position::Unit::Meter, aFrameSPtr } ;
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

}
}
}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


#include <exception>
#include <chrono>
#include <limits>
#include <iostream>
#include <string>
#include <vector>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>

#include "calculate.hpp"

namespace fs = boost::filesystem;
namespace po = boost::program_options;

template<typename Parser>
void run(std::string, std::vector<std::string>, const po::variables_map&);

int main(int argc, char *argv[]) {
    fs::path program{argv[0]};
    po::variables_map arguments;
    std::string string;
    std::vector<std::string> variables;

    po::options_description named_args("Options");
    named_args.add_options()
        ("expression,e", po::value<std::string>(&string)->required(), "Expression to parse")
        ("help,h", "Show this help message")
        ("var,v", po::value<std::vector<std::string>>(&variables), "Add variable (var:value)")
        ("postfix,p", "Use postfix parser")
        ("complex,c", "Use complex parser")
        ("optimize,o", "Optimize expression")
        ("iter,i", po::value<std::size_t>()->default_value(1000), "Performance iterations")
        ("analysis,a", "Print analysis")
    ;
    po::positional_options_description positional_args;
    positional_args.add("expression", 1);

    try {
        po::store(
            po::command_line_parser(argc, argv)
                .options(named_args)
                .positional(positional_args)
                .run(),
            arguments
        );

        if (arguments.count("help")) {
            std::cout <<
                "Usage: " << program.filename() <<
                " [options] {-e [ --expression ]} <expression>"
                << std::endl;
            std::cout << named_args << std::endl;
            return 0;
        }
        po::notify(arguments);

        if (arguments.count("complex"))
            run<calculate::ComplexParser>(string, variables, arguments);
        else
            run<calculate::Parser>(string, variables, arguments);
    }
    catch (const po::error& error) {
        std::cerr << "Command line error: " << error.what() << std::endl;
        return 1;
    }
    catch (const calculate::BaseError& error) {
        std::cerr << error.what() << std::endl;
        return 2;
    }
    return 0;
}

template<typename Parser>
void run(
    std::string expression,
    std::vector<std::string> variables,
    const po::variables_map& arguments
) {
    using Type = typename Parser::Type;

    std::chrono::steady_clock::time_point begin;
    std::chrono::steady_clock::time_point::rep build_time, eval_time;
    std::size_t iterations = arguments["iter"].as<std::size_t>();
    Type result;

    auto parser = Parser{};
    auto& lexer = parser.lexer();

    if (arguments.count("optimize"))
        parser.optimize = true;

    std::vector<Type> values;
    if (arguments.count("var")) {
        for (auto& var : variables) {
            auto sep = var.find(":");
            if (sep == 0 || sep > var.size() - 2)
                throw po::error("bad variable input '" + var + "'");
            values.push_back(lexer.to_value(var.substr(sep + 1, var.size())));
            var = var.substr(0, sep);
        }
    }

    auto parse = arguments.count("postfix") ?
        &Parser::template from_postfix<const std::vector<std::string>&> :
        &Parser::template from_infix<const std::vector<std::string>&>;

    auto function = (parser.*parse)(expression, variables);
    if (arguments.count("analysis")) {
        begin = std::chrono::steady_clock::now();
        for (std::size_t i = 0; i < iterations; i++)
            (parser.*parse)(expression, variables);
        build_time = std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::steady_clock::now() - begin
        ).count() / iterations;
    }

    result = function(values);
    if (arguments.count("analysis")) {
        begin = std::chrono::steady_clock::now();
        for (std::size_t i = 0; i < iterations; i++)
            function(values);
        eval_time = std::chrono::duration_cast<std::chrono::nanoseconds>(
            std::chrono::steady_clock::now() - begin
        ).count() / iterations;
    }

    if (arguments.count("analysis")) {
        std::cout << "Infix notation:   " << function.infix() << "\n";
        std::cout << "Postfix notation: " << function.postfix() << "\n";
        if (arguments.count("var")) {
            std::cout << "Variables:         ";
            for (const auto& var : variables)
                std::cout << var << " ";
            std::cout << "\n";
            std::cout << "Values:            ";
            for (const auto& val : values)
                std::cout << lexer.to_string(val) << " ";
            std::cout << "\n";
        }
        std::cout << "Result:           " << lexer.to_string(result) << "\n";
        std::cout << "Iterations:       " << iterations << "\n";
        std::cout << "Building time:    " << build_time << " us" << "\n";
        std::cout << "Evaluation time:  " << eval_time << " ns" << std::endl;
    }
    else
        std::cout << lexer.to_string(result) << std::endl;
}