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

#ifndef SMOOTH__FEEDBACK__OCP_HPP_
#define SMOOTH__FEEDBACK__OCP_HPP_

/**
 * @file
 * @brief Optimal control problem definition.
 */

#include <Eigen/Core>
#include <smooth/diff.hpp>
#include <smooth/lie_group.hpp>

#include <iostream>

#include "traits.hpp"

namespace smooth::feedback {

// \cond
// Forward declaration
template<LieGroup _X, Manifold _U, int _Nq, int _Ncr, int _Nce>
struct OCPSolution;
// \endcond

/**
 * @brief Optimal control problem definition
 * @tparam _X state space
 * @tparam _U input space
 *
 * Problem is defined on the interval \f$ t \in [0, t_f] \f$.
 * \f[
 * \begin{cases}
 *  \min              & \theta(t_f, x_0, x_f, q)                                         \\
 *  \text{s.t.}       & x(0) = x_0                                                       \\
 *                    & x(t_f) = x_f                                                     \\
 *                    & \dot x(t) = f(t, x(t), u(t))                                     \\
 *                    & q = \int_{0}^{t_f} g(t, x(t), u(t)) \mathrm{d}t                  \\
 *                    & c_{rl} \leq c_r(t, x(t), u(t)) \leq c_{ru} \quad t \in [0, t_f]  \\
 *                    & c_{el} \leq c_e(t_f, x_0, x_f, q) \leq c_{eu}
 * \end{cases}
 * \f]
 *
 * The optimal control problem depends on arbitrary functions \f$ \theta, f, g, c_r, c_e \f$.
 * The type of those functions are template pararamters in this structure.
 *
 * @note To enable automatic differentiation \f$ \theta, f, g, c_r, c_e \f$ must be templated over
 * the scalar type.
 */
template<LieGroup _X, Manifold _U, typename Theta, typename F, typename G, typename CR, typename CE>
struct OCP
{
  /// @brief State space
  using X = _X;
  /// @brief Input space
  using U = _U;

  /// @brief State space dimension
  static constexpr int Nx = Dof<X>;
  /// @brief Input space dimension
  static constexpr int Nu = Dof<U>;
  /// @brief Number of integrals
  static constexpr int Nq = std::invoke_result_t<G, double, X, U>::SizeAtCompileTime;
  /// @brief Number of running constraints
  static constexpr int Ncr = std::invoke_result_t<CR, double, X, U>::SizeAtCompileTime;
  /// @brief Number of end constraints
  static constexpr int Nce =
    std::invoke_result_t<CE, double, X, X, Eigen::Matrix<double, Nq, 1>>::SizeAtCompileTime;

  /// @brief Solution type corresponding to this problem
  using Solution = OCPSolution<X, U, Nq, Ncr, Nce>;

  static_assert(Nx > 0, "Static size required");
  static_assert(Nu > 0, "Static size required");
  static_assert(Nq > 0, "Static size required");
  static_assert(Ncr > 0, "Static size required");
  static_assert(Nce > 0, "Static size required");

  /// @brief Objective function \f$ \theta : R \times X \times X \times R^{n_q} \rightarrow R \f$
  Theta theta;

  /// @brief System dynamics \f$ f : R \times X \times U \rightarrow Tangent<X> \f$
  F f;
  /// @brief Integrals \f$ g : R \times X \times U \rightarrow R^{n_q} \f$
  G g;

  /// @brief Running constraint \f$ c_r : R \times X \times U \rightarrow R^{n_{cr}} \f$
  CR cr;
  /// @brief Running constraint lower bound \f$ c_{rl} \in R^{n_{cr}} \f$
  Eigen::Vector<double, Ncr> crl = Eigen::Vector<double, Ncr>::Zero();
  /// @brief Running constraint upper bound \f$ c_{ru} \in R^{n_{cr}} \f$
  Eigen::Vector<double, Ncr> cru = Eigen::Vector<double, Ncr>::Zero();

  /// @brief End constraint \f$ c_e : R \times X \times X \times R^{n_q} \rightarrow R^{n_{ce}} \f$
  CE ce;
  /// @brief End constraint lower bound \f$ c_{el} \in R^{n_{ce}} \f$
  Eigen::Vector<double, Nce> cel = Eigen::Vector<double, Nce>::Zero();
  /// @brief End constraint upper bound \f$ c_{eu} \in R^{n_{ce}} \f$
  Eigen::Vector<double, Nce> ceu = Eigen::Vector<double, Nce>::Zero();
};

/// @brief Concept that is true for OCP specializations
template<typename T>
concept OCPType = traits::is_specialization_of_v<std::decay_t<T>, OCP>;

/// @brief Concept that is true for FlatOCP specializations
template<typename T>
concept FlatOCPType = OCPType<T> &&(smooth::traits::RnType<typename std::decay_t<T>::X> &&
                                      smooth::traits::RnType<typename std::decay_t<T>::U>);

/**
 * @brief Solution to OCP.
 */
template<LieGroup _X, Manifold _U, int _Nq, int _Ncr, int _Nce>
struct OCPSolution
{
  /// @brief State space
  using X = _X;
  /// @brief Input space
  using U = _U;

  /// @brief Number of integrals
  static constexpr int Nq = _Nq;
  /// @brief Number of running constraints
  static constexpr int Ncr = _Ncr;
  /// @brief Number of end constraints
  static constexpr int Nce = _Nce;

  ///@{
  /// @brief Initial and final time
  double t0, tf;
  //}@

  /// @brief Integral values
  Eigen::Vector<double, Nq> Q{};

  ///@{
  /// @brief Callable functions for state and input
  std::function<U(double)> u;
  std::function<X(double)> x;
  //}@

  /// @brief Multipliers for integral constraints
  Eigen::Vector<double, Nq> lambda_q{};

  /// @brief Multipliers for endpoint constraints
  Eigen::Vector<double, Nce> lambda_ce{};

  /// @brief Multipliers for dynamics equality constraint
  std::function<Eigen::Vector<double, Dof<X>>(double)> lambda_dyn{};

  /// @brief Multipliers for active running constraints
  std::function<Eigen::Vector<double, Ncr>(double)> lambda_cr{};
};

/**
 * @brief Test analytic derivatives for an OCP problem.
 *
 * @tparam DT differentiation method to compare against.
 *
 * @param ocp problem to test derivatives for
 * @param num_trials number of random points to test
 *
 * @todo Make it possible to test a subset of derivatives
 */
template<diff::Type DT = diff::Type::Numerical>
bool test_ocp_derivatives(OCPType auto & ocp, uint32_t num_trials = 1, double eps = 1e-4)
{
  using OCP = std::decay_t<decltype(ocp)>;

  using X = typename OCP::X;
  using U = typename OCP::U;
  using Q = Eigen::Vector<double, OCP::Nq>;

  if (!diff::detail::diffable_order1<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
    std::cout << "no jacobian for theta\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
    std::cout << "no hessian for theta\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.f), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for f\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.f), std::tuple<double, X, U>>) {
    std::cout << "no hessian for f\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.g), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for g\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.g), std::tuple<double, X, U>>) {
    std::cout << "no hessian for g\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.cr), std::tuple<double, X, U>>) {
    std::cout << "no jacobian for cr\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.cr), std::tuple<double, X, U>>) {
    std::cout << "no hessian for cr\n";
  }
  if (!diff::detail::diffable_order1<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
    std::cout << "no jacobian for ce\n";
  }
  if (!diff::detail::diffable_order2<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
    std::cout << "no hessian for ce\n";
  }

  const auto cmp = [&eps](const auto & m1, const auto & m2) {
    return (
      // clang-format off
      (m1.cols() == m2.cols())
      && (m1.rows() == m2.rows())
      && (
          m1.isApprox(m2, 1e-4) ||
          Eigen::MatrixXd(m1 - m2).cwiseAbs().maxCoeff() < eps
      )
      // clang-format on
    );
  };

  bool success = true;

  for (auto trial = 0u; trial < num_trials; ++trial) {
    // endpt parameters
    const double tf                        = 1 + static_cast<double>(std::rand()) / RAND_MAX;
    const X x0                             = Random<X>();
    const X xf                             = Random<X>();
    const Eigen::Vector<double, OCP::Nq> q = Eigen::Vector<double, OCP::Nq>::Random();
    const double t                         = 1 + static_cast<double>(std::rand()) / RAND_MAX;
    const X x                              = Random<X>();
    const U u                              = Random<U>();

    // theta
    if constexpr (diff::detail::diffable_order1<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.theta, wrt(tf, x0, xf, q));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.theta, wrt(tf, x0, xf, q));

      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of theta: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.theta), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def, d2f_def] =
        diff::dr<2, diff::Type::Analytic>(ocp.theta, wrt(tf, x0, xf, q));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.theta, wrt(tf, x0, xf, q));

      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of theta: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // end constraints
    if constexpr (diff::detail::diffable_order1<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.ce, wrt(tf, x0, xf, q));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.ce, wrt(tf, x0, xf, q));

      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of ce: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.ce), std::tuple<double, X, X, Q>>) {
      const auto [f_def, df_def, d2f_def] =
        diff::dr<2, diff::Type::Analytic>(ocp.ce, wrt(tf, x0, xf, q));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.ce, wrt(tf, x0, xf, q));

      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of ce: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // dynamics
    if constexpr (diff::detail::diffable_order1<decltype(ocp.f), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.f, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.f, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of f: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.f), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.f, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.f, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of f: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // integrand
    if constexpr (diff::detail::diffable_order1<decltype(ocp.g), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.g, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.g, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of g: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.g), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.g, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.g, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of g: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }

    // running constraints
    if constexpr (diff::detail::diffable_order1<decltype(ocp.cr), std::tuple<double, X, U>>) {
      const auto [f_def, df_def] = diff::dr<1, diff::Type::Analytic>(ocp.cr, wrt(t, x, u));
      const auto [f_num, df_num] = diff::dr<1, DT>(ocp.cr, wrt(t, x, u));
      if (!cmp(df_def, df_num)) {
        std::cout << "Error in 1st derivative of cr: got\n"
                  << Eigen::MatrixXd(df_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(df_num) << '\n';
        success = false;
      };
    }
    if constexpr (diff::detail::diffable_order2<decltype(ocp.cr), std::tuple<double, X, U>>) {
      const auto [f_def, df_def, d2f_def] = diff::dr<2, diff::Type::Analytic>(ocp.cr, wrt(t, x, u));
      const auto [f_num, df_num, d2f_num] = diff::dr<2, DT>(ocp.cr, wrt(t, x, u));
      if (!cmp(d2f_def, d2f_num)) {
        std::cout << "Error in 2nd derivative of cr: got\n"
                  << Eigen::MatrixXd(d2f_def) << "\nbut expected\n"
                  << Eigen::MatrixXd(d2f_num) << '\n';
        success = false;
      };
    }
  }

  return success;
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__OCP_HPP_


text:
/** \file
 * \page Node
 *
 * Source Files:
 *
 * *Node.cxx*
 *
 * (see also *RDataframe*, *UtilCSV37.cpp*, *Category.cpp* and *main.cpp*.)
 *
 * ##A Node Class to contain a variable
 *
 * @author David York
 * @date Sunday September 18, 2016
 * @version 0.8
 *
 * @brief This creates a Node class to contain the data and meta-data of a
 *  variable. The Variable Nodes ultimately formpart of the Dataframe class
 *  which is simply a vector of Nodes.
 *
 *  It's data, is implemented as a pointer to a vector containing the data.
 *
 *  In an Abstract sense, the Nodes are the variables, as contained for example
 *  in a vector of Nodes which is the Dataframe structure.
 *  The meta-data defines the variable fully and a void* pointer to a vector<any>
 *  containing the observation data of the#include "dataframe.cpp" variable.
 *
 *  Class methods allow maintenance and manipulation of the variable it's data
 *  as to variable name, variable (data) type variable id (or position) within
 *  any containing structure and the observation data itself.
 *
 * © 2016 David York
 */
#include <cstdio>
#include <cstdlib>
#include <cctype>
#include <iostream>
#include <ostream>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>
#include <map>
#include <tuple>
#include <algorithm>
#include <typeinfo>
#include <cxxabi.h>
#include <array>
#include <initializer_list>
#include <boost/any.hpp>

#include "node.hpp"

using namespace std;

/** default constructor */
        node::node() {
            varName = "";
            varType = "";
            varNumber = 0;
            ptrData = nullptr;
        }

        /** constructor with meta-data for variables and data as pointer to null
         *  @param r and integer value for the total number of rows of data at
         *           void* ptrData
         *  @param vName a string with the node and variable's name
         *  @param vType a string recording the variable type stored by the
         *         node at pointer ptrData
         *  @param vNumber an integer value with the node number (position
         *         in RDataframe)
         */
        node::node(int r, string vName, string vType, int vNumber) {
            nrows = r;
            varName = vName;
            varType = vType;
            varNumber = vNumber;
            ptrData = nullptr;
        }

        /** constructor with meta-data for category variable and data as pointer to null
         *  @param r and integer value for the total number of rows of data at
         *           void* ptrData
         *  @param vName a tring with the node and variable's name
         *  @param vType a string recording the variable type stored by the
         *         node at pointer ptrData
         *  @param vNumber an integer value with the node number (position
         *         in RDataframe)
         *  @param catSet a categorySet mapping levels. integers to names
         */
        node::node(int r, string vName, string vType, int vNumber, map<int, string>  catSet) {
            nrows = r;
            varName = vName;
            varType = vType;
            varNumber = vNumber;
            ptrData = nullptr;
        }

        /** constructor meta-data blank, add vector pointer to data only
         * @param void* pvData pointer to the vector of data
         */
        node::node(void* pvData) {
            varName = "";
            varType = "";
            varNumber = 0;
            ptrData = pvData;
        }

        /** constructor all meta data for variable and data as void pointer to
         *  a vector of actual type vector<vType>
         *  @param r and integer value for the total number of rows of data at
         *           void* ptrData
         *  @param vName a tring with the node and variable's name
         *  @param vType a string recording the variable type stored by the
         *         node at pointer ptrData
         *  @param vNumber an integer value with the node number (position
         *         in RDataframe)
         *  @param pvData a void pointer to the block of data (of type vType)
         *         in memory.
         */
        node::node(int r, string vName, string vType, int vNumber, void* pvData) {
            nrows = r;
            varName = vName;
            varType = vType;
            varNumber = vNumber;
            ptrData = pvData;
        }

        /** the Node class methods */
        //--------------------------------------------------------

        /** get a range of variable data from the node vector
         *  Remember! The varType needs to be used to type the data of the
         *  returned pointer
         *  @param start - the starting index for the range of data.
         *  @param the ending index for the range
         *  @return a (void) pointer to the start of memory containing the data range
         */
        void* node::getVarDataRange(int start, int ending) {
            if (varType == "int") {
                vector<int> vData = getNodeIntData(ptrData);
                static vector<int> extractData;
                for (int i = start; i <= ending; ++i) {
                    extractData.push_back(vData[i]);
                }

                cout << "extraction complete"<< endl;
                return &extractData;
		    }
		    else if (varType == "double") {
                vector<double> vData = getNodeDblData(ptrData);
                static vector<double> extractData;
                for (int i = start; i <= ending; ++i) {
                    extractData.push_back(vData[i]);
                }

                cout << "extraction complete"<< endl;
                return &extractData;
		    }
		    else if (varType == "string") {
                vector<string> vData = getNodeStrData(ptrData);
                static vector<string> extractData;
                for (int i = start; i <= ending; ++i) {
                    extractData.push_back(vData[i]);
                }

                cout << "extraction complete"<< endl;
                return &extractData;
		    }
		    else if (varType == "categoryType") {
                vector<int> vData = getNodeCatData(ptrData);
                static vector<int> extractData;
                for (int i = start; i <= ending; ++i) {
                    extractData.push_back(vData[i]);
                }

                cout << "extraction complete"<< endl;
                return &extractData;
		    }
		    else {
                cout << "Range Error" << endl;
                return nullptr;
		    }
        }

        /** get pointer to variable data from the node vector
         * requires knowledge of variable type to fetch pointer
         *  @param none
         *  @return a pointer to the start of memory containing the data vector
         */
        void* node::getPtrData() {
            return ptrData;
        }

        /** get all of the variable data from the node vector as a raw data
         * requires variable type to cast pointer
         *  @param none
         *  @return a pointer to the start of memory containing the recast data vector
         */
        void* node::getVarData() {
            if (varType == "int") {
                vector<int> vData;
                vData = getNodeIntData(ptrData);
                return &vData;
            }
            if (varType == "double") {
                vector<double> vData;
                vData = getNodeDblData(ptrData);
                return &vData;
            }
            if (varType == "string") {
                vector<string> vData;
                vData = getNodeStrData(ptrData);
                return &vData;
            }
            if (varType == "categoryType") {
                vector<int> vData;
                vData = getNodeCatData(ptrData);
                return &vData;
            }
            else return nullptr;
        }

        /** get the variable name (node meta-datum varName)
         *  @param none
         *  @return the variable name (string)
         */
        string node::getVarName() {
            return varName;
        }

        /** get the variable type (node meta-datum varType)
         *  @param none
         *  @return the variable type (string)
         */
        string node::getVarType() {
            return varType;
        } // ENDOF GETVARTYPE

        /** get the variable (position) number (node meta-datum varNumber)
         *  @param none
         *  @return the variable number (int)
         */
        int node::getVarNumber() {
            return varNumber;
        }

        /** get nrows, the number of data rows in the Node vector
         *  @param none
         *  @return nrows, an int with the number of data rows in vector
         */
        int node::getNRows() {
            return nrows;
        }

        /** get Node data as a vector of integers (OVERLOADED)
         *  @param pData a pointer to the Node data (implied or stated)
         *  @return ipData the pointer to a vector<int> with the Node data
         */
        vector<int> node::getNodeIntData () {
             ipData = (vector<int>*) ptrData;
             return *ipData;
        }
        vector<int> node::getNodeIntData (void* pData) {
             ipData = (vector<int>*) pData;
             return *ipData;
        }

        /** get Node data as a vector of doubles (OVERLOADED)
         *  @param pData a pointer to the Node data (implied or stated)
         *  @return ipData the pointer to a vector<double> with the Node data
         */
        vector<double> node::getNodeDblData () {
            dpData = (vector<double>*) ptrData;
            return *dpData;
         }
        vector<double> node::getNodeDblData (void* pData) {
            dpData = (vector<double>*) pData;
            return *dpData;
         }

         /** get Node data as a vector of strings (OVERLOADED)
         *  @param pData a pointer to the Node data (implied or stated)
         *  @return ipData the pointer to a vector<string> with the Node data
         */
         vector<std::string> node::getNodeStrData () {
             spData = (vector<string>*) ptrData;
             return *spData;
         }
         vector<std::string> node::getNodeStrData (void* pData) {
             spData = (vector<string>*) pData;
             return *spData;
         }

         /** get Node data as a vector of categories or factors (OVERLOADED)
         *  @param pData a pointer to the Node data (implied or stated)
         *  @return ipData the pointer to a vector<int> with the Node data
         */
         vector<int> node::getNodeCatData () {
             cpData = (vector<int>*) ptrData;
             return *cpData;}
         vector<int> node::getNodeCatData (void* pData) {
             cpData = (vector<int>*) pData;
             return *cpData;}

        //--------------------------------------------------------
        /** set the variable data, with a pointer to the data vector
         *  @param r, int with the number of rows in data vector (i.e its size)
         *  @param vName, string with variable name
         *  @param vType, string with variable type
         *  @param vNumber, int with variable (position number
         *  @param vData, pointer to data vector
         *  @return none, (void)
         */
        void node::setNodeContent(int r, string vName, string vType, int vNumber, void* pvData) {
            varName = vName;
            varType = vType;
            varNumber = vNumber;
            ptrData = pvData;
            nrows = r;
        }

        /** set the variable data, with a pointer to the data vector
         *  @param r, the number of rows/elements in data vector
         *  @return none (void)
         */
        void node::setVarDataRows(int r) {
            nrows = r;
        }

        /** set the variable data, with a pointer to the data vector
         *  @param pointer to data
         *  @return none (void)
         */
        void node::setVarData(void* pvData) {
            ptrData = pvData;
        }

        /** set the variable Name (nvector<int> year {data[0]};ode meta-datum varName)
         *  @param the variable name (string)
         *  @return none (void)
         */
        void node::setVarName(string vName) {
            varName = vName;
        }

        /** set / reset the variable type (node meta-datum varType) and convert
         *  vector and pointer type if necessary.
         *  @param the variable type (string)
         *  @return none (void)
         */
        void node::setVarType(string vType) {
            if(varType != "") {
            //initialize switches
            int x,y;
            if(varType == "int") x = 1;
            if(varType == "double") x = 2;
            if(varType == "string") x = 3;
            if(varType == "categoryType") x = 4;
            if(vType == "int") y = 1;
            if(vType == "double") y = 2;
            if(vType == "string") y = 3;
            if(vType == "categoryType") y = 4;
            std::vector<int> ivData;
            std::vector<double> dvData;
            std::vector<string> svData;
            switch(x) {
            case 1:{
                ivData = getNodeIntData(ptrData);
                switch (y) {
                case 1:{
                    //no change in vector type
                     break;
                }
                case 2:{
                    for (unsigned i =0; i < ivData.size(); ++i) {
                        dvData[i] = (double)(ivData[i]);
                    }
                    dpData = &dvData;
                    break;
                }
                case 3:{
                    for (unsigned i =0; i < ivData.size(); ++i) {
                        svData[i] = to_string(ivData[i]);
                    }
                    spData = &svData;
                    break;
                }
                case 4:{
                    for (unsigned i =0; i < ivData.size(); ++i) {
                        //direct cast of int to category but though
                        //no change in rank integer, check or add level name
                    }
                    break;
                }
                }
                break;
            }

            case 2:{
                std::vector<double> dvData = getNodeDblData(ptrData);
                switch (y) {
                case 1:{
                    for (unsigned i =0; i < dvData.size(); ++i) {
                        ivData[i] = (int)(dvData[i]);
                    }
                    ipData = &ivData;
                    break;
                }
                case 2:{
                    //no change in vector type
                    break;
                }
                case 3:{
                    for (unsigned i =0; i < dvData.size(); ++i) {
                        svData[i] = to_string(dvData[i]);
                    }
                    spData = &svData;
                    break;
                }
                case 4:{
                    for (unsigned i =0; i < dvData.size(); ++i) {
                        //direct cast of int to category but though
                        //no change in rank integer, check or add level name
                    }
                    break;
                }
                }
                break;
            }

            case 3:{
                vector<string> svData = getNodeStrData(ptrData);
                switch (y) {
                case 1:{
                    for (unsigned i =0; i < svData.size(); ++i) {
                        ivData[i] = stoi(svData[i]);
                    }
                    ipData = &ivData;
                    break;
                }
                case 2:{
                    for (unsigned i =0; i < svData.size(); ++i) {
                        dvData[i] = stod(svData[i]);
                    }
                    dpData = &dvData;
                    break;
                }
                case 3:{
                    //no change vector type
                    break;
                }
                case 4:{
                    for (unsigned i =0; i < svData.size(); ++i) {
                        //direct cast of int to category but though
                        //no change in rank integer, check or add level name
                    }
                    break;
                }
                }
                break;
            }

            case 4:{
                vector<int> cvData = getNodeCatData(ptrData);
                //Need to update to initialize levelname vector too
                switch (y) {
                case 1:{
                    for (unsigned i =0; i < cvData.size(); ++i) {}
                        //direct cast of int to category but though
                        //no change in rank integer, check or add level name
                    break;
                }
                case 2:{
                    for (unsigned i =0; i < cvData.size(); ++i) {}
                    break;
                }
                case 3:{
                    for (unsigned i =0; i < cvData.size(); ++i) {}
                    break;
                }
                case 4:{}
                    //no change in vector type
                    break;
                }
                }
                break;
            }
          }
            varType = vType;
        } // ENDOF SETVARTYPE

        /** set the variable (position)number (node meta-datum varNumber)
         *  @param the variable type (string)
         *  @return none (void)
         */
        void node::setVarNumber(int vNumber) {
            varNumber = vNumber;
        }

        //-------------------------------------------------
        /** display the whole node, meta-data and data
         *  @param nonefor (unsigned i =0; i < vData.size(); ++i) {}
         *  @return none (void)
         */
		void node::displayNode() {
		    cout << "Variable (Node) Contents" << endl;
		    displayName_Type();
		    cout << "position number: " << varNumber << endl;
		    cout << "number  of data vector items: " << nrows << endl;
		    if (varType == "int") {
                cout <<"All Data in Vector"<<endl;
                vector<int> vData = getNodeIntData(ptrData);
                for (int i = 0; i < vData.size(); ++i) {
                    cout << vData[i]<< ", ";
                }
                cout << endl << endl;
		    }
		    else if (varType == "double") {
                cout <<"All Data in Vector"<<endl;
                vector<double> vData = getNodeDblData(ptrData);
                for (int i = 0; i < vData.size(); ++i) {
                    cout << vData[i]<< ", ";
                }
                cout << endl << endl;
		    }
		    else if (varType == "string") {
                cout <<"All Data in Vector"<<endl;
                vector<string> vData = getNodeStrData(ptrData);
                for (int i = 0; i < vData.size(); ++i) {
                    cout << vData[i]<< ", ";
                }
                cout << endl << endl;
		    }
		    else if (varType == "categoryType") {
                cout <<"All Data in Vector"<<endl;
                vector<int> vData = getNodeCatData(ptrData);
                for (int i = 0; i < vData.size(); ++i) {
                    cout << vData[i]<< ", ";
                }
                cout << endl << endl;
		    }
		    else cout << "Unknown Variable type, Node may be corrupted of not initialized" << endl;
		}

        /** display the variable name and type
         *  @param none
         *  @return none (void)
         */
		void node::displayName_Type() {
		    cout << "Variable Name: " << varName << endl;
          cout << "Variable Type: " << varType << endl;
		}

        /** convert the node data to a string
         *  @param none
         *  @return the node as a continuous string (potentially a string stream
         */
		string node::toString() {
            string strVarName = varName;
            string strVarType = varType;
            string strVarNumber =  to_string(varNumber);
            string strVarData = "";
            int dataElement;

            for(int i = 0; i<nrows; ++i) {
                dataElement = (*(vector<int>*)ptrData)[i];
                strVarData = strVarData+" "+to_string(dataElement);
            }
			string retStr = strVarName+" "+strVarType+" "+strVarNumber+" "+strVarData;
			return retStr;
		}

ext: cpp

max_stars_repo_name: medmatix/libanalysis

max_issues_repo_name: medmatix/libanalysis

max_forks_repo_name: medmatix/libanalysis


text:
/*
 * Copyright (C) 2011-2015 Daniel Scharrer
 *
 * This software is provided 'as-is', without any express or implied
 * warranty.  In no event will the author(s) be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

/*!
 * \file
 *
 * Structures for file entries stored in Inno Setup files.
 */
#ifndef INNOEXTRACT_SETUP_FILE_HPP
#define INNOEXTRACT_SETUP_FILE_HPP

#include <string>
#include <iosfwd>

#include <boost/cstdint.hpp>

#include "setup/item.hpp"
#include "util/enum.hpp"
#include "util/flags.hpp"

namespace setup {

struct version;

struct file_entry : public item {

	FLAGS(flags,

		ConfirmOverwrite,
		NeverUninstall,
		RestartReplace,
		DeleteAfterInstall,
		RegisterServer,
		RegisterTypeLib,
		SharedFile,
		CompareTimeStamp,
		FontIsNotTrueType,
		SkipIfSourceDoesntExist,
		OverwriteReadOnly,
		OverwriteSameVersion,
		CustomDestName,
		OnlyIfDestFileExists,
		NoRegError,
		UninsRestartDelete,
		OnlyIfDoesntExist,
		IgnoreVersion,
		PromptIfOlder,
		DontCopy,
		UninsRemoveReadOnly,
		RecurseSubDirsExternal,
		ReplaceSameVersionIfContentsDiffer,
		DontVerifyChecksum,
		UninsNoSharedFilePrompt,
		CreateAllSubDirs,
		Bits32,
		Bits64,
		ExternalSizePreset,
		SetNtfsCompression,
		UnsetNtfsCompression,
		GacInstall,

		// obsolete options:
		IsReadmeFile
	);

	enum file_type {
		UserFile,
		UninstExe,
		RegSvrExe,
	};

	enum file_attributes {
		ReadOnly = 0x1
	};

	std::string source;
	std::string destination;
	std::string install_font_name;
	std::string strong_assembly_name;

	boost::uint32_t location; //!< index into the data entry list
	boost::uint32_t attributes;
	boost::uint64_t external_size;

	boost::int16_t permission; //!< index into the permission entry list

	flags options;

	file_type type;

	void load(std::istream & is, const version & version);

};

} // namespace setup

NAMED_FLAGS(setup::file_entry::flags)
NAMED_ENUM(setup::file_entry::file_type)

#endif // INNOEXTRACT_SETUP_FILE_HPP

ext: hpp


text:
// TODO: move to a library

#pragma once

#include <eosio/ship_protocol.hpp>

#include <abieos.hpp>
#include <boost/asio/connect.hpp>
#include <boost/asio/ip/tcp.hpp>
#include <boost/beast/core.hpp>
#include <boost/beast/websocket.hpp>
#include <fc/exception/exception.hpp>

namespace b1::ship_client {

namespace ship = eosio::ship_protocol;

enum request_flags {
   request_irreversible_only = 1,
   request_block             = 2,
   request_traces            = 4,
   request_deltas            = 8,
};

struct connection_callbacks {
   virtual ~connection_callbacks() = default;
   virtual void received_abi() {}
   virtual bool received(ship::get_status_result_v0& status, eosio::input_stream bin) { return true; }
   virtual bool received(ship::get_blocks_result_v0& result, eosio::input_stream bin) { return true; }
   virtual bool received(ship::get_blocks_result_v1& result, eosio::input_stream bin) { return true; }
   virtual void closed(bool retry) = 0;
};

struct connection_config {
   std::string host;
   std::string port;
};

struct abi_def_skip_table : eosio::abi_def {};

EOSIO_REFLECT(abi_def_skip_table, version, types, structs, actions, ricardian_clauses, error_messages, abi_extensions,
              variants);

struct connection : std::enable_shared_from_this<connection> {
   using error_code  = boost::system::error_code;
   using flat_buffer = boost::beast::flat_buffer;
   using tcp         = boost::asio::ip::tcp;
   using abi_type    = eosio::abi_type;

   connection_config                            config;
   std::shared_ptr<connection_callbacks>        callbacks;
   tcp::resolver                                resolver;
   boost::beast::websocket::stream<tcp::socket> stream;
   bool                                         have_abi  = false;
   abi_def_skip_table                           abi       = {};
   std::map<std::string, abi_type>              abi_types = {};

   connection(boost::asio::io_context& ioc, const connection_config& config,
              std::shared_ptr<connection_callbacks> callbacks)
       : config(config), callbacks(callbacks), resolver(ioc), stream(ioc) {

      stream.binary(true);
      stream.read_message_max(10ull * 1024 * 1024 * 1024);
   }

   void connect() {
      ilog("connect to ${h}:${p}", ("h", config.host)("p", config.port));
      resolver.async_resolve( //
            config.host, config.port,
            [self = shared_from_this(), this](error_code ec, tcp::resolver::results_type results) {
               enter_callback(ec, "resolve", [&] {
                  boost::asio::async_connect( //
                        stream.next_layer(), results.begin(), results.end(),
                        [self = shared_from_this(), this](error_code ec, auto&) {
                           enter_callback(ec, "connect", [&] {
                              stream.async_handshake( //
                                    config.host, "/", [self = shared_from_this(), this](error_code ec) {
                                       enter_callback(ec, "handshake", [&] { //
                                          start_read();
                                       });
                                    });
                           });
                        });
               });
            });
   }

   void start_read() {
      auto in_buffer = std::make_shared<flat_buffer>();
      stream.async_read(*in_buffer, [self = shared_from_this(), this, in_buffer](error_code ec, size_t) {
         enter_callback(ec, "async_read", [&] {
            if (!have_abi)
               receive_abi(in_buffer);
            else {
               if (!receive_result(in_buffer)) {
                  close(false);
                  return;
               }
            }
            start_read();
         });
      });
   }

   void receive_abi(const std::shared_ptr<flat_buffer>& p) {
      auto                     data = p->data();
      std::string              json{ (const char*)data.data(), data.size() };
      eosio::json_token_stream stream{ json.data() };
      from_json(abi, stream);
      std::string error;
      if (!abieos::check_abi_version(abi.version, error))
         throw std::runtime_error(error);
      eosio::abi a;
      convert(abi, a);
      abi_types = std::move(a.abi_types);
      have_abi  = true;
      if (callbacks)
         callbacks->received_abi();
   }

   bool receive_result(const std::shared_ptr<flat_buffer>& p) {
      auto                data = p->data();
      eosio::input_stream bin{ (const char*)data.data(), (const char*)data.data() + data.size() };
      auto                orig = bin;
      ship::result        result;
      from_bin(result, bin);
      return callbacks && std::visit([&](auto& r) { return callbacks->received(r, orig); }, result);
   }

   void request_blocks(uint32_t start_block_num, const std::vector<ship::block_position>& positions, int flags) {
      ship::get_blocks_request_v0 req;
      req.start_block_num        = start_block_num;
      req.end_block_num          = 0xffff'ffff;
      req.max_messages_in_flight = 0xffff'ffff;
      req.have_positions         = positions;
      req.irreversible_only      = flags & request_irreversible_only;
      req.fetch_block            = flags & request_block;
      req.fetch_traces           = flags & request_traces;
      req.fetch_deltas           = flags & request_deltas;
      send(req);
   }

   void request_blocks(const ship::get_status_result_v0& status, uint32_t start_block_num,
                       const std::vector<ship::block_position>& positions, int flags) {
      uint32_t nodeos_start = 0xffff'ffff;
      if (status.trace_begin_block < status.trace_end_block)
         nodeos_start = std::min(nodeos_start, status.trace_begin_block);
      if (status.chain_state_begin_block < status.chain_state_end_block)
         nodeos_start = std::min(nodeos_start, status.chain_state_begin_block);
      if (nodeos_start == 0xffff'ffff)
         nodeos_start = 0;
      request_blocks(std::max(start_block_num, nodeos_start), positions, flags);
   }

   const abi_type& get_type(const std::string& name) {
      auto it = abi_types.find(name);
      if (it == abi_types.end())
         throw std::runtime_error(std::string("unknown type ") + name);
      return it->second;
   }

   void send(const ship::request& req) {
      auto bin = std::make_shared<std::vector<char>>();
      eosio::convert_to_bin(req, *bin);
      stream.async_write(boost::asio::buffer(*bin), [self = shared_from_this(), bin, this](error_code ec, size_t) {
         enter_callback(ec, "async_write", [&] {});
      });
   }

   template <typename F>
   void catch_and_close(F f) {
      try {
         f();
      } catch (const std::exception& e) {
         elog("${e}", ("e", e.what()));
         close(false);
      } catch (...) {
         elog("unknown exception");
         close(false);
      }
   }

   template <typename F>
   void enter_callback(error_code ec, const char* what, F f) {
      if (ec)
         return on_fail(ec, what);
      catch_and_close(f);
   }

   void on_fail(error_code ec, const char* what) {
      try {
         elog("${w}: ${m}", ("w", what)("m", ec.message()));
         close(true);
      } catch (...) { elog("exception while closing"); }
   }

   void close(bool retry) {
      ilog("closing state-history socket");
      stream.next_layer().close();
      if (callbacks)
         callbacks->closed(retry);
      callbacks.reset();
   }
}; // connection

} // namespace b1::ship_client

max_stars_repo_name: forfreeday/eos

max_issues_repo_name: forfreeday/eos

max_forks_repo_name: forfreeday/eos


text:
#include "BSpline.h"

__pragma(warning(push, 0))
#include <Eigen/Eigen>
__pragma(warning(pop))

#include <iostream>

#include <omp.h>

namespace Chaf
{
	BSpline::BSpline()
	{
		m_T = { 0, 0, 0 };
	}

	double BSpline::genBasis(const std::vector<double>& T, double t, size_t i, size_t k)
	{
		if (k == 1)
		{
			if ((t >= T[i] && t < T[i + 1]) || (t >= T[i] && t <= T[i + 1] && T[i + 1] == T.back()))
			{
				return 1.0;
			}
			else
			{
				return 0.0;
			}
		}
		double left = 0.0;
		left = T[i + k - 1] - T[i] == 0.0 ? 0.0 : (t - T[i]) / (T[i + k - 1] - T[i]) * genBasis(T, t, i, k - 1);

		double right = 0.0;
		right = T[i + k] - T[i + 1] == 0.0 ? 0.0 : (T[i + k] - t) / (T[i + k] - T[i + 1]) * genBasis(T, t, i + 1, k - 1);

		return  left + right;
	}

	void BSpline::addPoint(const ImPlotPoint& p, PointType type)
	{
		Types.push_back(type);
		Control_Points_X.push_back(p.x);
		Control_Points_Y.push_back(p.y);
	}

	void BSpline::genDeBoorPoints(std::vector<double>& x, std::vector<double>& y)
	{
		std::vector<size_t> indices = { 1 };
		std::vector<double> s;
		for (size_t i = 0; i < Types.size(); i++)
		{
			indices.push_back(Types[i] + indices.back());
			s.push_back(static_cast<double>(i));
		}

		m_T.clear();
		m_T.push_back(0);
		m_T.push_back(0);
		m_T.push_back(0);
		for (size_t i = 0; i < Types.size(); i++)
		{
			for (size_t k = 0; k < Types[i]; k++)
			{
				m_T.push_back(static_cast<double>(i));
			}
		}
		m_T.push_back(static_cast<double>(Types.size() - 1));
		m_T.push_back(static_cast<double>(Types.size() - 1));
		m_T.push_back(static_cast<double>(Types.size() - 1));

		size_t n = m_T.size() - 4;

		x.resize(n);
		y.resize(n);
		Eigen::MatrixXd A(n, n);
		Eigen::MatrixXd b(n, 2);

		A.setZero();
		b.setZero();

		// Begin
		switch (Types[0])
		{
		case PointType::C2:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];
			break;
		case PointType::Line:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			A(2, 2) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];

			b(2, 0) = Control_Points_X[0];
			b(2, 1) = Control_Points_Y[0];
			break;
		case PointType::Sharp:
			A(0, 0) = 1;

			A(1, 0) = 2;
			A(1, 1) = -3;
			A(1, 2) = 1;

			A(2, 2) = 1;

			A(3, 3) = 1;

			b(0, 0) = Control_Points_X[0];
			b(0, 1) = Control_Points_Y[0];

			b(2, 0) = Control_Points_X[0];
			b(2, 1) = Control_Points_Y[0];

			b(3, 0) = Control_Points_X[0];
			b(3, 1) = Control_Points_Y[0];
			break;
		default:
			break;
		}

		// Inner
#pragma omp parallel for
		for (int i = 1; i < Types.size() - 1; i++)
		{
			size_t index = indices[i];

			if (Types[i] == PointType::C2)
			{
				A(index, index - 1) = genBasis(m_T, s[i], index - 1, 4);
				A(index, index) = genBasis(m_T, s[i], index, 4);
				A(index, index + 1) = genBasis(m_T, s[i], index + 1, 4);
				b(index, 0) = Control_Points_X[i];
				b(index, 1) = Control_Points_Y[i];
			}
			else if (Types[i] == PointType::Line)
			{
				if (Types[i - 1] != PointType::Line)
				{
					A(index, index) = 2;
					A(index, index + 1) = -3;
					A(index, index + 2) = 1;

					A(index + 1, index + 1) = 1;

					b(index + 1, 0) = Control_Points_X[i];
					b(index + 1, 1) = Control_Points_Y[i];
				}
				else if (Types[i + 1] != PointType::Line)
				{
					A(index, index) = 1;

					A(index + 1, index - 1) = 1;
					A(index + 1, index) = -3;
					A(index + 1, index + 1) = 2;

					b(index, 0) = Control_Points_X[i];
					b(index, 1) = Control_Points_Y[i];
				}
				else
				{
					A(index, index) = 1;

					A(index + 1, index + 1) = 1;

					b(index, 0) = Control_Points_X[i];
					b(index, 1) = Control_Points_Y[i];

					b(index + 1, 0) = Control_Points_X[i];
					b(index + 1, 1) = Control_Points_Y[i];
				}
			}
			else if (Types[i] == PointType::Sharp)
			{
				A(index, index - 1) = 2;
				A(index, index) = -3;
				A(index, index + 1) = 1;

				A(index + 1, index + 1) = 1;

				A(index + 2, index + 1) = 1;
				A(index + 2, index + 2) = -3;
				A(index + 2, index + 3) = 2;

				b(index + 1, 0) = Control_Points_X[i];
				b(index + 1, 1) = Control_Points_Y[i];
			}
		}

		// End
		switch (Types.back())
		{
		case PointType::C2:
			A(n - 2, n - 3) = 1;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 1) = 2;

			A(n - 1, n - 1) = 1;

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
			break;
		case PointType::Line:
			A(n - 3, n - 3) = 1;

			A(n - 2, n - 3) = 1;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 1) = 2;

			A(n - 1, n - 1) = 1;

			b(n - 3, 0) = Control_Points_X.back();
			b(n - 3, 1) = Control_Points_Y.back();

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
			break;
		case PointType::Sharp:
			A(n - 4, n - 4) = 1;

			A(n - 3, n - 3) = 1;

			A(n - 2, n - 1) = 2;
			A(n - 2, n - 2) = -3;
			A(n - 2, n - 3) = 1;

			A(n - 1, n - 1) = 1;

			b(n - 4, 0) = Control_Points_X.back();
			b(n - 4, 1) = Control_Points_Y.back();

			b(n - 3, 0) = Control_Points_X.back();
			b(n - 3, 1) = Control_Points_Y.back();

			b(n - 1, 0) = Control_Points_X.back();
			b(n - 1, 1) = Control_Points_Y.back();
		default:
			break;
		}

		std::cout << A << std::endl
			<< "===========" << std::endl;

		Eigen::MatrixXd res = A.colPivHouseholderQr().solve(b);

		for (size_t i = 0; i < x.size(); i++)
		{
			x[i] = res(i, 0);
			y[i] = res(i, 1);
		}
	}

	void BSpline::genBSpline(const std::vector<double>& px, const std::vector<double>& py, std::vector<double>& x, std::vector<double>& y, size_t sample)
	{
		x.resize(sample+1);
		y.resize(sample+1);

#pragma omp parallel for
		for (int i = 0; i <= sample; i++)
		{
			double t = m_T.back() / static_cast<double>(sample) * static_cast<double>(i);
			auto p = genBSpline(px, py, t);
			x[i] = p.x;
			y[i] = p.y;
		}
	}

	void BSpline::clear()
	{
		m_T.clear();
		Control_Points_X.clear();
		Control_Points_Y.clear();
		Types.clear();
	}

	ImPlotPoint BSpline::genBSpline(const std::vector<double>& px, const std::vector<double>& py, double t)
	{
		ImPlotPoint result = { 0.0, 0.0 };
		for (size_t i = 0; i < px.size(); i++)
		{
			auto basis = genBasis(m_T, t, i, 4);
			result.x += basis * px[i];
			result.y += basis * py[i];
		}

		return result;
	}

}

max_stars_repo_name: Chaphlagical/CAGD

max_issues_repo_name: Chaphlagical/CAGD

max_forks_repo_name: Chaphlagical/CAGD


text:
/******************************************************************************
 * Copyright 2017 The Apollo Authors. All Rights Reserved.
 * Modifications Copyright (c) 2018 LG Electronics, Inc. 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.
 *****************************************************************************/

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include "modules/localization/msf/local_map/lossless_map/lossless_map.h"
#include "modules/localization/msf/local_map/lossless_map/lossless_map_config.h"
#include "modules/localization/msf/local_map/lossless_map/lossless_map_matrix.h"
#include "modules/localization/msf/local_map/lossless_map/lossless_map_pool.h"

#include "modules/localization/msf/local_map/lossy_map/lossy_map_2d.h"
#include "modules/localization/msf/local_map/lossy_map/lossy_map_config_2d.h"
#include "modules/localization/msf/local_map/lossy_map/lossy_map_matrix_2d.h"
#include "modules/localization/msf/local_map/lossy_map/lossy_map_node_2d.h"
#include "modules/localization/msf/local_map/lossy_map/lossy_map_pool_2d.h"

namespace apollo {
namespace localization {
namespace msf {

typedef LossyMap2D LossyMap;
typedef LossyMapNode2D LossyMapNode;
typedef LossyMapNodePool2D LossyMapNodePool;
typedef LossyMapMatrix2D LossyMapMatrix;
typedef LossyMapConfig2D LossyMapConfig;

MapNodeIndex GetMapIndexFromMapFolder(const std::string& map_folder) {
  MapNodeIndex index;
  char buf[100];
  sscanf(map_folder.c_str(), "/%03u/%05s/%02d/%08u/%08u", &index.resolution_id_,
         buf, &index.zone_id_, &index.m_, &index.n_);
  std::string zone = buf;
  // std::cout << zone << std::endl;
  if (zone == "south") {
    index.zone_id_ = -index.zone_id_;
  }
  std::cout << index << std::endl;
  return index;
}

bool GetAllMapIndex(const std::string& src_map_folder,
                    const std::string& dst_map_folder,
                    std::list<MapNodeIndex>* buf) {
  std::string src_map_path = src_map_folder + "/map";
  std::string dst_map_path = dst_map_folder + "/map";
  boost::filesystem::path src_map_path_boost(src_map_path);
  boost::filesystem::path dst_map_path_boost(dst_map_path);

  if (!boost::filesystem::exists(dst_map_path)) {
    boost::filesystem::create_directory(dst_map_path_boost);
  }

  // push path of map's index to list
  buf->clear();
  // std::deque<std::string> map_bin_path;
  boost::filesystem::recursive_directory_iterator end_iter;
  boost::filesystem::recursive_directory_iterator iter(src_map_path_boost);
  for (; iter != end_iter; ++iter) {
    if (!boost::filesystem::is_directory(*iter)) {
      if (iter->path().extension() == "") {
        // map_bin_path.push_back(iter->path().string());
        std::string tmp = iter->path().string();
        tmp = tmp.substr(src_map_path.length(), tmp.length());
        buf->push_back(GetMapIndexFromMapFolder(tmp));
      }
    } else {
      std::string tmp = iter->path().string();
      tmp = tmp.substr(src_map_path.length(), tmp.length());
      tmp = dst_map_path + tmp;
      boost::filesystem::path p(tmp);
      if (!boost::filesystem::exists(p)) {
        boost::filesystem::create_directory(p);
      }
    }
  }

  return true;
}

}  // namespace msf
}  // namespace localization
}  // namespace apollo

using apollo::localization::msf::LosslessMap;
using apollo::localization::msf::LosslessMapConfig;
using apollo::localization::msf::LosslessMapMatrix;
using apollo::localization::msf::LosslessMapNode;
using apollo::localization::msf::LosslessMapNodePool;
using apollo::localization::msf::LossyMap;
using apollo::localization::msf::LossyMapConfig;
using apollo::localization::msf::LossyMapMatrix;
using apollo::localization::msf::LossyMapNode;
using apollo::localization::msf::LossyMapNodePool;
using apollo::localization::msf::MapNodeIndex;

int main(int argc, char** argv) {
  boost::program_options::options_description boost_desc("Allowed options");
  boost_desc.add_options()("help", "produce help message")(
      "srcdir", boost::program_options::value<std::string>(),
      "provide the data base dir")("dstdir",
                                   boost::program_options::value<std::string>(),
                                   "provide the lossy map destination dir");

  boost::program_options::variables_map boost_args;
  boost::program_options::store(
      boost::program_options::parse_command_line(argc, argv, boost_desc),
      boost_args);
  boost::program_options::notify(boost_args);

  if (boost_args.count("help") || !boost_args.count("srcdir") ||
      !boost_args.count("dstdir")) {
    std::cout << boost_desc << std::endl;
    return 0;
  }

  const std::string src_path = boost_args["srcdir"].as<std::string>();
  const std::string dst_path = boost_args["dstdir"].as<std::string>();
  std::string src_map_folder = src_path + "/";

  LosslessMapConfig lossless_config("lossless_map");
  LosslessMapNodePool lossless_map_node_pool(25, 8);
  lossless_map_node_pool.Initial(&lossless_config);

  LosslessMap lossless_map(&lossless_config);
  lossless_map.InitThreadPool(1, 6);
  lossless_map.InitMapNodeCaches(12, 24);
  lossless_map.AttachMapNodePool(&lossless_map_node_pool);
  if (!lossless_map.SetMapFolderPath(src_map_folder)) {
    std::cerr << "Reflectance map folder is invalid!" << std::endl;
    return -1;
  }

  // create lossy map
  std::string dst_map_folder = dst_path + "/lossy_map/";
  if (!boost::filesystem::exists(dst_map_folder)) {
    boost::filesystem::create_directory(dst_map_folder);
  }

  std::list<MapNodeIndex> buf;
  GetAllMapIndex(src_map_folder, dst_map_folder, &buf);
  std::cout << "index size: " << buf.size() << std::endl;

  LosslessMapConfig config_transform_lossy("lossless_map");
  config_transform_lossy.Load(src_map_folder + "config.xml");
  config_transform_lossy.map_version_ = "lossy_map";
  config_transform_lossy.Save(dst_map_folder + "config.xml");

  std::cout << "lossy map directory structure has built." << std::endl;

  LossyMapConfig lossy_config("lossy_map");
  LossyMapNodePool lossy_map_node_pool(25, 8);
  lossy_map_node_pool.Initial(&lossy_config);
  LossyMap lossy_map(&lossy_config);
  lossy_map.InitThreadPool(1, 6);
  lossy_map.InitMapNodeCaches(12, 24);
  lossy_map.AttachMapNodePool(&lossy_map_node_pool);
  if (!lossy_map.SetMapFolderPath(dst_map_folder)) {
    std::cout << "lossy_map config xml not exist" << std::endl;
  }

  int index = 0;
  auto itr = buf.begin();
  for (; itr != buf.end(); ++itr, ++index) {
    // int single_alt = 0;
    // int double_alt = 0;
    // float delta_alt_max = 0.0;
    // float delta_alt_min = 100.0;
    // int delta_alt_minus_num = 0;
    // float alt_max = 0.0;
    // float alt_min = 100.0;

    LosslessMapNode* lossless_node =
        static_cast<LosslessMapNode*>(lossless_map.GetMapNodeSafe(*itr));
    if (lossless_node == NULL) {
      std::cerr << "index: " << index << " is a NULL pointer!" << std::endl;
      continue;
    }
    LosslessMapMatrix& lossless_matrix =
        static_cast<LosslessMapMatrix&>(lossless_node->GetMapCellMatrix());

    LossyMapNode* lossy_node =
        static_cast<LossyMapNode*>(lossy_map.GetMapNodeSafe(*itr));
    LossyMapMatrix& lossy_matrix =
        static_cast<LossyMapMatrix&>(lossy_node->GetMapCellMatrix());

    int rows = lossless_config.map_node_size_y_;
    int cols = lossless_config.map_node_size_x_;
    for (int row = 0; row < rows; ++row) {
      for (int col = 0; col < cols; ++col) {
        float intensity = lossless_node->GetValue(row, col);
        float intensity_var = lossless_node->GetVar(row, col);
        unsigned int count = lossless_node->GetCount(row, col);

        // Read altitude
        float altitude_ground = 0.0;
        float altitude_avg = 0.0;
        bool is_ground_useful = false;
        std::vector<float> layer_alts;
        std::vector<unsigned int> layer_counts;
        lossless_matrix.GetMapCell(row, col).GetCount(&layer_counts);
        lossless_matrix.GetMapCell(row, col).GetAlt(&layer_alts);
        if (layer_counts.size() == 0 || layer_alts.size() == 0) {
          altitude_avg = lossless_node->GetAlt(row, col);
          is_ground_useful = false;
        } else {
          altitude_avg = lossless_node->GetAlt(row, col);
          altitude_ground = layer_alts[0];
          is_ground_useful = true;
        }

        lossy_matrix[row][col].intensity = intensity;
        lossy_matrix[row][col].intensity_var = intensity_var;
        lossy_matrix[row][col].count = count;
        lossy_matrix[row][col].altitude = altitude_avg;
        lossy_matrix[row][col].altitude_ground = altitude_ground;
        lossy_matrix[row][col].is_ground_useful = is_ground_useful;
      }
    }
    lossy_node->SetIsChanged(true);
  }

  return 0;
}

ext: cc


text:
#include <xmlrpc_server/xmlrpc_server.hpp>
#include <boost/tokenizer.hpp>
#include <boost/bind.hpp>
#include <xmlrpc_server/xmlrpc_utility.hpp>
#include <xmlrpc_server/xmlrpc_request_handler.hpp>
#include <iostream>

namespace FBi
{

/**
 * @brief Constructor for XMLRPC server.
 *
 * @param[in] server The HTTP server.
 * @param[in] dispatcher The HTTP request dispatcher.
 */
XMLRPC_Server::XMLRPC_Server(const boost::shared_ptr<IServer>& server, const boost::shared_ptr<IHttpRequestDispatcher>& dispatcher)
	: http_server_(server)
{
	// Register the XMLRPC request handler.
	boost::shared_ptr<IHttpRequestHandler> xmlrpc_request_handler(new XMLRPC_RequestHandler(method_call_map_));
	dispatcher->RegisterHttpRequestHandler("POST", "/RPC2", xmlrpc_request_handler);
	dispatcher->RegisterHttpRequestHandler("POST", "/RPC", xmlrpc_request_handler);
	dispatcher->RegisterHttpRequestHandler("POST", "/", xmlrpc_request_handler);
}

XMLRPC_Server::~XMLRPC_Server()
{
	this->StopServer();
}

/// Function to start the XMLRPC server.
void XMLRPC_Server::StartServer()
{
	http_server_->StartServer();
	add_introspective_methods();
}

/// Function to stop the XMLRPC server.
void XMLRPC_Server::StopServer()
{
	http_server_->StopServer();
}

/**
 * @brief Function to register the callback function for the max connection number fit the limitation.
 *
 * @param[in] callback The callback function to be registered.
 */
void XMLRPC_Server::RegisterMeetMaxConnectionCallback(const MeetMaxConnectionNumCallback& callback)
{
	http_server_->RegisterMeetMaxConnectionCallback(callback);
}

/**
 * @brief Function for register the methods of XMLRPC. It will throw exception when the method name has already existed or the callback function is invalid.
 *
 * @param[in] callback The XMLRPC callback function to register.
 * @param[in] out_signature The signature of output of XMLRPC method for introspection. It allows only one signature of output in the specification of XMLRPC.
 * @param[in] method_name The XMLRPC method name.
 * @param[in] in_signature The signature of input of XMLRPC method for introspection.
 * @param[in] method_help The help information of method for introspection.
 * @param[in] is_public The flag to control the information of method to be shown when introspect. true: The information will be shown.
 * @exception "The method name has already existed or can not find the callback."
 */
void XMLRPC_Server::AddMethod(const XMLRPC_Method_Callback& callback, const std::string& out_signature, const std::string& method_name, const std::string& in_signature, const std::string& method_help, bool is_public)
{
	if((method_call_map_.find(method_name) == method_call_map_.end()) && callback)
	{
		this->internal_add_method(callback, out_signature, method_name, in_signature, method_help, is_public);
	}
	else
	{
		// FixMe:
		// Throw exception here
		// The method name has already existed.
		throw "The method name has already existed or can not find the callback.";
	}
}

/**
 * @brief Function to unregister the XMLRPC method.
 *
 * @param[in] method_name The registered method name when use AddMethod function.
 */
void XMLRPC_Server::RemoveMethod(const std::string& method_name)
{
	// This method should be used carefully in the multi-threaded environment.
	// Because we don't use any lock here.
	MethodCallMapIter iter = method_call_map_.find(method_name);

	if(iter != method_call_map_.end())
	{
		method_call_map_.erase(iter);
	}
}

/**
 * @brief Tokenizer for the output signature (The separator are dot and space).
 *
 * @param[in] signature_str The string of signature.
 * @param[out] signature_vector The output vector of each signature.
 */
void XMLRPC_Server::signature_tokenizer(const std::string& signature_str, std::vector<std::string>& signature_vector)
{
	typedef boost::tokenizer<boost::char_separator<char> > tokenizer_t;
	boost::char_separator<char> sep(", ");
	tokenizer_t tokens(signature_str, sep);

	for(tokenizer_t::iterator tok_iter = tokens.begin(); tok_iter != tokens.end(); ++tok_iter)
		signature_vector.push_back(*tok_iter);
}

/**
 * @brief Private function for register the introspection methods of XMLRPC.
 *
 * @param[in] callback The XMLRPC callback function to register.
 * @param[in] out_signature The signature of output of XMLRPC method for introspection. It allows only one signature of output in the specification of XMLRPC.
 * @param[in] method_name The XMLRPC method name.
 * @param[in] in_signature The signature of input of XMLRPC method for introspection.
 * @param[in] method_help The help information of method for introspection.
 * @param[in] is_public The flag to control the information of method to be shown when introspect. true: The information will be shown.
 */
void XMLRPC_Server::internal_add_method(const XMLRPC_Method_Callback& callback, const std::string& out_signature, const std::string& method_name, const std::string& in_signature, const std::string& method_help, bool is_public)
{
	method_call_map_[method_name].callback = callback;
	method_call_map_[method_name].output_signature = out_signature;
	signature_tokenizer(in_signature, method_call_map_[method_name].input_signature);
	method_call_map_[method_name].method_help = method_help;
	method_call_map_[method_name].is_public = is_public;
}

/// Function to register the introspection methods (system.listMethods, system.methodSignature and system.methodHelp).
void XMLRPC_Server::add_introspective_methods()
{
	internal_add_method(boost::bind(&XMLRPC_Server::system_list_methods, this, _1, _2), "array", "system.listMethods", "", "This method returns a list of the methods the server has, by name.", true);
	internal_add_method(boost::bind(&XMLRPC_Server::system_method_signature, this, _1, _2), "array", "system.methodSignature", "string", "This method returns a description of the argument format a particular method expects.", true);
	internal_add_method(boost::bind(&XMLRPC_Server::system_method_help, this, _1, _2), "string", "system.methodHelp", "string", "This method returns a text description of a particular method.", true);
}

/**
 * @brief Callback function of system.listMethods.
 *
 * @param[in] vector The XMLRPC parameters.
 * @param[out] resp The return values of this callback.
 */
void XMLRPC_Server::system_list_methods(const XMLRPC_Value_List& /*vector*/, MethodResponse& resp)
{
	MethodCallMapConstIter iter = method_call_map_.begin();
	XMLRPCArray array;
	for(; iter != method_call_map_.end(); iter++)
	{
		if(iter->second.is_public)
		{
			array.push_back(iter->first);
		}
	}

	resp.AddArray(array);
}

/**
 * @brief Callback function of system.methodSignature.
 *
 * @param[in] vector The XMLRPC parameters.
 * @param[out] resp The return values of this callback.
 */
void XMLRPC_Server::system_method_signature(const XMLRPC_Value_List& vector, MethodResponse& resp)
{
	std::string method_name = (std::string)vector.at(0);
	MethodCallMapConstIter iter = method_call_map_.find(method_name);
	if((iter != method_call_map_.end()) && (iter->second.is_public))
	{
		XMLRPCArray array;

		// Add output signature to array
		array.push_back(iter->second.output_signature);

		// Add intput signature to array
		for(size_t i = 0; i < iter->second.input_signature.size(); ++i)
			array.push_back(iter->second.input_signature[i]);

		resp.AddArray(array);
	}
	else
	{
		resp.SetFault(-32602, XMLRPC_Utility::GetXMLRPCErrorString(-32602));
	}

}

/**
 * @brief Callback function of system.methodHelp.
 *
 * @param[in] vector The XMLRPC parameters.
 * @param[out] resp The return values of this callback.
 */
void XMLRPC_Server::system_method_help(const XMLRPC_Value_List& vector, MethodResponse& resp)
{
	std::string method_name = (std::string)vector.at(0);
	MethodCallMapConstIter iter = method_call_map_.find(method_name);
	if((iter != method_call_map_.end()) && (iter->second.is_public))
	{
		resp.AddString(iter->second.method_help);
	}
	else
	{
		resp.SetFault(-32602, XMLRPC_Utility::GetXMLRPCErrorString(-32602));
	}
}

} // namespace FBi


text:
// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <gmock/gmock.h>
#include <Eigen/Dense>

#include <mars/buffer.h>
#include <mars/sensors/imu/imu_sensor_class.h>
#include <mars/sensors/pose/pose_sensor_class.h>
#include <mars/sensors/position/position_sensor_class.h>
#include <mars/time.h>
#include <mars/type_definitions/buffer_entry_type.h>

class mars_buffer_test : public testing::Test
{
public:
};

///
/// \brief Test that the CTOR arguments are handled correct
///
TEST_F(mars_buffer_test, CTOR)
{
  // Test buffer size setting by class init argument
  mars::Buffer buffer_ctor_1(100);
  ASSERT_EQ(buffer_ctor_1.get_max_buffer_size(), 100);
  mars::Buffer buffer_ctor_2(-1 * 100);
  ASSERT_EQ(buffer_ctor_2.get_max_buffer_size(), 100);

  // Test setter of max buffer size
  mars::Buffer buffer_setter_1(100);
  buffer_setter_1.set_max_buffer_size(200);
  ASSERT_EQ(buffer_setter_1.get_max_buffer_size(), 200);
  buffer_setter_1.set_max_buffer_size(-1 * 200);
  ASSERT_EQ(buffer_setter_1.get_max_buffer_size(), 200);
}

///
/// \brief Ensure that all entry getter return false on an empty buffer
///
TEST_F(mars_buffer_test, GETTER_EMPTY_BUFFER_RETURN)
{
  const int max_buffer_size = 100;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose", core_states_sptr);

  try
  {
    ASSERT_EQ(buffer.IsEmpty(), 1);
  }
  catch (const std::exception& e)
  {
    std::cout << e.what();
    exit(EXIT_FAILURE);
  }

  // Empty buffer return tests
  mars::BufferEntryType latest_state;
  int latest_state_return = buffer.get_latest_state(&latest_state);
  ASSERT_EQ(latest_state_return, 0);

  mars::BufferEntryType oldest_state;
  int oldest_state_return = buffer.get_oldest_state(&oldest_state);
  ASSERT_EQ(oldest_state_return, 0);

  mars::BufferEntryType latest_entry;
  int latest_entry_return = buffer.get_latest_entry(&latest_entry);
  ASSERT_EQ(latest_entry_return, 0);

  mars::BufferEntryType latest_init_state;
  int latest_init_state_return = buffer.get_latest_init_state(&latest_init_state);
  ASSERT_EQ(latest_init_state_return, 0);

  mars::BufferEntryType latest_sensor_handle_state;
  int latest_sensor_handle_state_return =
      buffer.get_latest_sensor_handle_state(pose_sensor_sptr, &latest_sensor_handle_state);
  ASSERT_EQ(latest_sensor_handle_state_return, 0);

  // With index argument
  int latest_sensor_state_idx;
  buffer.get_latest_sensor_handle_state(pose_sensor_sptr, &latest_sensor_handle_state, &latest_sensor_state_idx);
  ASSERT_EQ(latest_sensor_state_idx, -1);

  mars::BufferEntryType latest_sensor_handle_measurement;
  int latest_sensor_handle_measurement_return =
      buffer.get_latest_sensor_handle_measurement(pose_sensor_sptr, &latest_sensor_handle_measurement);
  ASSERT_EQ(latest_sensor_handle_measurement_return, 0);

  mars::BufferEntryType closest_state;
  mars::Time timestamp(1);
  int closest_state_return = buffer.get_closest_state(timestamp, &closest_state);
  ASSERT_EQ(closest_state_return, 0);

  // With index argument
  int closest_state_index;
  closest_state_return = buffer.get_closest_state(timestamp, &closest_state, &closest_state_index);
  ASSERT_EQ(closest_state_index, -1);

  mars::BufferEntryType entry_at_idx;
  int index = 1;
  int entry_at_idx_return = buffer.get_entry_at_idx(index, &entry_at_idx);
  ASSERT_EQ(entry_at_idx_return, 0);
}

///
/// \brief Test that old entrys are removed if max_buffer_size is reached
///
TEST_F(mars_buffer_test, STORAGE_MAX_ENTRY)
{
  // Force removal of overflowing buffer entrys
  const int num_test_entrys = 20;
  const int max_buffer_size = 10;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);

  mars::Time timestamp(0);
  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  for (int k = num_test_entrys; k > 0; k--)
  {
    mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_1_sptr, 1);
    buffer.AddEntrySorted(entry);
  }

  std::cout << "Buffer Length: " << buffer.get_length() << std::endl;
  buffer.PrintBufferEntrys();

  ASSERT_EQ(buffer.get_length(), max_buffer_size);
}

TEST_F(mars_buffer_test, LATEST_ENTRY)
{
  const int num_test_entrys = 10;
  const int max_buffer_size = 100;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_2_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_2", core_states_sptr);

  mars::Time current_timestamp(0);
  int core_dummy = 13;
  int sensor_dummy = 15;

  for (int k = num_test_entrys; k > 0; k--)
  {
    mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

    current_timestamp = current_timestamp + mars::Time(1);

    if (k % 2 == 0)
    {
      mars::BufferEntryType entry(current_timestamp, data, pose_sensor_1_sptr, 1);
      buffer.AddEntrySorted(entry);
    }
    else
    {
      mars::BufferEntryType entry(current_timestamp, data, pose_sensor_2_sptr, 4);
      buffer.AddEntrySorted(entry);
    }
  }

  std::cout << buffer.get_length() << std::endl;
  buffer.PrintBufferEntrys();

  mars::BufferEntryType latest_entry;
  buffer.get_latest_entry(&latest_entry);

  std::cout << "Picked:\n" << latest_entry << std::endl;
  EXPECT_EQ(latest_entry.timestamp_, current_timestamp);
}

TEST_F(mars_buffer_test, OLDEST_LATEST_STATE_RETURN)
{
  const int num_test_entrys = 10;
  const int max_buffer_size = 100;
  mars::Buffer buffer(max_buffer_size);

  // Empty buffer return tests
  mars::BufferEntryType latest_state_empty;
  int latest_emtpy_state_return = buffer.get_latest_state(&latest_state_empty);
  ASSERT_EQ(latest_emtpy_state_return, 0);

  mars::BufferEntryType oldest_state_empty;
  int oldest_empty_state_return = buffer.get_oldest_state(&oldest_state_empty);
  ASSERT_EQ(oldest_empty_state_return, 0);

  // Filled buffer return tests
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_2_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_2", core_states_sptr);

  mars::Time timestamp(0);
  int core_dummy = 13;
  int sensor_dummy = 15;

  for (int k = num_test_entrys; k > 0; k--)
  {
    mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

    if (k % 2 == 0)
    {
      mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_1_sptr,
                                  mars::BufferMetadataType::sensor_state);
      buffer.AddEntrySorted(entry);
    }
    else
    {
      mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_2_sptr,
                                  mars::BufferMetadataType::measurement);
      buffer.AddEntrySorted(entry);
    }
  }

  std::cout << buffer.get_length() << std::endl;
  buffer.PrintBufferEntrys();

  mars::BufferEntryType latest_state_full;
  int latest_filled_state_return = buffer.get_latest_state(&latest_state_full);
  ASSERT_EQ(latest_filled_state_return, 1);
  ASSERT_EQ(latest_state_full.metadata_, mars::BufferMetadataType::sensor_state);
  std::cout << "Picked:\n" << latest_state_full << std::endl;

  mars::BufferEntryType oldest_state_full;
  int oldest_filled_state_return = buffer.get_oldest_state(&oldest_state_full);
  ASSERT_EQ(oldest_filled_state_return, 1);
  ASSERT_EQ(oldest_state_full.metadata_, mars::BufferMetadataType::sensor_state);
  std::cout << "Picked:\n" << oldest_state_full << std::endl;
}

///
/// \brief Test that resetting the buffer removes all entrys
///
TEST_F(mars_buffer_test, RESET_BUFFER)
{
  const int num_test_entrys = 100;
  const int max_buffer_size = 110;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_2_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_2", core_states_sptr);

  mars::Time timestamp(0);
  int core_dummy = 13;
  int sensor_dummy = 15;

  for (int k = num_test_entrys; k > 0; k--)
  {
    mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

    if (k % 2 == 0)
    {
      mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_1_sptr, 1);
      buffer.AddEntrySorted(entry);
    }
    else
    {
      mars::BufferEntryType entry(timestamp + mars::Time(1), data, pose_sensor_2_sptr, 1);
      buffer.AddEntrySorted(entry);
    }
  }

  std::cout << buffer.get_length() << std::endl;
  buffer.PrintBufferEntrys();
  ASSERT_EQ(buffer.get_length(), num_test_entrys);
  ASSERT_EQ(buffer.IsEmpty(), 0);

  buffer.ResetBufferData();
  std::cout << buffer.get_length() << std::endl;

  // Check if buffer is reset
  ASSERT_EQ(buffer.get_length(), 0);
  ASSERT_EQ(buffer.IsEmpty(), 1);
}

TEST_F(mars_buffer_test, GET_ENTRY_METHODS)
{
  mars::Buffer buffer;

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_2_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_2", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state));
  buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_2_sptr, mars::BufferMetadataType::init_state));
  buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::core_state));
  buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_2_sptr, mars::BufferMetadataType::core_state));
  buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement_ooo));
  buffer.AddEntrySorted(mars::BufferEntryType(10, data, pose_sensor_2_sptr, mars::BufferMetadataType::measurement_ooo));

  mars::BufferEntryType latest_entry_return;
  buffer.get_latest_entry(&latest_entry_return);
  ASSERT_EQ(latest_entry_return.timestamp_, 10);

  mars::BufferEntryType oldest_state_return;
  buffer.get_oldest_state(&oldest_state_return);
  ASSERT_EQ(oldest_state_return.timestamp_, 1);

  mars::BufferEntryType oldest_core_state_return;
  buffer.get_oldest_core_state(&oldest_core_state_return);
  ASSERT_EQ(oldest_core_state_return.timestamp_, 3);

  mars::BufferEntryType latest_init_state_return;
  buffer.get_latest_init_state(&latest_init_state_return);
  ASSERT_EQ(latest_init_state_return.timestamp_, 2);

  mars::BufferEntryType latest_state_return;
  buffer.get_latest_state(&latest_state_return);
  ASSERT_EQ(latest_state_return.timestamp_, 6);

  mars::BufferEntryType latest_sensor1_handle_state_return;
  buffer.get_latest_sensor_handle_state(pose_sensor_1_sptr, &latest_sensor1_handle_state_return);
  ASSERT_EQ(latest_sensor1_handle_state_return.timestamp_, 5);

  int latest_sensor1_handle_state_index;
  buffer.get_latest_sensor_handle_state(pose_sensor_1_sptr, &latest_sensor1_handle_state_return,
                                        &latest_sensor1_handle_state_index);
  ASSERT_EQ(latest_sensor1_handle_state_index, 5);

  mars::BufferEntryType latest_sensor2_handle_state_return;
  buffer.get_latest_sensor_handle_state(pose_sensor_2_sptr, &latest_sensor2_handle_state_return);
  ASSERT_EQ(latest_sensor2_handle_state_return.timestamp_, 6);

  int latest_sensor2_handle_state_index;
  buffer.get_latest_sensor_handle_state(pose_sensor_2_sptr, &latest_sensor2_handle_state_return,
                                        &latest_sensor2_handle_state_index);
  ASSERT_EQ(latest_sensor2_handle_state_index, 6);

  mars::BufferEntryType latest_sensor1_handle_measurement_return;
  buffer.get_latest_sensor_handle_measurement(pose_sensor_1_sptr, &latest_sensor1_handle_measurement_return);
  ASSERT_EQ(latest_sensor1_handle_measurement_return.timestamp_, 9);

  mars::BufferEntryType latest_sensor2_handle_measurement_return;
  buffer.get_latest_sensor_handle_measurement(pose_sensor_2_sptr, &latest_sensor2_handle_measurement_return);
  ASSERT_EQ(latest_sensor2_handle_measurement_return.timestamp_, 10);
}

TEST_F(mars_buffer_test, GET_CLOSEST_STATE)
{
  mars::Buffer buffer;

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));

  // Buffer is not empty but has no state, needs to return false

  mars::BufferEntryType no_state_entry;
  int no_state_return = buffer.get_closest_state(2, &no_state_entry);
  ASSERT_EQ(no_state_return, 0);

  // With index
  int closest_state_idx_no_state;
  buffer.get_closest_state(2, &no_state_entry, &closest_state_idx_no_state);
  ASSERT_EQ(closest_state_idx_no_state, -1);

  buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state));
  buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  // Equal timestamp
  mars::Time search_timestamp(6);
  mars::BufferEntryType equal_timestamp_return;
  buffer.get_closest_state(search_timestamp, &equal_timestamp_return);
  ASSERT_EQ(equal_timestamp_return.timestamp_, search_timestamp);

  // Equal time distance - needs to return the newer state
  mars::Time search_timestamp_2(8);
  mars::BufferEntryType equal_timestamp_distance_return;
  buffer.get_closest_state(search_timestamp_2, &equal_timestamp_distance_return);
  ASSERT_EQ(equal_timestamp_distance_return.timestamp_, 9);

  // Timestamp closer to older state
  mars::Time search_timestamp_3(6.1);
  mars::BufferEntryType close_to_older_state_return;
  buffer.get_closest_state(search_timestamp_3, &close_to_older_state_return);
  ASSERT_EQ(close_to_older_state_return.timestamp_, 6);

  // Timestamp closer to newer state
  mars::Time search_timestamp_4(6.9);
  mars::BufferEntryType close_to_newer_state_return;
  buffer.get_closest_state(search_timestamp_4, &close_to_newer_state_return);
  ASSERT_EQ(close_to_newer_state_return.timestamp_, 7);

  // Timestamp newer than newer state
  mars::Time search_timestamp_5(10);
  mars::BufferEntryType newer_than_newer_state_return;
  buffer.get_closest_state(search_timestamp_5, &newer_than_newer_state_return);
  ASSERT_EQ(newer_than_newer_state_return.timestamp_, 9);

  // Test if correct entry index is returned
  mars::Time index_timestamp(6);
  int closest_state_idx;
  mars::BufferEntryType dummy_return;
  buffer.get_closest_state(index_timestamp, &dummy_return, &closest_state_idx);
  ASSERT_EQ(closest_state_idx, 6);
}

TEST_F(mars_buffer_test, GET_CLOSEST_STATE_ONLY_ONE_STATE_IN_BUFFER)
{
  mars::Buffer buffer;
  mars::BufferDataType data;
  mars::Time timestamp(0.0);

  // setup propagation sensor
  std::shared_ptr<mars::ImuSensorClass> imu_sensor_sptr = std::make_shared<mars::ImuSensorClass>("IMU");

  mars::BufferEntryType meas_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::measurement);
  buffer.AddEntrySorted(meas_entry);

  mars::BufferEntryType state_entry(timestamp, data, imu_sensor_sptr, mars::BufferMetadataType::core_state);
  buffer.AddEntrySorted(state_entry);

  mars::BufferEntryType result;
  bool status = buffer.get_closest_state(timestamp, &result);

  EXPECT_TRUE(status);
}

TEST_F(mars_buffer_test, GET_ENTRY_AT_INDEX)
{
  const int max_buffer_size = 10;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));

  // test return entry value
  mars::BufferEntryType entry_return;

  for (int k = 0; k < 4; k++)
  {
    buffer.get_entry_at_idx(k, &entry_return);
    ASSERT_EQ(entry_return.timestamp_, k);
  }

  // test return success status

  ASSERT_EQ(buffer.get_entry_at_idx(0, &entry_return), 1);
  ASSERT_EQ(buffer.get_entry_at_idx(3, &entry_return), 1);

  ASSERT_EQ(buffer.get_entry_at_idx(-1, &entry_return), 0);
  ASSERT_EQ(buffer.get_entry_at_idx(4, &entry_return), 0);
}

TEST_F(mars_buffer_test, ADD_SORTED)
{
  const int max_buffer_size = 50;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(3.2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  buffer.PrintBufferEntrys();
  ASSERT_EQ(buffer.IsSorted(), 1);
  ASSERT_EQ(buffer.get_length(), 7);
}

TEST_F(mars_buffer_test, REMOVE_STATES_STARTING_AT_IDX)
{
  const int max_buffer_size = 10;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  buffer.AddEntrySorted(mars::BufferEntryType(0, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));
  buffer.AddEntrySorted(mars::BufferEntryType(8, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(9, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  buffer.PrintBufferEntrys();
  buffer.DeleteStatesStartingAtIdx(4);
  buffer.PrintBufferEntrys();

  ASSERT_EQ(buffer.get_length(), 6);
}

TEST_F(mars_buffer_test, MULTI_SENSOR_TYPE_SETUP)
{
  const int max_buffer_size = 100;
  mars::Buffer buffer(max_buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);
  std::shared_ptr<mars::PositionSensorClass> position_sensor_1_sptr =
      std::make_shared<mars::PositionSensorClass>("Position_1", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state));
  buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(2, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  buffer.AddEntrySorted(mars::BufferEntryType(3, data, position_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(3, data, position_sensor_1_sptr, mars::BufferMetadataType::init_state));
  buffer.AddEntrySorted(mars::BufferEntryType(4, data, position_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(4, data, position_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  buffer.AddEntrySorted(mars::BufferEntryType(6, data, position_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(6, data, position_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  buffer.PrintBufferEntrys();
  ASSERT_EQ(buffer.IsSorted(), 1);
  ASSERT_EQ(buffer.get_length(), 14);

  // If the new timestamp matches with an existing one, then the new entry is added after the existing one
  mars::BufferEntryType entry_return_0;
  buffer.get_entry_at_idx(0, &entry_return_0);
  ASSERT_EQ(entry_return_0.metadata_, mars::BufferMetadataType::measurement);

  mars::BufferEntryType entry_return_1;
  buffer.get_entry_at_idx(1, &entry_return_1);
  ASSERT_EQ(entry_return_1.metadata_, mars::BufferMetadataType::init_state);
}

TEST_F(mars_buffer_test, INSERT_DATA_IDX_TEST)
{
  mars::Buffer buffer;

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  buffer.AddEntrySorted(mars::BufferEntryType(4, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(5, data, pose_sensor_1_sptr, mars::BufferMetadataType::init_state));
  buffer.AddEntrySorted(mars::BufferEntryType(6, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  buffer.AddEntrySorted(mars::BufferEntryType(7, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state));

  // New entry is newer than newest existing entry and needs to be inserted at idx 4
  int idx_newer = buffer.InsertDataAtTimestamp(
      mars::BufferEntryType(8, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  ASSERT_EQ(4, idx_newer);

  // add entry in the middle of the buffer
  int idx_mid = buffer.InsertDataAtTimestamp(
      mars::BufferEntryType(5.3, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  ASSERT_EQ(2, idx_mid);

  idx_mid = buffer.InsertDataAtTimestamp(
      mars::BufferEntryType(5.6, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  ASSERT_EQ(3, idx_mid);

  // New entry is older than oldest existing entry and needs to be inserted at idx 0
  int idx_older = buffer.InsertDataAtTimestamp(
      mars::BufferEntryType(1, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
  ASSERT_EQ(int(0), idx_older);
}

TEST_F(mars_buffer_test, INSERT_DATA_IDX_MAX_BUFFER_TEST)
{
  const int buffer_size = 10;
  mars::Buffer buffer(buffer_size);

  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);

  int core_dummy = 13;
  int sensor_dummy = 15;
  mars::BufferDataType data(std::make_shared<int>(core_dummy), std::make_shared<int>(sensor_dummy));

  // Fill the buffer until max_buffer_size and compare the returned index
  for (int k = 0; k < buffer_size; k++)
  {
    int idx = buffer.AddEntrySorted(
        mars::BufferEntryType(k, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
    ASSERT_EQ(k, idx);
  }

  // Add further entrys at the beginning of the buffer to trigger the deletion of old states and ensure that the correct
  // index is returned
  for (int k = 0; k < 5; k++)
  {
    int idx = buffer.AddEntrySorted(
        mars::BufferEntryType(buffer_size, data, pose_sensor_1_sptr, mars::BufferMetadataType::measurement));
    ASSERT_EQ(buffer_size - 1, idx);
  }
}

TEST_F(mars_buffer_test, CHECK_LAST_SENSOR_HANDLE)
{
  mars::Buffer buffer(10);
  std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_1_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_1", core_states_sptr);
  std::shared_ptr<mars::PoseSensorClass> pose_sensor_2_sptr =
      std::make_shared<mars::PoseSensorClass>("Pose_2", core_states_sptr);

  mars::BufferDataType data;
  mars::Time timestamp(1);

  mars::BufferEntryType entry(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state);
  buffer.AddEntrySorted(entry);
  timestamp = timestamp + mars::Time(1);

  EXPECT_TRUE(buffer.CheckForLastSensorHandle(pose_sensor_1_sptr));

  // Add an entry with another sensor instance
  mars::BufferEntryType entry2(timestamp, data, pose_sensor_2_sptr, mars::BufferMetadataType::sensor_state);
  buffer.AddEntrySorted(entry2);
  timestamp = timestamp + mars::Time(1);

  EXPECT_TRUE(buffer.CheckForLastSensorHandle(pose_sensor_1_sptr));

  // Add an entry for sensor 1
  mars::BufferEntryType entry3(timestamp, data, pose_sensor_1_sptr, mars::BufferMetadataType::sensor_state);
  buffer.AddEntrySorted(entry3);

  // Two entrys of sensor 1 exist
  EXPECT_FALSE(buffer.CheckForLastSensorHandle(pose_sensor_1_sptr));
}

TEST_F(mars_buffer_test, REMOVE_OVERFLOW_ENTRIES)
{
  // TODO
}

TEST_F(mars_buffer_test, INSERT_DATA_AT_IDX)
{
  // TODO
}