/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  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.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/**
 *
 * \brief This application performs MAP inference on Markov Nets
 * provided in standard UAI file format via Dual-Decomposition.
 *
 *
 *  \author Dhruv Batra
 */

#ifndef __DD_GRLAB_HPP__
#define __DD_GRLAB_HPP__

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>

#include <Eigen/Dense>
#include "eigen_serialization.hpp"

#include <graphlab.hpp>
#include <graphlab/macros_def.hpp>

using namespace std;

/**
 * \brief Eigen library vectors are used to store the potentials (log-space)
 */
typedef Eigen::VectorXd factor_type;
typedef Eigen::VectorXd vec;
typedef Eigen::MatrixXd mat;

/**
 * \brief The convergence threshold for each message.  Smaller values
 * imply tighter convergence but slower execution.
 *
 */
double TOLERANCE = 0.01;

/////////////////////////////////////////////////////////////////////////
// Edge and Vertex data and Graph Type
/**
 * \brief There is a vertex for each factor in the graph AND each singleton
 */
struct vertex_data
{
    int nvars;              // Number of variables in this factor.
    int degree;             // Degree of this factor (same as nvars for higher-order factors).
    vector<int> cards;      // Cardinality of each variable.
    vector<int> neighbors;  // Vertex ids of the neighbors.
    vec potentials;         // Potentials for each configuration of the factor.

    int best_configuration; // Index of the best configuration at a subgradient step.
                            // TODO: Maybe replace best_configuration by beliefs for the high order variables?
                            // In which case, beliefs would be vector<vec> beliefs.
    vec beliefs;            // Posterior values for the configurations after averaging (projected DD, unary variables only).

    vertex_data(): nvars(0) {}

    void load(graphlab::iarchive& arc)
    {
      arc >> nvars >> degree >> cards >> neighbors >> potentials >> best_configuration >> beliefs;
    }
    void save(graphlab::oarchive& arc) const
    {
      arc << nvars << degree << cards << neighbors << potentials << best_configuration << beliefs;
    }
}; // end of vertex_data

/**
 * \brief There is an edge connecting each factor to each singleton
 * in its scope.
 */
struct edge_data
{
    int varid; // Do we need this? (afm)
    int card; // Do we need this? (afm)
    vec potentials; // TODO: Unary potentials distributed evenly through the edges (i.e. unary potentials divided by degree).

    vec multiplier_messages; // Dual variables, i.e. Lagrangian multipliers.
    vec local_messages;      // Local MAP variables (for projected DD).

    edge_data(): varid(0), card(0) {}

    void load(graphlab::iarchive& arc) {
      arc >> varid >> card >> potentials >> multiplier_messages >> local_messages;
    }
    void save(graphlab::oarchive& arc) const {
      arc << varid << card << potentials << multiplier_messages << local_messages;
    }
};

/**
 * The graph type
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

/**
 * \brief The Dual Decomposition Vertex Program.
 */
struct dd_vertex_program :
  public graphlab::ivertex_program< graph_type, factor_type,
                                    graphlab::messages::sum_priority >,
  public graphlab::IS_POD_TYPE {

  /////////////////////////////////////////////////////////////////////////
  // Find the configuration index of a factor given the array of states.
  /////////////////////////////////////////////////////////////////////////
  int get_configuration_index(const graph_type::vertex_type& vertex,
                              const std::vector<int>& states) const {
    const vertex_data& vdata = vertex.data();
    int index = states[0];
    for (size_t i = 1; i < states.size(); ++i) {
      index *= vdata.cards[i];
      index += states[i];
    }
    return index;
  }

  /////////////////////////////////////////////////////////////////////////
  // Find the array of states corresponding to a factor configuration index.
  /////////////////////////////////////////////////////////////////////////
  void get_configuration_states(const graph_type::vertex_type& vertex,
                                int index, std::vector<int>* states) const {
    const vertex_data& vdata = vertex.data();
    int tmp = 1;
    for (size_t i = 1; i < states->size(); ++i) {
      tmp *= vdata.cards[i];
    }
    (*states)[0] = index / tmp;
    for (size_t i = 1; i < states->size(); ++i) {
      index = index % tmp;
      tmp /= vdata.cards[i];
      (*states)[i] = index / tmp;
    }
  }

  /**
   * \brief Given an edge and a vertex return the other vertex along
   * that edge.
   */
  inline vertex_type get_other_vertex(edge_type& edge,
                                      const vertex_type& vertex) const {
    return vertex.id() == edge.source().id()? edge.target() : edge.source();
  }; // end of other_vertex

  virtual edge_dir_type gather_edges(icontext_type& context,
                                     const vertex_type& vertex) const = 0;
  virtual factor_type gather(icontext_type& context, const vertex_type& vertex,
                             edge_type& edge) const = 0;
  virtual void apply(icontext_type& context, vertex_type& vertex,
                     const factor_type& total) = 0;
  virtual edge_dir_type scatter_edges(icontext_type& context,
                                      const vertex_type& vertex) const = 0;
  virtual void scatter(icontext_type& context, const vertex_type& vertex,
                       edge_type& edge) const = 0;
}; // end of class bp_vertex_program

////////////////////////////////////////////////////////////////////////////////
// This class implements the "symmetric" version of dual decomposition described
// in:
// D. Sontag, A. Globerson, T. Jaakkola.
// Introduction to Dual Decomposition for Inference.
// Optimization for Machine Learning, editors S. Sra, S. Nowozin, and S. J.
// Wright: MIT Press, 2011
////////////////////////////////////////////////////////////////////////////////

struct dd_vertex_program_symmetric : public dd_vertex_program {
  /**
   * \brief Since the MRF is undirected we will use all edges for gather and
   * scatter
   */
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of gather_edges

  /**
   * \brief The gather function takes a vertex and an edge as inputs and outputs
   a vector of numeric values. Vectors of numeric values will later be summed
   over all edges incident in this vertex. So, if the vertex is a unary factor,
   we can just return the vector of Lagrange multipliers stored in "edge.messages".
   Otherwise (if vertex is a general factor), things are a little more tricky.
   Suppose the factor is linked to K variables, with cardinalities C_1, ..., C_K.
   Suppose this edge is with respect to the k-th variable. Then, we return a
   vector of size C_1 + ... + C_K which is zero everywhere except in the
   k-th slot, where the Lagrange multipliers in "edge.messages" will be copied
   to. This way, when the "gather sum" takes place, and since all these slots
   are disjoint, we will just get the Lagrange multipliers of all the variables.
   */
  factor_type gather(icontext_type& context, const vertex_type& vertex,
                     edge_type& edge) const {
    cout << "gather begin" << endl;
    const vertex_type other_vertex = get_other_vertex(edge, vertex);
    const vertex_data& vdata = vertex.data();
    edge_data& edata = edge.data();

    if (vdata.nvars == 1) {
      // Unary factor.
      cout << "This unary factor has " << vertex.num_in_edges() <<
        " in edges and " << vertex.num_out_edges() << " out edges" << endl;
      return edata.multiplier_messages;
    } else {
      // General factor.
      factor_type messages;
      messages.resize(vdata.potentials.size());
      int offset = 0;
      int index_neighbor = -1;
      for (int k = 0; k < vdata.nvars; ++k) {
        int vertex_id = vdata.neighbors[k];
        if (vertex_id == other_vertex.id()) {
          index_neighbor = k;
          break;
        }
        offset += vdata.cards[k];
      }
      CHECK_GE(index_neighbor, 0);
      for (int state = 0; state < vdata.cards[index_neighbor]; ++state) {
        messages[offset + state] = -edata.multiplier_messages[state];
      }
      return messages;
    }
    cout << "gather end" << endl;
  }; // end of gather function

  /**
   * \brief The apply function takes a vertex and a vector of numeric values
   (a total) as input. For unary vertices, this will be the sum of Lagrange
   multipliers, and we just need to sum that to the vertex potential and compute
   the argmax. For general factors, the vector of numeric values, as stated above,
   will contain all the Lagrange multipliers of the neighboring variables.
   So we need to loop through all possible factor configurations, get the
   sequence of states of each configuration, fetch the Lagrange multipliers for
   those states, and add them to the factor potential. Then we compute the argmax.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const factor_type& total) {
    vertex_data& vdata = vertex.data();
    cout << "begin apply" << endl;
    if (vdata.nvars == 1) {
      // Unary factor.
      ASSERT_EQ(vdata.potentials.size(), total.size());
      vec belief = vdata.potentials + total;
      // Save the best configuration for this vertex.
      belief.maxCoeff(&vdata.best_configuration);
      cout << "vdata.best_configuration = " << vdata.best_configuration << endl;
    } else {
      // General factor.
      vec belief = vdata.potentials;
      int num_configurations = vdata.potentials.size();
      for (int index_configuration = 0;
           index_configuration < num_configurations;
           ++index_configuration) {
        vector<int> states(vdata.nvars, -1);
        // This could be made more efficient by defining an iterator over factor
        // configurations.
        get_configuration_states(vertex, index_configuration, &states);
        int offset = 0;
        for (int k = 0; k < vdata.nvars; ++k) {
          belief[index_configuration] += total[offset + states[k]];
          offset += vdata.cards[k];
        }
      }
      // Save the best configuration for this factor.
      belief.maxCoeff(&vdata.best_configuration);
    }
    cout << "end apply" << endl;
  }; // end of apply

  /**
   * \brief Since the MRF is undirected we will use all edges for gather and
   * scatter
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of scatter edges

  /**
   * \brief The scatter function takes a vertex and an edge as input.
   We just need to update the messages (Lagrange multipliers) by looking at the
   saved argmaxes.
   */
  void scatter(icontext_type& context, const vertex_type& vertex,
               edge_type& edge) const {
    const vertex_type other_vertex = get_other_vertex(edge, vertex);
    const vertex_type *unary_vertex;
    const vertex_type *factor_vertex;
    cout << "begin scatter" << endl;
    if (vertex.data().nvars == 1) {
      // Unary factor.
      unary_vertex = &vertex;
      factor_vertex = &other_vertex;
    } else {
      // General factor.
      unary_vertex = &other_vertex;
      factor_vertex = &vertex;
    }
    const vertex_data& vdata = unary_vertex->data();
    const vertex_data& other_vdata = factor_vertex->data();
    edge_data& edata = edge.data();
    double stepsize = 1.0; // TODO: Make this decay over iteration number.

    CHECK_GE(vdata.best_configuration, 0);
    CHECK_LT(vdata.best_configuration, vdata.cards[0]);
    edata.multiplier_messages[vdata.best_configuration] += stepsize;
    vector<int> states(other_vdata.nvars, -1);
    get_configuration_states(*factor_vertex, other_vdata.best_configuration, &states);
    int offset = 0;
    int index_neighbor = -1;
    for (int k = 0; k < other_vdata.nvars; ++k) {
      int vertex_id = other_vdata.neighbors[k];
      if (vertex_id == unary_vertex->id()) {
        index_neighbor = k;
        break;
      }
      offset += other_vdata.cards[k];
    }
    CHECK_GE(index_neighbor, 0);
    CHECK_GE(states[index_neighbor], 0);
    CHECK_LT(states[index_neighbor], other_vdata.cards[index_neighbor]);
    CHECK_EQ(other_vdata.cards[index_neighbor], vdata.cards[0]);
    edata.multiplier_messages[states[index_neighbor]] -= stepsize;
    cout << "end scatter" << endl;
  }; // end of scatter
}; // end of class bp_vertex_program_symmetric

////////////////////////////////////////////////////////////////////////////////
// This class implements the "projected" version of dual decomposition described
// in:
// Komodakis, N., Paragios, N., and Tziritas, G. (2007).
// MRF optimization via dual decomposition: Message-passing revisited.
// In Proc. of International Conference on Computer Vision.
//
// The formulation used is the one in Algorithm 1 of:
//
// André F. T. Martins, Mário A. T. Figueiredo, Pedro M. Q. Aguiar,
// Noah A. Smith, and Eric P. Xing.
// "An Augmented Lagrangian Approach to Constrained MAP Inference."
// International Conference on Machine Learning (ICML), 2011.
////////////////////////////////////////////////////////////////////////////////

struct dd_vertex_program_projected : public dd_vertex_program {
  /**
   * \brief Since the MRF is undirected we will use all edges for gather and
   * scatter
   */
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of gather_edges

  /**
   * \brief The gather function takes a vertex and an edge as inputs and outputs
   a vector of numeric values. Vectors of numeric values will later be summed
   over all edges incident in this vertex.
   If the vertex is a unary factor, compute the sum of all the local MAP variables,
   which in the "apply" function will serve to compute the global MAP.
   Otherwise (if vertex is a general factor), things are a little more tricky.
   Suppose the factor is linked to K variables, with cardinalities C_1, ..., C_K.
   Suppose this edge is with respect to the k-th variable. Then, we return a
   vector of size C_1 + ... + C_K which is zero everywhere except in the
   k-th slot, where the Lagrange multipliers in "edge.messages" will be copied
   to. This way, when the "gather sum" takes place, and since all these slots
   are disjoint, we will just get the Lagrange multipliers of all the variables.
   */
  factor_type gather(icontext_type& context, const vertex_type& vertex,
                     edge_type& edge) const {
    cout << "gather begin" << endl;
    const vertex_type other_vertex = get_other_vertex(edge, vertex);
    const vertex_data& vdata = vertex.data();
    edge_data& edata = edge.data();

    if (vdata.nvars == 1) {
      // Unary factor.
      cout << "This unary factor has " << vertex.num_in_edges() <<
        " in edges and " << vertex.num_out_edges() << " out edges" << endl;
      factor_type messages = edata.local_messages;
      return messages;
    } else {
      // General factor.
      factor_type messages;
      messages.resize(vdata.potentials.size());
      int offset = 0;
      int index_neighbor = -1;
      for (int k = 0; k < vdata.nvars; ++k) {
        int vertex_id = vdata.neighbors[k];
        if (vertex_id == other_vertex.id()) {
          index_neighbor = k;
          break;
        }
        offset += vdata.cards[k];
      }
      CHECK_GE(index_neighbor, 0);
      //const vec &unary_potential = other_vertex.data().potential;
      //int degree = other_vertex.data().degree;

      for (int state = 0; state < vdata.cards[index_neighbor]; ++state) {
        messages[offset + state] = edata.multiplier_messages[state];
        // TODO: somehow set the "edge potential" to be the potential of the
        // unary variable divided by the number of factors in which that
        // variable appears.
        messages[offset + state] += edata.potentials[state];
        //message[offset + state] += unary_potential[state] / static_cast<double>(degree);
      }
      return messages;
    }
    cout << "gather end" << endl;
  }; // end of gather function

  /**
   * \brief The apply function takes a vertex and a vector of numeric values
   (a total) as input.
   For a unary vertex, "total" will be the sum of local MAP vectors, and we
   just need to divide by the vertex degree and save the result as global MAP.
   For higher-order factors, "total" will contain all the Lagrange multipliers
   of the neighboring variables. So we need to loop through all possible factor
   configurations, get the sequence of states of each configuration, fetch the
   Lagrange multipliers for those states, and add them to the factor potential.
   Then we compute the argmax and save result to local MAP for each variable
   connected to the factor.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const factor_type& total) {
    vertex_data& vdata = vertex.data();
    cout << "begin apply" << endl;
    if (vdata.nvars == 1) {
      // Unary factor. Divide by vertex degree.
      vdata.beliefs = total / static_cast<double>(vdata.degree);
      return;
    } else {
      // General factor.
      vec beliefs = vdata.potentials;
      int num_configurations = vdata.potentials.size();
      for (int index_configuration = 0;
           index_configuration < num_configurations;
           ++index_configuration) {
        vector<int> states(vdata.nvars, -1);
        // This could be made more efficient by defining an iterator over factor
        // configurations.
        get_configuration_states(vertex, index_configuration, &states);
        int offset = 0;
        for (int k = 0; k < vdata.nvars; ++k) {
          beliefs[index_configuration] += total[offset + states[k]];
          offset += vdata.cards[k];
        }
      }
      // Save the best configuration for this factor.
      beliefs.maxCoeff(&vdata.best_configuration);
    }
    cout << "end apply" << endl;
  }; // end of apply

  /**
   * \brief Since the MRF is undirected we will use all edges for gather and
   * scatter
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of scatter edges

  /**
   * \brief The scatter function takes a vertex and an edge as input.
   (1) If the vertex is a unary factor, we update the messages (Lagrange multipliers)
   by subtracting the global MAP by the local MAP.
   (2) If the vertex is a higher order factor, this function will take the best
   configuration (obtained at the apply function) and save the local MAP
   at the corresponding edge.
   */
  void scatter(icontext_type& context, const vertex_type& vertex,
               edge_type& edge) const {
    const vertex_data& vdata = vertex.data();
    edge_data& edata = edge.data();
    cout << "begin scatter" << endl;
    if (vdata.nvars == 1) {
      // Unary factor. Update the messages (Lagrange multipliers).
      double stepsize = 1.0; // TODO: Make this decay over iteration number.
      edata.multiplier_messages += (vdata.beliefs - edata.local_messages) * stepsize;
    } else {
      // General factor. Update the local MAPs.
      const vertex_type &unary_vertex = get_other_vertex(edge, vertex);
      vector<int> states(vdata.nvars, -1);
      get_configuration_states(vertex, vdata.best_configuration, &states);
      int offset = 0;
      int index_neighbor = -1;
      for (int k = 0; k < vdata.nvars; ++k) {
        int vertex_id = vdata.neighbors[k];
        if (vertex_id == unary_vertex.id()) {
          index_neighbor = k;
          break;
        }
        offset += vdata.cards[k];
      }
      CHECK_GE(index_neighbor, 0);
      CHECK_GE(states[index_neighbor], 0);
      CHECK_LT(states[index_neighbor], vdata.cards[index_neighbor]);
      CHECK_EQ(vdata.cards[index_neighbor], unary_vertex.data().cards[0]);
      edata.local_messages.setZero();
      edata.local_messages[states[index_neighbor]] += 1.0;
    }
    cout << "end scatter" << endl;
  }; // end of scatter
}; // end of class dd_vertex_program_projected

#endif


/*
 * Copyright 2020 Ria Sonecha, Massachusetts Institute of Technology in Cambridge, MA, USA
 * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <thread>
#include <chrono>

#include "rotors_gazebo/Quaternion.h"
#include "rotors_gazebo/transform_datatypes.h"
#include "rotors_gazebo/parameters_ros.h"
#include <nav_msgs/Odometry.h>
#include "rotors_gazebo/Matrix3x3.h"
#include <ros/console.h>
#include <time.h>

#include <Eigen/Core>
#include <mav_msgs/conversions.h>
#include <mav_msgs/default_topics.h>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <trajectory_msgs/MultiDOFJointTrajectory.h>
#include <math.h>
#include "rotors_gazebo/spline_trajectory_generator.h"

#define DELTA_1   0.5
#define ALPHA_1   2
#define ALPHA_2   20
#define ALPHA_3   8
#define ALPHA_4   12
#define ALPHA_5   3
#define BETA_1    30
#define BETA_2    14
#define BETA_3    16
#define BETA_4    2
#define GAMMA_1   12
#define GAMMA_2   6
#define SAMPLING_TIME  10e-3       /* SAMPLING CONTROLLER TIME [s] - 100Hz */
#define START_SIMULATION_TIME 3   /* TIME GAZEBO NEEDS TO INITIALIZE THE ENVIRONMENT */

namespace rotors_gazebo {

  SplineTrajectoryGenerator::SplineTrajectoryGenerator(){

    // Parameters initializion
    a0_.setZero(); a1_.setZero(); a2_.setZero(); a3_.setZero(); a4_.setZero(); a5_.setZero();
    b0_.setZero(); b1_.setZero(); b2_.setZero(); b3_.setZero(); b4_.setZero();
    c0_.setZero(); c1_.setZero(); c2_.setZero(); c3_.setZero();

    g0_.setZero(); g1_.setZero(); g2_.setZero(); g3_.setZero(); g4_.setZero(); g5_.setZero();
    h0_.setZero(); h1_.setZero(); h2_.setZero(); h3_.setZero(); h4_.setZero();
    i0_.setZero(); i1_.setZero(); i2_.setZero(); i3_.setZero();

  }

  SplineTrajectoryGenerator::~SplineTrajectoryGenerator(){}

  void SplineTrajectoryGenerator::TrajectoryCallback(mav_msgs::EigenDroneState* odometry, double* time_final, double* time_init) {
    assert(odometry);
    assert(time_final);
    assert(time_init);

    double time_spline;
    time_spline = *time_final - *time_init;

    if (enable_parameter_computation_){
      ComputeSplineParameters(&time_final_);
      enable_parameter_computation_ = false;

      ROS_DEBUG("Publishing time_final_: %f", time_final_);
    }

    ROS_DEBUG("Publishing time_spline: %f", time_spline);

    // Desired Position
    double x_component, y_component, z_component;
    x_component = a5_.x() * pow(time_spline, 5) + a4_.x() * pow(time_spline, 4) + a3_.x() * pow(time_spline, 3) + a2_.x() * pow(time_spline, 2)
                  + a1_.x() * time_spline + a0_.x();
    y_component = a5_.y() * pow(time_spline, 5) + a4_.y() * pow(time_spline, 4) + a3_.y() * pow(time_spline, 3) + a2_.y() * pow(time_spline, 2)
                  + a1_.y() * time_spline + a0_.y();
    z_component = a5_.z() * pow(time_spline, 5) + a4_.z() * pow(time_spline, 4) + a3_.z() * pow(time_spline, 3) + a2_.z() * pow(time_spline, 2)
                  + a1_.z() * time_spline + a0_.z();

    ROS_DEBUG("Publishing position spline parameters along x-axis: [%f, %f, %f, %f, %f, %f].", a0_.x(), a1_.x(), a2_.x(), a3_.x(), a4_.x(), a5_.x());
    ROS_DEBUG("Publishing position spline parameters along y-axis: [%f, %f, %f, %f, %f, %f].", a0_.y(), a1_.y(), a2_.y(), a3_.y(), a4_.y(), a5_.y());
    ROS_DEBUG("Publishing position spline parameters along z-axis: [%f, %f, %f, %f, %f, %f].", a0_.z(), a1_.z(), a2_.z(), a3_.z(), a4_.z(), a5_.z());

    odometry->position_W = Eigen::Vector3f(x_component, y_component, z_component);

    ROS_DEBUG("Publishing position waypoint: [%f, %f, %f].", x_component, y_component, z_component);

    // Desired Linear Velocity
    x_component = b4_.x() * pow(time_spline, 4) + b3_.x() * pow(time_spline, 3) + b2_.x() * pow(time_spline, 2) + b1_.x() * time_spline
                  + b0_.x();
    y_component = b4_.y() * pow(time_spline, 4) + b3_.y() * pow(time_spline, 3) + b2_.y() * pow(time_spline, 2) + b1_.y() * time_spline
                  + b0_.y();
    z_component = b4_.z() * pow(time_spline, 4) + b3_.z() * pow(time_spline, 3) + b2_.z() * pow(time_spline, 2) + b1_.z() * time_spline
                  + b0_.z();

    ROS_DEBUG("Publishing velocity spline parameters along x-axis: [%f, %f, %f, %f, %f].", b0_.x(), b1_.x(), b2_.x(), b3_.x(), b4_.x());
    ROS_DEBUG("Publishing velocity spline parameters along y-axis: [%f, %f, %f, %f, %f].", b0_.y(), b1_.y(), b2_.y(), b3_.y(), b4_.y());
    ROS_DEBUG("Publishing velocity spline parameters along z-axis: [%f, %f, %f, %f, %f].", b0_.z(), b1_.z(), b2_.z(), b3_.z(), b4_.z());

    odometry->velocity = Eigen::Vector3f(x_component, y_component, z_component);

    ROS_DEBUG("Publishing velocity waypoint: [%f, %f, %f].", x_component, y_component, z_component);

    // Desired Acceleration
    x_component = c3_.x() * pow(time_spline, 3) + c2_.x() * pow(time_spline, 2) + c1_.x() * time_spline + c0_.x();
    y_component = c3_.y() * pow(time_spline, 3) + c2_.y() * pow(time_spline, 2) + c1_.y() * time_spline + c0_.y();
    z_component = c3_.z() * pow(time_spline, 3) + c2_.z() * pow(time_spline, 2) + c1_.z() * time_spline + c0_.z();

    ROS_DEBUG("Publishing acceleration spline parameters along y-axis: [%f, %f, %f, %f].", c0_.y(), c1_.y(), c2_.y(), c3_.y());
    ROS_DEBUG("Publishing acceleration spline parameters along x-axis: [%f, %f, %f, %f].", c0_.x(), c1_.x(), c2_.x(), c3_.x());
    ROS_DEBUG("Publishing acceleration spline parameters along z-axis: [%f, %f, %f, %f].", c0_.z(), c1_.z(), c2_.z(), c3_.z());

    odometry->acceleration = Eigen::Vector3f(x_component, y_component, z_component);

    ROS_DEBUG("Publishing acceleration waypoint: [%f, %f, %f].", x_component, y_component, z_component);

    // Desired Attitute
    rollDesRad_ = g5_.x() * pow(time_spline, 5) + g4_.x() * pow(time_spline, 4) + g3_.x() * pow(time_spline, 3) + g2_.x() * pow(time_spline, 2)
                + g1_.x() * time_spline + g0_.x();
    pitchDesRad_ = g5_.y() * pow(time_spline, 5) + g4_.y() * pow(time_spline, 4) + g3_.y() * pow(time_spline, 3) + g2_.y() * pow(time_spline, 2)
                + g1_.y() * time_spline + g0_.y();
    yawDesRad_ = g5_.z() * pow(time_spline, 5) + g4_.z() * pow(time_spline, 4) + g3_.z() * pow(time_spline, 3) + g2_.z() * pow(time_spline, 2)
                + g1_.z() * time_spline + g0_.z();

    ROS_DEBUG("Publishing orientation spline parameters along x-axis: [%f, %f, %f, %f, %f, %f].", g0_.x(), g1_.x(), g2_.x(), g3_.x(), g4_.x(), g5_.x());
    ROS_DEBUG("Publishing orientation spline parameters along y-axis: [%f, %f, %f, %f, %f, %f].", g0_.y(), g1_.y(), g2_.y(), g3_.y(), g4_.y(), g5_.y());
    ROS_DEBUG("Publishing orientation spline parameters along z-axis: [%f, %f, %f, %f, %f, %f].", g0_.z(), g1_.z(), g2_.z(), g3_.z(), g4_.z(), g5_.z());

    // Converts radians in quaternions
    double x, y, z, w;
    Euler2QuaternionCommandTrajectory(&x, &y, &z, &w);
    odometry->orientation_W_B = Eigen::Quaterniond(w, x, y, z);

    ROS_DEBUG("Publishing attitude waypoint: [%f, %f, %f].", rollDesRad_, pitchDesRad_, yawDesRad_);

    // Desired Angular Velocity
    double roll_component, pitch_component, yaw_component;
    roll_component = h4_.x() * pow(time_spline, 4) + h3_.x() * pow(time_spline, 3) + h2_.x() * pow(time_spline, 2) + h1_.x() * time_spline + h0_.x();
    pitch_component = h4_.y() * pow(time_spline, 4) + h3_.y() * pow(time_spline, 3) + h2_.y() * pow(time_spline, 2) + h1_.y() * time_spline + h0_.y();
    yaw_component = h4_.z() * pow(time_spline, 4) + h3_.z() * pow(time_spline, 3) + h2_.z() * pow(time_spline, 2) + h1_.z() * time_spline + h0_.z();

    ROS_DEBUG("Publishing angular velocity waypoint: [%f, %f, %f].", roll_component, pitch_component, yaw_component);

    // The angular rates are rotated from the inertial to the body frame
    // https://home.aero.polimi.it/trainelli/downloads/Bottasso_ThreeDimensionalRotations.pdf, eqs. 1.206 and 1.208
    double roll_component_B, pitch_component_B, yaw_component_B;
    roll_component_B = roll_component - sin(pitchDesRad_) * yaw_component;
    pitch_component_B = cos(rollDesRad_) * pitch_component + sin(rollDesRad_) * cos(pitchDesRad_) * yaw_component;
    yaw_component_B = -sin(rollDesRad_) * pitch_component + cos(rollDesRad_) * cos(pitchDesRad_) * yaw_component;

    odometry->angular_velocity_B = Eigen::Vector3f(roll_component_B, pitch_component_B, yaw_component_B);

    ROS_DEBUG("Publishing angular velocity waypoint in the body frame: [%f, %f, %f].", roll_component_B, pitch_component_B, yaw_component_B);

  }

  void SplineTrajectoryGenerator::ComputeSplineParameters(double* time_spline){
      assert(time_spline);

      /*        POSITION       */
      // Parameters used for the position and its derivatives. The coefficient
      // refers to the x, y and z-avis
      a0_ = position_initial_;

      ROS_DEBUG("Publishing position initial: [%f, %f, %f].", position_initial_[0], position_initial_[1], position_initial_[2]);
      ROS_DEBUG("Content of the a0 coefficient: [%f, %f, %f].", a0_[0], a0_[1], a0_[2]);

      a1_ = velocity_initial_;

      ROS_DEBUG("Publishing velocity initial: [%f, %f, %f].", velocity_initial_[0], velocity_initial_[1], velocity_initial_[2]);
      ROS_DEBUG("Content of the a1 coefficient: [%f, %f, %f].", a1_[0], a1_[1], a1_[2]);

      a2_.x() = acceleration_initial_.x() * DELTA_1;
      a2_.y() = acceleration_initial_.y() * DELTA_1;
      a2_.z() = acceleration_initial_.z() * DELTA_1;

      ROS_DEBUG("Publishing acceleration initial: [%f, %f, %f].", acceleration_initial_[0], acceleration_initial_[1], acceleration_initial_[2]);
      ROS_DEBUG("Content of the a2 coefficient: [%f, %f, %f].", a2_[0], a2_[1], a2_[2]);

      a3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.x() - position_initial_.x()) -
                (ALPHA_3 * velocity_final_.x() + ALPHA_4 * velocity_initial_.x()) * *time_spline -
                (ALPHA_5 * acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2));

      a3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.y() - position_initial_.y()) -
                (ALPHA_3 * velocity_final_.y() + ALPHA_4 * velocity_initial_.y()) * *time_spline -
                (ALPHA_5 * acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2));

      a3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.z() - position_initial_.z()) -
                (ALPHA_3 * velocity_final_.z() + ALPHA_4 * velocity_initial_.z()) * *time_spline -
                (ALPHA_5 * acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2));

      ROS_DEBUG("Content of the a3 coefficient: [%f, %f, %f].", a3_[0], a3_[1], a3_[2]);

      a4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.x() - position_final_.x()) +
                (BETA_2 * velocity_final_.x() + BETA_3 * velocity_initial_.x()) * *time_spline +
                (BETA_3 * acceleration_final_.x() - BETA_4 * acceleration_initial_.x()) * pow(*time_spline, 2));

      a4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.y() - position_final_.y()) +
                (BETA_2 * velocity_final_.y() + BETA_3 * velocity_initial_.y()) * *time_spline +
                (BETA_3 * acceleration_final_.y() - BETA_4 * acceleration_initial_.y()) * pow(*time_spline, 2));

      a4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.z() - position_final_.z()) +
                (BETA_2 * velocity_final_.z() + BETA_3 * velocity_initial_.z()) * *time_spline +
                (BETA_3 * acceleration_final_.z() - BETA_4 * acceleration_initial_.z()) * pow(*time_spline, 2));

      ROS_DEBUG("Content of the a4 coefficient: [%f, %f, %f].", a4_[0], a4_[1], a4_[2]);

      a5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.x() - position_initial_.x()) -
                GAMMA_2 * (velocity_final_.x() + velocity_initial_.x()) * *time_spline -
                (acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2));

      a5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.y() - position_initial_.y()) -
                GAMMA_2 * (velocity_final_.y() + velocity_initial_.y()) * *time_spline -
                (acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2));

      a5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.z() - position_initial_.z()) -
                GAMMA_2 * (velocity_final_.z() + velocity_initial_.z()) * *time_spline -
                (acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2));

      ROS_DEBUG("Content of the a5 coefficient: [%f, %f, %f].", a5_[0], a5_[1], a5_[2]);

      // The coefficients are used for having the velocity reference trajectory. The polynomial is computed
      // derivating the a one. In other words,
      // a5 * x^5 + a4 * x^4 + a3 * x^3 + a2 * x^2 + a1 * x + a0
      // 5 * a5 * x^4 + 4 * a4 * x^3 + 3 * a3 * x^2 + 2 * a2 * x + a1
      // b4 = 5 * a5 -- b3 = 4 * a4 -- b2 = 3 * a3 - b1 = 2 * a2 - b0 = a1
      // and so on
      b4_ = 5 * a5_;
      b3_ = 4 * a4_;
      b2_ = 3 * a3_;
      b1_ = 2 * a2_;
      b0_ = a1_;

      ROS_DEBUG("Content of the b0 coefficient: [%f, %f, %f].", b0_[0], b0_[1], b0_[2]);
      ROS_DEBUG("Content of the b1 coefficient: [%f, %f, %f].", b1_[0], b1_[1], b1_[2]);
      ROS_DEBUG("Content of the b2 coefficient: [%f, %f, %f].", b2_[0], b2_[1], b2_[2]);
      ROS_DEBUG("Content of the b3 coefficient: [%f, %f, %f].", b3_[0], b3_[1], b3_[2]);
      ROS_DEBUG("Content of the b4 coefficient: [%f, %f, %f].", b4_[0], b4_[1], b4_[2]);

      // The coefficients are used for having the acceleration reference trajectory. The polynomial is computed
      // derivating the b one. In other words,
      // 4 * b4 * x^4 + 3 * b3 * x^3 + 2 * b2 * x^2 + b1 * x
      // c3 = 4 * b4 -- c2 = 3 * b3 -- c1 = 2 * b2 - c0 = b1
      c3_ = 4 * b4_;
      c2_ = 3 * b3_;
      c1_ = 2 * b2_;
      c0_ = b1_;

      ROS_DEBUG("Content of the c0 coefficient: [%f, %f, %f].", c0_[0], c0_[1], c0_[2]);
      ROS_DEBUG("Content of the c1 coefficient: [%f, %f, %f].", c1_[0], c1_[1], c1_[2]);
      ROS_DEBUG("Content of the c2 coefficient: [%f, %f, %f].", c2_[0], c2_[1], c2_[2]);
      ROS_DEBUG("Content of the c3 coefficient: [%f, %f, %f].", c3_[0], c3_[1], c3_[2]);

      /*        ORIENTATION      */
      // Parameters used for the orientation and its derivatives. The coefficient
      // refers to the ROLL (X), PITCH (Y) and YAW (Z)
      g0_ = orientation_initial_;

      ROS_DEBUG("Publishing orientation initial: [%f, %f, %f].", orientation_initial_[0], orientation_initial_[1], orientation_initial_[2]);
      ROS_DEBUG("Content of the g0 coefficient: [%f, %f, %f].", g0_[0], g0_[1], g0_[2]);

      g1_ = angular_velocity_initial_;

      ROS_DEBUG("Publishing angular rate initial: [%f, %f, %f].", angular_velocity_initial_[0], angular_velocity_initial_[1], angular_velocity_initial_[2]);
      ROS_DEBUG("Content of the g1 coefficient: [%f, %f, %f].", g1_[0], g1_[1], g1_[2]);

      g2_.x() = angular_acceleration_initial_.x() * DELTA_1;
      g2_.y() = angular_acceleration_initial_.y() * DELTA_1;
      g2_.z() = angular_acceleration_initial_.z() * DELTA_1;

      ROS_DEBUG("Publishing angular acceleration initial: [%f, %f, %f].", angular_acceleration_initial_[0], angular_acceleration_initial_[1],
                angular_acceleration_initial_[2]);
      ROS_DEBUG("Content of the g2 coefficient: [%f, %f, %f].", g2_[0], g2_[1], g2_[2]);

      g3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.x() - orientation_initial_.x()) -
                (ALPHA_3 * angular_velocity_final_.x() + ALPHA_4 * angular_velocity_initial_.x()) * *time_spline -
                (ALPHA_5 * angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2));

      g3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.y() - orientation_initial_.y()) -
                (ALPHA_3 * angular_velocity_final_.y() + ALPHA_4 * angular_velocity_initial_.y()) * *time_spline -
                (ALPHA_5 * angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2));

      g3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.z() - orientation_initial_.z()) -
                (ALPHA_3 * angular_velocity_final_.z() + ALPHA_4 * angular_velocity_initial_.z()) * *time_spline -
                (ALPHA_5 * angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2));

      ROS_DEBUG("Content of the g3 coefficient: [%f, %f, %f].", g3_[0], g3_[1], g3_[2]);

      g4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.x() - orientation_final_.x()) +
                (BETA_2 * angular_velocity_final_.x() + BETA_3 * angular_velocity_initial_.x()) * *time_spline +
                (BETA_3 * angular_acceleration_final_.x() - BETA_4 * angular_acceleration_initial_.x()) * pow(*time_spline, 2));

      g4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.y() - orientation_final_.y()) +
                (BETA_2 * angular_velocity_final_.y() + BETA_3 * angular_velocity_initial_.y()) * *time_spline +
                (BETA_3 * angular_acceleration_final_.y() - BETA_4 * angular_acceleration_initial_.y()) * pow(*time_spline, 2));

      g4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.z() - orientation_final_.z()) +
                (BETA_2 * angular_velocity_final_.z() + BETA_3 * angular_velocity_initial_.z()) * *time_spline +
                (BETA_3 * angular_acceleration_final_.z() - BETA_4 * angular_acceleration_initial_.z()) * pow(*time_spline, 2));

      ROS_DEBUG("Content of the g4 coefficient: [%f, %f, %f].", g4_[0], g4_[1], g4_[2]);

      g5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.x() - orientation_initial_.x()) -
                GAMMA_2 * (angular_velocity_final_.x() + angular_velocity_initial_.x()) * *time_spline -
                (angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2));

      g5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.y() - orientation_initial_.y()) -
                GAMMA_2 * (angular_velocity_final_.y() + angular_velocity_initial_.y()) * *time_spline -
                (angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2));

      g5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.z() - orientation_initial_.z()) -
                GAMMA_2 * (angular_velocity_final_.z() + angular_velocity_initial_.z()) * *time_spline -
                (angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2));

      ROS_DEBUG("Content of the g5 coefficient: [%f, %f, %f].", g5_[0], g5_[1], g5_[2]);

      // The coefficients are used for having the angular velocity reference trajectory. The polynomial is computed
      // derivating the g one. In other words,
      // g5 * x^5 + g4 * x^4 + g3 * x^3 + g2 * x^2 + g1 * x + g0
      // 5 * g5 * x^4 + 4 * g4 * x^3 + 3 * g3 * x^2 + 2 * g2 * x + g1
      // h4 = 5 * g5 -- h3 = 4 * g4 -- h2 = 3 * g3 - h1 = 2 * g2 - h0 = g1
      // and so on
      h4_ = 5 * g5_;
      h3_ = 4 * g4_;
      h2_ = 3 * g3_;
      h1_ = 2 * g2_;
      h0_ = g1_;

      ROS_DEBUG("Content of the h0 coefficient: [%f, %f, %f].", h0_[0], h0_[1], h0_[2]);
      ROS_DEBUG("Content of the h1 coefficient: [%f, %f, %f].", h1_[0], h1_[1], h1_[2]);
      ROS_DEBUG("Content of the h2 coefficient: [%f, %f, %f].", h2_[0], h2_[1], h2_[2]);
      ROS_DEBUG("Content of the h3 coefficient: [%f, %f, %f].", h3_[0], h3_[1], h3_[2]);
      ROS_DEBUG("Content of the h4 coefficient: [%f, %f, %f].", h4_[0], h4_[1], h4_[2]);

      // The coefficients are used for having the angular acceleration reference trajectory. The polynomial is computed
      // derivating the b one. In other words,
      // 4 * h4 * x^4 + 3 * h3 * x^3 + 2 * h2 * x^2 + h1 * x
      // i3 = 4 * h4 -- i2 = 3 * h3 -- i1 = 2 * h2 - i0 = h1
      i3_ = 4 * h4_;
      i2_ = 3 * h3_;
      i1_ = 2 * h2_;
      i0_ = h1_;

      ROS_DEBUG("Content of the i0 coefficient: [%f, %f, %f].", i0_[0], i0_[1], i0_[2]);
      ROS_DEBUG("Content of the i1 coefficient: [%f, %f, %f].", i1_[0], i1_[1], i1_[2]);
      ROS_DEBUG("Content of the i2 coefficient: [%f, %f, %f].", i2_[0], i2_[1], i2_[2]);
      ROS_DEBUG("Content of the i3 coefficient: [%f, %f, %f].", i3_[0], i3_[1], i3_[2]);

  }

  void SplineTrajectoryGenerator::Euler2QuaternionCommandTrajectory(double* x, double* y, double* z, double* w) const {
      assert(x);
      assert(y);
      assert(z);
      assert(w);

      // Abbreviations for the various angular functions
      double cy = cos(yawDesRad_ * 0.5);
      double sy = sin(yawDesRad_ * 0.5);
      double cp = cos(pitchDesRad_ * 0.5);
      double sp = sin(pitchDesRad_ * 0.5);
      double cr = cos(rollDesRad_ * 0.5);
      double sr = sin(rollDesRad_ * 0.5);

      *w = cy * cp * cr + sy * sp * sr;
      *x = cy * cp * sr - sy * sp * cr;
      *y = sy * cp * sr + cy * sp * cr;
      *z = sy * cp * cr - cy * sp * sr;

      ROS_DEBUG("x Trajectory: %f, y Trajectory: %f, z Trajectory: %f, w Trajectory: %f", *x, *y, *z, *w);
    }

   void SplineTrajectoryGenerator::InitializeParams(){

     ros::NodeHandle pnh("~");

     // Parameters reading from rosparam.
     GetRosParameter(pnh, "time_final/time", time_final_, &time_final_);
     GetRosParameter(pnh, "position_initial/x", position_initial_.x(), &position_initial_.x());
     GetRosParameter(pnh, "position_initial/y", position_initial_.y(), &position_initial_.y());
     GetRosParameter(pnh, "position_initial/z", position_initial_.z(), &position_initial_.z());

     GetRosParameter(pnh, "position_final/x", position_final_.x(), &position_final_.x());
     GetRosParameter(pnh, "position_final/y", position_final_.y(), &position_final_.y());
     GetRosParameter(pnh, "position_final/z", position_final_.z(), &position_final_.z());

     GetRosParameter(pnh, "velocity_initial/x", velocity_initial_.x(), &velocity_initial_.x());
     GetRosParameter(pnh, "velocity_initial/y", velocity_initial_.y(), &velocity_initial_.y());
     GetRosParameter(pnh, "velocity_initial/z", velocity_initial_.z(), &velocity_initial_.z());

     GetRosParameter(pnh, "velocity_final/x", velocity_final_.x(), &velocity_final_.x());
     GetRosParameter(pnh, "velocity_final/y", velocity_final_.y(), &velocity_final_.y());
     GetRosParameter(pnh, "velocity_final/z", velocity_final_.z(), &velocity_final_.z());

     GetRosParameter(pnh, "acceleration_initial/x", acceleration_initial_.x(), &acceleration_initial_.x());
     GetRosParameter(pnh, "acceleration_initial/y", acceleration_initial_.y(), &acceleration_initial_.y());
     GetRosParameter(pnh, "acceleration_initial/z", acceleration_initial_.z(), &acceleration_initial_.z());

     GetRosParameter(pnh, "acceleration_final/x", acceleration_final_.x(), &acceleration_final_.x());
     GetRosParameter(pnh, "acceleration_final/y", acceleration_final_.y(), &acceleration_final_.y());
     GetRosParameter(pnh, "acceleration_final/z", acceleration_final_.z(), &acceleration_final_.z());

     GetRosParameter(pnh, "orientation_initial/roll", orientation_initial_.x(), &orientation_initial_.x());
     GetRosParameter(pnh, "orientation_initial/pitch", orientation_initial_.y(), &orientation_initial_.y());
     GetRosParameter(pnh, "orientation_initial/yaw", orientation_initial_.z(), &orientation_initial_.z());

     GetRosParameter(pnh, "orientation_final/roll", orientation_final_.x(), &orientation_final_.x());
     GetRosParameter(pnh, "orientation_final/pitch", orientation_final_.y(), &orientation_final_.y());
     GetRosParameter(pnh, "orientation_final/yaw", orientation_final_.z(), &orientation_final_.z());

     GetRosParameter(pnh, "angularRate_initial/roll", angular_velocity_initial_.x(), &angular_velocity_initial_.x());
     GetRosParameter(pnh, "angularRate_initial/pitch", angular_velocity_initial_.y(), &angular_velocity_initial_.y());
     GetRosParameter(pnh, "angularRate_initial/yaw", angular_velocity_initial_.z(), &angular_velocity_initial_.z());

     GetRosParameter(pnh, "angularRate_final/roll", angular_velocity_final_.x(), &angular_velocity_final_.x());
     GetRosParameter(pnh, "angularRate_final/pitch", angular_velocity_final_.y(), &angular_velocity_final_.y());
     GetRosParameter(pnh, "angularRate_final/yaw", angular_velocity_final_.z(), &angular_velocity_final_.z());

     GetRosParameter(pnh, "angularDoubleRate_initial/roll", angular_acceleration_initial_.x(), &angular_acceleration_initial_.x());
     GetRosParameter(pnh, "angularDoubleRate_initial/pitch", angular_acceleration_initial_.y(), &angular_acceleration_initial_.y());
     GetRosParameter(pnh, "angularDoubleRate_initial/yaw", angular_acceleration_initial_.z(), &angular_acceleration_initial_.z());

     GetRosParameter(pnh, "angularDoubleRate_final/roll", angular_acceleration_final_.x(), &angular_acceleration_final_.x());
     GetRosParameter(pnh, "angularDoubleRate_final/pitch", angular_acceleration_final_.y(), &angular_acceleration_final_.y());
     GetRosParameter(pnh, "angularDoubleRate_final/yaw", angular_acceleration_final_.z(), &angular_acceleration_final_.z());

   }

   template<typename T> inline void SplineTrajectoryGenerator::GetRosParameterHovering(const ros::NodeHandle& nh,
                                                            const std::string& key,
                                                            const T& default_value,
                                                            T* value) {

     ROS_ASSERT(value != nullptr);
     bool have_parameter = nh.getParam(key, *value);
     if (!have_parameter) {
       ROS_WARN_STREAM("[rosparam]: could not find parameter " << nh.getNamespace()
                       << "/" << key << ", setting to default: " << default_value);
       *value = default_value;
     }
   }

}


/**************************************************************************\
|
|    Copyright (C) 2009 Marc Stevens
|
|    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/>.
|
\**************************************************************************/

#include <vector>
#include <string>
#include <map>
#include <cmath>
#include <algorithm>

#include <hashclash/saveload_bz2.hpp>

#include "main.hpp"

#include <boost/lexical_cast.hpp>
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/convenience.hpp>
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <hashclash/sdr.hpp>
#include <hashclash/rng.hpp>
#include <hashclash/timer.hpp>
#include <hashclash/md5detail.hpp>

#include "distribution.hpp" // distribution tables
#include "storage.hpp"

void determine_nrblocks_distribution(birthday_parameters& parameters);

boost::mutex global_mutex;
#define LOCK_GLOBAL_MUTEX	boost::mutex::scoped_lock lock(global_mutex);

using namespace hashclash;
using namespace std;

/* LOCK_GLOBAL_MUTEX not needed */
uint32 ihv1[4], ihv2[4], ihv2mod[4], msg1[16], msg2[16], precomp1[4], precomp2[4];
uint32 hybridmask = 0, distinguishedpointmask = 0, maximumpathlength = 1;
unsigned maxblocks = 64;
bool memhardlimit = false;
unsigned parameterspathtyperange = 0;
unsigned procmodn = 0, procmodi = 0;
vector< pair<uint32,uint32> > singleblockdata;
/**/

/* LOCK_GLOBAL_MUTEX required */
uint32 colla1, collb1, collc1, colla2, collb2, collc2;
uint64 totwork = 0, totworkallproc = 0;
unsigned bestnrblocks = 64;
bool quit = false;
unsigned collrobinhoods = 0, collequalihvs = 0, collusefull = 0;
vector< vector<trail_type> > trail_distribution(0);
//vector< pair<trail_type, trail_type> > collisions_queue(0);
/**/

// LOCK_GLOBAL_MUTEX required
void status_line()
{
	unsigned totcoll = main_storage.get_totcoll();
	unsigned collqueue = main_storage.get_collqueuesize();
	if (procmodn > 1)
		cout << "Work: mine=2^(" << log(double(totwork))/log(double(2)) << ") all=2^(" << log(double(totworkallproc))/log(double(2)) << ")";
	else
		cout << "Work: 2^(" << log(double(totwork))/log(double(2)) << ")";
	cout << ", Coll.: " << main_storage.get_totcoll()
		 << "(uf=" << collusefull << ",nuf=" << collequalihvs
		 << ",?=" << (totcoll-collusefull-collequalihvs-collrobinhoods-collqueue)
		 << ",q=" << collqueue << ",rh=" << collrobinhoods
		 << "), Blocks: " << bestnrblocks << endl; //"     \r" << flush;
}

// LOCK_GLOBAL_MUTEX not needed
unsigned nrblocks(uint32 dihv[4])
{
	uint32 ptrmaskt2 = 0;
	for (unsigned j = 0; j < parameterspathtyperange; ++j)
		ptrmaskt2 |= 2<<j;
	sdr p1naf = naf(dihv[3]);
	uint32 p1nafmask = p1naf.mask;
	uint32 p2nafmask = naf(dihv[1]-dihv[3]).mask;
	p2nafmask = rotate_right(p2nafmask, 21);
	uint32 p1mask = p1nafmask;
	for (unsigned j = 0; j < parameterspathtyperange; ++j)
		p1mask |= p1nafmask << (j+1);
	p2nafmask &= ~p1mask;
	for (unsigned b = 0; b < 32; ++b)
		if (p2nafmask & (1<<b))
			p2nafmask &= ~(ptrmaskt2<<b);
	p1nafmask &= ~(p2nafmask<<1);
	p1nafmask &= ~(p1nafmask>>31);
	return hw(p1nafmask) + 2*hw(p2nafmask);
}

// LOCK_GLOBAL_MUTEX not needed
void precomputestate(uint32 ihv[4], uint32 block[16])
{
        #define HASHCLASH_MD5COMPRESS_STEP(f, a, b, c, d, m, ac, rc) \
                a += f(b, c, d) + m + ac; a = rotate_left(a,rc); a += b;

	uint32 a = ihv[0]; uint32 b = ihv[1]; uint32 c = ihv[2]; uint32 d = ihv[3];

	HASHCLASH_MD5COMPRESS_STEP(md5_ff, a, b, c, d, block[ 0], 0xd76aa478,  7);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, d, a, b, c, block[ 1], 0xe8c7b756, 12);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, c, d, a, b, block[ 2], 0x242070db, 17);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, b, c, d, a, block[ 3], 0xc1bdceee, 22);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, a, b, c, d, block[ 4], 0xf57c0faf,  7);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, d, a, b, c, block[ 5], 0x4787c62a, 12);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, c, d, a, b, block[ 6], 0xa8304613, 17);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, b, c, d, a, block[ 7], 0xfd469501, 22);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, a, b, c, d, block[ 8], 0x698098d8,  7);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, d, a, b, c, block[ 9], 0x8b44f7af, 12);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, c, d, a, b, block[10], 0xffff5bb1, 17);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, b, c, d, a, block[11], 0x895cd7be, 22);
	HASHCLASH_MD5COMPRESS_STEP(md5_ff, a, b, c, d, block[12], 0x6b901122,  7);

	ihv[0] = a; ihv[1] = b; ihv[2] = c; ihv[3] = d;
}

// LOCK_GLOBAL_MUTEX not needed
inline void birthday_step(uint32& x, uint32& y, uint32& z)
{
	uint32* block;
	uint32* precomp;
	uint32* ihv;
	if (x <= y) {
		precomp = precomp1;
		block = msg1;
		ihv = ihv1;
	} else {
		precomp = precomp2;
		block = msg2;
		ihv = ihv2mod;
	}
	{
		uint32 a = precomp[0], b = precomp[1], c = precomp[2], d = precomp[3];
		HASHCLASH_MD5COMPRESS_STEP(md5_ff, d, a, b, c, z, 0xfd987193, 12);
		HASHCLASH_MD5COMPRESS_STEP(md5_ff, c, d, a, b, x, 0xa679438e, 17);
		HASHCLASH_MD5COMPRESS_STEP(md5_ff, b, c, d, a, y, 0x49b40821, 22);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, a, b, c, d, block[ 1], 0xf61e2562,  5);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, d, a, b, c, block[ 6], 0xc040b340,  9);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, c, d, a, b, block[11], 0x265e5a51, 14);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, b, c, d, a, block[ 0], 0xe9b6c7aa, 20);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, a, b, c, d, block[ 5], 0xd62f105d,  5);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, d, a, b, c, block[10], 0x02441453,  9);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, c, d, a, b, y, 0xd8a1e681, 14);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, b, c, d, a, block[ 4], 0xe7d3fbc8, 20);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, a, b, c, d, block[ 9], 0x21e1cde6,  5);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, d, a, b, c, x, 0xc33707d6,  9);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, c, d, a, b, block[ 3], 0xf4d50d87, 14);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, b, c, d, a, block[ 8], 0x455a14ed, 20);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, a, b, c, d, z, 0xa9e3e905,  5);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, d, a, b, c, block[ 2], 0xfcefa3f8,  9);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, c, d, a, b, block[ 7], 0x676f02d9, 14);
		HASHCLASH_MD5COMPRESS_STEP(md5_gg, b, c, d, a, block[12], 0x8d2a4c8a, 20);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, a, b, c, d, block[ 5], 0xfffa3942,  4);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, d, a, b, c, block[ 8], 0x8771f681, 11);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, c, d, a, b, block[11], 0x6d9d6122, 16);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, b, c, d, a, x, 0xfde5380c, 23);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, a, b, c, d, block[ 1], 0xa4beea44,  4);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, d, a, b, c, block[ 4], 0x4bdecfa9, 11);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, c, d, a, b, block[ 7], 0xf6bb4b60, 16);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, b, c, d, a, block[10], 0xbebfbc70, 23);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, a, b, c, d, z, 0x289b7ec6,  4);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, d, a, b, c, block[ 0], 0xeaa127fa, 11);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, c, d, a, b, block[ 3], 0xd4ef3085, 16);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, b, c, d, a, block[ 6], 0x04881d05, 23);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, a, b, c, d, block[ 9], 0xd9d4d039,  4);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, d, a, b, c, block[12], 0xe6db99e5, 11);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, c, d, a, b, y, 0x1fa27cf8, 16);
		HASHCLASH_MD5COMPRESS_STEP(md5_hh, b, c, d, a, block[ 2], 0xc4ac5665, 23);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, a, b, c, d, block[ 0], 0xf4292244,  6);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, block[ 7], 0x432aff97, 10);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, x, 0xab9423a7, 15);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, b, c, d, a, block[ 5], 0xfc93a039, 21);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, a, b, c, d, block[12], 0x655b59c3,  6);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, block[ 3], 0x8f0ccc92, 10);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, block[10], 0xffeff47d, 15);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, b, c, d, a, block[ 1], 0x85845dd1, 21);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, a, b, c, d, block[ 8], 0x6fa87e4f,  6);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, y, 0xfe2ce6e0, 10);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, block[ 6], 0xa3014314, 15);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, b, c, d, a, z, 0x4e0811a1, 21);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, a, b, c, d, block[ 4], 0xf7537e82,  6);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, d, a, b, c, block[11], 0xbd3af235, 10);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, c, d, a, b, block[ 2], 0x2ad7d2bb, 15);
		HASHCLASH_MD5COMPRESS_STEP(md5_ii, b, c, d, a, block[ 9], 0xeb86d391, 21);
		a += ihv[0];
		b += ihv[1];
		c += ihv[2];
		d += ihv[3];
		if (maxblocks != 1) {
			x = a;
			y = d - c;
			z = (d - b) & hybridmask;
		} else {
			x = a;
			y = d;
			z = c & hybridmask;
		}
	}
}

// LOCK_GLOBAL_MUTEX not needed
void generate_trail(trail_type& trail)
{
	uint32 x = trail.start[0], y = trail.start[1], z = trail.start[2];
	trail.len = 1;
	birthday_step(x,y,z);
	while (trail.len <= maximumpathlength && 0!=(x & distinguishedpointmask)) {
		birthday_step(x,y,z);
		++trail.len;
	}
	trail.end[0] = x;
	trail.end[1] = y;
	trail.end[2] = z;
}

// do not use LOCK_GLOBAL_MUTEX
// function possibly calls LOCK_GLOBAL_MUTEX
void find_collision(const trail_type& trail1, const trail_type& trail2)
{
	uint32 a1 = trail1.start[0];
	uint32 b1 = trail1.start[1];
	uint32 c1 = trail1.start[2];
	uint32 a2 = trail2.start[0];
	uint32 b2 = trail2.start[1];
	uint32 c2 = trail2.start[2];
	uint32 len1 = trail1.len;
	uint32 len2 = trail2.len;
	// sanity check
	if (trail1.len > maximumpathlength || 0!=(trail1.end[0]&distinguishedpointmask)
	    || trail2.len > maximumpathlength || 0!=(trail2.end[0]&distinguishedpointmask))
	{
		LOCK_GLOBAL_MUTEX;
		cout << "X" << flush;
		++collrobinhoods; // have to categorize this collision
		return;
	}

	while (len1 > len2)
	{
		birthday_step(a1, b1, c1);
		--len1;
	}
	while (len2 > len1)
	{
		birthday_step(a2, b2, c2);
		--len2;
	}

	// check for robin hood
	if (a1 == a2 && b1 == b2 && c1 == c2)
	{
		LOCK_GLOBAL_MUTEX;
		++collrobinhoods;
		return;
	}

	uint32 oa1 = a1, oa2 = a2, ob1 = b1, ob2 = b2, oc1 = c1, oc2 = c2;
	while ((a1 != a2 || b1 != b2 || c1 != c2) && len1 > 0)
	{
		oa1 = a1; oa2 = a2; ob1 = b1; ob2 = b2; oc1 = c1; oc2 = c2;
		birthday_step(a1, b1, c1);
		birthday_step(a2, b2, c2);
		--len1;
	}
	if (len1 == 0 && (a1 != a2 || b1 != b2 || c1 != c2))
	{
		LOCK_GLOBAL_MUTEX;
		cerr << "find_collision(): len=0 (len1=" << trail1.len << ",len2=" << trail2.len << ")           " << endl;
		++collrobinhoods;
		return;
	}

	// check for same ihv birthday collision
	if ((oa1 <= ob1) == (oa2 <= ob2))
	{
		LOCK_GLOBAL_MUTEX;
		++collequalihvs;
		return;
	}

	uint32 ihvmsg1[4];
	uint32 ihvmsg2[4];
	if (oa2 <= ob2) {
		swap(oa2, oa1);
		swap(ob2, ob1);
		swap(oc2, oc1);
	}
	uint32 lmsg1[16];
	uint32 lmsg2[16];
	for (unsigned i = 0; i < 13; ++i)
	{
		lmsg1[i] = msg1[i];
		lmsg2[i] = msg2[i];
	}
	lmsg1[13] = oc1; lmsg1[14] = oa1; lmsg1[15] = ob1;
	ihvmsg1[0] = ihv1[0]; ihvmsg1[1] = ihv1[1];
	ihvmsg1[2] = ihv1[2]; ihvmsg1[3] = ihv1[3];
	md5compress(ihvmsg1, lmsg1);
	lmsg2[13] = oc2; lmsg2[14] = oa2; lmsg2[15] = ob2;
	ihvmsg2[0] = ihv2[0]; ihvmsg2[1] = ihv2[1];
	ihvmsg2[2] = ihv2[2]; ihvmsg2[3] = ihv2[3];
	md5compress(ihvmsg2, lmsg2);
	uint32 dihv[4] = { ihvmsg2[0]-ihvmsg1[0], ihvmsg2[1]-ihvmsg1[1],
					   ihvmsg2[2]-ihvmsg1[2], ihvmsg2[3]-ihvmsg1[3] };

	unsigned n = 64;
	if (maxblocks == 1) {
		if (dihv[0] == -(1<<5)
			&& dihv[3] == -(1<<5)+(1<<25)
			&& (dihv[2]&hybridmask) == (uint32(-(1<<5)+(1<<25)+(1<<23))&hybridmask)
			)
		{
			pair<uint32,uint32> bc(dihv[1],dihv[2]);
			if (binary_search(singleblockdata.begin(), singleblockdata.end(), bc))
				n = 1;
		} else if (dihv[0] == (1<<5)
			&& dihv[3] == (1<<5)-(1<<25)
			&& (dihv[2]&hybridmask) == (uint32((1<<5)-(1<<25)-(1<<23))&hybridmask)
			)
		{
			pair<uint32,uint32> bc(-dihv[1],-dihv[2]);
			if (binary_search(singleblockdata.begin(), singleblockdata.end(), bc))
				n = 1;
		} else
			cout << "bad!!" << endl;
	} else {
		if (dihv[0] != 0 && dihv[2] != dihv[3])
		{
			cerr << "dihv fails assumptions" << endl;
			return;
//			throw;
		}
		n = nrblocks(dihv);
	}
	LOCK_GLOBAL_MUTEX;
	++collusefull;
	if (quit == true)
		return;

	if (n < bestnrblocks) {
		bestnrblocks = n;
		cout << endl;
		status_line();
	}

	if (n <= maxblocks)
	{
		if (n >= bestnrblocks) {
			cout << endl;
			status_line();
		}
		colla1 = oa1;
		collb1 = ob1;
		collc1 = oc1;
		colla2 = oa2;
		collb2 = ob2;
		collc2 = oc2;
		cout << endl;

		cout << "IHV1   = {" << ihvmsg1[0] << "," << ihvmsg1[1] << "," << ihvmsg1[2] << "," << ihvmsg1[3] << "}" << endl;
		cout << "IHV2   = {" << ihvmsg2[0] << "," << ihvmsg2[1] << "," << ihvmsg2[2] << "," << ihvmsg2[3] << "}" << endl;
		cout << "dIHV   = {" << dihv[0] << "," << dihv[1] << "," << dihv[2] << "," << dihv[3] << "}" << endl;
		sdr v = naf(dihv[3]);
		sdr w = naf(dihv[1] - dihv[3]);
		cout << "Dv     = " << v << endl;
		cout << "Dw     = " << w << endl;
		cout << "Blocks = " << n << endl << endl;
		cout << "Msg1   = ";
		for (unsigned i = 0; i < 13; ++i)
			cout << msg1[i] << " ";
		cout << oc1 << " " << oa1 << " " << ob1 << endl;
		cout << "Msg2   = ";
		for (unsigned i = 0; i < 13; ++i)
			cout << msg2[i] << " ";
		cout << oc2 << " " << oa2 << " " << ob2 << endl;
#if 1
		quit = true;
#else
		string filename1 = workdir + "/birthdayblock1_" + boost::lexical_cast<string>(v)+"_" + boost::lexical_cast<string>(w)+".bin";
		string filename2 = workdir + "/birthdayblock2_" + boost::lexical_cast<string>(v)+"_" + boost::lexical_cast<string>(w)+".bin";
		ofstream of1(filename1.c_str(), ios::binary | ios::app);
		ofstream of2(filename2.c_str(), ios::binary | ios::app);
		save_block(of1,msg1);
		save_block(of2,msg2);
		cout << "Wrote birthdaycollision block to: " << endl;
		cout << "\t" << filename1 << endl << "\t" << filename2 << endl;
#endif
	}
}

// LOCK_GLOBAL_MUTEX required
void distribute_trail(const trail_type& newtrail)
{
	totwork += newtrail.len;
	totworkallproc += newtrail.len;
	uint32 procindex = newtrail.end[1] % procmodn;
	if (procindex == procmodi) {
		main_storage.insert_trail(newtrail);
	} else
		trail_distribution[procindex].push_back(newtrail);
}

// LOCK_GLOBAL_MUTEX required
struct trail_distribute_type {
	uint32 proci;
	uint64 totwork;
	vector< trail_type > trails;
	uint32 check;

	template<class Archive>
	void serialize(Archive& ar, const unsigned int file_version) {
		ar & boost::serialization::make_nvp("proci", proci);
		ar & boost::serialization::make_nvp("totwork", totwork);
		ar & boost::serialization::make_nvp("trails", trails);
		ar & boost::serialization::make_nvp("check", check);
	}
};

// do not use LOCK_GLOBAL_MUTEX
// function possibly calls LOCK_GLOBAL_MUTEX
void load_save_trails(bool dosave = true)
{
	static vector<uint64> workothers(procmodn, 0);
	static vector<unsigned> fileserialothers(procmodn, 0);
	static unsigned savepart = 0;
	try {
		boost::filesystem::directory_iterator dit(workdir + "/" + boost::lexical_cast<string>(procmodi)), ditend;
		for (; dit != ditend; ++dit)
		{
			boost::filesystem::path filepath = *dit;
			if (!exists(*dit)
				|| symbolic_link_exists(*dit)
				|| is_directory(*dit))
				continue;
#if BOOST_VERSION == 104300
			string filename = dit->leaf();
#else
			string filename = dit->path().filename().string();
#endif
			if (filename.size() < 16)
				continue;
			if (filename.substr(0, 12) != "birthdaydata")
				continue;
			if (filename.substr(filename.size()-4) != ".bin")
				continue;

			trail_distribute_type traildata;
			traildata.check = 0;
			try {
				load(traildata, binary_archive, *dit);
			} catch (exception& ) {} catch (...) {}
			if (traildata.check != 0x56139078) continue; // incomplete file
			try {
				boost::filesystem::remove(*dit);
			} catch (exception& ) {} catch (...) {}
			if (traildata.proci >= procmodn) continue;
			if (workothers[traildata.proci] < traildata.totwork)
				workothers[traildata.proci] = traildata.totwork;
			LOCK_GLOBAL_MUTEX;
			for (unsigned i = 0; i < traildata.trails.size(); ++i)
				if (traildata.trails[i].end[1] % procmodn != procmodi)
					cerr << "False trail loaded!!" << endl;
			main_storage.insert_trails(traildata.trails);
		}
	}
	catch (exception& ) {}
	catch (...) {}
	if (dosave) {
		try {
			trail_distribute_type traildata;
			traildata.proci = procmodi;
			traildata.totwork = totwork;
			traildata.check = 0x56139078;
			savepart = (savepart+1) % 64;
			for (unsigned i = 0; i < procmodn; ++i) {
				if ( (i%64) != savepart) continue;
				traildata.trails.clear();
				{
					LOCK_GLOBAL_MUTEX;
					swap(traildata.trails, trail_distribution[i]);
				}
				if (traildata.trails.size()) {
					try {
						++fileserialothers[i];
						string basefilename = workdir + "/" + boost::lexical_cast<string>(i)
							+ "/birthdaydata_" + boost::lexical_cast<string>(procmodi) + "_";
						string tmpfilename = "/tmp/birthdaydata_" + boost::lexical_cast<string>(i)
							+ "_" + boost::lexical_cast<string>(procmodi) + "_";
						save(traildata, binary_archive, tmpfilename + boost::lexical_cast<string>(fileserialothers[i]) + ".tmp");
						boost::filesystem::copy_file( tmpfilename + boost::lexical_cast<string>(fileserialothers[i]) + ".tmp",
							basefilename + boost::lexical_cast<string>(fileserialothers[i]) + ".bin");
						boost::filesystem::remove( tmpfilename + boost::lexical_cast<string>(fileserialothers[i]) + ".tmp" );
					}
					catch (exception& e) { cerr << e.what() << endl; }
					catch (...) {}
				}
			}
		}
		catch (exception& ) {}
		catch (...) {}
	}
	LOCK_GLOBAL_MUTEX;
	workothers[procmodi] = totwork;
	totworkallproc = 0;
	for (unsigned i = 0; i < workothers.size(); ++i)
		totworkallproc += workothers[i];
}

struct coll_less : public std::binary_function<pair<trail_type,trail_type>, pair<trail_type,trail_type>, bool>
{
	bool operator()(const pair<trail_type,trail_type>& _Left, const pair<trail_type,trail_type>& _Right) const {
		return _Left.first.len < _Right.first.len;
	}
};

struct birthday_thread {
	static uint32 id_counter; // = 1 (below class definition)
	uint32 id;
	int _cuda_device_nr;
	cuda_device _cuda_device;

	birthday_thread(int cuda_device_nr = -1)
		: _cuda_device_nr(cuda_device_nr), id(id_counter++)
	{}

	void loop_cuda(bool single = false)
	{
		vector<trail_type> work;
		vector< pair<trail_type, trail_type> > collisions;
		while (true)
		{
			uint64 seed;
			{
				LOCK_GLOBAL_MUTEX;
				seed = uint64(xrng128()) + (uint64(xrng128())<<32)+1111*procmodi;
				xrng128();
				xrng128();

				if (quit) return;
			}
#ifdef HAVE_CUDA
			if (_cuda_device_nr >= 0)
				_cuda_device.cuda_fill_trail_buffer(id, seed, work, collisions, bool(maxblocks == 1));
			else
				return;
#endif

			// insert the trails into the trail hash
			if (collisions.size() > 0)
				collisions.clear();
			else {
				LOCK_GLOBAL_MUTEX;
				for (unsigned i = 0; i < work.size(); ++i)
					distribute_trail(work[i]);
			}
			if (single)
				break;
		}
	}

	void loop(bool single = false)
	{
#ifdef HAVE_CUDA
		if (_cuda_device_nr >= 0) {
			loop_cuda(single);
			return;
		}
#endif // CUDA
		vector<trail_type> work(256);
		vector< pair<trail_type,trail_type> > collisions;
		while (true)
		{
			// generate a batch of new trail starting points
			{
				LOCK_GLOBAL_MUTEX;
				if (quit) return;
				main_storage.get_birthdaycollisions(collisions);
				work.resize(256);
				for (unsigned i = 0; i < work.size(); ++i)
				{
					work[i].start[0] = xrng128();
					work[i].start[1] = xrng128();
					work[i].start[2] = xrng128() & hybridmask;
					work[i].len = 0;
					xrng128();
					xrng128();
				}
			}
			if (collisions.size() > 0)
			{
				for (unsigned i = 0; i < collisions.size(); ++i)
					find_collision(collisions[i].first, collisions[i].second);
				collisions.clear();
				continue;
			}

			// generate the trails
			for (unsigned i = 0; i < work.size(); ++i)
				generate_trail(work[i]);

			// insert the trails into the trail hash
			{
				LOCK_GLOBAL_MUTEX;
				for (unsigned i = 0; i < work.size(); ++i)
					distribute_trail(work[i]);
			}

			if (single)
				break;
		}
	}

	void thread_run()
	{
		try
		{
			{
				LOCK_GLOBAL_MUTEX;
#ifdef HAVE_CUDA
				if (_cuda_device_nr >= 0) {
					cout << "Thread " << id << " created (CUDA)." << endl;
					if (!_cuda_device.init(_cuda_device_nr, ihv1, ihv2, ihv2mod, msg1, msg2, hybridmask, distinguishedpointmask, maximumpathlength))
						return;
//					_cuda_device.benchmark();
				} else
#endif // CUDA
				cout << "Thread " << id << " created." << endl;
			}
			loop();
			cout << "Thread " << id << " exited.          " << endl;
		}
		catch (...)
		{
			cerr << "Exception in thread " << id << "!         " << endl;
		}
		LOCK_GLOBAL_MUTEX;
	}

	void single_run()
	{
		loop(true);
	}

	void operator()()
	{
		thread_run();
	}
};
uint32 birthday_thread::id_counter = 1;
struct birthday_thread_shell {
	birthday_thread* bt;
	birthday_thread_shell(birthday_thread* _bt): bt(_bt) {}
	void operator()() { bt->operator()(); }
};

void birthday(birthday_parameters& parameters)
{
	procmodn = parameters.modn;
	procmodi = parameters.modi;
	trail_distribution.resize(procmodn);

	// add the modi parameter to the seed
	addseed(parameters.modi);

	hybridmask = (parameters.hybridbits==0) ? uint32(0) : (uint32(~0)>>(32-parameters.hybridbits));
	parameterspathtyperange = parameters.pathtyperange;
	memhardlimit = parameters.memhardlimit;
	uint64 maxtrails = 0;
	if (parameters.maxblocks > 16)
		parameters.maxblocks = 16;
	double logprob = log(dist[parameters.hybridbits][parameters.pathtyperange][parameters.maxblocks])/log(double(2));
	if (parameters.maxblocks == 1) {
		bool loaded = false;
		try {
			cout << "Loading 'singleblockdata.bin'..." << flush;
			load_bz2(singleblockdata, "singleblockdata", binary_archive);
			loaded = true;
		} catch (exception& e ) {} catch (...) {}
		if (!loaded) {
			cout << "failed!" << endl;
			cout << "Loading 'singleblockdata.txt.bz2'..." << flush;
			try {
				load_bz2(singleblockdata, "singleblockdata", text_archive);
				loaded = true;
				save_bz2(singleblockdata, "singleblockdata", binary_archive);
			} catch (exception& e) {} catch (...) {}
			if (!loaded) {
				cout << "failed!" << endl;
				return;
			}
		}
		cout << "done: " << singleblockdata.size() << " dIHVs loaded." << endl;
		logprob = (log(double(singleblockdata.size()))/log(2.0)) - (64 - parameters.hybridbits);
	}
	double estcomplexity = 32.825748 - 0.5*logprob + 0.5*double(parameters.hybridbits);
	double estcollisions = 1-logprob;

	if (parameters.maxmemory != 0)
	{
		maxtrails = uint64(parameters.maxmemory) << 19;
		maxtrails /= sizeof(trail_type);
	}
	if (parameters.logpathlength < 0)
	{
		double comppertrail = pow(double(2),estcomplexity)/double(maxtrails);
		double logcpt = log(comppertrail)/log(double(2));
		parameters.logpathlength = unsigned(logcpt+0.9);
	}
	if (parameters.maxmemory == 0)
	{
		maxtrails = pow(double(2),estcomplexity-double(parameters.logpathlength));
		uint64 tmp = (maxtrails*sizeof(trail_type))>>19;
		parameters.maxmemory = unsigned(tmp);
	}
	cout << "Maximum amount of memory in MB for trails: " << parameters.maxmemory << " (local: " << parameters.maxmemory/parameters.modn << ")" << endl;
	cout << "Estimated number of trails that will be stored:    " << maxtrails << " (local: " << maxtrails/parameters.modn << ")" << endl;
	cout << "Estimated number of trails that will be generated: " << uint64(pow(double(2),estcomplexity-parameters.logpathlength)) << endl;
	cout << "Estimated complexity per trail: 2^(" << parameters.logpathlength << ")" << endl;
	cout << "Estimated complexity on trails: 2^(" << estcomplexity << ")" << endl;
	cout << "Estimated complexity on collisions: 2^(" << parameters.logpathlength + estcollisions + 1.321928 << ")" << endl;
	cout << endl;

	distinguishedpointmask = 0;
	unsigned pathlen = parameters.logpathlength;
	for (unsigned k = 0; k < unsigned(parameters.logpathlength) && k < 32; ++k)
		distinguishedpointmask |= 1<<k;

	uint64 mpl = (uint64(1) << parameters.logpathlength)*20;
	maximumpathlength = (mpl>>31) ? 0x7FFFFFFF : mpl;

	maxblocks = parameters.maxblocks;

	for (unsigned k = 0; k < 4; ++k)
	{
		precomp1[k] = ihv1[k] = parameters.ihv1[k];
		precomp2[k] = ihv2[k] = parameters.ihv2[k];
		ihv2mod[k] = ihv2[k];
	}
	if (maxblocks == 1) {
		ihv2mod[0] -= -(1<<5);
		ihv2mod[3] -= -(1<<5) + (1<<25);
		ihv2mod[2] -= -(1<<5) + (1<<25) + (1<<23);
	}
	for (unsigned k = 0; k < 16; ++k)
	{
		msg1[k] = parameters.msg1[k];
		msg2[k] = parameters.msg2[k];
	}
	precomputestate(precomp1, msg1);
	precomputestate(precomp2, msg2);

	main_storage.set_parameters(parameters);
	main_storage.reserve_memory(maxtrails/parameters.modn);

	if (parameters.threads == 0 || parameters.threads > boost::thread::hardware_concurrency())
		parameters.threads = boost::thread::hardware_concurrency();

	int cuda_dev_cnt = 0;
#ifdef HAVE_CUDA
	if (parameters.cuda_enabled)
	{
		try
		{
			cuda_dev_cnt = get_num_cuda_devices();
		}
		catch (std::exception& e)
		{
			std::cerr << "CUDA ERROR: " << e.what() << std::endl;
			cuda_dev_cnt = 0;
		}
		std::cout << "Found " << cuda_dev_cnt << " CUDA devices." << std::endl;
	}
	// if possible save 1 cpucore for better cuda latency
	if (parameters.threads > 1 && cuda_dev_cnt > 0 && parameters.threads == boost::thread::hardware_concurrency())
		--parameters.threads;
#endif

	boost::thread_group threads;
	vector<birthday_thread*> threads_data(parameters.threads, 0);
	for (unsigned i = 0; i < threads_data.size(); ++i)
	{
		threads_data[i] = new birthday_thread();
		threads.create_thread( birthday_thread_shell(threads_data[i]) );
	}
#ifdef HAVE_CUDA
	if (parameters.cuda_enabled) {
		for (int i = 0; i < cuda_dev_cnt; ++i) {
			threads_data.push_back(new birthday_thread(i));
			threads.create_thread( birthday_thread_shell(threads_data[threads_data.size()-1]) );
		}
	}
#endif
	if (threads_data.size() == 0)
		quit = true;

	timer save_timer(true);
	while (!quit)
	{
		boost::this_thread::sleep(boost::posix_time::seconds(10));
		if (procmodn > 1) {
			if (save_timer.time() > 60) {
				load_save_trails();
				save_timer.start();
			} //else load_save_trails(false);
		}
		LOCK_GLOBAL_MUTEX;
		status_line();
	}
	cout << endl << "Waiting for threads to finish..." << flush;
	threads.join_all();
	cout << "done." << endl;

	for (unsigned i = 0; i < threads_data.size(); ++i)
		delete threads_data[i];

}


/***************************************************************************
 *   Copyright (C) 2006 by Nicola Bellotto                                 *
 *   nbellotto@lincoln.ac.uk                                                    *
 *                                                                         *
 *   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 2 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, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "ukf.h"
#include <float.h>
#include <boost/numeric/ublas/io.hpp>
#include "bayes_tracking/BayesFilter/matSup.hpp"
#include "bayes_tracking/BayesFilter/bayesFlt.hpp"

using namespace Bayesian_filter;
using namespace Bayesian_filter_matrix;

UKFilter::UKFilter(std::size_t x_size) :
        Kalman_state_filter(x_size),
        Unscented_scheme(x_size),
        z_p(Empty)
{
    UKFilter::x_size = x_size;
    UKFilter::XX_size = 2*x_size+1;
    FM::Vec x0(x_size);
    x0.clear();
    FM::SymMatrix P0(x_size, x_size);
    P0.clear();
    init(x0, P0);
}

UKFilter::UKFilter(const FM::Vec& x0, const FM::SymMatrix& P0) :
        Kalman_state_filter(x0.size()),
        Unscented_scheme(x0.size()),
        z_p(Empty)
{
    UKFilter::x_size = x0.size();
    UKFilter::XX_size = 2*x0.size()+1;
    init(x0, P0);
}

UKFilter::~UKFilter()
{
}

void UKFilter::init(const FM::Vec& x0, const FM::SymMatrix& P0)
{
    init_kalman(x0, P0);
}

void UKFilter::update(Additive_predict_model& predict_model,
                      Correlated_additive_observe_model& observe_model,
                      const FM::Vec& z)
{
    predict(predict_model);
    observe(observe_model, z);
}

void UKFilter::predict_observation(Correlated_additive_observe_model& observe_model, FM::Vec& z_pred, FM::SymMatrix& R_pred)
{
    std::size_t z_size = z_pred.size();
    ColMatrix zXX(z_size, 2*x_size+1);
    SymMatrix Xzz(z_size,z_size);
    Matrix Xxz(x_size,z_size);

    z_p.resize(z_size);
    observe_size (z_size);  // Dynamic sizing

    // Create unscented distribution
    kappa = observe_Kappa(x_size);
    Float x_kappa = Float(x_size) + kappa;
    unscented (XX, x, X, x_kappa);

    // Predict points of XX using supplied observation model
    {
        Vec zXXi(z_size), zXX0(z_size);
        zXX0 = static_cast<Correlated_additive_observe_model&>(observe_model).h( column(XX,0) );
        column(zXX,0) = zXX0;
        for (std::size_t i = 1; i < XX.size2(); ++i) {
            zXXi = static_cast<Correlated_additive_observe_model&>(observe_model).h( column(XX,i) );
            // Normalise relative to zXX0
            observe_model.normalise (zXXi, zXX0);
            column(zXX,i) = zXXi;
        }
    }

    // Mean of predicted distribution: z_p
    noalias(z_p) = column(zXX,0) * kappa;
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        noalias(z_p) += column(zXX,i) / Float(2); // ISSUE uBlas may not be able to promote integer 2
    }
    z_pred = z_p /= x_kappa;

    // Covariance of observation predict: Xzz
    // Subtract mean from each point in zXX
    for (std::size_t i = 0; i < XX_size; ++i) {
        column(zXX,i).minus_assign (z_p);
    }
    // Center point, premult here by 2 for efficency
    {
        ColMatrix::Column zXX0 = column(zXX,0);
        noalias(Xzz) = FM::outer_prod(zXX0, zXX0);
        Xzz *= 2*kappa;
    }
    // Remaining unscented points
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        ColMatrix::Column zXXi = column(zXX,i);
        noalias(Xzz) += FM::outer_prod(zXXi, zXXi);
    }
    Xzz /= 2*x_kappa;

    /** Fix for non-positive semidefinite covariance **/
    {
        ColMatrix::Column zXX0 = column(zXX,0);      // mean zp already subtracted above
        noalias(Xzz) += FM::outer_prod(zXX0, zXX0);  // modified covariance about mean
    }

    R_pred = Xzz;

//    // Correlation of state with observation: Xxz
//    // Center point, premult here by 2 for efficency
//    {
//       noalias(Xxz) = FM::outer_prod(column(XX,0) - x, column(zXX,0));
//       Xxz *= 2*kappa;
//    }
//    // Remaining unscented points
//    for (std::size_t i = 1; i < zXX.size2(); ++i) {
//       noalias(Xxz) += FM::outer_prod(column(XX,i) - x, column(zXX,i));
//    }
//    Xxz /= 2* (Float(x_size) + kappa);
}

Bayes_base::Float UKFilter::observeInnovation(Correlated_additive_observe_model& h, const Vec& si, const SymMatrix& Si)
/*
 * Observation fusion
 *  Pre : x,X
 *  Post: x,X is PSD
 */
{
    std::size_t z_size = si.size();
    ColMatrix zXX (z_size, 2*x_size+1);
    Vec zp(z_size);
    SymMatrix Xzz(z_size,z_size);
    Matrix Xxz(x_size,z_size);
    Matrix W(x_size,z_size);

    observe_size (si.size());   // Dynamic sizing

    // Create unscented distribution
    kappa = observe_Kappa(x_size);
    Float x_kappa = Float(x_size) + kappa;
    unscented (XX, x, X, x_kappa);

    // Predict points of XX using supplied observation model
    {
        Vec zXXi(z_size), zXX0(z_size);
        zXX0 = h.h( column(XX,0) );
        column(zXX,0) = zXX0;
        for (std::size_t i = 1; i < XX.size2(); ++i) {
            zXXi = h.h( column(XX,i) );
            // Normalise relative to zXX0
            h.normalise (zXXi, zXX0);
            column(zXX,i) = zXXi;
        }
    }

    // Mean of predicted distribution: zp
    noalias(zp) = column(zXX,0) * kappa;
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        noalias(zp) += column(zXX,i) / Float(2); // ISSUE uBlas may not be able to promote integer 2
    }
    zp /= x_kappa;

    // Covariance of observation predict: Xzz
    // Subtract mean from each point in zXX
    for (std::size_t i = 0; i < XX_size; ++i) {
        column(zXX,i).minus_assign (zp);
    }
    // Center point, premult here by 2 for efficency
    {
        ColMatrix::Column zXX0 = column(zXX,0);
        noalias(Xzz) = FM::outer_prod(zXX0, zXX0);
        Xzz *= 2*kappa;
    }
    // Remaining unscented points
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        ColMatrix::Column zXXi = column(zXX,i);
        noalias(Xzz) += FM::outer_prod(zXXi, zXXi);
    }
    Xzz /= 2*x_kappa;

    /** Fix for non-positive semidefinite covariance **/
    {
        ColMatrix::Column zXX0 = column(zXX,0);      // mean zp already subtracted above
        noalias(Xzz) += FM::outer_prod(zXX0, zXX0);  // modified covariance about mean
    }

    // Correlation of state with observation: Xxz
    // Center point, premult here by 2 for efficency
    {
        noalias(Xxz) = FM::outer_prod(column(XX,0) - x, column(zXX,0));
        Xxz *= 2*kappa;
    }
    // Remaining unscented points
    for (std::size_t i = 1; i < zXX.size2(); ++i) {
        noalias(Xxz) += FM::outer_prod(column(XX,i) - x, column(zXX,i));
    }
    Xxz /= 2* (Float(x_size) + kappa);

    // Innovation covariance
    S = Xzz;
    noalias(S) += h.Z;
    // Inverse innovation covariance
    Float rcond = UdUinversePD (SI, S);
    rclimit.check_PD(rcond, "S not PD in observeInnovation");
    // Kalman gain
    noalias(W) = prod(Xxz,SI);

    // Store innovation
    noalias(s) = si;

    // Filter update
    noalias(x) += prod(W,s);
    RowMatrix WStemp(W.size1(), S.size2());
    // update state cov with modified innovation cov
    noalias(X) -= prod_SPD(W, Si, WStemp);

    return rcond;
}

double UKFilter::logLikelihood() {
    SymMatrix Si(S.size1(), S.size2());
    Float detS;
    Float rcond = UdUinversePD(Si, detS, S);  // Si = inv(S)
    Numerical_rcond rclimit;
    rclimit.check_PD(rcond, "S not PD in UKFilter::logLikelihood");
    // exp(-0.5 * (s' * Si * s)) / sqrt(2pi^ns * |S|)
    return -0.5*(inner_prod(trans(s),prod(Si, s))) - 0.5*((double)s.size()*log(2*M_PI)+log(detS));
}

void UKFilter::unscented(FM::ColMatrix& XX, const FM::Vec& x, const FM::SymMatrix& X, Float scale) {
    UTriMatrix Sigma(x_size, x_size);

    // Get a upper Cholesky factoriation
    Float rcond = UCfactor(Sigma, X);
    rclimit.check_PSD(rcond, "X not PSD in UKFilter::unscented(...)");
    Sigma *= std::sqrt(scale);

    // Generate XX with the same sample Mean and Covar as before
    column(XX,0) = x;

    for (std::size_t c = 0; c < x_size; ++c) {
        UTriMatrix::Column SigmaCol = column(Sigma,c);
        noalias(column(XX,c+1)) = x  + SigmaCol;
        noalias(column(XX,x_size+c+1)) = x - SigmaCol;
    }
}