//
// Created by tkhamvilai on 4/4/19.
//

#ifndef DIST_MILP_NOC_HPP
#define DIST_MILP_NOC_HPP

#include "MY_MACROS.hpp"
#include "MathHelperFunctions.hpp"
#include <iostream>
#include <vector>
#include <algorithm>
#include <list>
#include <Eigen/Dense>

class NOC{
public:
    int N_Row_CRs;
    int N_Col_CRs;
    int N_apps;
    int *N_Row_apps;
    int *N_Col_apps;

    int N_CRs;
    int N_paths;
    int *N_nodes_apps; // # of nodes corresponds to a given app
    int *N_links_apps; // # of links corresponds to a given app
    int N_nodes;
    int N_links;
    int N_neighbors;

    Eigen::MatrixXi G; // Incident matrix of the network
    Eigen::MatrixXi H; // Incident matrix of the application
    Eigen::MatrixXi A; // Adjacency matrix of the network
    Eigen::MatrixXi D; // Degree matrix of the network
    Eigen::MatrixXi L; // Laplacian matrix of the network

    Eigen::MatrixXi X_CRs_nodes;
    Eigen::MatrixXi X_paths_links;
    Eigen::MatrixXi R_apps;
    Eigen::MatrixXi M_apps;
    std::vector<Eigen::MatrixXi> X_comm_paths;
    Eigen::MatrixXi X_CRs_nodes_old;

    Eigen::MatrixXi X_CRs_nodes_received;

    int *nodes_on_CRs; // node to run on each CR, solver sends
    Eigen::MatrixXi nodes_on_CRs_received; // node to run on each CR, receives from multiple allocators
    int *apps_on_CRs; // app to run on each CR
    int node_to_run; // node to run on each CR, each node receives
    int app_to_run; // app to run on each CR
    int *comm_path_to_use; //communication path to use for communicating from the reallocators to others
    Eigen::MatrixXi comm_path_to_use_received; // comm path to use on each path, receives from multiple allocators
    int *path_to_run;

    std::vector<std::vector<int> > disconnected_sets;
    std::vector<int> Fault_Isolated_CRs_ind;
    int *Fault_Isolated_CRs;

    int N_Faults_CR;
    int prev_N_Faults_CR;
    int *Fault_CRs;
    int *Fault_Internal_CRs; // faulty status of each node, solver receives
    int fault_internal_status_CR; // faulty status of each node, each node sends
    int *Fault_External_CRs;

    int N_Faults_Paths;
    int prev_N_Faults_Paths;
    int *Fault_Paths; // overall fault paths due to both node failures and path failures
    int *Fault_Paths_receive; // faulty status of each path, solver receives
    int *Fault_Paths_send; // faulty status of neighbor paths, each node sends; -1 if node&path not adjacent, 1 if adjacent path is dead, 0 if adjacent path is alive

    int solver_status;
    int obj_val;
    int var_size;
    int con_size;

    int *app_color;
    int allocator_app_ind;
    int allocator_app_num;
    std::vector<int> allocator_nodes_ind;

    NOC(int N_Row_CRs, int N_Col_CRs, int N_apps, int N_Row_apps[], int N_Col_apps[], int app_color[], int allocator_app_ind, int allocator_app_num);

    void CreateTopology(const char* topo);
    void CreateSquareTopology();
    void CreateAuxMatrices(const char* topo);
    Eigen::MatrixXi CreateIncidentMatrixSquareTopology(int N_Row, int N_Col);
    void CreateNeighborMatrixSquareTopology();
    void CreateDecisionMatrices();

    int get_last_node_from_app(int app);
    int get_app_from_node(int node);
    int get_app_from_link(int node);

    void Update_State();
    void Clear_State();

    void Voter(int rank, int step);
    void App_Voter(int rank, int step);
    void Path_Voter(int step);
    Eigen::MatrixXi get_X_from_nodes(Eigen::MatrixXi nodes_on_CRs_received_voted, int N_rows, int N_cols);
    void Disp();
};

#endif //DIST_MILP_NOC_HPP


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/function/abs.hpp>
#include <boost/simd/function/bitwise_notand.hpp>
#include <boost/simd/function/bitofsign.hpp>
#include <boost/simd/function/signnz.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

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

  BOOST_DISPATCH_OVERLOAD ( copysign_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return saturated_(abs)(a0)*signnz(a1);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( copysign_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return bitwise_or(bitofsign(a1), bitwise_notand(Signmask<A0>(), a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( copysign_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::floating_<A0> >
                          , bd::scalar_< bd::floating_<A0> >
                                    )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 a0, A0 a1) const BOOST_NOEXCEPT
    {
      return std::copysign(a0, a1);
    }
  };

} } }

#endif


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

#include <gtest/gtest.h>

#include <Eigen/Core>
#include <smooth/so3.hpp>

#include "smooth/feedback/collocation/mesh_function.hpp"

using namespace Eigen;

/// Reduce a hstacked Hessian (as returned by smooth::diff) with multipliers
MatrixXd blocksum(const VectorXd & lambda, const MatrixXd & in)
{
  MatrixXd ret = MatrixXd::Zero(in.rows(), in.rows());
  for (auto i = 0u; i < lambda.size(); ++i) {
    ret += lambda(i) * in.block(0, i * in.rows(), in.rows(), in.rows());
  }
  return ret;
}

/// Sample analytic mesh function (does not allocate)
struct Functor
{
  SparseMatrix<double> jac, hess;

  Functor()
  {
    jac.resize(3, 6);

    jac.coeffRef(0, 0) = 0.;
    jac.coeffRef(0, 1) = 0.;
    jac.coeffRef(0, 5) = 0.;

    jac.coeffRef(1, 3) = 0.;

    jac.coeffRef(2, 0) = 0.;
    jac.coeffRef(2, 2) = 0.;
    jac.coeffRef(2, 4) = 0.;

    jac.makeCompressed();

    hess.resize(6, 3 * 6);

    hess.coeffRef(0, 0 * 6 + 1) = 0;
    hess.coeffRef(0, 0 * 6 + 5) = 0;

    hess.coeffRef(1, 0 * 6 + 0) = 0;
    hess.coeffRef(1, 0 * 6 + 1) = 0;
    hess.coeffRef(1, 0 * 6 + 5) = 0;

    hess.coeffRef(5, 0 * 6 + 0) = 0;
    hess.coeffRef(5, 0 * 6 + 1) = 0;

    // second component

    hess.coeffRef(3, 1 * 6 + 3) = 0;

    // third component

    hess.coeffRef(0, 2 * 6 + 0) = 0;
    hess.coeffRef(0, 2 * 6 + 2) = 0;
    hess.coeffRef(0, 2 * 6 + 4) = 0;

    hess.coeffRef(2, 2 * 6 + 0) = 0;
    hess.coeffRef(2, 2 * 6 + 4) = 0;

    hess.coeffRef(4, 2 * 6 + 0) = 0;
    hess.coeffRef(4, 2 * 6 + 2) = 0;

    hess.makeCompressed();
  }

  template<typename T>
  Vector3<T> operator()(T t, const Vector3<T> & x, const Vector2<T> & u)
  {
    return Vector3<T>{
      t * x.x() * x.x() * u.y(),
      x.z() * x.z(),
      t * t * x.y() * u.x(),
    };
  }

  const SparseMatrix<double> & jacobian(double t, const Vector3d & x, const Vector2d & u)
  {
    jac.coeffRef(0, 0) = x.x() * x.x() * u.y();  // df0 / dt
    jac.coeffRef(0, 1) = t * 2 * x.x() * u.y();  // df0 / dx0
    jac.coeffRef(0, 5) = t * x.x() * x.x();      // df0 / du1

    jac.coeffRef(1, 3) = 2 * x.z();  // df1 / dx2

    jac.coeffRef(2, 0) = 2 * t * x.y() * u.x();  // df2 / dt
    jac.coeffRef(2, 2) = t * t * u.x();          // df2 / dx1
    jac.coeffRef(2, 4) = t * t * x.y();          // df2 / du1

    assert(jac.isCompressed());

    return jac;
  }

  const SparseMatrix<double> & hessian(double t, const Vector3d & x, const Vector2d & u)
  {
    // first component

    hess.coeffRef(0, 0 * 6 + 1) = 2 * x.x() * u.y();  // d2f0 / dtx0
    hess.coeffRef(0, 0 * 6 + 5) = x.x() * x.x();      // d2f0 / dtu1

    hess.coeffRef(1, 0 * 6 + 0) = 2 * x.x() * u.y();  // d2f0 / dx0t
    hess.coeffRef(1, 0 * 6 + 1) = 2 * t * u.y();      // d2f0 / dx0x0
    hess.coeffRef(1, 0 * 6 + 5) = 2 * t * x.x();      // d2f0 / dx0u1

    hess.coeffRef(5, 0 * 6 + 0) = x.x() * x.x();  // d2f0 / du1t
    hess.coeffRef(5, 0 * 6 + 1) = 2 * t * x.x();  // d2f0 / du1x0

    // second component

    hess.coeffRef(3, 1 * 6 + 3) = 2;  // d2f1 / dx2x2

    // third component

    hess.coeffRef(0, 2 * 6 + 0) = 2 * x.y() * u.x();  // d2f2 / dtt
    hess.coeffRef(0, 2 * 6 + 2) = 2 * t * u.x();      // d2f2 / dtx1
    hess.coeffRef(0, 2 * 6 + 4) = 2 * t * x.y();      // d2f2 / dtu0

    hess.coeffRef(2, 2 * 6 + 0) = 2 * t * u.x();  // d2f2 / dx1t
    hess.coeffRef(2, 2 * 6 + 4) = t * t;          // d2f2 / dx1u0

    hess.coeffRef(4, 2 * 6 + 0) = 2 * t * x.y();  // d2f2 / du0t
    hess.coeffRef(4, 2 * 6 + 2) = t * t;          // d2f2 / du0x1

    assert(hess.isCompressed());

    return hess;
  }
};

class MeshFunction_Random : public testing::Test
{
protected:
  Functor f{};

  static constexpr int nx = 3, nu = 2;

  // mesh
  std::size_t N;
  smooth::feedback::Mesh<4, 6> mesh;

  // variables
  double t0 = 1;
  double tf = 6;

  Matrix<double, nx, -1> X;
  Matrix<double, nu, -1> U;

  VectorXd xflat;
  VectorXd uflat;

  void SetUp() override
  {
    mesh.refine_ph(0, 8);
    mesh.refine_ph(0, 5);

    N = mesh.N_colloc();

    X.setRandom(nx, N + 1);
    U.setRandom(nu, N);

    xflat = X.reshaped();
    uflat = U.reshaped();
  }
};

TEST_F(MeshFunction_Random, TestTheTest)
{
  const double t                    = 1;
  const Eigen::Vector<double, nx> x = Eigen::Vector<double, nx>::Random();
  const Eigen::Vector<double, nu> u = Eigen::Vector<double, nu>::Random();

  const auto [fn, dfn, d2fn] = smooth::diff::dr<2>(f, smooth::wrt(t, x, u));
  const auto [fa, dfa, d2fa] =
    smooth::diff::dr<2, smooth::diff::Type::Analytic>(f, smooth::wrt(t, x, u));

  ASSERT_TRUE(fn.isApprox(fa));
  ASSERT_TRUE(dfn.isApprox(Eigen::MatrixXd(dfa), 1e-3));
  ASSERT_TRUE(d2fn.isApprox(Eigen::MatrixXd(d2fa), 1e-3));
}

TEST_F(MeshFunction_Random, EvalDerivative)
{
  using namespace smooth;

  // numerical derivatives of function
  auto fd =
    [&](double t0var, double tfvar, const VectorXd & xvar, const VectorXd & uvar) -> VectorXd {
    Eigen::Matrix<double, nx, -1> Xvar = xvar.reshaped(nx, N + 1);
    Eigen::Matrix<double, nu, -1> Uvar = uvar.reshaped(nu, N);
    feedback::MeshValue<0> o;
    feedback::mesh_eval(o, mesh, f, t0var, tfvar, Xvar.colwise(), Uvar.colwise());
    return o.F;
  };
  auto [fval, dfval, d2fval] = diff::dr<2, diff::Type::Numerical>(fd, wrt(t0, tf, xflat, uflat));

  // numerical derivative of jacobian
  auto fdj =
    [&](double t0var, double tfvar, const VectorXd & xvar, const VectorXd & uvar) -> VectorXd {
    Eigen::Matrix<double, nx, -1> Xvar = xvar.reshaped(nx, N + 1);
    Eigen::Matrix<double, nu, -1> Uvar = uvar.reshaped(nu, N);
    feedback::MeshValue<1> o;
    // NOTE: can not double-differentiate!
    feedback::mesh_eval<1, diff::Type::Analytic>(
      o, mesh, f, t0var, tfvar, Xvar.colwise(), Uvar.colwise());
    return Eigen::MatrixXd(o.dF).transpose().reshaped();
  };
  auto [u0_, d2fval_j] = diff::dr<1, diff::Type::Numerical>(fdj, wrt(t0, tf, xflat, uflat));

  // analytic (analytic inner)
  {
    feedback::MeshValue<2> out;
    out.lambda.setRandom(nx * N);
    feedback::mesh_eval<2, diff::Type::Analytic>(out, mesh, f, t0, tf, X.colwise(), U.colwise());

    // compare values
    ASSERT_TRUE(MatrixXd(out.dF).isApprox(dfval, 1e-3));

    MatrixXd hess = MatrixXd(out.d2F).selfadjointView<Upper>();
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval), 1e-3));
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval_j.transpose()), 1e-3));
  }

  // analytic (numerical inner)
  {
    feedback::MeshValue<2> out;
    out.lambda.setRandom(nx * N);
    feedback::mesh_eval<2, diff::Type::Numerical>(out, mesh, f, t0, tf, X.colwise(), U.colwise());

    // compare values
    ASSERT_TRUE(MatrixXd(out.dF).isApprox(dfval, 1e-3));

    MatrixXd hess = MatrixXd(out.d2F).selfadjointView<Upper>();
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval), 1e-3));
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval_j.transpose()), 1e-3));
  }
}

TEST_F(MeshFunction_Random, IntegralDerivative)
{
  using namespace smooth;

  // numerical derivative of function
  auto fd =
    [&](double t0var, double tfvar, const VectorXd & xvar, const VectorXd & uvar) -> VectorXd {
    Eigen::Matrix<double, nx, -1> Xvar = xvar.reshaped(nx, N + 1);
    Eigen::Matrix<double, nu, -1> Uvar = uvar.reshaped(nu, N);
    feedback::MeshValue<0> o;
    feedback::mesh_integrate(o, mesh, f, t0var, tfvar, Xvar.colwise(), Uvar.colwise());
    return o.F;
  };
  auto [fval, dfval, d2fval] = diff::dr<2>(fd, wrt(t0, tf, xflat, uflat));

  // numerical derivative of jacobian
  auto fdj =
    [&](double t0var, double tfvar, const VectorXd & xvar, const VectorXd & uvar) -> VectorXd {
    Eigen::Matrix<double, nx, -1> Xvar = xvar.reshaped(nx, N + 1);
    Eigen::Matrix<double, nu, -1> Uvar = uvar.reshaped(nu, N);
    feedback::MeshValue<1> o;
    // NOTE: can not double-differentiate!
    feedback::mesh_integrate<1, diff::Type::Analytic>(
      o, mesh, f, t0var, tfvar, Xvar.colwise(), Uvar.colwise());
    return Eigen::MatrixXd(o.dF).transpose().reshaped();
  };
  auto [u0_, d2fval_j] = diff::dr<1, diff::Type::Numerical>(fdj, wrt(t0, tf, xflat, uflat));

  // analytic (analytic inner)
  {
    feedback::MeshValue<2> out;
    out.lambda.setRandom(nx);
    feedback::mesh_integrate<2, diff::Type::Analytic>(
      out, mesh, f, t0, tf, X.colwise(), U.colwise());

    // compare values
    ASSERT_TRUE(MatrixXd(out.dF).isApprox(dfval, 1e-3));
    MatrixXd hess = MatrixXd(out.d2F).selfadjointView<Upper>();
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval), 1e-3));
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval_j.transpose()), 1e-3));
  }

  // analytic (numerical inner)
  {
    feedback::MeshValue<2> out;
    out.lambda.setRandom(nx);
    feedback::mesh_integrate<2, diff::Type::Numerical>(
      out, mesh, f, t0, tf, X.colwise(), U.colwise());

    // compare values
    ASSERT_TRUE(MatrixXd(out.dF).isApprox(dfval, 1e-3));
    MatrixXd hess = MatrixXd(out.d2F).selfadjointView<Upper>();
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval), 1e-3));
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval_j.transpose()), 1e-3));
  }
}

TEST_F(MeshFunction_Random, DynDerivative)
{
  using namespace smooth;

  // numerical derivative of function
  auto fd =
    [&](double t0var, double tfvar, const VectorXd & xvar, const VectorXd & uvar) -> VectorXd {
    Eigen::Matrix<double, nx, -1> Xvar = xvar.reshaped(nx, N + 1);
    Eigen::Matrix<double, nu, -1> Uvar = uvar.reshaped(nu, N);
    feedback::MeshValue<0> o;
    feedback::mesh_dyn(o, mesh, f, t0var, tfvar, Xvar.colwise(), Uvar.colwise());
    return o.F;
  };
  auto [fval, dfval, d2fval] = diff::dr<2, diff::Type::Numerical>(fd, wrt(t0, tf, xflat, uflat));

  // numerical derivative of jacobian
  auto fdj =
    [&](double t0var, double tfvar, const VectorXd & xvar, const VectorXd & uvar) -> VectorXd {
    Eigen::Matrix<double, nx, -1> Xvar = xvar.reshaped(nx, N + 1);
    Eigen::Matrix<double, nu, -1> Uvar = uvar.reshaped(nu, N);
    feedback::MeshValue<1> o;
    // NOTE: can not double-differentiate with Numerical!
    feedback::mesh_dyn<1, diff::Type::Analytic>(
      o, mesh, f, t0var, tfvar, Xvar.colwise(), Uvar.colwise());
    return Eigen::MatrixXd(o.dF).transpose().reshaped();
  };
  auto [u0_, d2fval_j] = diff::dr<1, diff::Type::Numerical>(fdj, wrt(t0, tf, xflat, uflat));

  // analytic (analytic inner)
  {
    feedback::MeshValue<2> out;
    out.lambda.setRandom(nx * N);
    feedback::mesh_dyn<2, diff::Type::Analytic>(out, mesh, f, t0, tf, X.colwise(), U.colwise());

    // compare values
    ASSERT_TRUE(MatrixXd(out.dF).isApprox(dfval, 1e-3));
    MatrixXd hess = MatrixXd(out.d2F).selfadjointView<Upper>();
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval), 1e-3));
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval_j.transpose()), 1e-3));
  }

  // analytic (numerical inner)
  {
    feedback::MeshValue<2> out;
    out.lambda.setRandom(nx * N);
    feedback::mesh_dyn<2, diff::Type::Numerical>(out, mesh, f, t0, tf, X.colwise(), U.colwise());

    // compare values
    ASSERT_TRUE(MatrixXd(out.dF).isApprox(dfval, 1e-3));
    MatrixXd hess = MatrixXd(out.d2F).selfadjointView<Upper>();
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval), 1e-3));
    ASSERT_TRUE(hess.isApprox(blocksum(out.lambda, d2fval_j.transpose()), 1e-3));
  }
}

TEST_F(MeshFunction_Random, EvalAllocationAnalytic)
{
  smooth::feedback::MeshValue<2> out;
  out.lambda.setRandom(nx * N);

  // first call: not allocated
  ASSERT_FALSE(out.allocated);
  smooth::feedback::mesh_eval<2, smooth::diff::Type::Analytic>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  ASSERT_FALSE(out.dF.isCompressed());
  ASSERT_FALSE(out.d2F.isCompressed());

  out.dF.makeCompressed();
  out.d2F.makeCompressed();

  // second call: allocated
  ASSERT_TRUE(out.allocated);
  smooth::feedback::mesh_eval<2, smooth::diff::Type::Analytic>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  // expect to still be compressed (means we only touched existing coeffs)
  ASSERT_TRUE(out.dF.isCompressed());
  ASSERT_TRUE(out.d2F.isCompressed());
}

TEST_F(MeshFunction_Random, EvalAllocationNumerical)
{
  smooth::feedback::MeshValue<2> out;
  out.lambda.setRandom(nx * N);

  // first call: not allocated
  ASSERT_FALSE(out.allocated);
  smooth::feedback::mesh_eval<2, smooth::diff::Type::Numerical>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  ASSERT_FALSE(out.dF.isCompressed());
  ASSERT_FALSE(out.d2F.isCompressed());

  out.dF.makeCompressed();
  out.d2F.makeCompressed();

  // numerical eval gives non-sparse derivatives, so we expect all allocated memory to be used
  ASSERT_EQ(out.dF.nonZeros(), out.dF.data().size());
  ASSERT_EQ(out.d2F.nonZeros(), out.d2F.data().size());

  // second call: allocated
  ASSERT_TRUE(out.allocated);
  smooth::feedback::mesh_eval<2, smooth::diff::Type::Numerical>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  // expect to still be compressed (means we only touched existing coeffs)
  ASSERT_TRUE(out.dF.isCompressed());
  ASSERT_TRUE(out.d2F.isCompressed());
}

TEST_F(MeshFunction_Random, IntegrateAllocationAnalytic)
{
  smooth::feedback::MeshValue<2> out;
  out.lambda.setRandom(nx);

  // first call: not allocated
  ASSERT_FALSE(out.allocated);
  smooth::feedback::mesh_integrate<2, smooth::diff::Type::Analytic>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  ASSERT_FALSE(out.dF.isCompressed());
  ASSERT_FALSE(out.d2F.isCompressed());

  out.dF.makeCompressed();
  out.d2F.makeCompressed();

  // second call: allocated
  ASSERT_TRUE(out.allocated);
  smooth::feedback::mesh_integrate<2, smooth::diff::Type::Analytic>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  // expect to still be compressed (means we only touched existing coeffs)
  ASSERT_TRUE(out.dF.isCompressed());
  ASSERT_TRUE(out.d2F.isCompressed());
}

TEST_F(MeshFunction_Random, IntegrateAllocationNumerical)
{
  smooth::feedback::MeshValue<2> out;
  out.lambda.setRandom(nx);

  // first call: not allocated
  ASSERT_FALSE(out.allocated);
  smooth::feedback::mesh_integrate<2, smooth::diff::Type::Numerical>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  ASSERT_FALSE(out.dF.isCompressed());
  ASSERT_FALSE(out.d2F.isCompressed());

  // numerical eval gives non-sparse derivatives, so we expect all allocated memory to be used
  ASSERT_EQ(out.dF.nonZeros(), out.dF.data().size());
  ASSERT_EQ(out.d2F.nonZeros(), out.d2F.data().size());

  out.dF.makeCompressed();
  out.d2F.makeCompressed();

  // second call: allocated
  ASSERT_TRUE(out.allocated);
  smooth::feedback::mesh_integrate<2, smooth::diff::Type::Numerical>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  // expect to still be compressed (means we only touched existing coeffs)
  ASSERT_TRUE(out.dF.isCompressed());
  ASSERT_TRUE(out.d2F.isCompressed());
}

TEST_F(MeshFunction_Random, DynAllocationAnalytic)
{
  smooth::feedback::MeshValue<2> out;
  out.lambda.setRandom(nx * N);

  // first call: not allocated
  ASSERT_FALSE(out.allocated);
  smooth::feedback::mesh_dyn<2, smooth::diff::Type::Analytic>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  ASSERT_FALSE(out.dF.isCompressed());
  ASSERT_FALSE(out.d2F.isCompressed());

  out.dF.makeCompressed();
  out.d2F.makeCompressed();

  // second call: allocated
  ASSERT_TRUE(out.allocated);
  smooth::feedback::mesh_dyn<2, smooth::diff::Type::Analytic>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  // expect to still be compressed (means we only touched existing coeffs)
  ASSERT_TRUE(out.dF.isCompressed());
  ASSERT_TRUE(out.d2F.isCompressed());
}

TEST_F(MeshFunction_Random, DynAllocationNumerical)
{
  smooth::feedback::MeshValue<2> out;
  out.lambda.setRandom(nx * N);

  // first call: not allocated
  ASSERT_FALSE(out.allocated);
  smooth::feedback::mesh_dyn<2, smooth::diff::Type::Numerical>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  ASSERT_FALSE(out.dF.isCompressed());
  ASSERT_FALSE(out.d2F.isCompressed());

  // numerical eval gives non-sparse derivatives, so we expect all allocated memory to be used
  ASSERT_EQ(out.dF.nonZeros(), out.dF.data().size());
  ASSERT_EQ(out.d2F.nonZeros(), out.d2F.data().size());

  out.dF.makeCompressed();
  out.d2F.makeCompressed();

  // second call: allocated
  ASSERT_TRUE(out.allocated);
  smooth::feedback::mesh_dyn<2, smooth::diff::Type::Numerical>(
    out, mesh, f, t0, tf, X.colwise(), U.colwise());

  // expect to still be compressed (means we only touched existing coeffs)
  ASSERT_TRUE(out.dF.isCompressed());
  ASSERT_TRUE(out.d2F.isCompressed());
}

/**
 * @brief Test using time-defined trajectory x(t) = 0.1 t^2 - 0.4 t + 0.2
 */
class MeshFunction_Traj1 : public testing::Test
{
protected:
  static constexpr std::size_t nx = 1, nu = 0, nq = 1;

  static constexpr auto df_dt =
    []<typename T>(const T & t, const Vector<T, nx> &, const Vector<T, nu> &) -> Vector<T, nx> {
    return Vector<T, nx>{{0.2 * t - 0.4}};
  };

  // variables
  const double t0 = 3, tf = 5;
  Matrix<double, nx, -1> X;
  Matrix<double, nu, -1> U;

  // mesh
  smooth::feedback::Mesh<5, 5> m;

  void SetUp() override
  {
    m.refine_ph(0, 40);
    const auto N = m.N_colloc();

    X.setRandom(nx, N + 1);
    U.setRandom(nu, N);

    for (const auto & [i, tau] : smooth::utils::zip(std::views::iota(0u), m.all_nodes())) {
      const auto s = t0 + (tf - t0) * tau;
      X(i)         = 0.1 * s * s - 0.4 * s + 0.2;
    }
  }
};

TEST_F(MeshFunction_Traj1, Integral)
{
  const auto g =
    []<typename T>(const T &, const Vector<T, nx> & x, const Vector<T, nu> &) -> Vector<T, nq> {
    return Vector<T, nq>{{0.1 + x.squaredNorm()}};
  };

  smooth::feedback::MeshValue<0> out;
  smooth::feedback::mesh_integrate(out, m, g, t0, tf, X.colwise(), U.colwise());
  ASSERT_NEAR(out.F.x(), 0.217333 + 0.1 * (tf - t0), 1e-4);
}

TEST_F(MeshFunction_Traj1, Dynamics)
{
  smooth::feedback::MeshValue<1> out;
  smooth::feedback::mesh_dyn<1>(out, m, df_dt, t0, tf, X.colwise(), U.colwise());

  ASSERT_LE(out.F.cwiseAbs().maxCoeff(), 1e-8);
}

/**
 * @brief Test using time-defined trajectory x(t) = 1.5 exp(-t)
 */
class MeshFunction_Traj2 : public testing::Test
{
protected:
  static constexpr std::size_t nx = 1, nu = 0, nq = 1;

  static constexpr auto df_dt =
    []<typename T>(const T &, const Vector<T, nx> & x, const Vector<T, nu> &) -> Vector<T, nx> {
    return Vector<T, nx>{{-x.x()}};
  };

  // variables
  const double t0 = 3, tf = 5;
  Matrix<double, nx, -1> X;
  Matrix<double, nu, -1> U;

  // mesh
  smooth::feedback::Mesh<5, 5> m;

  void SetUp() override
  {
    m.refine_ph(0, 40);
    const auto N = m.N_colloc();

    X.setRandom(nx, N + 1);
    U.setRandom(nu, N);

    for (const auto & [i, tau] : smooth::utils::zip(std::views::iota(0u), m.all_nodes())) {
      const auto s = t0 + (tf - t0) * tau;
      X(i)         = 1.5 * exp(-s);
    }
  }
};

TEST_F(MeshFunction_Traj2, Integral)
{
  const auto g =
    []<typename T>(const T &, const Vector<T, nx> & x, const Vector<T, nu> &) -> Vector<T, nq> {
    return Vector<T, nq>{{x.squaredNorm()}};
  };

  smooth::feedback::MeshValue<0> out;
  smooth::feedback::mesh_integrate(out, m, g, t0, tf, X.colwise(), U.colwise());
  ASSERT_NEAR(out.F.x(), 0.00273752, 1e-4);
}

TEST_F(MeshFunction_Traj2, Dynamics)
{
  smooth::feedback::MeshValue<1> out;
  smooth::feedback::mesh_dyn<1>(out, m, df_dt, t0, tf, X.colwise(), U.colwise());
  ASSERT_LE(out.F.cwiseAbs().maxCoeff(), 1e-8);
}


#include <random>

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <Eigen/Dense>
#include <unsupported/Eigen/AutoDiff>

#include <geometry/camera.h>
#include <ceres/jet.h>
#include <ceres/rotation.h>

class CameraFixture : public ::testing::Test {
 public:
  CameraFixture() {
    pixels.resize(pixels_count, 2);

    std::mt19937 gen(42);
    std::uniform_real_distribution<> rand(-0.5, 0.5);
    for (int i = 0; i < pixels_count; ++i) {
      const auto x = rand(gen) * max_width;
      const auto y = rand(gen) * max_height;
      pixels.row(i) << x, y;
    }

    distortion.resize(2);
    distortion << -0.1, 0.03;
    distortion_brown.resize(5);
    distortion_brown << -0.1, 0.03, 0.001, 0.001, 0.002;
    distortion_fisheye.resize(4);
    distortion_fisheye << -0.1, 0.03, 0.001, 0.005;
    principal_point << 0.1, -0.05;

    for (int i = 0; i < 3; ++i) {
      point_adiff[i].value() = point[i] = (i + 1) / 10.0;
    }
  }

  double ComputeError(const Eigen::MatrixX2d& other) {
    double error = 0;
    for (int i = 0; i < pixels_count; ++i) {
      error += (other.row(i) - pixels.row(i)).norm();
    }
    return error/pixels_count;
  }

  template <class MAT>
  void RunJacobianEval(const Camera& camera, ProjectionType projection,
                       MAT* jacobian) {
    const VecXd camera_params = camera.GetParametersValues();
    const int size_params = 3 + camera_params.size();

    // Prepare Eigen's Autodiff structures
    for (int i = 0; i < 3; ++i) {
      point_adiff[i].derivatives() = Eigen::VectorXd::Unit(size_params, i);
    }
    camera_adiff.resize(camera_params.size());
    for (int i = 0; i < camera_params.size(); ++i) {
      camera_adiff(i).value() = camera_params(i);
      camera_adiff(i).derivatives() = Eigen::VectorXd::Unit(size_params, 3 + i);
    }

    // Run project with Autodiff types to get expected jacobian
    Dispatch<ProjectFunction>(projection, point_adiff, camera_adiff.data(),
                              projection_expected);

    // Analytical derivatives
    Dispatch<ProjectDerivativesFunction>(
        projection, point, camera_params.data(), projected, jacobian->data());
  }

  template <class MAT>
  void CheckJacobian(const MAT& jacobian, int size_params) {
    const double eps = 1e-12;
    for (int i = 0; i < 2; ++i) {
      for (int j = 0; j < size_params; ++j) {
        ASSERT_NEAR(projection_expected[i].derivatives()(j), jacobian(i, j), eps);
      }
      ASSERT_NEAR(projection_expected[i].value(), projected[i], eps);
    }
  }

  int pixels_count{10000};
  Eigen::MatrixX2d pixels;

  double max_width{0.90};
  double max_height{0.75};
  double focal{0.4};
  double new_focal{0.8};
  double new_ar{0.9};

  int pixel_width{4000};
  int pixel_height{3000};

  Eigen::VectorXd distortion;
  Eigen::VectorXd distortion_brown;
  Eigen::VectorXd distortion_fisheye;
  Eigen::Vector2d principal_point;

  double point[3];
  typedef Eigen::AutoDiffScalar<Eigen::VectorXd> AScalar;
  AScalar point_adiff[3];
  VecX<AScalar> camera_adiff;

  AScalar projection_expected[2];
  double projected[2];
};

TEST_F(CameraFixture, PerspectiveIsConsistent){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, BrownIsConsistent){
  Camera camera = Camera::CreateBrownCamera(focal, 1.0, principal_point, distortion_brown);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, FisheyeIsConsistent){
  Camera camera = Camera::CreateFisheyeCamera(focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, FisheyeExtendedIsConsistent){
  Camera camera = Camera::CreateFisheyeExtendedCamera(focal, 1.0, principal_point, distortion_fisheye);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, DualIsConsistent){
  Camera camera = Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);
  const auto projected = camera.ProjectMany(camera.BearingsMany(pixels));
  ASSERT_LT(ComputeError(projected), 2e-7);
}

TEST_F(CameraFixture, SphericalIsConsistent){
  Camera camera = Camera::CreateSphericalCamera();
  const auto projected = camera.Project(camera.Bearing(Eigen::Vector2d(0.2, 0.2)));
  ASSERT_EQ(projected(0), 0.2);
  ASSERT_EQ(projected(1), 0.2);
}

TEST_F(CameraFixture, SphericalReturnCorrectTypes){
  Camera camera = Camera::CreateSphericalCamera();
  const auto types = camera.GetParametersTypes();
  ASSERT_EQ(1, types.size());
  ASSERT_EQ(Camera::Parameters::None, types[0]);
}

TEST_F(CameraFixture, PerspectiveReturnCorrectTypes){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<Camera::Parameters>(
      {Camera::Parameters::K1, Camera::Parameters::K2,
       Camera::Parameters::Focal});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, FisheyeReturnCorrectTypes) {
  Camera camera = Camera::CreateFisheyeCamera(focal, distortion[0], distortion[1]);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<Camera::Parameters>(
      {Camera::Parameters::K1, Camera::Parameters::K2,
       Camera::Parameters::Focal});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, FisheyeExtendedReturnCorrectTypes) {
  Camera camera = Camera::CreateFisheyeExtendedCamera(focal, 1.0, principal_point, distortion_fisheye);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<Camera::Parameters>(
      {Camera::Parameters::K1, Camera::Parameters::K2, Camera::Parameters::K3,
       Camera::Parameters::K4, Camera::Parameters::Focal, Camera::Parameters::AspectRatio,
       Camera::Parameters::Cx, Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, BrownReturnCorrectTypes){
  Camera camera = Camera::CreateBrownCamera(focal, 1.0, principal_point, distortion_brown);
  const auto types = camera.GetParametersTypes();

  const auto expected = std::vector<Camera::Parameters>(
      {Camera::Parameters::K1, Camera::Parameters::K2, Camera::Parameters::K3,
       Camera::Parameters::P1, Camera::Parameters::P2,
       Camera::Parameters::Focal, Camera::Parameters::AspectRatio,
       Camera::Parameters::Cx, Camera::Parameters::Cy});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, DualReturnCorrectTypes){
  Camera camera = Camera::Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);
  const auto types = camera.GetParametersTypes();
  const auto expected = std::vector<Camera::Parameters>(
      {Camera::Parameters::Transition, Camera::Parameters::K1,
       Camera::Parameters::K2, Camera::Parameters::Focal});
  ASSERT_THAT(expected, ::testing::ContainerEq(types));
}

TEST_F(CameraFixture, PerspectiveReturnCorrectValues){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();

  Eigen::VectorXd expected(3);
  expected << distortion[0], distortion[1], focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, FisheyeReturnCorrectValues) {
  Camera camera = Camera::CreateFisheyeCamera(focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();

  Eigen::VectorXd expected(3);
  expected << distortion[0], distortion[1], focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, FisheyeExtendedReturnCorrectValues) {
  Camera camera = Camera::CreateFisheyeExtendedCamera(focal, 1.0, principal_point, distortion_fisheye);
  const auto values = camera.GetParametersValues();

  Eigen::VectorXd expected(8);
  expected << distortion_fisheye, focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, BrownReturnCorrectValues){
  Camera camera = Camera::CreateBrownCamera(focal, 1.0, principal_point, distortion_brown);
  const auto values = camera.GetParametersValues();

  Eigen::VectorXd expected(9);
  expected << distortion_brown, focal, 1.0, principal_point;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, DualReturnCorrectValues){
  Camera camera = Camera::Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();

  Eigen::VectorXd expected(4);
  expected << 0.5, distortion[0], distortion[1], focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, PerspectiveReturnCorrectK){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  expected(0, 0) = expected(1, 1) = focal;
  ASSERT_EQ(expected, camera.GetProjectionMatrix());
}

TEST_F(CameraFixture, CanSetParameter){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);

  const double new_focal = 0.1;
  camera.SetParameterValue(Camera::Parameters::Focal, new_focal);
  const auto values = camera.GetParametersValues();

  Eigen::VectorXd expected(3);
  expected << distortion[0], distortion[1], new_focal;
  ASSERT_EQ(expected, values);
}

TEST_F(CameraFixture, DoSetParameter){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();
  EXPECT_NO_THROW(camera.SetParameterValue(Camera::Parameters::Focal, 0.1));
}

TEST_F(CameraFixture, DoesntSetParameter){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);
  const auto values = camera.GetParametersValues();
  EXPECT_ANY_THROW(camera.SetParameterValue(Camera::Parameters::AspectRatio, 0.0));
}

TEST_F(CameraFixture, PerspectiveReturnCorrectKScaled){
  Camera camera = Camera::CreatePerspectiveCamera(focal, distortion[0], distortion[1]);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  expected(0, 0) = expected(1, 1) = focal*std::max(pixel_width, pixel_height);
  expected(0, 2) = pixel_width*0.5;
  expected(1, 2) = pixel_height*0.5;
  ASSERT_EQ(expected, camera.GetProjectionMatrixScaled(pixel_width, pixel_height));
}

TEST_F(CameraFixture, BrownReturnCorrectK){
  Camera camera = Camera::CreateBrownCamera(focal, new_ar, principal_point, distortion_brown);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  expected(0, 0) = focal;
  expected(1, 1) = new_ar*focal;
  expected(0, 2) = principal_point(0);
  expected(1, 2) = principal_point(1);
  ASSERT_EQ(expected, camera.GetProjectionMatrix());
}

TEST_F(CameraFixture, BrownReturnCorrectKScaled){
  Camera camera = Camera::CreateBrownCamera(focal, new_ar, principal_point, distortion_brown);

  Eigen::Matrix3d expected = Eigen::Matrix3d::Identity();
  const auto normalizer = std::max(pixel_width, pixel_height);
  expected(0, 0) = focal*normalizer;
  expected(1, 1) = (focal*new_ar)*normalizer;
  expected(0, 2) = principal_point(0)*normalizer + pixel_width*0.5;
  expected(1, 2) = principal_point(1)*normalizer + pixel_height*0.5;

  ASSERT_EQ(expected, camera.GetProjectionMatrixScaled(pixel_width, pixel_height));
}

TEST_F(CameraFixture, ComputePerspectiveAnalyticalDerivatives){
  const Camera camera = Camera::CreatePerspectiveCamera(focal, -0.1, 0.01);

  const VecXd camera_params = camera.GetParametersValues();
  const int size_params = 3 + camera_params.size();

  Eigen::Matrix<double, 2, 6, Eigen::RowMajor> jacobian;
  RunJacobianEval(camera, ProjectionType::PERSPECTIVE, &jacobian);
  CheckJacobian(jacobian, size_params);
}

TEST_F(CameraFixture, ComputeFisheyeAnalyticalDerivatives){
  const Camera camera = Camera::CreateFisheyeCamera(focal, -0.1, 0.01);

  const VecXd camera_params = camera.GetParametersValues();
  const int size_params = 3 + camera_params.size();

  Eigen::Matrix<double, 2, 6, Eigen::RowMajor> jacobian;
  RunJacobianEval(camera, ProjectionType::FISHEYE, &jacobian);
  CheckJacobian(jacobian, size_params);
}

TEST_F(CameraFixture, ComputeFisheyeExtendedAnalyticalDerivatives){
  const Camera camera = Camera::CreateFisheyeExtendedCamera(focal, new_ar, principal_point, distortion_fisheye);

  const VecXd camera_params = camera.GetParametersValues();
  const int size_params = 3 + camera_params.size();

  Eigen::Matrix<double, 2, 11, Eigen::RowMajor> jacobian;
  RunJacobianEval(camera, ProjectionType::FISHEYE_OPENCV, &jacobian);
  CheckJacobian(jacobian, size_params);
}

TEST_F(CameraFixture, ComputeBrownAnalyticalDerivatives){
  const Camera camera =
      Camera::CreateBrownCamera(focal, new_ar, principal_point, distortion_brown);

  const VecXd camera_params = camera.GetParametersValues();
  const int size_params = 3 + camera_params.size();

  Eigen::Matrix<double, 2, 12, Eigen::RowMajor> jacobian;
  RunJacobianEval(camera, ProjectionType::BROWN, &jacobian);
  CheckJacobian(jacobian, size_params);
}

TEST_F(CameraFixture, ComputeSphericalAnalyticalDerivatives){
  const Camera camera = Camera::CreateSphericalCamera();

  const VecXd camera_params = camera.GetParametersValues();
  const int size_params = 3;

  Eigen::Matrix<double, 2, 3, Eigen::RowMajor> jacobian;
  RunJacobianEval(camera, ProjectionType::SPHERICAL, &jacobian);
  CheckJacobian(jacobian, size_params);
}

TEST_F(CameraFixture, ComputeDualAnalyticalDerivatives){
  const Camera camera = Camera::Camera::CreateDualCamera(0.5, focal, distortion[0], distortion[1]);

  const VecXd camera_params = camera.GetParametersValues();
  const int size_params = 3 + camera_params.size();

  Eigen::Matrix<double, 2, 7, Eigen::RowMajor> jacobian;
  RunJacobianEval(camera, ProjectionType::DUAL, &jacobian);
  CheckJacobian(jacobian, size_params);
}

class FunctionFixture : public ::testing::Test {
 public:
  /* Evaluates the following function :
   *
   * f = a * exp( b - c * log( d * (e/x - f)^2 ) )
   *
   * Decomposed as follows :
   *
   * f1 = a * exp(x)
   * f2 = b - c * log(x)
   * f3 = d * x^2
   * f4 = e/x - f
   * f = f1(f2(f3(f4))) */

  struct F1 : public CameraFunctor<1, 1, 1>{
    template <class T>
    static void Apply(const T* in, const T* p, T* out) {
      out[0] = p[0] * exp(in[0]);
    }
    template <class T, bool COMP_PARAM>
    static void ForwardDerivatives(const T* in, const T* p,
                                   T* out, T* jacobian) {
      jacobian[0] = p[0] * exp(in[0]);
      if (COMP_PARAM) {
        jacobian[1] = exp(in[0]);
      }
      Apply(in, p, out);
    }
  };
  struct F2 : public CameraFunctor<1, 2, 1>{
    template <class T>
    static void Apply(const T* in, const T* p, T* out) {
      out[0] = p[0] - p[1] * log(in[0]);
    }
    template <class T, bool COMP_PARAM>
    static void ForwardDerivatives(const T* in, const T* p,
                                   T* out, T* jacobian) {
      jacobian[0] = -p[1]/in[0];
      if (COMP_PARAM) {
        jacobian[1] = T(1.0);
        jacobian[2] = -log(in[0]);
      }
      Apply(in, p, out);
    }
  };
  struct F3 : public CameraFunctor<1, 1, 1>{
    template <class T>
    static void Apply(const T* in, const T* p, T* out) {
      out[0] = p[0] * in[0] * in[0];
    }
    template <class T, bool COMP_PARAM>
    static void ForwardDerivatives(const T* in, const T* p,
                                   T* out, T* jacobian) {
      jacobian[0] = T(2.0) * p[0] * in[0];
      if (COMP_PARAM) {
        jacobian[1] = in[0] * in[0];
      }
      Apply(in, p, out);
    }
  };
  struct F4 : public CameraFunctor<1, 2, 1>{
    template <class T>
    static void Apply(const T* in, const T* p, T* out) {
      out[0] = p[0] / in[0] - p[1];
    }
    template <class T, bool COMP_PARAM>
    static void ForwardDerivatives(const T* in, const T* p,
                                   T* out, T* jacobian) {
      jacobian[0] = -p[0] / (in[0] * in[0] );
      if (COMP_PARAM) {
        jacobian[1] = T(1.0)/in[0];
        jacobian[2] = -T(1.0);
      }
      Apply(in, p, out);
    }
  };
};

TEST_F(FunctionFixture, EvaluatesCorrectly) {
  double in[] = {1.0};

  /* Parameters needs to be stored in the order of evaluation :
   * params f4 |params f3 | params f2 | params f1 */
  double parameters[] = {4.0, 2.0, 3.0, 1.0, 2.0, 3.0};
  double evaluated = 0;
  ComposeFunctions<double, F1, F2, F3, F4>(in, parameters, &evaluated);

  const double expected = 3.0 * exp( 1.0 - 2.0 * log( 3.0 * std::pow(4.0/in[0] - 2.0, 2) ) );
  ASSERT_NEAR(expected, evaluated, 1e-20);
}

TEST_F(FunctionFixture, EvaluatesDerivativesCorrectly) {
  double in[] = {1.0};

  /* Parameters needs to be stored in the order of evaluation :
   * params f4 |params f3 | params f2 | params f1 */
  double parameters[] = {4.0, 2.0, 3.0, 1.0, 2.0, 3.0};
  double evaluated = 0;
  double jacobian[7];
  ComposeForwardDerivatives<double, F1, F2, F3, F4>(in, parameters, &evaluated, &jacobian[0]);

  const double expected = 3.0 * exp( 1.0 - 2.0 * log( 3.0 * std::pow(4.0/in[0] - 2.0, 2) ) );
  ASSERT_NEAR(expected, evaluated, 1e-20);

  /* Jacobian ordering : d_input | d_parameters */
  typedef Eigen::AutoDiffScalar<Eigen::VectorXd> AScalar;
  VecX<AScalar> eval_adiff(1);
  eval_adiff(0).value() = in[0];
  eval_adiff(0).derivatives() = Eigen::VectorXd::Unit(7, 0);
  VecX<AScalar> parameters_adiff(6);
  for (int i = 0; i < 6; ++i) {
    parameters_adiff[i].value() = parameters[i];
    parameters_adiff[i].derivatives() = Eigen::VectorXd::Unit(7, i+1);
  }

  AScalar evaluated_addif;
  ComposeFunctions<AScalar, F1, F2, F3, F4>(
      eval_adiff.data(), parameters_adiff.data(), &evaluated_addif);
  for(int i = 0; i < 7; ++i){
    ASSERT_NEAR(evaluated_addif.derivatives()(i), jacobian[i], 1e-20);
  }
}

class PoseFixture : public ::testing::Test {
 public:
  const double point[3] = {1.0, 2.0, 3.0};
  const double rt[6] = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6};
};

TEST_F(PoseFixture, EvaluatesCorrectly) {
  double transformed[3] = {0., 0., 0.};

  /* Parameters order : angle_axis | pose_center */
  Pose::Forward(point, rt, transformed);

  /* Use Ceres as groundtruth */
  const double pt[3] = {
    point[0] - rt[Pose::Tx],
    point[1] - rt[Pose::Ty],
    point[2] - rt[Pose::Tz],
  };
  const double Rt[3] = {
    -rt[Pose::Rx],
    -rt[Pose::Ry],
    -rt[Pose::Rz]
  };
  double expected[] = {1., 1., 1.};
  ceres::AngleAxisRotatePoint(Rt, pt, expected);

  for(int i = 0; i < 3; ++i){
    ASSERT_NEAR(expected[i], transformed[i], 1e-15);
  }
}

TEST_F(PoseFixture, EvaluatesDerivativesCorrectly) {
  typedef Eigen::AutoDiffScalar<Eigen::VectorXd> AScalar;

  VecX<AScalar> point_adiff(3);
  constexpr int size = 9;

  /* Autodifferentaied as a reference */
  for (int i = 0; i < 3; ++i) {
    point_adiff(i).value() = point[i];
    point_adiff(i).derivatives() = Eigen::VectorXd::Unit(size, i);
  }
  VecX<AScalar> rt_adiff(6);
  for (int i = 0; i < 6; ++i) {
    rt_adiff[i].value() = rt[i];
    rt_adiff[i].derivatives() = Eigen::VectorXd::Unit(size, 3 + i);
  }
  VecX<AScalar> expected_adiff(3);
  Pose::Forward(point_adiff.data(), rt_adiff.data(), expected_adiff.data());

  /* Analytic version */
  double transformed[] = {0., 0., 0.};
  double jacobian[size * 3];
  Pose::ForwardDerivatives<double, true>(&point[0], &rt[0], &transformed[0],
                                         &jacobian[0]);

  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < size; ++j) {
      ASSERT_NEAR(expected_adiff(i).derivatives()(j), jacobian[i * size + j], 1e-15);
    }
  }
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

//////////////////////////////////////////////////////////////////////////////
// Important: Keep this file in sync with the Overview in the README
//////////////////////////////////////////////////////////////////////////////

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/integral.hpp>
#include <boost/hana/list/instance.hpp>
#include <boost/hana/type.hpp>

#include <cassert>
#include <string>
#include <type_traits>
using namespace boost::hana;

struct President { std::string name; };
struct Car       { std::string name; };
struct City      { std::string name; };
auto name = [](auto x) { return x.name; };

int main() {
    // Heterogeneous sequences for value-level metaprogramming.
    auto stuff = list(President{"Obama"}, Car{"Toyota"}, City{"Quebec"});
    assert(reverse(fmap(name, stuff)) == list("Quebec", "Toyota", "Obama"));

    // Type-level metaprogramming works too.
    auto types = fmap(compose(metafunction<std::add_pointer>, decltype_), stuff);

    static_assert(std::is_same<
        decltype(head(types))::type, President*
    >{}, "");

    static_assert(std::is_same<
        decltype(at(int_<1>, types))::type, Car*
    >{}, "");

    static_assert(std::is_same<
        decltype(last(types))::type, City*
    >{}, "");

    // No compile-time information is lost (the assertion is done at compile-time).
    BOOST_HANA_CONSTANT_ASSERT(length(permutations(stuff)) == int_<3 * 2 * 1>);
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.bitwise toolbox - ilog2/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.bitwise components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <boost/simd/bitwise/include/functions/ilog2.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/dispatch/meta/ignore_unused.hpp>

NT2_TEST_CASE_TPL ( ilog2_real__1_0,  BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::ilog2;
  using boost::simd::tag::ilog2_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<ilog2_(T)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  typedef typename boost::dispatch::meta::as_integer<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;

  // specific values tests
  NT2_TEST_EQUAL(ilog2(boost::simd::Two<T>()), 1);
  NT2_TEST_EQUAL(ilog2(boost::simd::Three<T>()), 1);
  NT2_TEST_EQUAL(ilog2(boost::simd::Four<T> ()), 2);
  NT2_TEST_EQUAL(ilog2(boost::simd::Pi<T> ()), 1);
  NT2_TEST_EQUAL(ilog2(boost::simd::One<T>()), 0);
} // end of test for real_

NT2_TEST_CASE_TPL ( ilog2_signed_int__1_0,  BOOST_SIMD_INTEGRAL_SIGNED_TYPES)
{

  using boost::simd::ilog2;
  using boost::simd::tag::ilog2_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<ilog2_(T)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  typedef typename boost::dispatch::meta::as_integer<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;

  // specific values tests
  NT2_TEST_EQUAL(ilog2(boost::simd::One<T>()), 0);
  NT2_TEST_EQUAL(ilog2(boost::simd::Two<T>()), 1);
} // end of test for signed_int_

 NT2_TEST_CASE_TPL ( ilog2_unsigned_int__1_0,  BOOST_SIMD_UNSIGNED_TYPES)
 {

  using boost::simd::ilog2;
  using boost::simd::tag::ilog2_;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<ilog2_(T)>::type r_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type sr_t;
  typedef typename boost::simd::meta::scalar_of<r_t>::type ssr_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type wished_r_t;

  // return type conformity test
  NT2_TEST( (boost::is_same < r_t, wished_r_t >::value) );
  std::cout << std::endl;

  // specific values tests
  NT2_TEST_EQUAL(ilog2(boost::simd::One<T>()), 0u);
  NT2_TEST_EQUAL(ilog2(boost::simd::Two<T>()),1u);

  T j = 1;
  for(T i=2; i < boost::simd::Valmax<T>()/2; i*= 2)
    {
      std::cout << "i = " << i << " j = " <<  j << std::endl;
      NT2_TEST_EQUAL(ilog2(T(i)),j);
      NT2_TEST_EQUAL(ilog2(T(i+1)),j);
      ++j;
    }
 } // end of test for unsigned_int_


/*    Copyright (c) 2010-2015, Delft University of Technology
 *    All rights reserved.
 *
 *    Redistribution and use in source and binary forms, with or without modification, are
 *    permitted provided that the following conditions are met:
 *      - Redistributions of source code must retain the above copyright notice, this list of
 *        conditions and the following disclaimer.
 *      - Redistributions in binary form must reproduce the above copyright notice, this list of
 *        conditions and the following disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *      - Neither the name of the Delft University of Technology nor the names of its contributors
 *        may be used to endorse or promote products derived from this software without specific
 *        prior written permission.
 *
 *    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS
 *    OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *    MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *    COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 *    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
 *    GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *    AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 *    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
 *    OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *    Changelog
 *      YYMMDD    Author            Comment
 *      111103    S. Billemont      File created.
 *      120906    T. Secretin       Reviewed code. Moved transform implementation to .cpp file.
 *
 *    References
 *
 *    Notes
 *
 */

#include <boost/lexical_cast.hpp>

#include "Tudat/InputOutput/linearFieldTransform.h"

namespace tudat
{
namespace input_output
{

//! Transform input string.
boost::shared_ptr< std::string > LinearFieldTransform::transform( const std::string& input )
{
    // Transform string to double.
    const double number = boost::lexical_cast< double >( input );

    // Perform transformation.
    const double result = slope * number + intercept;

    // Return pointer to transformed value, in string format.
    return boost::shared_ptr< std::string >( new std::string(
                                                 boost::lexical_cast< std::string >( result ) ) );
}

} // namespace input_output
} // namespace tudat


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_TEST_CENTROID_HPP
#define BOOST_GEOMETRY_TEST_CENTROID_HPP

// Test-functionality, shared between single and multi tests

#include <boost/variant/variant.hpp>

#include <geometry_test_common.hpp>

#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/algorithms/centroid.hpp>
#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/geometries/adapted/boost_tuple.hpp>

#include <boost/geometry/io/wkt/read.hpp>

template <std::size_t D>
struct check_result
{
    template <typename Point1, typename Point2>
    static void apply(Point1 const& actual, Point2 const& expected)
    {
        check_result<D-1>::apply(actual, expected);
        BOOST_CHECK_CLOSE(bg::get<D-1>(actual), bg::get<D-1>(expected), 0.001);
    }
};

template <>
struct check_result<0>
{
    template <typename Point1, typename Point2>
    static void apply(Point1 const&, Point2 const&)
    {}
};

template <typename CalculationType, typename Geometry, typename Point>
void test_with_other_calculation_type(Geometry const& geometry, Point& c1)
{
    typedef typename bg::point_type<Geometry>::type point_type;
    // Calculate it with user defined strategy
    point_type c2;
    bg::centroid(geometry, c2,
        bg::strategy::centroid::bashein_detmer<point_type, point_type, CalculationType>());

    std::cout << typeid(CalculationType).name() << ": " << std::setprecision(20)
        << bg::get<0>(c2) << " " << bg::get<1>(c2)
        << " -> difference: " << bg::distance(c1, c2)
        << std::endl;
}

template <typename Geometry, typename T>
void test_centroid(std::string const& wkt, T const& d1, T const& d2, T const& d3 = T(), T const& d4 = T(), T const& d5 = T())
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    typedef typename bg::point_type<Geometry>::type point_type;
    point_type c1;

    bg::centroid(geometry, c1);
    check_result<bg::dimension<Geometry>::type::value>::apply(c1, boost::make_tuple(d1, d2, d3, d4, d5));

    boost::variant<Geometry> v(geometry);
    bg::centroid(v, c1);

#ifdef REPORT_RESULTS
    std::cout << "normal: " << std::setprecision(20) << bg::get<0>(c1) << " " << bg::get<1>(c1) << std::endl;

    //test_with_other_calculation_type<long long>(geometry, c1);
    test_with_other_calculation_type<float>(geometry, c1);
    test_with_other_calculation_type<long double>(geometry, c1);
#if defined(HAVE_TTMATH)
    test_with_other_calculation_type<ttmath_big>(geometry, c1);
#endif

#endif
}

template <typename Geometry>
void test_centroid_exception()
{
    Geometry geometry;
    try
    {
        typename bg::point_type<Geometry>::type c;
        bg::centroid(geometry, c);
    }
    catch(bg::centroid_exception const& )
    {
        return;
    }
    BOOST_CHECK_MESSAGE(false, "A centroid_exception should have been thrown" );
}

#endif


#ifndef COURSE_MANAGER_H
#define COURSE_MANAGER_H

#include <Eigen/Dense>
#include <array>
#include <cmath>
#include <iostream>
#include <limits>
#include <stdexcept>
#include <unordered_map>
#include "MPCCourse.hpp"
#include "Pose.hpp"
#include "rapidcsv.h"

namespace pathtrack_tools
{
    ///
    /// @class CourseManager
    /// @brief Manage ego car reference path (x,y,z,yaw), reference speed (speed) and drivable area (delta_x_e<-[now not
    /// supported], delta_y_e). Their info are decided by user setting (csv format) or upper class planner.
    ///

    class CourseManager
    {
    public:
        /**
     * @brief Default constructor
     *
     */
        CourseManager(/* args */);
        CourseManager(const int curvature_smoothing_num, const double max_curvature_change_rate, const double speed_reduction_rate, const double deceleration_rate_for_stop);

        /**
     * @brief Destroy the CourseManager object
     *
     */
        ~CourseManager();

        /**
     * @brief Get the reference path size
     *
     * @return int
     */
        int get_path_size() const;

        /**
     * @brief clear reference path
     *
     */
        void clear_path();

        /**
     * @brief Get the mpc cource object for test
     *
     * @return MPCCourse
     */
        MPCCourse get_mpc_course() const;

        /**
     * @brief Set the course info from csv file and calculate curvature of path
     *
     * @param csv_file_path
     */
        void set_course_from_csv(const std::string &csv_path);

        /**
     * @brief Get curvature of the reference path in pose_x_f with interpolation．
     *
     * @param pose_x_f : ego vehicle x in frenet-serret coordinate
     * @return curvature of reference path
     */
        double get_curvature(const double &pose_x_f);

        /**
     * @brief Get reference speed in pose_x_f with interpolation
     *
     * @param pose_x_f : ego vehicle x in frenet-serret coordinate
     * @return reference speed[m/s]
     */
        double get_speed(const double &pose_x_f);

        /**
     * @brief Get drivable course width in pose_x_f
     *
     * @param pose_x_f : ego vehicle x in frenet-serret coordinate
     * @return drivable width, which is often course width[m]
     */
        double get_drivable_width(const double &pose_x_f);

    private:
        MPCCourse mpc_course_; //!< @brief driving course interface using in MPC
        // Parameters for path smoothing and filtering
        const int curvature_smoothing_num_ = 10;        //!< @brief Smoothing value for curvature calculation
        const double max_curvature_change_rate_ = 0.1;  //!< @brief Saturate value for curvature change rate [1/m^2]
        const double speed_reduction_rate_ = 0.1;       //!< @brief Reduce the speed reference based on the rate of curvature change; v_ref' = v_ref * exp
                                                        //!< (-speed_reduction_rate * curvature_rate^2)
        const double deceleration_rate_for_stop_ = 0.1; //!< @brief Reduce the speed reference for stopping; v_ref'  = v_ref * (1 - exp(-deceleration_rate_for_stop
                                                        //!< * (x_goal -x_f))), recommend the same value as a a_min in MPC formulation

        // Parameters for lookup table (xf) -> (nearest index)
        std::unordered_map<double, int> hash_xf2index_; // Hash that connects x_f to the nearest index
        const double hash_resolution_ = 0.01;           // resolution of x_f [m]
        const double redundant_xf_ = 5.0;               // Reserve the lookup table for the extra -redundant_xf[m] from the initial value of mpc_course.

        // variables for linear interpolate
        int nearest_index_;           // Nearest waypoint index
        double nearest_ratio_;        // The internal fraction of nearest_index.
        int second_nearest_index_;    // second nearest reference pointのindex
        double second_nearest_ratio_; // The internal fraction of second nearest_index
        double current_pose_x_f_;     // Position of vehicle in the same prediction step

        void set_accumulated_path_length(const double &offset, MPCCourse *mpc_course);

        std::vector<double> calc_accumulated_path_length(const double &offset, const MPCCourse &mpc_course) const;

        void set_path_curvature(const int &smoothing_num, MPCCourse *mpc_course);

        void set_path_yaw(MPCCourse *mpc_course);

        std::vector<double> calc_path_curvature(const int &smoothing_num, const MPCCourse &mpc_course) const;

        double calc_distance(const std::array<double, 2> &p1, const std::array<double, 2> &p2) const;

        void deceleration_for_stop(const std::vector<double> &accumulated_path_length, std::vector<double> *ref_speed);

        void filtering_based_curvature_rate(const std::vector<double> &accumulated_path_length, std::vector<double> *path_curvature, std::vector<double> *ref_speed);

        void set_hash_xf2index(const MPCCourse &mpc_course, std::unordered_map<double, int> *hash_xf2index);

        double round_resolution(const double &raw_x_f) const;

        inline int search_nearest_index(const MPCCourse &mpc_course, const double &pose_x_f, const int &last_nearest_index) const noexcept;

        int search_nearest_index(const MPCCourse &mpc_course, const Pose &pose) const; // Not used now

        inline int lookup_xf_to_nearest_index(const std::unordered_map<double, int> &hash_xf2index, const double &pose_x_f) const;

        void set_internal_ratio(const MPCCourse &mpc_course, const double &pose_x_f, const int &nearest_index);

        double linear_interpolate(const std::vector<double> &reference_vec, const int &nearest_index, const double &nearest_ratio, const int &second_nearest_index,
                                  const double &second_nearest_ratio) const noexcept;
    };

} // namespace pathtrack_tools

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/integral.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/type.hpp>
using namespace boost::hana;
using namespace literals;

int main() {
    BOOST_HANA_CONSTANT_ASSERT(0_c == llong<0>);
    BOOST_HANA_CONSTANT_ASSERT(1_c == llong<1>);
    BOOST_HANA_CONSTANT_ASSERT(12_c == llong<12>);
    BOOST_HANA_CONSTANT_ASSERT(123_c == llong<123>);
    BOOST_HANA_CONSTANT_ASSERT(1234567_c == llong<1234567>);
    BOOST_HANA_CONSTANT_ASSERT(-34_c == llong<-34>);

    BOOST_HANA_CONSTANT_ASSERT(decltype_(-1234_c) == decltype_(llong<-1234>));
    BOOST_HANA_CONSTANT_ASSERT(-12_c < 0_c);
}