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

#define DEFAULT_LOGGER "betting"
#include <graphene/chain/betting_market_object.hpp>
#include <graphene/chain/database.hpp>
#include <boost/integer/common_factor_rt.hpp>

#include <boost/msm/back/state_machine.hpp>
#include <boost/msm/front/state_machine_def.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/msm/back/tools.hpp>

namespace graphene { namespace chain {
   enum class betting_market_state {
      unresolved,
      frozen,
      closed,
      graded,
      canceled,
      settled
   };
} }
FC_REFLECT_ENUM(graphene::chain::betting_market_state,
                (unresolved)
                (frozen)
                (closed)
                (graded)
                (canceled)
                (settled))

namespace graphene { namespace chain {

namespace msm = boost::msm;
namespace mpl = boost::mpl;

/* static */ share_type bet_object::get_approximate_matching_amount(share_type bet_amount, bet_multiplier_type backer_multiplier, bet_type back_or_lay, bool round_up /* = false */)
{
   fc::uint128_t amount_to_match_128 = bet_amount.value;

   if (back_or_lay == bet_type::back)
   {
       amount_to_match_128 *= backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION;
       if (round_up)
         amount_to_match_128 += GRAPHENE_BETTING_ODDS_PRECISION - 1;
       amount_to_match_128 /= GRAPHENE_BETTING_ODDS_PRECISION;
   }
   else
   {
       amount_to_match_128 *= GRAPHENE_BETTING_ODDS_PRECISION;
       if (round_up)
         amount_to_match_128 += backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION - 1;
       amount_to_match_128 /= backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION;
   }
   return amount_to_match_128.to_uint64();
}

share_type bet_object::get_approximate_matching_amount(bool round_up /* = false */) const
{
   return get_approximate_matching_amount(amount_to_bet.amount, backer_multiplier, back_or_lay, round_up);
}

/* static */ share_type bet_object::get_exact_matching_amount(share_type bet_amount, bet_multiplier_type backer_multiplier, bet_type back_or_lay)
{
   share_type back_ratio;
   share_type lay_ratio;
   std::tie(back_ratio, lay_ratio) = get_ratio(backer_multiplier);
   if (back_or_lay == bet_type::back)
      return bet_amount / back_ratio * lay_ratio;
   else
      return bet_amount / lay_ratio * back_ratio;
}

share_type bet_object::get_exact_matching_amount() const
{
   return get_exact_matching_amount(amount_to_bet.amount, backer_multiplier, back_or_lay);
}

/* static */ std::pair<share_type, share_type> bet_object::get_ratio(bet_multiplier_type backer_multiplier)
{
   share_type gcd = boost::integer::gcd(GRAPHENE_BETTING_ODDS_PRECISION, static_cast<int32_t>(backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION));
   return std::make_pair(GRAPHENE_BETTING_ODDS_PRECISION / gcd, (backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION) / gcd);
}

std::pair<share_type, share_type> bet_object::get_ratio() const
{
   return get_ratio(backer_multiplier);
}

share_type bet_object::get_minimum_matchable_amount() const
{
   share_type gcd = boost::integer::gcd(GRAPHENE_BETTING_ODDS_PRECISION, static_cast<int32_t>(backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION));
   return (back_or_lay == bet_type::back ? GRAPHENE_BETTING_ODDS_PRECISION : backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION) / gcd;
}

share_type bet_object::get_minimum_matching_amount() const
{
   share_type gcd = boost::integer::gcd(GRAPHENE_BETTING_ODDS_PRECISION, static_cast<int32_t>(backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION));
   return (back_or_lay == bet_type::lay ? GRAPHENE_BETTING_ODDS_PRECISION : backer_multiplier - GRAPHENE_BETTING_ODDS_PRECISION) / gcd;
}

share_type betting_market_position_object::reduce()
{
   share_type additional_not_cancel_balance = std::min(pay_if_payout_condition, pay_if_not_payout_condition);
   if (additional_not_cancel_balance == 0)
      return 0;
   pay_if_payout_condition -= additional_not_cancel_balance;
   pay_if_not_payout_condition -= additional_not_cancel_balance;
   pay_if_not_canceled += additional_not_cancel_balance;

   share_type immediate_winnings = std::min(pay_if_canceled, pay_if_not_canceled);
   if (immediate_winnings == 0)
      return 0;
   pay_if_canceled -= immediate_winnings;
   pay_if_not_canceled -= immediate_winnings;
   return immediate_winnings;
}

// betting market object implementation
namespace
{
   // Events -- most events happen when the witnesses publish an update operation with a new
   // status, so if they publish an event with the status set to `frozen`, we'll generate a `frozen_event`
   struct unresolved_event
   {
      database& db;
      unresolved_event(database& db) : db(db) {}
   };
   struct frozen_event
   {
      database& db;
      frozen_event(database& db) : db(db) {}
   };
   struct closed_event
   {
      database& db;
      closed_event(database& db) : db(db) {}
   };
   struct graded_event
   {
      database& db;
      betting_market_resolution_type new_grading;
      graded_event(database& db, betting_market_resolution_type new_grading) : db(db), new_grading(new_grading) {}
   };
   struct settled_event
   {
      database& db;
      settled_event(database& db) : db(db) {}
   };
   struct canceled_event
   {
      database& db;
      canceled_event(database& db) : db(db) {}
   };

   // Events
   struct betting_market_state_machine_ : public msm::front::state_machine_def<betting_market_state_machine_>
   {
      // disable a few state machine features we don't use for performance
      typedef int no_exception_thrown;
      typedef int no_message_queue;

      // States
      struct unresolved : public msm::front::state<>{
         template <class Event>
         void on_entry(const Event& event, betting_market_state_machine_& fsm) {
            dlog("betting market ${id} -> unresolved", ("id", fsm.betting_market_obj->id));
         }
      };
      struct frozen : public msm::front::state<>{
         template <class Event>
         void on_entry(const Event& event, betting_market_state_machine_& fsm) {
            dlog("betting market ${id} -> frozen", ("id", fsm.betting_market_obj->id));
         }
      };
      struct closed : public msm::front::state<>{
         template <class Event>
         void on_entry(const Event& event, betting_market_state_machine_& fsm) {
            dlog("betting market ${id} -> closed", ("id", fsm.betting_market_obj->id));
         }
      };
      struct graded : public msm::front::state<>{
         void on_entry(const graded_event& event, betting_market_state_machine_& fsm) {
            dlog("betting market ${id} -> graded", ("id", fsm.betting_market_obj->id));
            fsm.betting_market_obj->resolution = event.new_grading;
         }
      };
      struct settled : public msm::front::state<>{
         template <class Event>
         void on_entry(const Event& event, betting_market_state_machine_& fsm) {
            dlog("betting market ${id} -> settled", ("id", fsm.betting_market_obj->id));
         }
      };
      struct canceled : public msm::front::state<>{
         void on_entry(const canceled_event& event, betting_market_state_machine_& fsm) {
            dlog("betting market ${id} -> canceled", ("id", fsm.betting_market_obj->id));
            fsm.betting_market_obj->resolution = betting_market_resolution_type::cancel;
            fsm.betting_market_obj->cancel_all_bets(event.db);
         }
      };

      typedef unresolved initial_state;
      typedef betting_market_state_machine_ x; // makes transition table cleaner

      // Transition table for betting market
      struct transition_table : mpl::vector<
      //    Start                       Event                         Next                       Action               Guard
      //  +---------------------------+-----------------------------+----------------------------+---------------------+----------------------+
      _row < unresolved,               frozen_event,                 frozen >,
      _row < unresolved,               closed_event,                 closed >,
      _row < unresolved,               canceled_event,               canceled >,
      //  +---------------------------+-----------------------------+----------------------------+---------------------+----------------------+
      _row < frozen,                   unresolved_event,             unresolved >,
      _row < frozen,                   closed_event,                 closed >,
      _row < frozen,                   canceled_event,               canceled >,
      //  +---------------------------+-----------------------------+----------------------------+---------------------+----------------------+
      _row < closed,                   graded_event,                 graded >,
      _row < closed,                   canceled_event,               canceled >,
      //  +---------------------------+-----------------------------+----------------------------+---------------------+----------------------+
      _row < graded,                   settled_event,                settled >,
      _row < graded,                   canceled_event,               canceled >
      //  +---------------------------+-----------------------------+----------------------------+---------------------+----------------------+
      > {};

      template <class Fsm, class Event>
      void no_transition(Event const& e, Fsm& ,int state)
      {
         FC_THROW_EXCEPTION(graphene::chain::no_transition, "No transition");
      }

      template <class Fsm>
      void no_transition(canceled_event const& e, Fsm&, int state)
      {
         //ignore transitions from settled to canceled state
         //and from canceled to canceled state
      }

      betting_market_object* betting_market_obj;
      betting_market_state_machine_(betting_market_object* betting_market_obj) : betting_market_obj(betting_market_obj) {}
   };
   typedef msm::back::state_machine<betting_market_state_machine_> betting_market_state_machine;

} // end anonymous namespace

class betting_market_object::impl {
public:
   betting_market_state_machine state_machine;

   impl(betting_market_object* self) : state_machine(self) {}
};

betting_market_object::betting_market_object() :
   my(new impl(this))
{
}

betting_market_object::betting_market_object(const betting_market_object& rhs) :
   graphene::db::abstract_object<betting_market_object>(rhs),
   group_id(rhs.group_id),
   description(rhs.description),
   payout_condition(rhs.payout_condition),
   resolution(rhs.resolution),
   my(new impl(this))
{
   my->state_machine = rhs.my->state_machine;
   my->state_machine.betting_market_obj = this;
}

betting_market_object& betting_market_object::operator=(const betting_market_object& rhs)
{
   //graphene::db::abstract_object<betting_market_object>::operator=(rhs);
   id = rhs.id;
   group_id = rhs.group_id;
   description = rhs.description;
   payout_condition = rhs.payout_condition;
   resolution = rhs.resolution;

   my->state_machine = rhs.my->state_machine;
   my->state_machine.betting_market_obj = this;

   return *this;
}

betting_market_object::~betting_market_object()
{
}

namespace {

   bool verify_betting_market_status_constants()
   {
      unsigned error_count = 0;
      typedef msm::back::generate_state_set<betting_market_state_machine::stt>::type all_states;
      static char const* filled_state_names[mpl::size<all_states>::value];
      mpl::for_each<all_states,boost::msm::wrap<mpl::placeholders::_1> >
         (msm::back::fill_state_names<betting_market_state_machine::stt>(filled_state_names));
      for (unsigned i = 0; i < mpl::size<all_states>::value; ++i)
      {
         try
         {
            // this is an approximate test, the state name provided by typeinfo will be mangled, but should
            // at least contain the string we're looking for
            const char* fc_reflected_value_name = fc::reflector<betting_market_state>::to_string((betting_market_state)i);
            if (!strstr(filled_state_names[i], fc_reflected_value_name))
            {
               fc_elog(fc::logger::get("default"),
                       "Error, state string mismatch between fc and boost::msm for int value ${int_value}: "
                       "boost::msm -> ${boost_string}, fc::reflect -> ${fc_string}",
                       ("int_value", i)("boost_string", filled_state_names[i])("fc_string", fc_reflected_value_name));
               ++error_count;
            }
         }
         catch (const fc::bad_cast_exception&)
         {
            fc_elog(fc::logger::get("default"),
                    "Error, no reflection for value ${int_value} in enum betting_market_status",
                    ("int_value", i));
            ++error_count;
         }
      }
      if (error_count == 0)
         dlog("Betting market status constants are correct");
      else
         wlog("There were ${count} errors in the betting market status constants", ("count", error_count));

      return error_count == 0;
   }
} // end anonymous namespace

betting_market_status betting_market_object::get_status() const
{
   static bool state_constants_are_correct = verify_betting_market_status_constants();
   (void)&state_constants_are_correct;
   betting_market_state state = (betting_market_state)my->state_machine.current_state()[0];

   edump((state));

   switch (state)
   {
      case betting_market_state::unresolved:
         return betting_market_status::unresolved;
      case betting_market_state::frozen:
         return betting_market_status::frozen;
      case betting_market_state::closed:
         return betting_market_status::unresolved;
      case betting_market_state::canceled:
         return betting_market_status::canceled;
      case betting_market_state::graded:
         return betting_market_status::graded;
      case betting_market_state::settled:
         return betting_market_status::settled;
      default:
         FC_THROW("Unexpected betting market state");
   };
}

void betting_market_object::cancel_all_unmatched_bets(database& db) const
{
   const auto& bet_odds_idx = db.get_index_type<bet_object_index>().indices().get<by_odds>();

   // first, cancel all bets on the active books
   auto book_itr = bet_odds_idx.lower_bound(std::make_tuple(id));
   auto book_end = bet_odds_idx.upper_bound(std::make_tuple(id));
   while (book_itr != book_end)
   {
      auto old_book_itr = book_itr;
      ++book_itr;
      db.cancel_bet(*old_book_itr, true);
   }

   // then, cancel any delayed bets on that market.  We don't have an index for
   // that, so walk through all delayed bets
   book_itr = bet_odds_idx.begin();
   while (book_itr != bet_odds_idx.end() &&
          book_itr->end_of_delay)
   {
      auto old_book_itr = book_itr;
      ++book_itr;
      if (old_book_itr->betting_market_id == id)
         db.cancel_bet(*old_book_itr, true);
   }
}

void betting_market_object::cancel_all_bets(database& db) const
{
    const auto& bets_by_market_id = db.get_index_type<bet_object_index>().indices().get<by_betting_market_id>();

    auto bet_it = bets_by_market_id.lower_bound(id);
    auto bet_it_end = bets_by_market_id.upper_bound(id);
    while (bet_it != bet_it_end)
    {
        auto old_bet_it = bet_it;
        ++bet_it;
        db.cancel_bet(*old_bet_it, true);
    }
}

void betting_market_object::pack_impl(std::ostream& stream) const
{
   boost::archive::binary_oarchive oa(stream, boost::archive::no_header|boost::archive::no_codecvt|boost::archive::no_xml_tag_checking);
   oa << my->state_machine;
}

void betting_market_object::unpack_impl(std::istream& stream)
{
   boost::archive::binary_iarchive ia(stream, boost::archive::no_header|boost::archive::no_codecvt|boost::archive::no_xml_tag_checking);
   ia >> my->state_machine;
}

void betting_market_object::on_unresolved_event(database& db)
{
   my->state_machine.process_event(unresolved_event(db));
}

void betting_market_object::on_frozen_event(database& db)
{
   my->state_machine.process_event(frozen_event(db));
}

void betting_market_object::on_closed_event(database& db)
{
   my->state_machine.process_event(closed_event(db));
}

void betting_market_object::on_graded_event(database& db, betting_market_resolution_type new_grading)
{
   my->state_machine.process_event(graded_event(db, new_grading));
}

void betting_market_object::on_settled_event(database& db)
{
   my->state_machine.process_event(settled_event(db));
}

void betting_market_object::on_canceled_event(database& db)
{
   my->state_machine.process_event(canceled_event(db));
}

} } // graphene::chain

namespace fc {
   // Manually reflect betting_market_object to variant to properly reflect "state"
   void to_variant(const graphene::chain::betting_market_object& event_obj, fc::variant& v)
   {
      fc::mutable_variant_object o;
      o("id", event_obj.id)
       ("group_id", event_obj.group_id)
       ("description", event_obj.description)
       ("payout_condition", event_obj.payout_condition)
       ("resolution", event_obj.resolution)
       ("status", event_obj.get_status());

      v = o;
   }

   // Manually reflect betting_market_object to variant to properly reflect "state"
   void from_variant(const fc::variant& v, graphene::chain::betting_market_object& event_obj)
   {
      event_obj.id = v["id"].as<graphene::chain::betting_market_id_type>();
      event_obj.group_id = v["name"].as<graphene::chain::betting_market_group_id_type>();
      event_obj.description = v["description"].as<graphene::chain::internationalized_string_type>();
      event_obj.payout_condition = v["payout_condition"].as<graphene::chain::internationalized_string_type>();
      event_obj.resolution = v["resolution"].as<fc::optional<graphene::chain::betting_market_resolution_type>>();
      graphene::chain::betting_market_status status = v["status"].as<graphene::chain::betting_market_status>();
      const_cast<int*>(event_obj.my->state_machine.current_state())[0] = (int)status;
   }
} //end namespace fc


//
// Created by erik on 1/2/17.
//

#include "Spawn.h"
#include "game/State.h"
#include "game/entity/entity.h"
#include "physics/PhysicsWorld.h"
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>

using namespace spatacs;
using namespace game;
using namespace events;

Spawn::Spawn(const length_vec& pos, const velocity_vec& vel) : mPosition(pos), mVelocity(vel)
{

}

GameEntityHandle Spawn::spawn(EventContext& context) const
{
    return context.state.entity_manager().create();
}

SpawnProjectile::SpawnProjectile(GameEntityID shooter, length_vec pos, velocity_vec vel, mass_t mass, length_t rad,
                                 game::Damage dmg) :
        Spawn(pos, vel),
        mShooter(shooter),
        mMass(mass),
        mRadius(rad),
        mDamage(dmg)
{

}

void SpawnProjectile::apply(EventContext& context) const
{
    GameEntityHandle entity = spawn(context);
    entity.add<components::Timer>(10.0_s);
    entity.add<components::Parent>(mShooter);
    entity.add<components::Warhead>(mDamage);
    entity.add_tag<components::TimedDespawn>();

    physics::Object obj(mPosition, mVelocity, mMass, entity.id().getID());
    obj.addFixture( mRadius );
    auto pid = context.world.spawn(obj);

    entity.add<components::PhysicsData>(pid, mMass, mPosition, mVelocity);
}

SpawnShip::SpawnShip(std::uint64_t team, std::string name, std::string type, const length_vec& position) :
        Spawn(position, velocity_vec{0,0,0}),
        mTeam(team),
        mName(std::move(name)),
        mType(std::move(type))
{

}
/*
namespace
{
    game::AmmoData getAmmoData(const std::string& type)
    {
        boost::property_tree::ptree tree;
        boost::property_tree::xml_parser::read_xml("data/ammunition.xml", tree);
        auto& source = tree.get_child("ammunition." + type);

        game::AmmoData data;
        data.mass   = source.get<mass_t>("mass");
        data.charge = source.get<energy_t>("energy");
        data.name   = type;
        data.damage.armour_pierce   = source.get<double>("AP", 0.0);
        data.damage.high_explosive  = source.get<double>("HE", 0.0);
        data.damage.shield_overload = source.get<double>("SO", 0.0);
        return data;
    }
}
*/
void SpawnShip::apply(EventContext& context) const
{
    boost::property_tree::ptree tree;
    boost::property_tree::xml_parser::read_xml("data/"+mType+".xml", tree);
    auto data = tree.get_child("ship");
    auto mass = data.get<mass_t>("mass");

    GameEntityHandle entity = spawn(context);
    entity.add<components::Name>(mName);
    entity.add<components::Affiliation>(mTeam);

    physics::Object obj(mPosition, velocity_vec{0, 0, 0}, mass, entity.id().getID());
    /*obj.addFixture( ship->radius() ).setUserdata(0); // ship
    obj.addFixture( ship->radius() + 25.0_m ).setUserdata(1); // shield
     */
    auto pid = context.world.spawn(obj);

    entity.add<components::PhysicsData>(pid, mass, mPosition, mVelocity);

    /*
    // now add the ammo
    for(auto& ammo : mAmmo)
    {
        ship->components().apply( game::systems::AddAmmunition(getAmmoData(ammo.type), ammo.amount) );
    }

    mass_t rest = game::systems::fill_fuel(ship->components(), mFuel);
    if( rest > 0.0_kg )
        std::cerr << "Could not fit total fuel into tank!" << std::endl;
     */
}

void SpawnShip::addAmmunition(std::string name, std::size_t amount)
{
    mAmmo.emplace_back( std::move(name), amount);
}

void SpawnShip::setFuel(mass_t f)
{
    mFuel = f;
}

SpawnShip::AmmoData::AmmoData(const std::string& type, size_t amount) : type(type), amount(amount)
{}


////////////////////////////////////////////////////////////////////////////////
// Copyright 2019 FZI Research Center for Information Technology
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived from this
// software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
////////////////////////////////////////////////////////////////////////////////

//-----------------------------------------------------------------------------
/*!\file    JacobianTransposeSolver.cpp
 *
 * \author  Stefan Scherzinger <scherzin@fzi.de>
 * \date    2020/03/26
 *
 */
//-----------------------------------------------------------------------------

// this package
#include <cartesian_controller_base/JacobianTransposeSolver.h>

// Pluginlib
#include <pluginlib/class_list_macros.h>

// other
#include <boost/algorithm/clamp.hpp>

/**
 * \class cartesian_controller_base::JacobianTransposeSolver
 *
 * Users may explicitly specify this solver with \a "jacobian_transpose" as \a
 * ik_solver in their controllers.yaml configuration file for each controller:
 *
 * \code{.yaml}
 * <name_of_your_controller>:
 *     type: "<type_of_your_controller>"
 *     ik_solver: "jacobian_transpose"
 *     ...
 * \endcode
 *
 */
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::JacobianTransposeSolver, cartesian_controller_base::IKSolver)

namespace cartesian_controller_base{

  JacobianTransposeSolver::JacobianTransposeSolver()
  {
  }

  JacobianTransposeSolver::~JacobianTransposeSolver(){}

  trajectory_msgs::JointTrajectoryPoint JacobianTransposeSolver::getJointControlCmds(
        ros::Duration period,
        const ctrl::Vector6D& net_force)
  {
    // Compute joint jacobian
    m_jnt_jacobian_solver->JntToJac(m_current_positions,m_jnt_jacobian);

    // Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f) \f$
    m_current_accelerations.data = m_jnt_jacobian.data.transpose() * net_force;

    // Integrate once, starting with zero motion
    m_current_velocities.data = 0.5 * m_current_accelerations.data * period.toSec();

    // Integrate twice, starting with zero motion
    m_current_positions.data = m_last_positions.data + 0.5 * m_current_velocities.data * period.toSec();

    // Make sure positions stay in allowed margins
    applyJointLimits();

    // Apply results
    trajectory_msgs::JointTrajectoryPoint control_cmd;
    for (int i = 0; i < m_number_joints; ++i)
    {
      control_cmd.positions.push_back(m_current_positions(i));
      control_cmd.velocities.push_back(m_current_velocities(i));

      // Accelerations should be left empty. Those values will be interpreted
      // by most hardware joint drivers as max. tolerated values. As a
      // consequence, the robot will move very slowly.
    }
    control_cmd.time_from_start = period; // valid for this duration

    return control_cmd;
  }

  bool JacobianTransposeSolver::init(ros::NodeHandle& nh,
                                     const KDL::Chain& chain,
                                     const KDL::JntArray& upper_pos_limits,
                                     const KDL::JntArray& lower_pos_limits)
  {
    IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits);

    m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
    m_jnt_jacobian.resize(m_number_joints);

    return true;
  }
} // namespace


#include "dense-reconstruction/pmvs-common.h"

#include <map>
#include <stdlib.h>

#include <Eigen/Core>
#include <aslam/cameras/camera-factory.h>
#include <aslam/cameras/camera-pinhole.h>
#include <aslam/cameras/camera-unified-projection.h>
#include <aslam/cameras/camera.h>
#include <aslam/common/timer.h>
#include <aslam/common/undistort-helpers.h>
#include <aslam/frames/visual-frame.h>
#include <aslam/frames/visual-nframe.h>
#include <glog/logging.h>
#include <landmark-triangulation/pose-interpolator.h>
#include <maplab-common/accessors.h>
#include <maplab-common/file-system-tools.h>
#include <maplab-common/vector-window-operations.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/imgproc/types_c.h>
#include <vi-map/unique-id.h>

namespace dense_reconstruction {
ObserverCamera::ObserverCamera(
    const vi_map::VIMap& vi_map, const PmvsConfig& config,
    const vi_map::MissionId& _mission_id, const aslam::CameraId& _camera_id,
    const unsigned int _frame_idx, const bool _is_optional_camera)
    : mission_id(_mission_id),
      camera_id(_camera_id),
      frame_idx(_frame_idx),
      is_optional_camera(_is_optional_camera) {
  VLOG(2) << "Create ObserverCamera from camera " << camera_id;
  const aslam::Camera* camera = nullptr;
  if (is_optional_camera) {
    const backend::CameraWithExtrinsics& cam_with_extrinsics =
        vi_map.getMission(mission_id)
            .getOptionalCameraWithExtrinsics(camera_id);
    camera = cam_with_extrinsics.second.get();
    T_B_C = cam_with_extrinsics.first.inverse();
    VLOG(2) << "  -> is optional camera.";
  } else {
    const aslam::NCamera& n_camera =
        vi_map.getSensorManager().getNCameraForMission(mission_id);
    camera = &(n_camera.getCamera(frame_idx));
    T_B_C = n_camera.get_T_C_B(frame_idx).inverse();
    VLOG(2) << "  -> is nframe camera.";
  }

  const aslam::Camera::Type camera_type = camera->getType();
  aslam::Camera::Ptr original_camera;
  switch (camera_type) {
    case aslam::Camera::Type::kPinhole:
      VLOG(2) << "  -> is pinhole camera.";
      if (camera->getDistortion().getType() !=
          aslam::Distortion::Type::kNoDistortion) {
        undistorter = aslam::createMappedUndistorter(
            *camera, config.pmvs_undistortion_alpha,
            config.pmvs_undistortion_scale,
            config.kUndistortionInterpolationMethod);

        camera_matrix_undistorted = static_cast<const aslam::PinholeCamera&>(
                                        undistorter->getOutputCamera())
                                        .getCameraMatrix();

        VLOG(2)
            << "  -> is distorted, initializing pinhole mapped undistorter!";
        CHECK(undistorter != nullptr);
      } else {
        camera_matrix_undistorted =
            static_cast<const aslam::PinholeCamera*>(camera)->getCameraMatrix();
        VLOG(2) << "  -> is already undistorted!";
      }
      break;
    case aslam::Camera::Type::kUnifiedProjection:
      VLOG(2) << "  -> is unified projection camera.";
      if (camera->getDistortion().getType() !=
          aslam::Distortion::Type::kNoDistortion) {
        undistorter = aslam::createMappedUndistorterToPinhole(
            *static_cast<const aslam::UnifiedProjectionCamera*>(camera),
            config.pmvs_undistortion_alpha, config.pmvs_undistortion_scale,
            config.kUndistortionInterpolationMethod);

        camera_matrix_undistorted =
            static_cast<const aslam::UnifiedProjectionCamera&>(
                undistorter->getOutputCamera())
                .getCameraMatrix();
        VLOG(2) << "  -> is distorted, initializing unified projection mapped "
                   "undistorter!";
        CHECK(undistorter != nullptr);
      } else {
        camera_matrix_undistorted =
            static_cast<const aslam::UnifiedProjectionCamera*>(camera)
                ->getCameraMatrix();
        VLOG(2) << "  -> is already undistorted!";
      }

      break;
    default:
      LOG(FATAL) << "Unsupported camera type: " << camera_type;
  }
}

ObserverCamera::ObserverCamera(
    const vi_map::VIMap& vi_map, const PmvsConfig& config,
    const vi_map::MissionId& _mission_id, const aslam::CameraId& _camera_id)
    : ObserverCamera(vi_map, config, _mission_id, _camera_id, 0u, true) {}

void ObserverCamera::undistortImage(
    const cv::Mat& image, cv::Mat* undistorted_image) const {
  CHECK(!image.empty());
  CHECK_NOTNULL(undistorted_image);
  CHECK(undistorter != nullptr);
  undistorter->processImage(image, undistorted_image);
}

ObserverPose::ObserverPose(
    const vi_map::VIMap& vi_map, const size_t _camera_number,
    const vi_map::MissionId& _mission_id,
    const pose_graph::VertexId& _vertex_id, const int64_t _timestamp_ns,
    const ObserverCamera& observer_camera,
    const backend::ResourceType& _image_type)
    : camera_number(_camera_number),
      mission_id(_mission_id),
      vertex_id(_vertex_id),
      timestamp_ns(_timestamp_ns),
      camera_id(observer_camera.camera_id),
      is_optional_camera_image(observer_camera.is_optional_camera),
      image_type(_image_type),
      frame_idx(observer_camera.frame_idx) {
  aslam::Transformation T_M_I;
  if (needsInterpolation(vi_map)) {
    Eigen::Matrix<int64_t, 1, Eigen::Dynamic> pose_timestamps;
    pose_timestamps.resize(1);
    pose_timestamps(0, 0) = timestamp_ns;
    aslam::TransformationVector poses_M_I;
    landmark_triangulation::PoseInterpolator pose_interpolator;
    pose_interpolator.getPosesAtTime(
        vi_map, mission_id, pose_timestamps, &poses_M_I);
    T_M_I = poses_M_I.at(0u);
  } else {
    T_M_I = vi_map.getVertex(vertex_id).get_T_M_I();
  }

  const aslam::Transformation& T_G_M =
      vi_map.getMissionBaseFrameForMission(mission_id).get_T_G_M();

  T_G_C = T_G_M * T_M_I * observer_camera.T_B_C;
  p_G = T_G_C.getPosition();

  const Eigen::Matrix<double, 3, 4> T_GC =
      T_G_C.inverse().getTransformationMatrix().template topLeftCorner<3, 4>();

  P_undistorted = observer_camera.camera_matrix_undistorted * T_GC;
}

void ObserverPose::loadImage(
    const vi_map::VIMap& vi_map, cv::Mat* image) const {
  CHECK_NOTNULL(image);
  if (is_optional_camera_image) {
    CHECK(
        vi_map.getOptionalCameraResource(
            vi_map.getMission(mission_id), image_type, camera_id, timestamp_ns,
            image));
  } else {
    const vi_map::Vertex& vertex = vi_map.getVertex(vertex_id);
    CHECK(vi_map.getFrameResource(vertex, frame_idx, image_type, image));
  }
}

bool ObserverPose::needsUndistortion() const {
  return (image_type == backend::ResourceType::kRawImage) ||
         (image_type == backend::ResourceType::kRawColorImage);
}

bool ObserverPose::needsInterpolation(const vi_map::VIMap& vi_map) const {
  return vi_map.getVertex(vertex_id).getMinTimestampNanoseconds() !=
         timestamp_ns;
}

}  // namespace dense_reconstruction


//-------------------------------------------------------------------------//
//
// Copyright 2017 Sascha Kaden
//
// 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.
//
//-------------------------------------------------------------------------//

#ifndef COLLISIONDETECTIONSPHERE_HPP
#define COLLISIONDETECTIONSPHERE_HPP

#include <Eigen/Geometry>

#include <ippp/environment/cad/CadProcessing.h>
#include <ippp/modules/collisionDetection/CollisionDetection.hpp>

namespace ippp {

/*!
* \brief   Class for AABB collision detection with the Eigen AABB structure, which is saved in the Mesh container.
* \author  Sascha Kaden
* \date    2017-02-19
*/
template <unsigned int dim>
class CollisionDetectionSphere : public CollisionDetection<dim> {
  public:
    CollisionDetectionSphere(const std::shared_ptr<Environment> &environment,
                             const CollisionRequest &request = CollisionRequest());
    bool checkConfig(const Vector<dim> &config, CollisionRequest *request = nullptr, CollisionResult *result = nullptr);
    bool checkTrajectory(std::vector<Vector<dim>> &configs) override;

  private:
    bool checkObstacles(const AABB &robotAABB, CollisionResult *result);
    bool checkRobots(const std::vector<AABB> &robotAABB, CollisionResult *result);
    double checkSphere(const AABB &a, const AABB &b);

    bool m_multiRobot = false;
    std::vector<AABB> m_robotAABBs;
    std::vector<AABB> m_obstacleAABBs;
    std::vector<std::shared_ptr<RobotBase>> m_robots;

    using CollisionDetection<dim>::m_environment;
};

/*!
*  \brief      Constructor of the class CollisionDetectionAABB
*  \author     Sascha Kaden
*  \param[in]  Environment
*  \date       2017-02-19
*/
template <unsigned int dim>
CollisionDetectionSphere<dim>::CollisionDetectionSphere(const std::shared_ptr<Environment> &environment,
                                                        const CollisionRequest &request)
    : CollisionDetection<dim>("CollisionDetectionSphere", environment, request) {
    if (environment->numRobots() > 1)
        m_multiRobot = true;

    m_robots = m_environment->getRobots();

    for (auto robot : environment->getRobots())
        m_robotAABBs.push_back(robot->getBaseModel()->getAABB());

    for (auto obstacle : environment->getObstacles())
        m_obstacleAABBs.push_back(obstacle->getAABB());
}

/*!
*  \brief      Check for collision
*  \author     Sascha Kaden
*  \param[in]  configuration
*  \param[out] binary result of collision (true if in collision or config is empty)
*  \date       2016-05-25
*/
template <unsigned int dim>
bool CollisionDetectionSphere<dim>::checkConfig(const Vector<dim> &config, CollisionRequest *request, CollisionResult *result) {
    CollisionRequest collisionRequest;
    if (request)
        collisionRequest = *request;
    else
        collisionRequest = this->m_request;

    if (m_multiRobot && collisionRequest.checkInterRobot) {
        // compute the new AABBs of the robots with the configuration
        std::vector<VectorX> singleConfigs = util::splitVec<dim>(config, m_environment->getRobotDimSizes());
        std::vector<AABB> robotAABBs;
        for (unsigned int i = 0; i < m_robots.size(); ++i) {
            auto trafo = m_robots[i]->getTransformation(singleConfigs[i]);
            robotAABBs.push_back(util::translateAABB(m_robotAABBs[i], trafo));
        }
        // check collisions
        if (checkRobots(robotAABBs, result))
            return true;
        for (auto &robotAABB : robotAABBs)
            if (checkObstacles(robotAABB, result))
                return true;
    }
    if (collisionRequest.checkObstacle) {
        auto trafo = m_robots[0]->getTransformation(config);
        AABB robotAABB = util::translateAABB(m_robotAABBs[0], trafo);
        return checkObstacles(robotAABB, result);
    }

    return false;
}

/*!
*  \brief      Check collision of a trajectory of configurations
*  \author     Sascha Kaden
*  \param[in]  vector of configurations
*  \param[out] binary result of collision (true if in collision)
*  \date       2016-05-25
*/
template <unsigned int dim>
bool CollisionDetectionSphere<dim>::checkTrajectory(std::vector<Vector<dim>> &configs) {
    if (configs.empty())
        return false;

    for (auto &config : configs)
        if (checkConfig(config))
            return true;

    return false;
}

template <unsigned int dim>
bool CollisionDetectionSphere<dim>::checkRobots(const std::vector<AABB> &robots, CollisionResult *result) {
    if (result) {
        double dist;
        for (auto a = robots.begin(); a != robots.end() - 1; ++a) {
            for (auto b = robots.begin() + 1; b != robots.end(); ++b) {
                dist = checkSphere(*a, *b);
                if (dist < result->minDist)
                    result->minDist = dist;
                if (dist < result->minRobotDist)
                    result->minRobotDist = dist;
                if (dist == 0) {
                    result->collision = true;
                    return true;
                }
            }
        }
    } else {
        for (auto a = robots.begin(); a != robots.end() - 1; ++a)
            for (auto b = robots.begin() + 1; b != robots.end(); ++b)
                if (checkSphere(*a, *b) < 0)
                    return true;
    }

    return false;
}

template <unsigned int dim>
bool CollisionDetectionSphere<dim>::checkObstacles(const AABB &robotAABB, CollisionResult *result) {
    if (result) {
        double dist;
        for (auto &obstacle : m_obstacleAABBs) {
            dist = checkSphere(robotAABB, obstacle);
            if (dist < result->minDist)
                result->minDist = dist;
            if (dist < result->minObstacleDist)
                result->minObstacleDist = dist;
            if (dist == 0) {
                result->collision = true;
                return true;
            }
        }
    } else {
        for (auto &obstacle : m_obstacleAABBs)
            if (checkSphere(robotAABB, obstacle) < 0)
                return true;
    }

    return false;
}

template <unsigned int dim>
double CollisionDetectionSphere<dim>::checkSphere(const AABB &a, const AABB &b) {
    // Book: Real-Time Collision Detection page 88
    Vector3 d = a.center() - b.center();
    double dist2 = d.dot(d);
    // Spheres intersect if squared distance is less than squared sum of radii
    double radiusSum = a.diagonal().norm() + b.diagonal().norm();
    return dist2 - (radiusSum * radiusSum);
}

} /* namespace ippp */

#endif /* COLLISIONDETECTIONAABB_HPP */


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/deinterleave_first.hpp>
#include <boost/simd/function/deinterleave_second.hpp>
#include <array>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( deinterleave_
                          , (typename T, typename X)
                          , bd::cpu_
                          , bs::pack_< bd::unspecified_<T>, X >
                          , bs::pack_< bd::unspecified_<T>, X >
                          )
  {
    static_assert ( T::static_size >= 2
                  , "deinterleave_first requires at least two elements"
                  );

    BOOST_FORCEINLINE std::array<T,2> operator()(T const& x, T const& y) const BOOST_NOEXCEPT
    {
      return { deinterleave_first(x,y), deinterleave_second(x,y) };
    }
  };
} } }

#endif


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#ifndef GRAPH_BLOCKMODEL_MULTICANONICAL_HH
#define GRAPH_BLOCKMODEL_MULTICANONICAL_HH

#include "config.h"

#include <vector>

#include "graph_tool.hh"
#include "../support/graph_state.hh"
#include "graph_blockmodel_util.hh"
#include <boost/mpl/vector.hpp>

namespace graph_tool
{
using namespace boost;
using namespace std;

#define MULTICANONICAL_BLOCK_STATE_params(State)                               \
    ((__class__,&, mpl::vector<python::object>, 1))                            \
    ((state, &, State&, 0))                                                    \
    ((hist, &, std::vector<size_t>&, 0))                                       \
    ((dens, &, std::vector<double>&, 0))                                       \
    ((S_min, , double, 0))                                                     \
    ((S_max, , double, 0))                                                     \
    ((f, , double, 0))                                                         \
    ((S, , double, 0))                                                         \
    ((E,, size_t, 0))                                                          \
    ((verbose,, bool, 0))

template <class State>
struct Multicanonical
{
    GEN_STATE_BASE(MulticanonicalBlockStateBase,
                   MULTICANONICAL_BLOCK_STATE_params(State))

    template <class... Ts>
    class MulticanonicalBlockState
        : public MulticanonicalBlockStateBase<Ts...>
    {
    public:
        GET_PARAMS_USING(MulticanonicalBlockStateBase<Ts...>,
                         MULTICANONICAL_BLOCK_STATE_params(State))
        GET_PARAMS_TYPEDEF(Ts, MULTICANONICAL_BLOCK_STATE_params(State))

        template <class... ATs,
                  typename std::enable_if_t<sizeof...(ATs) ==
                                            sizeof...(Ts)>* = nullptr>
        MulticanonicalBlockState(ATs&&... as)
           : MulticanonicalBlockStateBase<Ts...>(as...),
            _i(get_bin(_S))
        {
        }

        int _i;
        double _dS;
        size_t _null_move = null_group;

        int get_bin(double S)
        {
            return std::floor((_hist.size() - 1) *
                              ((S - _S_min) / (_S_max - _S_min)));
        };

        bool skip_node(size_t v)
        {
            return _state.skip_node(v);
        }

        size_t node_state(size_t v)
        {
            return _state.node_state(v);
        }

        size_t node_weight(size_t v)
        {
            return _state.node_weight(v);
        }

        template <class RNG>
        size_t move_proposal(size_t v, RNG& rng)
        {
            return _state.move_proposal(v, rng);
        }

        auto virtual_move_dS(size_t v, size_t nr)
        {
            auto dS = _state.virtual_move_dS(v, nr);
            double nS = _S + get<0>(dS);
            if (nS < _S_min || nS >= _S_max)
            {
                get<0>(dS) = numeric_limits<double>::infinity();
            }
            else
            {
                int j = get_bin(nS);
                get<1>(dS) += _dens[_i] - _dens[j];
            }
            _dS = get<0>(dS);
            return dS;
        }

        void perform_move(size_t v, size_t nr)
        {
            _state.perform_move(v, nr);
            _S += _dS;
            _i = get_bin(_S);
        }

        bool is_deterministic()
        {
            return _state.is_deterministic();
        }

        bool is_sequential()
        {
            return _state.is_sequential();
        }

        auto& get_vlist()
        {
            return _state.get_vlist();
        }

        double get_beta()
        {
            return 1;
        }

        size_t get_niter()
        {
            return _state.get_niter();
        }

        void step(size_t, size_t)
        {
            _hist[_i]++;
            _dens[_i] += _f;
        }
    };
};

} // graph_tool namespace

#endif //GRAPH_BLOCKMODEL_MULTICANONICAL_HH


#include "extractor/intersection/intersection_analysis.hpp"
#include "extractor/intersection/coordinate_extractor.hpp"

#include "util/assert.hpp"
#include "util/bearing.hpp"
#include "util/coordinate_calculation.hpp"

#include <boost/optional/optional_io.hpp>

namespace osrm
{
namespace extractor
{
namespace intersection
{

IntersectionEdges getIncomingEdges(const util::NodeBasedDynamicGraph &graph,
                                   const NodeID intersection_node)
{
    IntersectionEdges result;

    for (const auto outgoing_edge : graph.GetAdjacentEdgeRange(intersection_node))
    {
        const auto from_node = graph.GetTarget(outgoing_edge);
        const auto incoming_edge = graph.FindEdge(from_node, intersection_node);

        if (!graph.GetEdgeData(incoming_edge).reversed)
        {
            result.push_back({from_node, incoming_edge});
        }
    }

    // Enforce ordering of incoming edges
    std::sort(result.begin(), result.end());
    return result;
}

IntersectionEdges getOutgoingEdges(const util::NodeBasedDynamicGraph &graph,
                                   const NodeID intersection_node)
{
    IntersectionEdges result;

    for (const auto outgoing_edge : graph.GetAdjacentEdgeRange(intersection_node))
    {
        result.push_back({intersection_node, outgoing_edge});
    }

    BOOST_ASSERT(std::is_sorted(result.begin(), result.end()));
    return result;
}

std::vector<util::Coordinate>
getEdgeCoordinates(const extractor::CompressedEdgeContainer &compressed_geometries,
                   const std::vector<util::Coordinate> &node_coordinates,
                   const NodeID from_node,
                   const EdgeID edge,
                   const NodeID to_node)
{
    if (!compressed_geometries.HasEntryForID(edge))
        return {node_coordinates[from_node], node_coordinates[to_node]};

    BOOST_ASSERT(from_node < node_coordinates.size());
    BOOST_ASSERT(to_node < node_coordinates.size());

    // extracts the geometry in coordinates from the compressed edge container
    std::vector<util::Coordinate> result;
    const auto &geometry = compressed_geometries.GetBucketReference(edge);
    result.reserve(geometry.size() + 1);

    result.push_back(node_coordinates[from_node]);
    std::transform(geometry.begin(),
                   geometry.end(),
                   std::back_inserter(result),
                   [&node_coordinates](const auto &compressed_edge) {
                       return node_coordinates[compressed_edge.node_id];
                   });

    // filter duplicated coordinates
    result.erase(std::unique(result.begin(), result.end()), result.end());
    return result;
}

namespace
{
double findAngleBisector(double alpha, double beta)
{
    alpha *= M_PI / 180.;
    beta *= M_PI / 180.;
    const auto average =
        180. * std::atan2(std::sin(alpha) + std::sin(beta), std::cos(alpha) + std::cos(beta)) /
        M_PI;
    return std::fmod(average + 360., 360.);
}

double findClosestOppositeBearing(const IntersectionEdgeGeometries &edge_geometries,
                                  const double bearing)
{
    BOOST_ASSERT(!edge_geometries.empty());
    const auto min = std::min_element(
        edge_geometries.begin(),
        edge_geometries.end(),
        [bearing = util::bearing::reverse(bearing)](const auto &lhs, const auto &rhs) {
            return util::angularDeviation(lhs.perceived_bearing, bearing) <
                   util::angularDeviation(rhs.perceived_bearing, bearing);
        });
    return util::bearing::reverse(min->perceived_bearing);
}

std::pair<bool, double> findMergedBearing(const util::NodeBasedDynamicGraph &graph,
                                          const IntersectionEdgeGeometries &edge_geometries,
                                          std::size_t lhs_index,
                                          std::size_t rhs_index,
                                          bool neighbor_intersection)
{
    // Function returns a pair with a flag and a value of bearing for merged roads
    // If the flag is false the bearing must not be used as a merged value at neighbor intersections

    using util::angularDeviation;
    using util::bearing::angleBetween;

    const auto &lhs = edge_geometries[lhs_index];
    const auto &rhs = edge_geometries[rhs_index];
    BOOST_ASSERT(graph.GetEdgeData(lhs.edge).reversed != graph.GetEdgeData(rhs.edge).reversed);

    const auto &entry = graph.GetEdgeData(lhs.edge).reversed ? rhs : lhs;
    const auto opposite_bearing =
        findClosestOppositeBearing(edge_geometries, entry.perceived_bearing);
    const auto merged_bearing = findAngleBisector(rhs.perceived_bearing, lhs.perceived_bearing);

    if (angularDeviation(angleBetween(opposite_bearing, entry.perceived_bearing), STRAIGHT_ANGLE) <
        MAXIMAL_ALLOWED_NO_TURN_DEVIATION)
    {
        // In some intersections, turning roads can introduce artificial turns if we merge here.
        // Consider a scenario like:
        //
        //  a     .  g - f
        //  |   .
        //  | .
        //  |.
        // d-b--------e
        //  |
        //  c
        //
        // Merging `bgf` and `be` would introduce an angle, even though d-b-e is perfectly straight
        // We don't change the angle, if such an opposite road exists
        return {false, entry.perceived_bearing};
    }

    if (neighbor_intersection)
    {
        // Check that the merged bearing makes both turns closer to straight line
        const auto turn_angle_lhs = angleBetween(opposite_bearing, lhs.perceived_bearing);
        const auto turn_angle_rhs = angleBetween(opposite_bearing, rhs.perceived_bearing);
        const auto turn_angle_new = angleBetween(opposite_bearing, merged_bearing);

        if (util::angularDeviation(turn_angle_lhs, STRAIGHT_ANGLE) <
                util::angularDeviation(turn_angle_new, STRAIGHT_ANGLE) ||
            util::angularDeviation(turn_angle_rhs, STRAIGHT_ANGLE) <
                util::angularDeviation(turn_angle_new, STRAIGHT_ANGLE))
            return {false, opposite_bearing};
    }

    return {true, merged_bearing};
}

bool isRoadsPairMergeable(const MergableRoadDetector &detector,
                          const IntersectionEdgeGeometries &edge_geometries,
                          const NodeID intersection_node,
                          const std::size_t index)
{
    const auto size = edge_geometries.size();
    BOOST_ASSERT(index < size);

    const auto &llhs = edge_geometries[(index + size - 1) % size];
    const auto &lhs = edge_geometries[index];
    const auto &rhs = edge_geometries[(index + 1) % size];
    const auto &rrhs = edge_geometries[(index + 2) % size];

    // TODO: check IsDistinctFrom - it is an angle and name-only check
    // also check CanMergeRoad for all merging scenarios
    return detector.IsDistinctFrom({llhs.edge, llhs.perceived_bearing, llhs.length},
                                   {lhs.edge, lhs.perceived_bearing, lhs.length}) &&
           detector.CanMergeRoad(intersection_node,
                                 {lhs.edge, lhs.perceived_bearing, lhs.length},
                                 {rhs.edge, rhs.perceived_bearing, rhs.length}) &&
           detector.IsDistinctFrom({rhs.edge, rhs.perceived_bearing, rhs.length},
                                   {rrhs.edge, rrhs.perceived_bearing, rrhs.length});
}

auto getIntersectionLanes(const util::NodeBasedDynamicGraph &graph, const NodeID intersection_node)
{
    std::uint8_t max_lanes_intersection = 0;
    for (auto outgoing_edge : graph.GetAdjacentEdgeRange(intersection_node))
    {
        max_lanes_intersection =
            std::max(max_lanes_intersection,
                     graph.GetEdgeData(outgoing_edge).flags.road_classification.GetNumberOfLanes());
    }
    return max_lanes_intersection;
}

template <bool USE_CLOSE_COORDINATE>
IntersectionEdgeGeometries
getIntersectionOutgoingGeometries(const util::NodeBasedDynamicGraph &graph,
                                  const extractor::CompressedEdgeContainer &compressed_geometries,
                                  const std::vector<util::Coordinate> &node_coordinates,
                                  const NodeID intersection_node)
{
    IntersectionEdgeGeometries edge_geometries;

    // TODO: keep CoordinateExtractor to reproduce bearings, simplify later
    const CoordinateExtractor coordinate_extractor(graph, compressed_geometries, node_coordinates);

    const auto max_lanes_intersection = getIntersectionLanes(graph, intersection_node);

    // Collect outgoing edges
    for (const auto outgoing_edge : graph.GetAdjacentEdgeRange(intersection_node))
    {
        const auto remote_node = graph.GetTarget(outgoing_edge);

        const auto &geometry = getEdgeCoordinates(
            compressed_geometries, node_coordinates, intersection_node, outgoing_edge, remote_node);

        // OSRM_ASSERT(geometry.size() >= 2, node_coordinates[intersection_node]);

        const auto close_coordinate =
            coordinate_extractor.ExtractCoordinateAtLength(2. /*m*/, geometry);
        const auto initial_bearing =
            util::coordinate_calculation::bearing(geometry[0], close_coordinate);

        const auto representative_coordinate =
            USE_CLOSE_COORDINATE || graph.GetOutDegree(intersection_node) <= 2
                ? coordinate_extractor.GetCoordinateCloseToTurn(
                      intersection_node, outgoing_edge, false, remote_node)
                : coordinate_extractor.ExtractRepresentativeCoordinate(intersection_node,
                                                                       outgoing_edge,
                                                                       false,
                                                                       remote_node,
                                                                       max_lanes_intersection,
                                                                       geometry);
        const auto perceived_bearing =
            util::coordinate_calculation::bearing(geometry[0], representative_coordinate);

        const auto edge_length = util::coordinate_calculation::getLength(
            geometry.begin(), geometry.end(), util::coordinate_calculation::haversineDistance);

        edge_geometries.push_back({outgoing_edge, initial_bearing, perceived_bearing, edge_length});
    }

    // Sort edges in the clockwise bearings order
    std::sort(edge_geometries.begin(), edge_geometries.end(), [](const auto &lhs, const auto &rhs) {
        return lhs.perceived_bearing < rhs.perceived_bearing;
    });
    return edge_geometries;
}
}

std::pair<IntersectionEdgeGeometries, std::unordered_set<EdgeID>>
getIntersectionGeometries(const util::NodeBasedDynamicGraph &graph,
                          const extractor::CompressedEdgeContainer &compressed_geometries,
                          const std::vector<util::Coordinate> &node_coordinates,
                          const MergableRoadDetector &detector,
                          const NodeID intersection_node)
{
    IntersectionEdgeGeometries edge_geometries = getIntersectionOutgoingGeometries<false>(
        graph, compressed_geometries, node_coordinates, intersection_node);

    const auto edges_number = edge_geometries.size();

    std::vector<bool> merged_edges(edges_number, false);

    // TODO: intersection views do not contain merged and not allowed edges
    // but contain other restricted edges that are used in TurnAnalysis,
    // to be deleted after TurnAnalysis refactoring
    std::unordered_set<EdgeID> merged_edge_ids;

    if (edges_number >= 3)
    { // Adjust bearings of mergeable roads
        for (std::size_t index = 0; index < edges_number; ++index)
        {
            if (isRoadsPairMergeable(detector, edge_geometries, intersection_node, index))
            { // Merge bearings of roads left & right
                const auto next = (index + 1) % edges_number;
                auto &lhs = edge_geometries[index];
                auto &rhs = edge_geometries[next];
                merged_edges[index] = true;
                merged_edges[next] = true;

                const auto merge = findMergedBearing(graph, edge_geometries, index, next, false);

                lhs.perceived_bearing = lhs.initial_bearing = merge.second;
                rhs.perceived_bearing = rhs.initial_bearing = merge.second;

                // Only one of the edges must be reversed, mark it as merged to remove from
                // intersection view
                BOOST_ASSERT(graph.GetEdgeData(lhs.edge).reversed ^
                             graph.GetEdgeData(rhs.edge).reversed);
                merged_edge_ids.insert(graph.GetEdgeData(lhs.edge).reversed ? lhs.edge : rhs.edge);
            }
        }
    }

    if (edges_number >= 2)
    { // Adjust bearings of roads that will be merged at the neighbor intersections
        const double constexpr PRUNING_DISTANCE = 30.;

        for (std::size_t index = 0; index < edges_number; ++index)
        {
            auto &edge_geometry = edge_geometries[index];

            // Don't adjust bearings of roads that were merged at the current intersection
            // or have neighbor intersection farer than the pruning distance
            if (merged_edges[index] || edge_geometry.length > PRUNING_DISTANCE)
                continue;

            const auto neighbor_intersection_node = graph.GetTarget(edge_geometry.edge);

            const auto neighbor_geometries = getIntersectionOutgoingGeometries<false>(
                graph, compressed_geometries, node_coordinates, neighbor_intersection_node);

            const auto neighbor_edges = neighbor_geometries.size();
            if (neighbor_edges <= 1)
                continue;

            const auto neighbor_curr = std::distance(
                neighbor_geometries.begin(),
                std::find_if(neighbor_geometries.begin(),
                             neighbor_geometries.end(),
                             [&graph, &intersection_node](const auto &road) {
                                 return graph.GetTarget(road.edge) == intersection_node;
                             }));
            BOOST_ASSERT(static_cast<std::size_t>(neighbor_curr) != neighbor_geometries.size());
            const auto neighbor_prev = (neighbor_curr + neighbor_edges - 1) % neighbor_edges;
            const auto neighbor_next = (neighbor_curr + 1) % neighbor_edges;

            if (isRoadsPairMergeable(
                    detector, neighbor_geometries, neighbor_intersection_node, neighbor_prev))
            { // Neighbor intersection has mergable neighbor_prev and neighbor_curr roads
                BOOST_ASSERT(!isRoadsPairMergeable(
                    detector, neighbor_geometries, neighbor_intersection_node, neighbor_curr));

                // TODO: merge with an angle bisector, but not a reversed closed turn, to be
                // checked as a difference with the previous implementation
                const auto merge = findMergedBearing(
                    graph, neighbor_geometries, neighbor_prev, neighbor_curr, true);

                if (merge.first)
                {
                    const auto offset = util::angularDeviation(
                        merge.second, neighbor_geometries[neighbor_curr].perceived_bearing);

                    // Adjust bearing of AB at the node A if at the node B roads BA (neighbor_curr)
                    // and BC (neighbor_prev) will be merged and will have merged bearing Bb.
                    // The adjustment value is ∠bBA with negative sign (counter-clockwise) to Aa
                    //     A ~~~ a
                    //       \
                    //  b --- B ---
                    //       /
                    //     C
                    edge_geometry.perceived_bearing = edge_geometry.initial_bearing =
                        std::fmod(edge_geometry.perceived_bearing + 360. - offset, 360.);
                }
            }
            else if (isRoadsPairMergeable(
                         detector, neighbor_geometries, neighbor_intersection_node, neighbor_curr))
            { // Neighbor intersection has mergable neighbor_curr and neighbor_next roads
                BOOST_ASSERT(!isRoadsPairMergeable(
                    detector, neighbor_geometries, neighbor_intersection_node, neighbor_prev));

                // TODO: merge with an angle bisector, but not a reversed closed turn, to be
                // checked as a difference with the previous implementation
                const auto merge = findMergedBearing(
                    graph, neighbor_geometries, neighbor_curr, neighbor_next, true);
                if (merge.first)
                {
                    const auto offset = util::angularDeviation(
                        merge.second, neighbor_geometries[neighbor_curr].perceived_bearing);

                    // Adjust bearing of AB at the node A if at the node B roads BA (neighbor_curr)
                    // and BC (neighbor_next) will be merged and will have merged bearing Bb.
                    // The adjustment value is ∠bBA with positive sign (clockwise) to Aa
                    //     a ~~~ A
                    //         /
                    //    --- B --- b
                    //         \
                    //          C
                    edge_geometry.perceived_bearing = edge_geometry.initial_bearing =
                        std::fmod(edge_geometry.perceived_bearing + offset, 360.);
                }
            }
        }
    }

    // Add incoming edges with reversed bearings
    edge_geometries.resize(2 * edges_number);
    for (std::size_t index = 0; index < edges_number; ++index)
    {
        const auto &geometry = edge_geometries[index];
        const auto remote_node = graph.GetTarget(geometry.edge);
        const auto incoming_edge = graph.FindEdge(remote_node, intersection_node);
        edge_geometries[edges_number + index] = {incoming_edge,
                                                 util::bearing::reverse(geometry.initial_bearing),
                                                 util::bearing::reverse(geometry.perceived_bearing),
                                                 geometry.length};
    }

    // Enforce ordering of edges by IDs
    std::sort(edge_geometries.begin(), edge_geometries.end());

    return std::make_pair(edge_geometries, merged_edge_ids);
}

inline auto findEdge(const IntersectionEdgeGeometries &geometries, const EdgeID &edge)
{
    const auto it = std::lower_bound(
        geometries.begin(), geometries.end(), edge, [](const auto &geometry, const auto edge) {
            return geometry.edge < edge;
        });
    BOOST_ASSERT(it != geometries.end() && it->edge == edge);
    return it;
}

double findEdgeBearing(const IntersectionEdgeGeometries &geometries, const EdgeID &edge)
{
    return findEdge(geometries, edge)->perceived_bearing;
}

double findEdgeLength(const IntersectionEdgeGeometries &geometries, const EdgeID &edge)
{
    return findEdge(geometries, edge)->length;
}

template <typename RestrictionsRange>
bool isTurnRestricted(const RestrictionsRange &restrictions, const NodeID to)
{
    // Check turn restrictions to find a node that is the only allowed target when coming from a
    // node to an intersection
    //     d
    //     |
    // a - b - c  and `only_straight_on ab | bc would return `c` for `a,b`
    const auto is_only = std::find_if(restrictions.first,
                                      restrictions.second,
                                      [](const auto &pair) { return pair.second->is_only; });
    if (is_only != restrictions.second)
        return is_only->second->AsNodeRestriction().to != to;

    // Check if explicitly forbidden
    const auto no_turn =
        std::find_if(restrictions.first, restrictions.second, [&to](const auto &restriction) {
            return restriction.second->AsNodeRestriction().to == to;
        });

    return no_turn != restrictions.second;
}

bool isTurnAllowed(const util::NodeBasedDynamicGraph &graph,
                   const EdgeBasedNodeDataContainer &node_data_container,
                   const RestrictionMap &restriction_map,
                   const std::unordered_set<NodeID> &barrier_nodes,
                   const IntersectionEdgeGeometries &geometries,
                   const TurnLanesIndexedArray &turn_lanes_data,
                   const IntersectionEdge &from,
                   const IntersectionEdge &to)
{
    BOOST_ASSERT(graph.GetTarget(from.edge) == to.node);

    // TODO: to use TurnAnalysis all outgoing edges are required, to be removed later
    if (graph.GetEdgeData(from.edge).reversed || graph.GetEdgeData(to.edge).reversed)
        return false;

    const auto intersection_node = to.node;
    const auto destination_node = graph.GetTarget(to.edge);
    auto const &restrictions = restriction_map.Restrictions(from.node, intersection_node);

    // Check if turn is explicitly restricted by a turn restriction
    if (isTurnRestricted(restrictions, destination_node))
        return false;

    // Precompute reversed bearing of the `from` edge
    const auto from_edge_reversed_bearing =
        util::bearing::reverse(findEdgeBearing(geometries, from.edge));

    // Collect some information about the intersection
    // 1) number of allowed exits and adjacent bidirectional edges
    std::uint32_t allowed_exits = 0, bidirectional_edges = 0;
    // 2) edge IDs of roundabouts edges
    EdgeID roundabout_from = SPECIAL_EDGEID, roundabout_to = SPECIAL_EDGEID;
    double roundabout_from_angle = 0., roundabout_to_angle = 0.;

    for (const auto eid : graph.GetAdjacentEdgeRange(intersection_node))
    {
        const auto &edge_data = graph.GetEdgeData(eid);
        const auto &edge_class = edge_data.flags;
        const auto to_node = graph.GetTarget(eid);
        const auto reverse_edge = graph.FindEdge(to_node, intersection_node);
        BOOST_ASSERT(reverse_edge != SPECIAL_EDGEID);

        const auto is_exit_edge = !edge_data.reversed && !isTurnRestricted(restrictions, to_node);
        const auto is_bidirectional = !graph.GetEdgeData(reverse_edge).reversed;
        allowed_exits += is_exit_edge;
        bidirectional_edges += is_bidirectional;

        if (edge_class.roundabout || edge_class.circular)
        {
            if (edge_data.reversed)
            {
                // "Linked Roundabouts" is an example of tie between two linked roundabouts
                // A tie breaker for that maximizes ∠(roundabout_from_bearing, ¬from_edge_bearing)
                const auto angle = util::bearing::angleBetween(
                    findEdgeBearing(geometries, reverse_edge), from_edge_reversed_bearing);
                if (angle > roundabout_from_angle)
                {
                    roundabout_from = reverse_edge;
                    roundabout_from_angle = angle;
                }
            }
            else
            {
                // a tie breaker that maximizes ∠(¬from_edge_bearing, roundabout_to_bearing)
                const auto angle = util::bearing::angleBetween(from_edge_reversed_bearing,
                                                               findEdgeBearing(geometries, eid));
                if (angle > roundabout_to_angle)
                {
                    roundabout_to = eid;
                    roundabout_to_angle = angle;
                }
            }
        }
    }

    // 3) if the intersection has a barrier
    const bool is_barrier_node = barrier_nodes.find(intersection_node) != barrier_nodes.end();

    // Check a U-turn
    if (from.node == destination_node)
    {
        // Allow U-turns before barrier nodes
        if (is_barrier_node)
            return true;

        // Allow U-turns at dead-ends
        if (graph.GetAdjacentEdgeRange(intersection_node).size() == 1)
            return true;

        // Allow U-turns at dead-ends if there is at most one bidirectional road at the intersection
        // The condition allows U-turns d→a→d and c→b→c ("Bike - Around the Block" test)
        //   a→b
        //   ↕ ↕
        //   d↔c
        if (allowed_exits == 1 || bidirectional_edges <= 1)
            return true;

        // Allow U-turn if the incoming edge has a U-turn lane
        // TODO: revisit the use-case, related PR #2753
        const auto &incoming_edge_annotation_id = graph.GetEdgeData(from.edge).annotation_data;
        const auto lane_description_id = static_cast<std::size_t>(
            node_data_container.GetAnnotation(incoming_edge_annotation_id).lane_description_id);
        if (lane_description_id != INVALID_LANE_DESCRIPTIONID)
        {
            const auto &turn_lane_offsets = std::get<0>(turn_lanes_data);
            const auto &turn_lanes = std::get<1>(turn_lanes_data);
            BOOST_ASSERT(lane_description_id + 1 < turn_lane_offsets.size());

            if (std::any_of(turn_lanes.begin() + turn_lane_offsets[lane_description_id],
                            turn_lanes.begin() + turn_lane_offsets[lane_description_id + 1],
                            [](const auto &lane) { return lane & TurnLaneType::uturn; }))
                return true;
        }

        // Don't allow U-turns on usual intersections
        return false;
    }

    // Don't allow turns via barriers for not U-turn maneuvers
    if (is_barrier_node)
        return false;

    // Check for roundabouts exits in the opposite direction of roundabout flow
    if (roundabout_from != SPECIAL_EDGEID && roundabout_to != SPECIAL_EDGEID)
    {
        // Get bearings of edges
        const auto roundabout_from_bearing = findEdgeBearing(geometries, roundabout_from);
        const auto roundabout_to_bearing = findEdgeBearing(geometries, roundabout_to);
        const auto to_edge_bearing = findEdgeBearing(geometries, to.edge);

        // Get angles from the roundabout edge to three other edges
        const auto roundabout_angle =
            util::bearing::angleBetween(roundabout_from_bearing, roundabout_to_bearing);
        const auto roundabout_from_angle =
            util::bearing::angleBetween(roundabout_from_bearing, from_edge_reversed_bearing);
        const auto roundabout_to_angle =
            util::bearing::angleBetween(roundabout_from_bearing, to_edge_bearing);

        // Restrict turning over a roundabout if `roundabout_to_angle` is in
        // a sector between `roundabout_from_bearing` to `from_bearing` (shaded area)
        //
        //    roundabout_angle = 270°         roundabout_angle = 90°
        //  roundabout_from_angle = 150°    roundabout_from_angle = 150°
        //   roundabout_to_angle = 90°       roundabout_to_angle = 270°
        //
        //             150°                            150°
        //              v░░░░░░                ░░░░░░░░░v
        //             v░░░░░░░                ░░░░░░░░v
        // 270° <-ooo- v -ttt-> 90°       270° <-ttt- v -ooo-> 90°
        //             ^░░░░░░░                ░░░░░░░^
        //             r░░░░░░░                ░░░░░░░r
        //             r░░░░░░░                ░░░░░░░r
        if ((roundabout_from_angle < roundabout_angle &&
             roundabout_to_angle < roundabout_from_angle) ||
            (roundabout_from_angle > roundabout_angle &&
             roundabout_to_angle > roundabout_from_angle))
            return false;
    }

    return true;
}

// The function adapts intersection geometry data to TurnAnalysis
IntersectionView convertToIntersectionView(const util::NodeBasedDynamicGraph &graph,
                                           const EdgeBasedNodeDataContainer &node_data_container,
                                           const RestrictionMap &restriction_map,
                                           const std::unordered_set<NodeID> &barrier_nodes,
                                           const IntersectionEdgeGeometries &edge_geometries,
                                           const TurnLanesIndexedArray &turn_lanes_data,
                                           const IntersectionEdge &incoming_edge,
                                           const IntersectionEdges &outgoing_edges,
                                           const std::unordered_set<EdgeID> &merged_edges)
{
    using util::bearing::angleBetween;

    const auto edge_it = findEdge(edge_geometries, incoming_edge.edge);
    const auto incoming_bearing = edge_it->perceived_bearing;
    const auto initial_incoming_bearing = edge_it->initial_bearing;

    using IntersectionViewDataWithAngle = std::pair<IntersectionViewData, double>;
    std::vector<IntersectionViewDataWithAngle> pre_intersection_view;
    IntersectionViewData uturn{{SPECIAL_EDGEID, 0., 0.}, false, 0.};
    std::size_t allowed_uturns_number = 0;
    for (const auto &outgoing_edge : outgoing_edges)
    {
        const auto is_uturn = [](const auto angle) {
            return std::fabs(angle) < std::numeric_limits<double>::epsilon();
        };

        const auto edge_it = findEdge(edge_geometries, outgoing_edge.edge);
        const auto segment_length = edge_it->length;
        const auto is_merged = merged_edges.count(outgoing_edge.edge) != 0;
        const auto is_turn_allowed = intersection::isTurnAllowed(graph,
                                                                 node_data_container,
                                                                 restriction_map,
                                                                 barrier_nodes,
                                                                 edge_geometries,
                                                                 turn_lanes_data,
                                                                 incoming_edge,
                                                                 outgoing_edge);

        // Compute angles
        const auto outgoing_bearing = edge_it->perceived_bearing;
        const auto initial_outgoing_bearing = edge_it->initial_bearing;
        auto turn_angle = std::fmod(
            std::round(angleBetween(incoming_bearing, outgoing_bearing) * 1e8) / 1e8, 360.);
        auto initial_angle = angleBetween(initial_incoming_bearing, initial_outgoing_bearing);

        // If angle of the allowed turn is in a neighborhood of 0° (±15°) but the initial OSM angle
        // is in the opposite semi-plane then assume explicitly a U-turn to avoid incorrect
        // adjustments due to numerical noise in selection of representative_coordinate
        if (is_turn_allowed &&
            ((turn_angle < 15 && initial_angle > 180) || (turn_angle > 345 && initial_angle < 180)))
        {
            turn_angle = 0;
            initial_angle = 0;
        }

        const auto is_uturn_angle = is_uturn(turn_angle);

        IntersectionViewData road{
            {outgoing_edge.edge, outgoing_bearing, segment_length}, is_turn_allowed, turn_angle};

        if (graph.GetTarget(outgoing_edge.edge) == incoming_edge.node)
        { // Save the true U-turn road to add later if no allowed U-turns will be added
            uturn = road;
        }
        else if (is_turn_allowed || (!is_merged && !is_uturn_angle))
        { // Add roads that have allowed entry or not U-turns and not merged
            allowed_uturns_number += is_uturn_angle;

            // Adjust computed initial turn angle for non-U-turn road edge cases:
            // 1) use 0° or 360° if the road has 0° initial angle
            // 2) use turn angle if the smallest arc between turn and initial angles passes 0°
            const auto use_turn_angle = (turn_angle > 270 && initial_angle < 90) ||
                                        (turn_angle < 90 && initial_angle > 270);
            const auto adjusted_angle = is_uturn(initial_angle)
                                            ? (turn_angle > 180. ? 360. : 0.)
                                            : use_turn_angle ? turn_angle : initial_angle;
            pre_intersection_view.push_back({road, adjusted_angle});
        }
    }

    BOOST_ASSERT(uturn.eid != SPECIAL_EDGEID);
    if (uturn.entry_allowed || allowed_uturns_number == 0)
    { // Add the true U-turn if it is allowed or no other U-turns found
        pre_intersection_view.insert(pre_intersection_view.begin(), {uturn, 0});
    }

    // Order roads in counter-clockwise order starting from the U-turn edge in the OSM order
    std::stable_sort(pre_intersection_view.begin(),
                     pre_intersection_view.end(),
                     [](const auto &lhs, const auto &rhs) {
                         return std::tie(lhs.second, lhs.first.angle) <
                                std::tie(rhs.second, rhs.first.angle);
                     });

    // Adjust perceived bearings to keep the initial OSM order with respect to the first edge
    for (auto curr = pre_intersection_view.begin(), next = std::next(curr);
         next != pre_intersection_view.end();
         ++curr, ++next)
    {
        // Check that the perceived angles order is the same as the initial OSM one
        if (next->first.angle < curr->first.angle)
        { // If the true bearing is out of the initial order (next before current) then
            // adjust the next road angle to keep the order. The adjustment angle is at most
            // 0.5° or a half-angle between the current angle and 360° to prevent overlapping
            const auto angle_adjustment =
                std::min(.5, util::restrictAngleToValidRange(360. - curr->first.angle) / 2.);
            next->first.angle =
                util::restrictAngleToValidRange(curr->first.angle + angle_adjustment);
        }
    }

    // Copy intersection view data
    IntersectionView intersection_view;
    intersection_view.reserve(pre_intersection_view.size());
    std::transform(pre_intersection_view.begin(),
                   pre_intersection_view.end(),
                   std::back_inserter(intersection_view),
                   [](const auto &road) { return road.first; });

    return intersection_view;
}

//                                               a
//                                               |
//                                               |
//                                               v
// For an intersection from_node --via_eid--> turn_node ----> c
//                                               ^
//                                               |
//                                               |
//                                               b
// This functions returns _all_ turns as if the graph was undirected.
// That means we not only get (from_node, turn_node, c) in the above example
// but also (from_node, turn_node, a), (from_node, turn_node, b). These turns are
// marked as invalid and only needed for intersection classification.
template <bool USE_CLOSE_COORDINATE>
IntersectionView getConnectedRoads(const util::NodeBasedDynamicGraph &graph,
                                   const EdgeBasedNodeDataContainer &node_data_container,
                                   const std::vector<util::Coordinate> &node_coordinates,
                                   const extractor::CompressedEdgeContainer &compressed_geometries,
                                   const RestrictionMap &node_restriction_map,
                                   const std::unordered_set<NodeID> &barrier_nodes,
                                   const TurnLanesIndexedArray &turn_lanes_data,
                                   const IntersectionEdge &incoming_edge)
{
    const auto intersection_node = graph.GetTarget(incoming_edge.edge);
    const auto &outgoing_edges = intersection::getOutgoingEdges(graph, intersection_node);
    auto edge_geometries = getIntersectionOutgoingGeometries<USE_CLOSE_COORDINATE>(
        graph, compressed_geometries, node_coordinates, intersection_node);

    // Add incoming edges with reversed bearings
    const auto edges_number = edge_geometries.size();
    edge_geometries.resize(2 * edges_number);
    for (std::size_t index = 0; index < edges_number; ++index)
    {
        const auto &geometry = edge_geometries[index];
        const auto remote_node = graph.GetTarget(geometry.edge);
        const auto incoming_edge = graph.FindEdge(remote_node, intersection_node);
        edge_geometries[edges_number + index] = {incoming_edge,
                                                 util::bearing::reverse(geometry.initial_bearing),
                                                 util::bearing::reverse(geometry.perceived_bearing),
                                                 geometry.length};
    }

    // Enforce ordering of edges by IDs
    std::sort(edge_geometries.begin(), edge_geometries.end());

    return convertToIntersectionView(graph,
                                     node_data_container,
                                     node_restriction_map,
                                     barrier_nodes,
                                     edge_geometries,
                                     turn_lanes_data,
                                     incoming_edge,
                                     outgoing_edges,
                                     std::unordered_set<EdgeID>());
}

template IntersectionView
getConnectedRoads<false>(const util::NodeBasedDynamicGraph &graph,
                         const EdgeBasedNodeDataContainer &node_data_container,
                         const std::vector<util::Coordinate> &node_coordinates,
                         const extractor::CompressedEdgeContainer &compressed_geometries,
                         const RestrictionMap &node_restriction_map,
                         const std::unordered_set<NodeID> &barrier_nodes,
                         const TurnLanesIndexedArray &turn_lanes_data,
                         const IntersectionEdge &incoming_edge);

template IntersectionView
getConnectedRoads<true>(const util::NodeBasedDynamicGraph &graph,
                        const EdgeBasedNodeDataContainer &node_data_container,
                        const std::vector<util::Coordinate> &node_coordinates,
                        const extractor::CompressedEdgeContainer &compressed_geometries,
                        const RestrictionMap &node_restriction_map,
                        const std::unordered_set<NodeID> &barrier_nodes,
                        const TurnLanesIndexedArray &turn_lanes_data,
                        const IntersectionEdge &incoming_edge);

IntersectionEdge skipDegreeTwoNodes(const util::NodeBasedDynamicGraph &graph, IntersectionEdge road)
{
    std::unordered_set<NodeID> visited_nodes;
    (void)visited_nodes;

    // Skip trivial nodes without generating the intersection in between, stop at the very first
    // intersection of degree > 2
    const auto starting_node = road.node;
    auto next_node = graph.GetTarget(road.edge);
    while (graph.GetOutDegree(next_node) == 2 && next_node != starting_node)
    {
        BOOST_ASSERT(visited_nodes.insert(next_node).second);
        const auto next_edge = graph.BeginEdges(next_node);
        road.edge = graph.GetTarget(next_edge) == road.node ? next_edge + 1 : next_edge;
        road.node = next_node;
        next_node = graph.GetTarget(road.edge);
    }

    return road;
}
}
}
}


#include "pcl_ros/point_cloud.h"
#include <Eigen/Dense>
#include <dynamic_reconfigure/server.h>
#include <laser_geometry/laser_geometry.h>
#include <laserscan_merger/laserscan_mergerConfig.h>
#include <pcl/conversions.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/transforms.h>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h>
#include <string.h>
#include <tf/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

using namespace std;
using namespace pcl;
using namespace laserscan_merger;

class LaserscanMerger {
public:
  LaserscanMerger(ros::NodeHandle &n, tf2_ros::Buffer *tf);
  void scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan,
                    std::string topic);
  void pointcloud_to_laserscan(Eigen::MatrixXf points,
                               pcl::PCLPointCloud2 *merged_cloud);
  void reconfigureCallback(laserscan_mergerConfig &config, uint32_t level);

private:
  ros::NodeHandle node_;
  laser_geometry::LaserProjection projector_;
  tf2_ros::Buffer *tf_buffer_;
  // tf::TransformListener tf_listener_;

  ros::Publisher point_cloud_publisher_;
  ros::Publisher laser_scan_publisher_;
  vector<ros::Subscriber> scan_subscribers_;
  vector<bool> clouds_modified_;

  vector<pcl::PCLPointCloud2> clouds_;
  vector<string> input_topics_;

  void laserscan_topic_parser();

  double angle_min_;
  double angle_max_;
  double angle_increment_;
  double time_increment_;
  double scan_time_;
  double range_min_;
  double range_max_;

  string destination_frame_;
  string cloud_destination_topic_;
  string scan_destination_topic_;
  string laserscan_topics_;
};

void LaserscanMerger::reconfigureCallback(laserscan_mergerConfig &config,
                                          uint32_t level) {
  this->angle_min_ = config.angle_min;
  this->angle_max_ = config.angle_max;
  this->angle_increment_ = config.angle_increment;
  this->time_increment_ = config.time_increment;
  this->scan_time_ = config.scan_time;
  this->range_min_ = config.range_min;
  this->range_max_ = config.range_max;
}

LaserscanMerger::LaserscanMerger(ros::NodeHandle &n, tf2_ros::Buffer *tf) {
  ros::NodeHandle nh("~");
  node_ = n;
  tf_buffer_ = tf;
  // tf2_ros::TransformListener tf_listener(tf_buffer_);

  nh.param<std::string>("destination_frame", destination_frame_, "cart_frame");
  nh.param<std::string>("cloud_destination_topic", cloud_destination_topic_,
                        "/merged_cloud");
  nh.param<std::string>("scan_destination_topic", scan_destination_topic_,
                        "/scan_multi");
  nh.param<std::string>("laserscan_topics", laserscan_topics_, "");
  nh.param("angle_min", angle_min_, -2.36);
  nh.param("angle_max", angle_max_, 2.36);
  nh.param("angle_increment", angle_increment_, 0.0058);
  nh.param("scan_time", scan_time_, 0.0333333);
  nh.param("range_min", range_min_, 0.45);
  nh.param("range_max", range_max_, 25.0);

  this->laserscan_topic_parser();

  point_cloud_publisher_ = node_.advertise<sensor_msgs::PointCloud2>(
      cloud_destination_topic_.c_str(), 1, false);
  laser_scan_publisher_ = node_.advertise<sensor_msgs::LaserScan>(
      scan_destination_topic_.c_str(), 1, false);
}

void LaserscanMerger::laserscan_topic_parser() {
  // LaserScan topics to subscribe
  ros::master::V_TopicInfo topics;
  ros::master::getTopics(topics);

  istringstream iss(laserscan_topics_);
  vector<string> tokens;
  copy(istream_iterator<string>(iss), istream_iterator<string>(),
       back_inserter<vector<string>>(tokens));
  vector<string> tmp_input_topics;
  for (int i = 0; i < tokens.size(); ++i) {
    for (int j = 0; j < topics.size(); ++j) {
      if ((tokens[i].compare(topics[j].name) == 0) &&
          (topics[j].datatype.compare("sensor_msgs/LaserScan") == 0)) {
        tmp_input_topics.push_back(topics[j].name);
      }
    }
  }

  sort(tmp_input_topics.begin(), tmp_input_topics.end());
  std::vector<string>::iterator last =
      std::unique(tmp_input_topics.begin(), tmp_input_topics.end());
  tmp_input_topics.erase(last, tmp_input_topics.end());

  // Do not re-subscribe if the topics are the same
  if ((tmp_input_topics.size() != input_topics_.size()) ||
      !equal(tmp_input_topics.begin(), tmp_input_topics.end(),
             input_topics_.begin())) {

    // Unsubscribe from previous topics
    for (int i = 0; i < scan_subscribers_.size(); ++i)
      scan_subscribers_[i].shutdown();

    input_topics_ = tmp_input_topics;
    if (input_topics_.size() > 0) {
      scan_subscribers_.resize(input_topics_.size());
      clouds_modified_.resize(input_topics_.size());
      clouds_.resize(input_topics_.size());
      ROS_INFO("Subscribing to topics\t%ld", scan_subscribers_.size());
      for (int i = 0; i < input_topics_.size(); ++i) {
        scan_subscribers_[i] = node_.subscribe<sensor_msgs::LaserScan>(
            input_topics_[i].c_str(), 1,
            boost::bind(&LaserscanMerger::scanCallback, this, _1,
                        input_topics_[i]));
        clouds_modified_[i] = false;
        cout << input_topics_[i] << " ";
      }
    } else
      ROS_INFO("Not subscribed to any topic.");
  }
}

void LaserscanMerger::scanCallback(const sensor_msgs::LaserScan::ConstPtr &scan,
                                   std::string topic) {

  // Verify that TF knows how to transform from the received scan to the
  // destination scan frame
  /*tfListener_.waitForTransform(scan->header.frame_id.c_str(),
                               destination_frame_.c_str(), scan->header.stamp,
                               ros::Duration(1));
  projector_.transformLaserScanToPointCloud(
      scan->header.frame_id, *scan, tmpCloud1, tfListener_,
      laser_geometry::channel_option::Distance);
  try {
    tfListener_.transformPointCloud(destination_frame_.c_str(), tmpCloud1,
                                    tmpCloud2);
  } catch (tf::TransformException ex) {
    ROS_ERROR("%s", ex.what());
    return;
  }*/

  // Verify that TF knows how to transform from the received scan to the
  // destination scan frame
  // tf_buffer_.waitForTransform(destination_frame_.c_str(),
  // scan->header.frame_id.c_str(), scan->header.stamp, ros::Duration(1));
  // printf("Received topic %s in the frame %s\n", topic.c_str(),
  //       scan->header.frame_id.c_str());
  bool ok = true;
  try {
    bool ok = tf_buffer_->canTransform(
        destination_frame_.c_str(), scan->header.frame_id.c_str(),
        scan->header.stamp +
            ros::Duration().fromSec(scan->ranges.size() * scan->time_increment),
        ros::Duration(1.0));
  } catch (tf2::TransformException &ex) {
    ROS_ERROR("tranformException. Could NOT transform from %s to %s: %s",
              scan->header.frame_id.c_str(), destination_frame_.c_str(),
              ex.what());
    return;
  } catch (tf2::LookupException &ex) {
    ROS_ERROR("LookupException. Could NOT transform from %s to %s: %s",
              scan->header.frame_id.c_str(), destination_frame_.c_str(),
              ex.what());
  }
  if (!ok) {
    ROS_ERROR("Could NOT transform from %s to %s.",
              scan->header.frame_id.c_str(), destination_frame_.c_str());
    return;
  }

  // The concatenation of pointclouds (pcl::concatena) is giving me problems
  // because of different fiels in the pointclouds. And using the
  // channel_options in the transformLaserScanToPointCloud is not working
  // properly. So, I'm filling the intensity field in all the scans.
  sensor_msgs::LaserScan laser = *scan;
  if (scan->intensities.empty())
    laser.intensities.resize(scan->ranges.size(), 0.0);

  sensor_msgs::PointCloud2 cloud;
  // sensor_msgs::PointCloud cloud;
  if (scan->header.frame_id != destination_frame_) {
    try {
      projector_.transformLaserScanToPointCloud(
          destination_frame_, laser, cloud,
          *tf_buffer_); // laser_geometry::channel_option::Distance
      // | None | Intensity
      // projector_.transformLaserScanToPointCloud(destination_frame_, laser,
      //                                          cloud, tf_listener_);
    } catch (const std::exception &ex) {
      ROS_ERROR("[%s] Topic: %s. Error transforming laser to point cloud in "
                "other frame: %s",
                topic.c_str(), ros::this_node::getName().c_str(), ex.what());
      return;
    }
  } else {
    try {
      projector_.projectLaser(laser, cloud);
    } catch (const std::exception &ex) {
      ROS_ERROR("[%s] Error projecting laser to point cloud: %s",
                ros::this_node::getName().c_str(), ex.what());
      return;
    }
  }

  for (int i = 0; i < input_topics_.size(); ++i) {
    if (topic.compare(input_topics_[i]) == 0) {
      // sensor_msgs::convertPointCloudToPointCloud2(tmpCloud2, tmpCloud3);
      pcl_conversions::toPCL(cloud, clouds_[i]);
      clouds_modified_[i] = true;
    }
  }

  // Count how many scans we have
  int totalClouds = 0;
  for (int i = 0; i < clouds_modified_.size(); ++i)
    if (clouds_modified_[i])
      ++totalClouds;

  // Go ahead only if all subscribed scans have arrived
  if (totalClouds == clouds_modified_.size()) {
    pcl::PCLPointCloud2 merged_cloud = clouds_[0];
    clouds_modified_[0] = false;

    for (int i = 1; i < clouds_modified_.size(); ++i) {
      pcl::concatenatePointCloud(merged_cloud, clouds_[i],
                                 merged_cloud); // Deprecated

      // if (!pcl::concatenate(merged_cloud, clouds[i],
      // merged_cloud))
      //  ROS_ERROR("[%s] Concatenate pointclouds failed!",
      //            ros::this_node::getName().c_str());
      // merged_cloud += clouds_[i];
      clouds_modified_[i] = false;
    }

    point_cloud_publisher_.publish(merged_cloud);

    Eigen::MatrixXf points;
    getPointCloudAsEigen(merged_cloud, points);

    pointcloud_to_laserscan(points, &merged_cloud);
  }
}

void LaserscanMerger::pointcloud_to_laserscan(
    Eigen::MatrixXf points, pcl::PCLPointCloud2 *merged_cloud) {
  sensor_msgs::LaserScanPtr output(new sensor_msgs::LaserScan());
  output->header = pcl_conversions::fromPCL(merged_cloud->header);
  output->header.frame_id = destination_frame_.c_str();
  output->header.stamp = ros::Time::now(); // fixes #265
  output->angle_min = this->angle_min_;
  output->angle_max = this->angle_max_;
  output->angle_increment = this->angle_increment_;
  output->time_increment = this->time_increment_;
  output->scan_time = this->scan_time_;
  output->range_min = this->range_min_;
  output->range_max = this->range_max_;

  uint32_t ranges_size = std::ceil((output->angle_max - output->angle_min) /
                                   output->angle_increment);
  output->ranges.assign(ranges_size, output->range_max + 1.0);

  for (int i = 0; i < points.cols(); i++) {
    const float &x = points(0, i);
    const float &y = points(1, i);
    const float &z = points(2, i);

    if (std::isnan(x) || std::isnan(y) || std::isnan(z)) {
      ROS_DEBUG("rejected for nan in point(%f, %f, %f)\n", x, y, z);
      continue;
    }

    double range_sq = y * y + x * x;
    double range_min_sq_ = output->range_min * output->range_min;
    if (range_sq < range_min_sq_) {
      ROS_DEBUG(
          "rejected for range %f below minimum value %f. Point: (%f, %f, %f)",
          range_sq, range_min_sq_, x, y, z);
      continue;
    }

    double angle = atan2(y, x);
    if (angle < output->angle_min || angle > output->angle_max) {
      ROS_DEBUG("rejected for angle %f not in range (%f, %f)\n", angle,
                output->angle_min, output->angle_max);
      continue;
    }
    int index = (angle - output->angle_min) / output->angle_increment;

    if (output->ranges[index] * output->ranges[index] > range_sq)
      output->ranges[index] = sqrt(range_sq);
  }

  laser_scan_publisher_.publish(output);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "laser_multi_merger");

  ros::NodeHandle nh;

  tf2_ros::Buffer tf_buffer;
  tf2_ros::TransformListener tf_listener(tf_buffer);

  LaserscanMerger _laser_merger(nh, &tf_buffer);

  dynamic_reconfigure::Server<laserscan_mergerConfig> server;
  dynamic_reconfigure::Server<laserscan_mergerConfig>::CallbackType f;

  f = boost::bind(&LaserscanMerger::reconfigureCallback, &_laser_merger, _1,
                  _2);
  server.setCallback(f);

  ros::spin();

  return 0;
}