/* main.cpp - main entry point for the command-line tool `dm`
 *
 * The current implementation is specific to chains of integrators;
 * however, the design is intended to be general to facilitate later
 * treatment of other kinds of systems.
 *
 * While some infrastructure is in place for building without ROS, it
 * is as yet not operational.
 *
 *
 * This is part of https://fmrchallenge.org
 * This is free software, released under the Apache License, Version 2.0.
 * You may obtain a copy of the License at https://www.apache.org/licenses/LICENSE-2.0
 */

#ifdef USE_ROS
#include <ros/ros.h>
#include "integrator_chains_msgs/VectorStamped.h"
#include "integrator_chains_msgs/LabelStamped.h"
#include "integrator_chains_msgs/ProblemInstanceJSON.h"
#include "integrator_chains_msgs/DMMode.h"
#endif

#include "problem.hpp"

#include <iostream>
#include <fstream>
#include <string>
#include <utility>
#include <list>
#include <cstdlib>
#include <cstdio>
#include <errno.h>
#include <cassert>

#include <Eigen/Dense>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>

using namespace integrator_chains;

/** Flow computation for the problem domain: scaling chains of integrators.

   The trajectories are the solutions of a linear time-invariant control
   system. Control input is applied using a zero-order hold, i.e., applied
   constantly during the duration given as the first parameter of step().

   Let m be the highest order of derivative, and let n be the dimension of the
   output space.  The state variable indexing is such that the first output
   variable is the first state variable, the second output variable is the
   second state variable, etc. Thus the component systems are interleaved in
   the sense that the first subsystem is formed from state variable indices
   1, 1+n, 1+2n, ..., 1+(m-1)n, and the input to this subsystem is applied at
   state variable index 1+(m-1)n.

   \ingroup integrator_chains */
class TrajectoryGenerator {
public:
    TrajectoryGenerator( int numdim_output, int highest_order_deriv );

    /* Consult the class description regarding the assumed arrangement of the
       state variables, i.e., the indexing and the assumption that the system is
       a chain of integrators. */
    TrajectoryGenerator( Eigen::VectorXd Xinit, int numdim_output );

    void clear();

    /** Forward Euler integration for duration dt using constant input U */
    double step( double dt, const Eigen::VectorXd &U );

    double get_time() const
        { return t; }
    double get_state_dim() const
        { return X.size(); }
    double get_output_dim() const
        { return numdim_output; }

    /** Output vector */
    double operator[]( int i ) const;

    /** Access to full, current state vector */
    double get_state( int i ) const
        { return X[i]; }

private:
    Eigen::VectorXd X;
    double t;  // Current time
    int highest_order_deriv;
    int numdim_output;

    Eigen::VectorXd Xinit;
};

TrajectoryGenerator::TrajectoryGenerator( int numdim_output, int highest_order_deriv )
    : t(0.0),
      highest_order_deriv(highest_order_deriv), numdim_output(numdim_output)
{
    X.resize( numdim_output*highest_order_deriv );
    X.setZero();
    Xinit = X;
}

TrajectoryGenerator::TrajectoryGenerator( Eigen::VectorXd Xinit, int numdim_output  )
    : t(0.0), Xinit(Xinit), X(Xinit), numdim_output(numdim_output)
{
    assert( X.size() % numdim_output == 0 );
    highest_order_deriv = X.size()/numdim_output;
}

void TrajectoryGenerator::clear()
{
    X = Xinit;
    t = 0.0;
}

double TrajectoryGenerator::step( double dt, const Eigen::VectorXd &U )
{
    assert( U.size() > 0 );
    if (U.size() > numdim_output) {
        ROS_ERROR( "Input vector has too many elements." );
        return 0.0/0.0;
    }

    int numdim_state = X.size();

    int i, j;
    for (j = 0; j < numdim_output; j++) {
        for (i = 0; i <= highest_order_deriv-2; i++) {
            X[i*numdim_output+j] += dt*X[(i+1)*numdim_output+j];
        }
        X[(highest_order_deriv-1)*numdim_output+j] += dt*U[j];
    }

    t += dt;
    return t;
}

double TrajectoryGenerator::operator[]( int i ) const
{
    if (i < numdim_output) {
        return X[i];
    } else {
        ROS_ERROR( "Invalid index into output vector: %d", i );
        return 0.0/0.0;
    }
}

template <typename T>
class Queue {
public:
    void enqueue( int, T );
    std::pair<int, T> dequeue();
    int size();
    int peek_id();
    T peek();
private:
    boost::mutex mtx_;
    std::list< std::pair<int, T> > Q;
};

template <typename T>
void Queue<T>::enqueue( int k, T obj )
{
    mtx_.lock();
    Q.push_front( std::pair<int, T>( k, obj ) );
    mtx_.unlock();
}

template <typename T>
std::pair<int, T> Queue<T>::dequeue()
{
    assert( Q.size() > 0 );
    mtx_.lock();
    std::pair<int, T> x = Q.back();
    Q.pop_back();
    mtx_.unlock();
    return x;
}

template <typename T>
int Queue<T>::size()
{
    mtx_.lock();
    int sz = Q.size();
    mtx_.unlock();
    return sz;
}

template <typename T>
int Queue<T>::peek_id()
{
    assert( this->size() > 0 );
    mtx_.lock();
    int id = Q.back().first;
    mtx_.unlock();
    return id;
}

template <typename T>
T Queue<T>::peek()
{
    assert( this->size() > 0 );
    mtx_.lock();
    T value = Q.back().second;
    mtx_.unlock();
    return value;
}

/** \ingroup integrator_chains */
class TGThread {
public:
    TGThread( ros::NodeHandle &nh );
    ~TGThread();
    void inputcb( const integrator_chains_msgs::VectorStamped &vs );
    bool mode_request( integrator_chains_msgs::DMMode::Request &req,
                       integrator_chains_msgs::DMMode::Response &res );
    void run();
    void pubstate( const TrajectoryGenerator &tg, Eigen::VectorXd &Y );

    void inc_trial();
    int get_trial_number();

    /* Spin up a scribe thread to record trial data.

       The scribe thread is responsible for freeing each message in the state
       and problem instance queues. It can be terminated using stop_monitoring()

       Other assumptions: All of the following assumptions ignore the case of
       thread interruption, e.g., due to stop_monitoring() or ROS shutdown.
       Interruption causes the superseding assumption to be made that no more
       messages will arrive and everything from the queues is safe to delete.
       Upon arrival of an item associated with trial number t+1, all items
       associated with trial t are safe to be freed. The arrival of the problem
       instance description for trial t+1 implies that no more state nor input
       items will be enqueued for trial t. The first state message should not be
       recorded because it is redundant with the Xinit in the problem instance
       description. Thus ignoring the first state message, the numbers of inputs
       and remaining states for the current trial are equal unless the first
       input is empty (vector of zero length), in which case the controller has
       declared "unrealizable". For each trial, the problem instance and any
       compromising details should not be recorded until after the controller
       has begun the trial. The problem instance of trial t is safe to be freed
       by the scribe thread only if the problem instance of trial t+1 has
       arrived (consistent with an assumption above) or monitoring has ended. */
    void start_monitoring( std::string filename, bool append_mode=false );

    /* No-op if no scribe is active. */
    void stop_monitoring();

private:
    boost::mutex trial_mtx_;
    boost::mutex mtx_;
    bool fresh_input;
    Eigen::VectorXd U;

    Problem *probinstance;
    Labeler labeler;

    /* -1 indicates uninitialized. Counting begins at 0. */
    int trial_number;

    enum DMMode { ready, waiting, running, paused, resetting, restarting };
    DMMode dmmode;

    ros::NodeHandle &nh_;
    ros::ServiceServer mode_srv;
    ros::Publisher problemJSONpub;
    ros::Publisher statepub;
    ros::Publisher loutpub;
    ros::Subscriber inputsub;

    /* param_name is a name in the ROS Parameter Server. It should be a string
       of the form "J K", where the range will be parsed as [J, K]. Return true
       iff success. If return value is false, then the contents of \p range are
       undefined (and may have changed). */
    bool parse_range_str( const std::string param_name, Eigen::Vector2i &range );
    bool parse_range_str( const std::string param_name, Eigen::Vector2d &range );

    /* Like parse_range_str() but for arbitrarily long arrays. If
       expected_length is positive, then exactly that many values must be read
       for the call to be considered successful. Otherwise (default), continue
       to read values until no more are detected. */
    bool parse_array_str( const std::string &param_name, Eigen::VectorXd &array,
                          int expected_length=-1 );

    Queue<ros::Time> times_recording;
    Queue<Problem *> instances_recording;
    Queue<Eigen::VectorXd> inputs_recording;
    Queue<integrator_chains_msgs::VectorStamped *> states_recording;
    boost::thread *scribethread;
    void tgt_scribe( std::string filename, bool append_mode );
};

void TGThread::tgt_scribe( std::string filename, bool append_mode )
{
    std::ofstream outf( filename.c_str(),
                        std::ios_base::out
                        | (append_mode ?
                           std::ios_base::app : std::ios_base::trunc) );
    bool first_trial = true;
    std::pair<int, Problem *> tp( -1, nullptr );

    ros::Rate polling_rate( 1000 );
    while (get_trial_number() < 0 && ros::ok())
        polling_rate.sleep();

    ROS_INFO( "dynamaestro: Saving results to %s", filename.c_str() );
    outf << "\"trials\": [\n" << std::endl;
    while (!((get_trial_number() < 0 || !ros::ok()
              || boost::this_thread::interruption_requested())
             && instances_recording.size() == 0
             && states_recording.size() == 0
             && inputs_recording.size() == 0)) {

        if (instances_recording.size() == 0) {
            polling_rate.sleep();
            continue;
        }

        if (tp.second)
            delete tp.second;
        std::pair<int, Problem *> tp = instances_recording.dequeue();
        std::pair<int, ros::Time> tdurationt = times_recording.dequeue();
        assert( tdurationt.first == tp.first && tdurationt.second.nsec == 0 );

        // Wait for first state to be sure that it is safe to begin recording
        while (ros::ok() && get_trial_number() >= 0
               && states_recording.size() == 0
               && instances_recording.size() == 0)
            polling_rate.sleep();

        // Instance generated and queued but never revealed to the controller?
        if (states_recording.size() == 0 || states_recording.peek_id() != tp.first)
            continue;

        std::pair<int, ros::Time> tstartt = times_recording.dequeue();
        assert( tstartt.first == tp.first );

        if (first_trial) {
            first_trial = false;
        } else {
            outf << "," << std::endl;
        }
        outf << "{\n  \"duration\": " << tdurationt.second.sec << ",\n"
             << "  \"start_time\": [" << tstartt.second.sec
             << ", " << tstartt.second.nsec << "],\n"
             << "  \"problem_instance\": " << *tp.second;

        // Wait for first input to get realizability declaration
        while (ros::ok() && get_trial_number() >= 0
               && inputs_recording.size() == 0
               && instances_recording.size() == 0) {
            polling_rate.sleep();
        }

        // Did the controller time-out, and the dm progress to the next trial?
        if (inputs_recording.size() == 0 || inputs_recording.peek_id() != tp.first) {
            outf << "}" << std::endl;
            continue;
        }

        // Drop initial state because it is already in the instance description
        states_recording.dequeue();

        bool at_least_one = false;
        while ((inputs_recording.size() > 0
                && inputs_recording.peek_id() == tp.first)
               || (ros::ok() && get_trial_number() >= 0
                   && !boost::this_thread::interruption_requested()
                   && instances_recording.size() == 0)) {

            if (inputs_recording.size() == 0
                || (states_recording.size() == 0 && inputs_recording.peek().size() > 0)) {
                polling_rate.sleep();
                continue;
            }

            std::pair<int, Eigen::VectorXd> tinput = inputs_recording.dequeue();
            if (!at_least_one) {
                std::pair<int, ros::Time> tdecisiont = times_recording.dequeue();
                assert( tdecisiont.first == tinput.first );
                outf << ",\n  \"decision_time\": [" << tdecisiont.second.sec
                     << ", " << tdecisiont.second.nsec << "]," << std::endl;

                if (tinput.second.size() == 0) {
                    outf << "  \"realizable\": false" << std::endl;
                    break;
                } else {
                    outf << "  \"realizable\": true," << std::endl;
                }
                at_least_one = true;
                outf << "  \"trajectory\": [" << std::endl;

            } else {
                outf << "," << std::endl;
            }

            std::pair<int, integrator_chains_msgs::VectorStamped *> tstate = states_recording.dequeue();

            outf << "[" << tstate.second->header.stamp.sec
                 << ", " << tstate.second->header.stamp.nsec;
            for (int i = 0; i < tinput.second.size(); i++)
                outf << ", " << tinput.second(i);
            for (int i = 0; i < tstate.second->v.point.size(); i++)
                outf << ", " << tstate.second->v.point[i];
            outf << "]";
            delete tstate.second;

        }
        if (at_least_one)
            outf << "]" << std::endl;

        outf << "}";

    }
    outf << "\n]" << std::endl;
    outf.close();
    if (tp.second)
        delete tp.second;
}

void TGThread::start_monitoring( std::string filename, bool append_mode )
{
    assert( scribethread == nullptr );
    scribethread = new boost::thread( boost::bind( &TGThread::tgt_scribe,
                                                   this, _1, append_mode ),
                                      filename );
}

void TGThread::stop_monitoring()
{
    if (scribethread) {
        scribethread->interrupt();
        ROS_INFO( "dynamaestro: Waiting for scribe thread to finish..." );
        scribethread->join();
        delete scribethread;
        scribethread = nullptr;
    }
}

bool TGThread::parse_array_str( const std::string &param_name, Eigen::VectorXd &array,
                                int expected_length )
{
    // Try to handle special case of singletons
    if (expected_length <= 1) {
        double singleton;
        if (nh_.getParam( param_name, singleton )) {
            array.resize( 1 );
            array(0) = singleton;
            return true;
        }
    }
    if (expected_length <= 1) {
        int singleton;
        if (nh_.getParam( param_name, singleton )) {
            array.resize( 1 );
            array(0) = singleton;
            return true;
        }
    }

    std::list<double> buffer;
    std::string array_str;
    if (!nh_.getParam( param_name, array_str ))
        return false;

    if (expected_length > 0) {
        array.resize( expected_length );
    }

    double current_value;
    char *endptr;
    errno = 0;
    current_value = strtod( array_str.data(), &endptr );
    if (errno || array_str.data() == endptr) {
        std::cerr << "dynamaestro: Malformed array string in \""
                  << param_name << "\"" << std::endl;
        return false;
    }
    if (expected_length > 0) {
        array(0) = current_value;
    } else {
        buffer.push_back( current_value );
    }

    char *prev_endptr = nullptr;
    int read_count = 1;
    while (prev_endptr != endptr) {
        prev_endptr = endptr;
        errno = 0;
        current_value = strtod( prev_endptr, &endptr );
        if (errno) {
            std::cerr << "dynamaestro: Malformed array string in \""
                      << param_name << "\"" << std::endl;
            return false;
        }
        if (prev_endptr == endptr)
            break;

        if (expected_length > 0) {
            array(read_count) = current_value;
        } else {
            buffer.push_back( current_value );
        }

        read_count++;
        if (expected_length > 0 && read_count == expected_length)
            break;
    }

    if (expected_length <= 0) {
        array.resize( read_count );
        int i = 0;
        for (std::list<double>::iterator it_buff = buffer.begin();
             it_buff != buffer.end(); it_buff++) {
            array(i) = *it_buff;
            i++;
        }
    } else if (read_count != expected_length) {
        return false;
    }

    return true;
}

bool TGThread::parse_range_str( const std::string param_name, Eigen::Vector2i &range )
{
    std::string range_str;
    if (!nh_.getParam( param_name, range_str ))
        return false;

    char *endptr;
    errno = 0;
    range(0) = strtol( range_str.data(), &endptr, 10 );
    if (errno || endptr == range_str.data()) {
        std::cerr << "dynamaestro: Malformed range string in \""
                  << param_name << "\"" << std::endl;
        return false;
    }
    char *prev_endptr = endptr;
    errno = 0;
    range(1) = strtol( prev_endptr, &endptr, 10 );
    if (errno || endptr == prev_endptr) {
        std::cerr << "dynamaestro: Malformed range string in \""
                  << param_name << "\"" << std::endl;
        return false;
    }

    return true;
}

bool TGThread::parse_range_str( const std::string param_name, Eigen::Vector2d &range )
{
    std::string range_str;
    if (!nh_.getParam( param_name, range_str ))
        return false;

    char *endptr;
    errno = 0;
    range(0) = strtod( range_str.data(), &endptr );
    if (errno || endptr == range_str.data()) {
        std::cerr << "dynamaestro: Malformed range string in \""
                  << param_name << "\"" << std::endl;
        return false;
    }
    char *prev_endptr = endptr;
    errno = 0;
    range(1) = strtod( prev_endptr, &endptr );
    if (errno || endptr == prev_endptr) {
        std::cerr << "dynamaestro: Malformed range string in \""
                  << param_name << "\"" << std::endl;
        return false;
    }

    return true;
}

int TGThread::get_trial_number()
{
    trial_mtx_.lock();
    int x = trial_number;
    trial_mtx_.unlock();
    return x;
}

void TGThread::inc_trial()
{
    trial_mtx_.lock();
    trial_number += 1;
    trial_mtx_.unlock();
}

TGThread::TGThread( ros::NodeHandle &nh )
    : nh_(nh),
      fresh_input(false),
      U(),
      probinstance(nullptr),
      labeler(),
      trial_number(-1),
      dmmode(paused),
      scribethread(nullptr)
{
    mode_srv = nh_.advertiseService( "mode", &TGThread::mode_request, this );
    problemJSONpub = nh_.advertise<integrator_chains_msgs::ProblemInstanceJSON>( "probleminstance_JSON", 1, true );
    statepub = nh_.advertise<integrator_chains_msgs::VectorStamped>( "state", 10, true );
    loutpub = nh_.advertise<integrator_chains_msgs::LabelStamped>( "loutput", 10, true );
    inputsub = nh_.subscribe( "input", 1, &TGThread::inputcb, this );
}

TGThread::~TGThread()
{
    if (scribethread) {
        stop_monitoring();
    } else if (probinstance) {
        delete probinstance;
    }
}

void TGThread::pubstate( const TrajectoryGenerator &tg, Eigen::VectorXd &Y )
{
    integrator_chains_msgs::VectorStamped *pt = new integrator_chains_msgs::VectorStamped();
    pt->header.frame_id = std::string( "map" );
    pt->header.stamp = ros::Time::now();
    for (int i = 0; i < tg.get_state_dim(); i++) {
        pt->v.point.push_back( tg.get_state( i ) );
        if (i < tg.get_output_dim())
            Y(i) = tg[i];
    }
    statepub.publish( *pt );

    std::list<std::string> label = labeler.get_label( Y );
    integrator_chains_msgs::LabelStamped lbl;
    lbl.header.frame_id = pt->header.frame_id;
    lbl.header.stamp = pt->header.stamp;
    lbl.label.resize( label.size() );
    int i = 0;
    for (std::list<std::string>::iterator it_label = label.begin();
         it_label != label.end(); it_label++) {
        lbl.label[i] = *it_label;
        i++;
    }
    loutpub.publish( lbl );

    if (scribethread) {
        states_recording.enqueue( get_trial_number(), pt );
    } else {
        delete pt;
    }
}

bool TGThread::mode_request( integrator_chains_msgs::DMMode::Request &req,
                             integrator_chains_msgs::DMMode::Response &res )
{
    switch (req.mode) {
    case integrator_chains_msgs::DMMode::Request::UNPAUSE:
        if (dmmode != running)
            dmmode = running;
        res.result = true;
        break;

    case integrator_chains_msgs::DMMode::Request::READY:
        if (dmmode == ready) {
            res.result = true;
        } else {
            res.result = false;
        }
        break;

    case integrator_chains_msgs::DMMode::Request::START:
        if (dmmode == ready) {
            dmmode = waiting;
            res.result = true;
        } else {
            res.result = false;
        }
        break;

    case integrator_chains_msgs::DMMode::Request::PAUSE:
        if (dmmode == running) {
            dmmode = paused;
            fresh_input = false;
        }
        res.result = true;
        break;

    case integrator_chains_msgs::DMMode::Request::RESTART:
        dmmode = restarting;
        ROS_INFO( "integrator_chains_msgs: Restarting trial..." );
        res.result = true;
        break;

    case integrator_chains_msgs::DMMode::Request::RESET:
        dmmode = resetting;
        ROS_INFO( "dynamaestro: Stopping current trial and generating a new instance..." );
        res.result = true;
        break;

    default:
        return false;
    }

    return true;
}

void TGThread::inputcb( const integrator_chains_msgs::VectorStamped &vs )
{
    assert( probinstance != nullptr );

    mtx_.lock();

    if (vs.v.point.size() == 0) {
        U.resize( 0 );  // Controller declares trial as not realizable.
    } else if (U.size() > 0) {
        for (int i = 0; i < probinstance->get_numdim_output(); i++)
            U[i] = vs.v.point[i];
    }
    fresh_input = true;

    mtx_.unlock();
}

void TGThread::run()
{
    inc_trial();
    bool realizable;  // To be decided by the controller

    Eigen::Vector2i duration_bounds;
    if (!parse_range_str( "duration_bounds", duration_bounds )) {
        duration_bounds << 30, 90;
    }
    ROS_INFO( "dynamaestro: Using [%d, %d] as range of integers for sampling "
              "the trial duration.",
              duration_bounds(0), duration_bounds(1) );

    int nominal_duration = duration_bounds(0);
    if (duration_bounds(0) != duration_bounds(1))
        nominal_duration += std::rand() % (1+duration_bounds(1)-duration_bounds(0));

    Eigen::Vector2i numdim_output_bounds;
    if (!parse_range_str( "output_dim_bounds", numdim_output_bounds )) {
        numdim_output_bounds << 1, 3;
    }
    ROS_INFO( "dynamaestro: Using [%d, %d] as sampling range for number of "
              "dimensions of the output space.",
              numdim_output_bounds(0), numdim_output_bounds(1) );

    Eigen::Vector2i num_integrators_bounds;
    if (!parse_range_str( "number_integrators_bounds", num_integrators_bounds )) {
        num_integrators_bounds << 1, 3;
    }
    ROS_INFO( "dynamaestro: Using [%d, %d] as sampling range for number of "
              "integrators.",
              num_integrators_bounds(0), num_integrators_bounds(1) );

    Eigen::Vector2i number_goals_bounds;
    if (!parse_range_str( "number_goals_bounds", number_goals_bounds )) {
        number_goals_bounds << 1, 4;
    }
    ROS_INFO( "dynamaestro: Using [%d, %d] as sampling range for number of "
              "goal polytopes.",
              number_goals_bounds(0), number_goals_bounds(1) );

    Eigen::Vector2i number_obstacles_bounds;
    if (!parse_range_str( "number_obstacles_bounds", number_obstacles_bounds )) {
        number_obstacles_bounds << 0, 4;
    }
    ROS_INFO( "dynamaestro: Using [%d, %d] as sampling range for number of "
              "obstacle polytopes.",
              number_obstacles_bounds(0), number_obstacles_bounds(1) );

    Eigen::VectorXd Y_box;
    if (!parse_array_str( "Y", Y_box )
        || (Y_box.size() > 1 && Y_box.size() < numdim_output_bounds(1)*2)) {
        Y_box.resize( 2*numdim_output_bounds(1) );
        for (int i = 0; i < numdim_output_bounds(1); i++) {
            Y_box(2*i+1) = 10;
            Y_box(2*i) = -Y_box(2*i+1);
        }
    } else if (Y_box.size() == 1) {
        double Y_ref = Y_box(0);
        Y_box.resize( 2*numdim_output_bounds(1) );
        for (int i = 0; i < numdim_output_bounds(1); i++) {
            Y_box(2*i+1) = Y_ref;
            Y_box(2*i) = -Y_box(2*i+1);
        }
    }

    Eigen::VectorXd U_box;
    if (!parse_array_str( "U", U_box )
        || (U_box.size() > 1 && U_box.size() < numdim_output_bounds(1)*2)) {
        U_box.resize( 2*numdim_output_bounds(1) );
        for (int i = 0; i < numdim_output_bounds(1); i++) {
            U_box(2*i+1) = 1;
            U_box(2*i) = -U_box(2*i+1);
        }
    } else if (U_box.size() == 1) {
        double U_ref = U_box(0);
        U_box.resize( 2*numdim_output_bounds(1) );
        for (int i = 0; i < numdim_output_bounds(1); i++) {
            U_box(2*i+1) = U_ref;
            U_box(2*i) = -U_box(2*i+1);
        }
    }

    Eigen::VectorXd Xinit_bounds;

    Eigen::Vector2d period_bounds;
    if (!parse_range_str( "period_bounds", period_bounds )) {
        period_bounds << 0.01, 0.05;
    }
    ROS_INFO( "dynamaestro: Using [%f, %f] as sampling range for number of "
              "discretization period.",
              period_bounds(0), period_bounds(1) );

    probinstance = Problem::random( numdim_output_bounds,
                                    num_integrators_bounds,
                                    Y_box, U_box,
                                    number_goals_bounds,
                                    number_obstacles_bounds,
                                    period_bounds,
                                    Y_box );
    if (probinstance->get_highest_order_deriv() > 1)
        probinstance->Xinit.tail( (probinstance->get_highest_order_deriv()-1)*probinstance->get_numdim_output() ).setZero();
    labeler.importProblem( *probinstance );

    U.setZero( probinstance->get_numdim_output() );
    Eigen::VectorXd Y( probinstance->get_numdim_output() );

    TrajectoryGenerator tg( probinstance->Xinit,
                            probinstance->get_numdim_output() );

    ros::Rate rate( 1/probinstance->period );
    Eigen::VectorXd defaultU;
    defaultU.setZero( U.size() );

    if (scribethread) {
        times_recording.enqueue( get_trial_number(),
                                 ros::Time( nominal_duration, 0 ) );
        instances_recording.enqueue( get_trial_number(), probinstance );
    }

    ros::Duration trial_duration;

    ros::Rate polling_rate( 100 );

    /* For this single spinOnce() to successfully clear any remaining input, it
       must be that the Subscriber to /input has queue length of 1. */
    ros::spinOnce();
    fresh_input = false;

    dmmode = ready;
    while (dmmode == ready) {
        if (!ros::ok())
            return;
        ros::spinOnce();
        polling_rate.sleep();
    }
    assert( dmmode == waiting );

    integrator_chains_msgs::ProblemInstanceJSON probinstance_msg;
    probinstance_msg.stamp = ros::Time::now();
    probinstance_msg.problemjson = probinstance->dumpJSON();
    problemJSONpub.publish( probinstance_msg );
    ros::Time startt( probinstance_msg.stamp.sec,
                      probinstance_msg.stamp.nsec );
    if (scribethread)
        times_recording.enqueue( get_trial_number(), startt );

    nh_.setParam( "number_integrators",
                  probinstance->get_highest_order_deriv() );
    ROS_INFO( "dynamaestro: Using %d as number of integrators.",
              probinstance->get_highest_order_deriv() );

    nh_.setParam( "output_dim", probinstance->get_numdim_output() );
    ROS_INFO( "dynamaestro: Using %d as dimension of the output space.",
              probinstance->get_numdim_output() );

    nh_.setParam( "period", probinstance->period );
    ROS_INFO( "dynamaestro: Using %f seconds as the period.",
              probinstance->period );

    // Send initial state, before any input is applied or time has begun.
    pubstate( tg, Y );
    ros::spinOnce();

    bool first_input = true;
    while (ros::ok() && dmmode != resetting && (trial_duration = ros::Time::now() - startt).toSec() < nominal_duration) {
        if (dmmode == running) {
            if (fresh_input) {
                mtx_.lock();
                fresh_input = false;

                if (scribethread)
                    inputs_recording.enqueue( get_trial_number(), U );

                if (first_input) {
                    if (scribethread)
                        times_recording.enqueue( get_trial_number(),
                                                 ros::Time::now() );
                    first_input = false;
                    if (U.size() == 0) { // Controller declares "unrealizable"
                        mtx_.unlock();
                        realizable = false;
                        break;
                    } else {
                        realizable = true;
                    }
                }

                tg.step( probinstance->period, U );

                mtx_.unlock();
            } else {
                if (scribethread)
                    inputs_recording.enqueue( get_trial_number(), defaultU );
                tg.step( probinstance->period, defaultU );
            }

            pubstate( tg, Y );

        } else if (dmmode == restarting) {
            dmmode = paused;
            mtx_.lock();
            fresh_input = false;
            mtx_.unlock();
            tg.clear();
            U.setZero();
            pubstate( tg, Y );
            dmmode = waiting;
        } else if (dmmode == waiting) {
            if (fresh_input) {
                dmmode = running;
            }
        }
        ros::spinOnce();
        rate.sleep();
    }
    dmmode = resetting;
    integrator_chains_msgs::VectorStamped pt;
    // Send empty state message to indicate that trial has ended.
    statepub.publish( pt );

    // Handle special cases of trial termination:
    if (ros::ok() && trial_duration.toSec() >= nominal_duration) {
        // The trial end because the nominal (hidden) duration was reached
        ROS_INFO( "dynamaestro: Trial ended after %f s (threshold is %d s).",
                  trial_duration.toSec(), nominal_duration );
    } else if (ros::ok() && !first_input && !realizable) {
        ROS_INFO( "dynamaestro: Controller declared \"unrealizable\" after %f s (trial duration threshold is %d s).",
                  trial_duration.toSec(), nominal_duration );
    }

    // Clear registered instance
    ROS_INFO( "dynamaestro: Clearing instance summary from Parameter Server." );
    nh_.setParam( "number_integrators", -1 );
    nh_.setParam( "output_dim", -1 );
    nh_.setParam( "period", -1.0 );
    fresh_input = false;
    labeler.clear();
    if (scribethread == nullptr)
        delete probinstance;
    probinstance = nullptr;
}

void tgthread( ros::NodeHandle &nhp )
{
    TGThread tgt( nhp );
    std::string filename;
    if (nhp.getParam( "results_file", filename ))
        tgt.start_monitoring( filename, true );
    int number_trials = 0;
    bool counting_trials = nhp.getParam( "/number_trials", number_trials );
    if (counting_trials)
        ROS_INFO( "dynamaestro: Initiated with request for %d trials.",
                  number_trials );
    int trial_counter = 0;
    while ((!counting_trials || trial_counter < number_trials)
           && ros::ok()) {
        tgt.run();
        if (counting_trials)
            trial_counter++;
    }

    if (counting_trials && ros::ok()) {
        ROS_INFO( "dynamaestro: Completed %d trials.", trial_counter );
        tgt.stop_monitoring();
    }
}

int main( int argc, char **argv )
{
#ifdef USE_ROS
    ros::init( argc, argv, "dynamaestro" );
    ros::NodeHandle nh( "~" );

    time_t seed = time( nullptr );
    std::srand( seed );
    ROS_INFO( "dynamaestro: Using %ld as the PRNG seed.", seed );

    boost::thread tgmain( tgthread, nh );
    tgmain.join();
    nh.shutdown();

#endif
    return 0;
}


/* includes //{ */

#include <sdf/sdf.hh>

#include <ignition/gazebo/components/Actuators.hh>
#include <ignition/gazebo/components/Gravity.hh>
#include <ignition/gazebo/components/Inertial.hh>
#include <ignition/gazebo/components/Link.hh>
#include <ignition/gazebo/components/ParentEntity.hh>
#include <ignition/gazebo/components/World.hh>
#include <ignition/gazebo/Link.hh>
#include <ignition/gazebo/Model.hh>
#include <ignition/gazebo/System.hh>

#include <ignition/msgs/actuators.pb.h>
#include <ignition/msgs/twist.pb.h>

#include <ignition/transport/Node.hh>

#include <ignition/math/eigen3/Conversions.hh>
#include <ignition/math/Inertial.hh>
#include <ignition/math/Vector3.hh>

#include <ignition/common/Profiler.hh>

#include <ignition/plugin/Register.hh>

#include <Eigen/Geometry>
#include <memory>

#include <limits>

#include <Common.h>
#include <SE3Controller.h>

//}

/* using //{ */

using namespace ignition;
using namespace gazebo;
using namespace systems;
using namespace multicopter_control;

//}

/* defines //{ */

#define MAIN_THREAD_RATE 100.0

//}

/* class MRSMultirotorController //{ */

namespace ignition
{
namespace gazebo
{
inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE
{
namespace systems
{
class IGNITION_GAZEBO_VISIBLE MRSMultirotorController : public System, public ISystemConfigure, public ISystemPreUpdate {

public:
  MRSMultirotorController() = default;

  void Configure(const Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, EntityComponentManager &ecm, EventManager &_eventMgr) override;

  void PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &ecm) override;

private:
  bool is_initialized_ = false;

  // | ----------------------- parameters ----------------------- |

  math::Vector3d _maximum_linear_velocity_;
  math::Vector3d _maximum_angular_velocity_;

  std::string _robot_namespace_;
  std::string _cmd_vel_topic_{"cmd_vel"};
  std::string _enable_topic_{"enable"};
  std::string _feedforward_topic_{"feedforward"};

  std::string _gazebo_model_entity_name_;

  // noise parameters
  // noise to be added to the UAV states received from the simulator
  multicopter_control::NoiseParameters _noise_parameters_;

  // | -------------------- plugin interface -------------------- |

  void OnTwist(const msgs::Twist &msg);
  void OnEnable(const msgs::Boolean &msg);
  void PublishRotorVelocities(ignition::gazebo::EntityComponentManager &ecm, const Eigen::VectorXd &vels);

  // gazebo links and models
  Model  _gazebo_model_{kNullEntity};
  Entity _gazebo_model_entity_;

  transport::Node ignition_node_;

  Eigen::VectorXd rotor_velocities_;
  msgs::Actuators rotor_velocities_msg_;

  std::unique_ptr<multicopter_control::SE3Controller> multirotor_controller_ptr_;

  std::atomic<bool> is_active_{true};

  // --------------------------------------------------------------
  // |               subscriber to velocity command               |
  // --------------------------------------------------------------

  std::optional<msgs::Twist> cmd_vel_;
  std::mutex                 mutex_cmd_vel_;
  bool cmd_vel_received_ = false;

  // --------------------------------------------------------------
  // |            subscriber for feedforward reference            |
  // --------------------------------------------------------------

  void callbackFeedForward(const msgs::Twist &msg);

  std::optional<msgs::Twist> feedforward_;
  std::mutex                 mutex_feedforward_;
  bool feedforward_received_ = false;
};

}  // namespace systems
}  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
}  // namespace gazebo
}  // namespace ignition

//}

// | -------------------- plugin interface -------------------- |

/* Configure() //{ */

void MRSMultirotorController::Configure(const Entity &entity, const std::shared_ptr<const sdf::Element> &sdf, EntityComponentManager &ecm, EventManager &) {

  int    argc = 0;
  char **argv = NULL;

  ignmsg << "[mrs_multirotor_controller]: initializing" << std::endl;

  {  // gazebo model

    _gazebo_model_ = Model(entity);

    if (!_gazebo_model_.Valid(ecm)) {
      ignerr << "MRSMultirotorController plugin should be attached to a model "
             << "entity. Failed to initialize." << std::endl;
      return;
    }
  }

  auto sdf_clone = sdf->Clone();

  {  // get the com link name

    if (sdf_clone->HasElement("comLinkName")) {
      _gazebo_model_entity_name_ = sdf_clone->Get<std::string>("comLinkName");
    }

    if (_gazebo_model_entity_name_.empty()) {
      ignerr << "found an empty comLinkName parameter. Failed to initialize.\n";
      return;
    }
  }

  {  // get the link entity

    _gazebo_model_entity_ = _gazebo_model_.LinkByName(ecm, _gazebo_model_entity_name_);

    if (_gazebo_model_entity_ == kNullEntity) {
      ignerr << "Link " << _gazebo_model_entity_name_ << " could not be found. Failed to initialize.\n";
      return;
    }
  }

  createFrameDataComponents(ecm, _gazebo_model_entity_);

  VehicleParameters vehicleParams;

  math::Inertiald vehicleInertial;

  // Compute the vehicle's moment of inertia and mass assuming that all the
  // links in the model belong to the vehicle.
  for (const Entity &link : ecm.ChildrenByComponents(_gazebo_model_.Entity(), components::Link())) {
    auto inertial = ecm.Component<components::Inertial>(link);
    if (nullptr == inertial) {
      ignerr << "Could not find inertial component on on link " << _gazebo_model_entity_name_ << std::endl;
      return;
    }
    vehicleInertial += inertial->Data();
  }

  vehicleParams.mass    = vehicleInertial.MassMatrix().Mass();
  vehicleParams.inertia = math::eigen3::convert(vehicleInertial.Moi());

  if (sdf_clone->HasElement("rotorConfiguration")) {
    vehicleParams.rotorConfiguration = loadRotorConfiguration(ecm, sdf_clone->GetElement("rotorConfiguration"), _gazebo_model_, _gazebo_model_entity_);
  } else {
    ignerr << "Please specify rotorConfiguration.\n";
  }

  rotor_velocities_.resize(vehicleParams.rotorConfiguration.size());

  auto worldEntity = ecm.EntityByComponents(components::World());

  if (kNullEntity == worldEntity) {
    ignerr << "World entity missing." << std::endl;
    return;
  }

  // Get the world acceleration (defined in world frame)
  auto gravityComp = ecm.Component<components::Gravity>(worldEntity);

  if (nullptr == gravityComp) {
    ignerr << "World missing gravity." << std::endl;
    return;
  }

  vehicleParams.gravity = math::eigen3::convert(gravityComp->Data());

  SE3ControllerParameters controller_parameters;

  if (sdf_clone->HasElement("velocityGain")) {
    controller_parameters.velocity_gain = math::eigen3::convert(sdf_clone->Get<math::Vector3d>("velocityGain"));
  } else {
    ignerr << "Please specify velocityGain for MRSMultirotorController.\n";
    return;
  }

  if (sdf_clone->HasElement("attitudeGain")) {
    controller_parameters.attitude_gain = math::eigen3::convert(sdf_clone->Get<math::Vector3d>("attitudeGain"));
  } else {
    ignerr << "Please specify attitudeGain for MRSMultirotorController.\n";
    return;
  }

  if (sdf_clone->HasElement("angularRateGain")) {
    controller_parameters.angular_rate_gain = math::eigen3::convert(sdf_clone->Get<math::Vector3d>("angularRateGain"));
  } else {
    ignerr << "Please specify angularRateGain MRSMultirotorController.\n";
    return;
  }

  if (sdf_clone->HasElement("maximumLinearAcceleration")) {
    controller_parameters.max_linear_acceleration = math::eigen3::convert(sdf_clone->Get<math::Vector3d>("maximumLinearAcceleration"));
  } else {
    controller_parameters.max_linear_acceleration.setConstant(std::numeric_limits<double>::max());
  }

  if (sdf_clone->HasElement("_maximum_linear_velocity_")) {
    _maximum_linear_velocity_ = sdf_clone->Get<math::Vector3d>("_maximum_linear_velocity_").Abs();
  } else {
    _maximum_linear_velocity_.Set(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
  }

  if (sdf_clone->HasElement("_maximum_angular_velocity_")) {
    _maximum_angular_velocity_ = sdf_clone->Get<math::Vector3d>("_maximum_angular_velocity_").Abs();
  } else {
    _maximum_angular_velocity_.Set(std::numeric_limits<double>::max(), std::numeric_limits<double>::max(), std::numeric_limits<double>::max());
  }

  multirotor_controller_ptr_ = std::make_unique<multicopter_control::SE3Controller>(controller_parameters, vehicleParams);

  if (nullptr == multirotor_controller_ptr_) {
    ignerr << "Error while creating the LeeVelocityController\n";
    return;
  }

  math::Vector3d linearVelocityMean{0, 0, 0};
  sdf_clone->Get<math::Vector3d>("linearVelocityNoiseMean", linearVelocityMean, linearVelocityMean);

  math::Vector3d linearVelocityStdDev{0, 0, 0};
  sdf_clone->Get<math::Vector3d>("linearVelocityNoiseStdDev", linearVelocityStdDev, linearVelocityStdDev);

  math::Vector3d angularVelocityMean{0, 0, 0};
  sdf_clone->Get<math::Vector3d>("angularVelocityNoiseMean", angularVelocityMean, angularVelocityMean);

  math::Vector3d angularVelocityStdDev{0, 0, 0};
  sdf_clone->Get<math::Vector3d>("angularVelocityNoiseStdDev", angularVelocityStdDev, angularVelocityStdDev);

  _noise_parameters_.linearVelocityMean    = math::eigen3::convert(linearVelocityMean);
  _noise_parameters_.linearVelocityStdDev  = math::eigen3::convert(linearVelocityStdDev);
  _noise_parameters_.angularVelocityMean   = math::eigen3::convert(angularVelocityMean);
  _noise_parameters_.angularVelocityStdDev = math::eigen3::convert(angularVelocityStdDev);

  if (sdf_clone->HasElement("robotNamespace")) {
    _robot_namespace_ = transport::TopicUtils::AsValidTopic(sdf_clone->Get<std::string>("robotNamespace"));
    if (_robot_namespace_.empty()) {
      ignerr << "Robot namespace [" << sdf_clone->Get<std::string>("robotNamespace") << "] is invalid." << std::endl;
      return;
    }
  } else {
    ignerr << "Please specify a robotNamespace.\n";
    return;
  }

  sdf_clone->Get<std::string>("commandSubTopic", _cmd_vel_topic_, _cmd_vel_topic_);
  _cmd_vel_topic_ = transport::TopicUtils::AsValidTopic(_cmd_vel_topic_);
  if (_cmd_vel_topic_.empty()) {
    ignerr << "Invalid command sub-topic." << std::endl;
    return;
  }

  sdf_clone->Get<std::string>("enableSubTopic", _enable_topic_, _enable_topic_);
  _enable_topic_ = transport::TopicUtils::AsValidTopic(_enable_topic_);
  if (_enable_topic_.empty()) {
    ignerr << "Invalid enable sub-topic." << std::endl;
    return;
  }

  // Subscribe to actuator command messages
  std::string topic{_robot_namespace_ + "/" + _cmd_vel_topic_};

  ignition_node_.Subscribe(topic, &MRSMultirotorController::OnTwist, this);
  ignmsg << "MRSMultirotorController subscribing to Twist messages on [" << topic << "]" << std::endl;

  std::string enableTopic{_robot_namespace_ + "/" + _enable_topic_};
  ignition_node_.Subscribe(enableTopic, &MRSMultirotorController::OnEnable, this);
  ignmsg << "MRSMultirotorController subscribing to Boolean messages on [" << enableTopic << "]" << std::endl;

  // Create the Actuators component to take control of rotor speeds
  rotor_velocities_msg_.mutable_velocity()->Resize(rotor_velocities_.size(), 0);

  ecm.CreateComponent(_gazebo_model_.Entity(), components::Actuators(rotor_velocities_msg_));

  // --------------------------------------------------------------
  // |                   custom Ignition subscribers                   |
  // --------------------------------------------------------------

  std::string feedforwardTopic{_robot_namespace_ + "/" + _feedforward_topic_};
  ignition_node_.Subscribe(feedforwardTopic, &MRSMultirotorController::callbackFeedForward, this);
  ignmsg << "MRSMultirotorController subscribing to Twist messages on [" << feedforwardTopic << "]" << std::endl;

  // | ---------------- finish the initialization --------------- |

  ignmsg << "[mrs_multirotor_controller]: initialized" << std::endl;

  is_initialized_ = true;
}

//}

/* PreUpdate() //{ */

void MRSMultirotorController::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &ecm) {

  IGN_PROFILE("MRSMultirotorController::PreUpdate");

  if (!is_initialized_) {
    return;
  }

  msgs::Twist cmd_vel;

  {
    std::scoped_lock lock(mutex_cmd_vel_);

    if (!cmd_vel_.has_value()) {
      return;
    }

    cmd_vel = cmd_vel_.value();
  }

  msgs::Twist feedforward;

  {
    std::scoped_lock lock(mutex_feedforward_);

    if (feedforward_.has_value()) {
      feedforward = feedforward_.value();
    }
  }

  if (_info.dt < std::chrono::steady_clock::duration::zero()) {
    ignwarn << "Detected jump back in time [" << std::chrono::duration_cast<std::chrono::seconds>(_info.dt).count() << "s]. System may not work properly."
            << std::endl;
  }

  // nothing left to do if paused.
  if (_info.paused) {
    return;
  }

  if (!is_active_) {

    // if the last published rotor velocities were not 0, publish zero velocities
    if (rotor_velocities_.squaredNorm() > 0) {
      rotor_velocities_.setZero();
      this->PublishRotorVelocities(ecm, rotor_velocities_);

      // clear the cmd_vel_ so that the system waits for a new command after being renabled.
      std::lock_guard<std::mutex> lock(mutex_cmd_vel_);
      cmd_vel_.reset();
    }

    return;
  }

  // | ------------- prepare the velocity reference ------------- |

  EigenTwist cmd_vel_eigen;

  // saturate to maximum allowed velocity
  math::Vector3d linear = msgs::Convert(cmd_vel.linear());
  linear.Min(_maximum_linear_velocity_);
  linear.Max(-_maximum_linear_velocity_);

  // saturate to maximum allowed angular velocity
  math::Vector3d angular = msgs::Convert(cmd_vel.angular());
  angular.Min(_maximum_angular_velocity_);
  angular.Max(-_maximum_angular_velocity_);

  cmd_vel_eigen.linear  = math::eigen3::convert(linear);
  cmd_vel_eigen.angular = math::eigen3::convert(angular);

  // | ------------ prepare the feedforward reference ----------- |

  SE3ControllerFeedforward cmd_feedforward;

  cmd_feedforward.acceleration[0] = feedforward.linear().x();
  cmd_feedforward.acceleration[1] = feedforward.linear().y();
  cmd_feedforward.acceleration[2] = feedforward.linear().z();

  cmd_feedforward.jerk[0] = feedforward.angular().x();
  cmd_feedforward.jerk[1] = feedforward.angular().y();
  cmd_feedforward.jerk[2] = feedforward.angular().z();

  // | ------------ get the UAV model simulator data ------------ |

  std::optional<FrameData> frameData = getFrameData(ecm, _gazebo_model_entity_, _noise_parameters_);

  if (!frameData.has_value()) {
    return;
  }

  // | -------------- calculate the control action -------------- |

  rotor_velocities_ = multirotor_controller_ptr_->CalculateRotorVelocities(frameData.value(), cmd_vel_eigen, cmd_feedforward);

  // publish the control action
  PublishRotorVelocities(ecm, rotor_velocities_);
}

//}

/* OnTwist() //{ */

void MRSMultirotorController::OnTwist(const msgs::Twist &msg) {

  {
    std::scoped_lock lock(mutex_cmd_vel_);

    cmd_vel_ = msg;
  }

  if (!cmd_vel_received_) {
    ignmsg << "[mrs_multirotor_controller]: MRS controller at work" << std::endl;
    cmd_vel_received_ = true;
  }
}

//}

/* OnEnabled() //{ */

void MRSMultirotorController::OnEnable(const msgs::Boolean &msg) {

  is_active_ = msg.data();
}

//}

/* PublishRotorVelocities() //{ */

void MRSMultirotorController::PublishRotorVelocities(ignition::gazebo::EntityComponentManager &ecm, const Eigen::VectorXd &vels) {

  // check the size of the message
  if (vels.size() != rotor_velocities_msg_.velocity_size()) {
    rotor_velocities_msg_.mutable_velocity()->Resize(vels.size(), 0);
  }

  // fill in the velocities
  for (int i = 0; i < vels.size(); ++i) {
    rotor_velocities_msg_.set_velocity(i, vels(i));
  }

  // Publish the message by setting the Actuators component on the model entity.
  // This assumes that the MulticopterMotorModel system is attached to this model
  auto actuatorMsgComp = ecm.Component<components::Actuators>(_gazebo_model_.Entity());

  if (actuatorMsgComp) {

    auto compFunc = [](const msgs::Actuators &_a, const msgs::Actuators &_b) {
      return std::equal(_a.velocity().begin(), _a.velocity().end(), _b.velocity().begin());
    };

    auto state = actuatorMsgComp->SetData(rotor_velocities_msg_, compFunc) ? ComponentState::PeriodicChange : ComponentState::NoChange;

    ecm.SetChanged(_gazebo_model_.Entity(), components::Actuators::typeId, state);

  } else {
    ecm.CreateComponent(_gazebo_model_.Entity(), components::Actuators(rotor_velocities_msg_));
  }
}

//}

// | --------------------- other routines --------------------- |

/* callbackFeedForward() //{ */

void MRSMultirotorController::callbackFeedForward(const msgs::Twist &msg) {

  if (!is_initialized_) {
    return;
  }

  {
    std::scoped_lock lock(mutex_feedforward_);

    feedforward_ = msg;
  }

  if (!feedforward_received_) {
    ignmsg << "[mrs_multirotor_controller]: getting feedforward reference"
      << std::endl;
    feedforward_received_ = true;
  }
}

//}

//}

IGNITION_ADD_PLUGIN(MRSMultirotorController, ignition::gazebo::System, MRSMultirotorController::ISystemConfigure, MRSMultirotorController::ISystemPreUpdate)

IGNITION_ADD_PLUGIN_ALIAS(MRSMultirotorController, "ignition::gazebo::systems::MRSMultirotorController")


#define BOOST_TEST_MODULE initial

#include "milecsa.hpp"
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE( Equal )
{

    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(1.31f) << std::endl;
    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(1.301f) << std::endl;
    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(0.1f) << std::endl;
    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(1.1f) << std::endl;
    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(1.01f) << std::endl;
    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(1.001f) << std::endl;
    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(1.99f) << std::endl;
    std::cerr << "XDR " << milecsa::assets::XDR.value_to_string(1.999f) << std::endl;

    std::cerr << "MILE " << milecsa::assets::MILE.value_to_string(0.00001f) << std::endl;
    std::cerr << "MILE " << milecsa::assets::MILE.value_to_string(0.000001f) << std::endl;
    std::cerr << "MILE " << milecsa::assets::MILE.value_to_string(1.000001f) << std::endl;
    std::cerr << "MILE " << milecsa::assets::MILE.value_to_string(9.99999f) << std::endl;
    std::cerr << "MILE " << milecsa::assets::MILE.value_to_string(9.999999f) << std::endl;

    std::cerr << "NILL " << milecsa::assets::NILL.value_to_string(0.00001f) << std::endl;
    std::cerr << "NILL " << milecsa::assets::NILL.value_to_string(1000000.00001f) << std::endl;

    std::cerr << "Token from code 0: " << milecsa::assets::TokenFromCode(0).name << std::endl;
    std::cerr << "Token from code 1: " << milecsa::assets::TokenFromCode(1).name << std::endl;
    std::cerr << "Token from code 2: " << milecsa::assets::TokenFromCode(2).name << std::endl;

    BOOST_CHECK_EQUAL(milecsa::assets::TokenFromCode(milecsa::assets::XDR.code).code, milecsa::assets::XDR.code);
    BOOST_CHECK_EQUAL(milecsa::assets::TokenFromCode(milecsa::assets::MILE.code).code, milecsa::assets::MILE.code);
    BOOST_CHECK_EQUAL(milecsa::assets::TokenFromCode(10).code, milecsa::assets::NILL.code);
    BOOST_CHECK_EQUAL(milecsa::assets::XDR.value_to_string(1.10001f), "1.10");
    BOOST_CHECK_EQUAL(milecsa::assets::XDR.value_to_string(1.10901f), "1.10");
    BOOST_CHECK_EQUAL(milecsa::assets::XDR.value_to_string(1.99901f), "1.99");

    BOOST_CHECK_EQUAL(milecsa::assets::MILE.value_to_string(0.00001f), "0.00001");
    BOOST_CHECK_EQUAL(milecsa::assets::MILE.value_to_string(1.00001f), "1.00001");
    BOOST_CHECK_EQUAL(milecsa::assets::MILE.value_to_string(1.99999f), "1.99999");
    BOOST_CHECK_EQUAL(milecsa::assets::MILE.value_to_string(1.999999f),"1.99999");
    BOOST_CHECK_EQUAL(milecsa::assets::MILE.value_to_string(0.1f+0.2f),"0.30000");
    BOOST_CHECK_EQUAL(milecsa::assets::MILE.value_to_string(.29f*100.0f),"29.00000");
    BOOST_CHECK_EQUAL(milecsa::assets::MILE.value_to_string(0.000011f*10.0f),"0.00011");
}


#include "drake/solvers/fbstab/components/dense_variable.h"

#include <cmath>
#include <memory>
#include <stdexcept>

#include <Eigen/Dense>

#include "drake/solvers/fbstab/components/dense_data.h"

namespace drake {
namespace solvers {
namespace fbstab {

using MatrixXd = Eigen::MatrixXd;
using VectorXd = Eigen::VectorXd;

DenseVariable::DenseVariable(int nz, int nv) {
  if (nz <= 0 || nv <= 0) {
    throw std::runtime_error(
        "Inputs nz and nv to DenseVariable::DenseVariable must be positive.");
  }
  nz_ = nz;
  nv_ = nv;

  z_storage_ = std::make_unique<VectorXd>(nz_);
  v_storage_ = std::make_unique<VectorXd>(nv_);
  y_storage_ = std::make_unique<VectorXd>(nv_);

  z_ = z_storage_.get();
  v_ = v_storage_.get();
  y_ = y_storage_.get();
}

DenseVariable::DenseVariable(VectorXd* z, VectorXd* v, VectorXd* y) {
  if (z == nullptr || v == nullptr || y == nullptr) {
    throw std::runtime_error(
        "DenseVariable::DenseVariable requires non-null pointers.");
  }
  if (y->size() != v->size()) {
    throw std::runtime_error(
        "In DenseVariable::DenseVariable: v and y input size mismatch");
  }
  nz_ = z->size();
  nv_ = v->size();

  if (nz_ == 0 || nv_ == 0) {
    throw std::runtime_error(
        "Inputs to DenseVariable::DenseVariable must have nonzero sizes.");
  }
  z_ = z;
  v_ = v;
  y_ = y;
}

void DenseVariable::Fill(double a) {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "Cannot call DenseVariable::Fill unless data is linked.");
  }
  z_->setConstant(a);
  v_->setConstant(a);

  // Compute y = b - A*z
  if (a == 0.0) {
    *y_ = data_->b();
  } else {
    InitializeConstraintMargin();
  }
}

void DenseVariable::InitializeConstraintMargin() {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "Cannot call DenseVariable::InitializeConstraintMargin unless data is "
        "linked.");
  }
  y_->noalias() = data_->b() - data_->A() * (*z_);
}

void DenseVariable::axpy(double a, const DenseVariable& x) {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "Cannot call DenseVariable::axpy unless data is linked.");
  }
  (*z_) += a * x.z();
  (*v_) += a * x.v();
  (*y_) += a * (x.y() - data_->b());
}

void DenseVariable::Copy(const DenseVariable& x) {
  if (nz_ != x.nz_ || nv_ != x.nv_) {
    throw std::runtime_error("Sizes not equal in DenseVariable::Copy");
  }
  (*z_) = x.z();
  (*v_) = x.v();
  (*y_) = x.y();
  data_ = x.data_;
}

const DenseData* DenseVariable::data() const {
  if (data_ == nullptr) {
    throw std::runtime_error(
        "In DenseData::data: pointer to data requested before being assigned.");
  }

  return data_;
}
void DenseVariable::ProjectDuals() { *v_ = v_->cwiseMax(0); }

double DenseVariable::Norm() const {
  double t1 = z_->norm();
  double t2 = v_->norm();
  return sqrt(t1 * t1 + t2 * t2);
}

}  // namespace fbstab
}  // namespace solvers
}  // namespace drake


/****************************************************************************
 * Copyright (c) 2012-2019 by the ArborX authors                            *
 * All rights reserved.                                                     *
 *                                                                          *
 * This file is part of the ArborX library. ArborX is                       *
 * distributed under a BSD 3-clause license. For the licensing terms see    *
 * the LICENSE file in the top-level directory.                             *
 *                                                                          *
 * SPDX-License-Identifier: BSD-3-Clause                                    *
 ****************************************************************************/

#include <ArborX_BoostRTreeHelpers.hpp>
#include <ArborX_LinearBVH.hpp>

#include <Kokkos_Core.hpp>

#include <boost/program_options.hpp>

#include <chrono>
#include <cmath> // cbrt
#include <cstdlib>
#include <random>

#include <benchmark/benchmark.h>
#include <point_clouds.hpp>

#if defined(KOKKOS_ENABLE_SERIAL)
class BoostRTree
{
public:
  using DeviceType = Kokkos::Device<Kokkos::Serial, Kokkos::HostSpace>;
  using device_type = DeviceType;

  BoostRTree(Kokkos::View<ArborX::Point *, DeviceType> points)
  {
    _tree = BoostRTreeHelpers::makeRTree(points);
  }

  template <typename Query>
  void query(Kokkos::View<Query *, DeviceType> queries,
             Kokkos::View<int *, DeviceType> &indices,
             Kokkos::View<int *, DeviceType> &offset)
  {
    std::tie(offset, indices) =
        BoostRTreeHelpers::performQueries(_tree, queries);
  }

  template <typename Query>
  void query(Kokkos::View<Query *, DeviceType> queries,
             Kokkos::View<int *, DeviceType> &indices,
             Kokkos::View<int *, DeviceType> &offset, int)
  {
    std::tie(offset, indices) =
        BoostRTreeHelpers::performQueries(_tree, queries);
  }

private:
  BoostRTreeHelpers::RTree<ArborX::Point> _tree;
};
#endif

template <typename DeviceType>
Kokkos::View<ArborX::Point *, DeviceType>
constructPoints(int n_values, PointCloudType point_cloud_type)
{
  Kokkos::View<ArborX::Point *, DeviceType> random_points(
      Kokkos::ViewAllocateWithoutInitializing("random_points"), n_values);
  // Generate random points uniformely distributed within a box.  The edge
  // length of the box chosen such that object density (here objects will be
  // boxes 2x2x2 centered around a random point) will remain constant as
  // problem size is changed.
  auto const a = std::cbrt(n_values);
  generatePointCloud(point_cloud_type, a, random_points);

  return random_points;
}

template <typename DeviceType>
Kokkos::View<ArborX::Nearest<ArborX::Point> *, DeviceType>
makeNearestQueries(int n_values, int n_queries, int n_neighbors,
                   PointCloudType target_point_cloud_type)
{
  Kokkos::View<ArborX::Point *, DeviceType> random_points(
      Kokkos::ViewAllocateWithoutInitializing("random_points"), n_queries);
  auto const a = std::cbrt(n_values);
  generatePointCloud(target_point_cloud_type, a, random_points);

  Kokkos::View<ArborX::Nearest<ArborX::Point> *, DeviceType> queries(
      Kokkos::ViewAllocateWithoutInitializing("queries"), n_queries);
  using ExecutionSpace = typename DeviceType::execution_space;
  Kokkos::parallel_for(
      "bvh_driver:setup_knn_search_queries",
      Kokkos::RangePolicy<ExecutionSpace>(0, n_queries), KOKKOS_LAMBDA(int i) {
        queries(i) =
            ArborX::nearest<ArborX::Point>(random_points(i), n_neighbors);
      });
  Kokkos::fence();
  return queries;
}

template <typename DeviceType>
Kokkos::View<ArborX::Within *, DeviceType>
makeSpatialQueries(int n_values, int n_queries, int n_neighbors,
                   PointCloudType target_point_cloud_type)
{
  Kokkos::View<ArborX::Point *, DeviceType> random_points(
      Kokkos::ViewAllocateWithoutInitializing("random_points"), n_queries);
  auto const a = std::cbrt(n_values);
  generatePointCloud(target_point_cloud_type, a, random_points);

  Kokkos::View<ArborX::Within *, DeviceType> queries(
      Kokkos::ViewAllocateWithoutInitializing("queries"), n_queries);
  // radius chosen in order to control the number of results per query
  // NOTE: minus "1+sqrt(3)/2 \approx 1.37" matches the size of the boxes
  // inserted into the tree (mid-point between half-edge and half-diagonal)
  double const r =
      2. * std::cbrt(static_cast<double>(n_neighbors) * 3. / (4. * M_PI)) -
      (1. + std::sqrt(3.)) / 2.;
  using ExecutionSpace = typename DeviceType::execution_space;
  Kokkos::parallel_for("bvh_driver:setup_radius_search_queries",
                       Kokkos::RangePolicy<ExecutionSpace>(0, n_queries),
                       KOKKOS_LAMBDA(int i) {
                         queries(i) = ArborX::within(random_points(i), r);
                       });
  Kokkos::fence();
  return queries;
}

template <class TreeType>
void BM_construction(benchmark::State &state)
{
  using DeviceType = typename TreeType::device_type;
  int const n_values = state.range(0);
  PointCloudType point_cloud_type = static_cast<PointCloudType>(state.range(1));
  auto points = constructPoints<DeviceType>(n_values, point_cloud_type);

  for (auto _ : state)
  {
    auto const start = std::chrono::high_resolution_clock::now();
    TreeType index(points);
    auto const end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    state.SetIterationTime(elapsed_seconds.count());
  }
}

template <class TreeType>
void BM_knn_search(benchmark::State &state)
{
  using DeviceType = typename TreeType::device_type;
  int const n_values = state.range(0);
  int const n_queries = state.range(1);
  int const n_neighbors = state.range(2);
  PointCloudType const source_point_cloud_type =
      static_cast<PointCloudType>(state.range(3));
  PointCloudType const target_point_cloud_type =
      static_cast<PointCloudType>(state.range(4));

  TreeType index(
      constructPoints<DeviceType>(n_values, source_point_cloud_type));
  auto const queries = makeNearestQueries<DeviceType>(
      n_values, n_queries, n_neighbors, target_point_cloud_type);

  for (auto _ : state)
  {
    Kokkos::View<int *, DeviceType> offset("offset", 0);
    Kokkos::View<int *, DeviceType> indices("indices", 0);
    auto const start = std::chrono::high_resolution_clock::now();
    index.query(queries, indices, offset);
    auto const end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    state.SetIterationTime(elapsed_seconds.count());
  }
}

template <class TreeType>
void BM_radius_search(benchmark::State &state)
{
  using DeviceType = typename TreeType::device_type;
  int const n_values = state.range(0);
  int const n_queries = state.range(1);
  int const n_neighbors = state.range(2);
  int const buffer_size = state.range(3);
  PointCloudType const source_point_cloud_type =
      static_cast<PointCloudType>(state.range(4));
  PointCloudType const target_point_cloud_type =
      static_cast<PointCloudType>(state.range(5));

  TreeType index(
      constructPoints<DeviceType>(n_values, source_point_cloud_type));
  auto const queries = makeSpatialQueries<DeviceType>(
      n_values, n_queries, n_neighbors, target_point_cloud_type);

  bool first_pass = true;
  for (auto _ : state)
  {
    Kokkos::View<int *, DeviceType> offset("offset", 0);
    Kokkos::View<int *, DeviceType> indices("indices", 0);
    auto const start = std::chrono::high_resolution_clock::now();
    index.query(queries, indices, offset, buffer_size);
    auto const end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> elapsed_seconds = end - start;
    state.SetIterationTime(elapsed_seconds.count());

    if (first_pass)
    {
      auto offset_clone = ArborX::clone(offset);
      ArborX::adjacentDifference(offset, offset_clone);
      double const max = ArborX::max(offset_clone);
      double const avg = ArborX::lastElement(offset) / n_queries;
      auto offset_clone_subview = Kokkos::subview(
          offset_clone, std::make_pair(1, offset_clone.extent_int(0)));
      double const min = ArborX::min(offset_clone_subview);

      std::ostream &os = std::cout;
      os << "min number of neighbors " << min << "\n";
      os << "max number of neighbors " << max << "\n";
      os << "avg number of neighbors " << avg << "\n";

      first_pass = false;
    }
  }
}

class KokkosScopeGuard
{
public:
  KokkosScopeGuard(int &argc, char *argv[]) { Kokkos::initialize(argc, argv); }
  ~KokkosScopeGuard() { Kokkos::finalize(); }
};

#define REGISTER_BENCHMARK(TreeType)                                           \
  BENCHMARK_TEMPLATE(BM_construction, TreeType)                                \
      ->Args({n_values, source_point_cloud_type})                              \
      ->UseManualTime()                                                        \
      ->Unit(benchmark::kMicrosecond);                                         \
  BENCHMARK_TEMPLATE(BM_knn_search, TreeType)                                  \
      ->Args({n_values, n_queries, n_neighbors, source_point_cloud_type,       \
              target_point_cloud_type})                                        \
      ->UseManualTime()                                                        \
      ->Unit(benchmark::kMicrosecond);                                         \
  BENCHMARK_TEMPLATE(BM_radius_search, TreeType)                               \
      ->Args({n_values, n_queries, n_neighbors, buffer_size,                   \
              source_point_cloud_type, target_point_cloud_type})               \
      ->UseManualTime()                                                        \
      ->Unit(benchmark::kMicrosecond);

int main(int argc, char *argv[])
{
  KokkosScopeGuard guard(argc, argv);

  namespace bpo = boost::program_options;
  bpo::options_description desc("Allowed options");
  int n_values = 50000;
  int n_queries = 20000;
  int n_neighbors = 10;
  int buffer_size = 0;
  std::string source_pt_cloud = "filled_box";
  std::string target_pt_cloud = "filled_box";
  // clang-format off
    desc.add_options()
        ( "help", "produce help message" )
        ( "values", bpo::value<int>(&n_values)->default_value(50000), "number of indexable values (source)" )
        ( "queries", bpo::value<int>(&n_queries)->default_value(20000), "number of queries (target)" )
        ( "neighbors", bpo::value<int>(&n_neighbors)->default_value(10), "desired number of results per query" )
        ( "buffer", bpo::value<int>(&buffer_size)->default_value(0), "size for buffer optimization in radius search" )
        ( "source-point-cloud-type", bpo::value<std::string>(&source_pt_cloud)->default_value("filled_box"), "shape of the source point cloud"  )
        ( "target-point-cloud-type", bpo::value<std::string>(&target_pt_cloud)->default_value("filled_box"), "shape of the target point cloud"  )
    ;
  // clang-format on
  bpo::variables_map vm;
  bpo::parsed_options opts = bpo::command_line_parser(argc, argv)
                                 .options(desc)
                                 .allow_unregistered()
                                 .run();
  bpo::store(opts, vm);
  bpo::notify(vm);

  if (vm.count("help"))
  {
    // Full list of options consists of Kokkos + Boost.Program_options +
    // Google Benchmark and we still need to call benchmark::Initialize() to
    // get those printed to the standard output.
    std::cout << desc << "\n";
    int ac = 2;
    char *av[] = {(char *)"ignored", (char *)"--help"};
    // benchmark::Initialize() calls exit(0) when `--help` so register
    // Kokkos::finalize() to be called on normal program termination.
    std::atexit(Kokkos::finalize);
    benchmark::Initialize(&ac, av);
    return 1;
  }
  else
  {
    benchmark::Initialize(&argc, argv);
    // Throw if some of the arguments have not been recognized.
    std::ignore = bpo::command_line_parser(argc, argv)
                      .options(bpo::options_description(""))
                      .run();
  }

  // Google benchmark only supports integer arguments (see
  // https://github.com/google/benchmark/issues/387), so we map the string to
  // an enum.
  std::map<std::string, PointCloudType> to_point_cloud_enum;
  to_point_cloud_enum["filled_box"] = PointCloudType::filled_box;
  to_point_cloud_enum["hollow_box"] = PointCloudType::hollow_box;
  to_point_cloud_enum["filled_sphere"] = PointCloudType::filled_sphere;
  to_point_cloud_enum["hollow_sphere"] = PointCloudType::hollow_sphere;
  int source_point_cloud_type = to_point_cloud_enum.at(source_pt_cloud);
  int target_point_cloud_type = to_point_cloud_enum.at(target_pt_cloud);

#ifdef KOKKOS_ENABLE_SERIAL
  using Serial = Kokkos::Serial::device_type;
  REGISTER_BENCHMARK(ArborX::BVH<Serial>);
#endif

#ifdef KOKKOS_ENABLE_OPENMP
  using OpenMP = Kokkos::OpenMP::device_type;
  REGISTER_BENCHMARK(ArborX::BVH<OpenMP>);
#endif

#ifdef KOKKOS_ENABLE_CUDA
  using Cuda = Kokkos::Cuda::device_type;
  REGISTER_BENCHMARK(ArborX::BVH<Cuda>);
  using CudaUVM = Kokkos::Device<Kokkos::Cuda, Kokkos::CudaUVMSpace>;
  REGISTER_BENCHMARK(ArborX::BVH<CudaUVM>);
#endif

#if defined(KOKKOS_ENABLE_SERIAL)
  REGISTER_BENCHMARK(BoostRTree);
#endif

  benchmark::RunSpecifiedBenchmarks();

  return EXIT_SUCCESS;
}


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

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

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_AREA_CARTESIAN_HPP
#define BOOST_GEOMETRY_STRATEGIES_AREA_CARTESIAN_HPP

#include <boost/geometry/strategy/cartesian/area.hpp>
#include <boost/geometry/strategy/cartesian/area_box.hpp>

#include <boost/geometry/strategies/area/services.hpp>
#include <boost/geometry/strategies/detail.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace area
{

template <typename CalculationType = void>
struct cartesian : strategies::detail::cartesian_base
{
    template <typename Geometry>
    static auto area(Geometry const&,
                     std::enable_if_t<! util::is_box<Geometry>::value> * = nullptr)
    {
        return strategy::area::cartesian<CalculationType>();
    }

    template <typename Geometry>
    static auto area(Geometry const&,
                     std::enable_if_t<util::is_box<Geometry>::value> * = nullptr)
    {
        return strategy::area::cartesian_box<CalculationType>();
    }
};

namespace services
{

template <typename Geometry>
struct default_strategy<Geometry, cartesian_tag>
{
    using type = strategies::area::cartesian<>;
};

template <typename CT>
struct strategy_converter<strategy::area::cartesian<CT> >
{
    static auto get(strategy::area::cartesian<CT> const&)
    {
        return strategies::area::cartesian<CT>();
    }
};

} // namespace services

}} // namespace strategies::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_AREA_CARTESIAN_HPP