/***************************************************************
 * Name:      MultiRNGMain.cpp
 * Purpose:   Code for Application Frame
 * Author:    Uli Koehler ()
 * Created:   2008-01-20
 * Copyright: Uli Koehler ()
 * License: GPLv2
 **************************************************************/

#include "wx_pch.h"
#include "MultiRNGMain.h"
#include <wx/msgdlg.h>

//(*InternalHeaders(MultiRNGFrame)
#include <wx/settings.h>
#include <wx/string.h>
#include <wx/intl.h>
//*)
///Custom include
#include "Generators.h"

#include <boost/random.hpp>

//helper functions
enum wxbuildinfoformat {
    short_f, long_f };

wxString wxbuildinfo(wxbuildinfoformat format)
{
    wxString wxbuild(wxVERSION_STRING);

    if (format == long_f )
    {
#if defined(__WXMSW__)
        wxbuild << _T("-Windows");
#elif defined(__UNIX__)
        wxbuild << _T("-Linux");
#endif

#if wxUSE_UNICODE
        wxbuild << _T("-Unicode build");
#else
        wxbuild << _T("-ANSI build");
#endif // wxUSE_UNICODE
    }

    return wxbuild;
}

//(*IdInit(MultiRNGFrame)
const long MultiRNGFrame::ID_TEXTCTRL23 = wxNewId();
const long MultiRNGFrame::ID_ALGOLABEL = wxNewId();
const long MultiRNGFrame::ID_ALGOCHOICE = wxNewId();
const long MultiRNGFrame::ID_AMOUNTLABEL = wxNewId();
const long MultiRNGFrame::ID_AMOUNTFIELD = wxNewId();
const long MultiRNGFrame::ID_FILELABEL = wxNewId();
const long MultiRNGFrame::ID_FILEFIELD = wxNewId();
const long MultiRNGFrame::ID_OKBUTTON = wxNewId();
const long MultiRNGFrame::ID_DISTCHOICE = wxNewId();
const long MultiRNGFrame::ID_STATICTEXT1 = wxNewId();
const long MultiRNGFrame::ID_TEXTCTRL1 = wxNewId();
//*)

BEGIN_EVENT_TABLE(MultiRNGFrame,wxFrame)
    //(*EventTable(MultiRNGFrame)
    //*)
END_EVENT_TABLE()

MultiRNGFrame::MultiRNGFrame(wxWindow* parent,wxWindowID id)
{
    //(*Initialize(MultiRNGFrame)
    Create(parent, wxID_ANY, _("MultiRNG"), wxDefaultPosition, wxDefaultSize, wxDEFAULT_FRAME_STYLE, _T("wxID_ANY"));
    SetClientSize(wxSize(585,174));
    distributionLabel = new wxTextCtrl(this, ID_TEXTCTRL23, _("Range:"), wxPoint(224,8), wxSize(56,21), wxTE_READONLY|wxTE_RIGHT|wxNO_BORDER, wxDefaultValidator, _T("ID_TEXTCTRL23"));
    distributionLabel->SetBackgroundColour(wxSystemSettings::GetColour(wxSYS_COLOUR_APPWORKSPACE));
    algorithmLabel = new wxStaticText(this, ID_ALGOLABEL, _("Algorithm:"), wxPoint(8,16), wxDefaultSize, 0, _T("ID_ALGOLABEL"));
    algorithmChoice = new wxChoice(this, ID_ALGOCHOICE, wxPoint(64,8), wxSize(160,27), 0, 0, 0, wxDefaultValidator, _T("ID_ALGOCHOICE"));
    amountLabel = new wxStaticText(this, ID_AMOUNTLABEL, _("Amount:"), wxPoint(8,48), wxDefaultSize, 0, _T("ID_AMOUNTLABEL"));
    amountField = new wxTextCtrl(this, ID_AMOUNTFIELD, _("100"), wxPoint(56,48), wxDefaultSize, 0, wxDefaultValidator, _T("ID_AMOUNTFIELD"));
    fileLabel = new wxStaticText(this, ID_FILELABEL, _("File:"), wxPoint(144,48), wxDefaultSize, 0, _T("ID_FILELABEL"));
    filenameField = new wxTextCtrl(this, ID_FILEFIELD, _("random.txt"), wxPoint(176,48), wxSize(216,21), 0, wxDefaultValidator, _T("ID_FILEFIELD"));
    okButton = new wxButton(this, ID_OKBUTTON, _("OK"), wxPoint(8,136), wxSize(384,23), 0, wxDefaultValidator, _T("ID_OKBUTTON"));
    distributionChoice = new wxChoice(this, ID_DISTCHOICE, wxPoint(296,8), wxSize(152,32), 0, 0, 0, wxDefaultValidator, _T("ID_DISTCHOICE"));
    distParam1Label = new wxStaticText(this, ID_STATICTEXT1, _("Dist param 1:"), wxPoint(24,96), wxDefaultSize, 0, _T("ID_STATICTEXT1"));
    distParam1Field = new wxTextCtrl(this, ID_TEXTCTRL1, _("10.0"), wxPoint(104,96), wxDefaultSize, 0, wxDefaultValidator, _T("ID_TEXTCTRL1"));

    Connect(ID_OKBUTTON,wxEVT_COMMAND_BUTTON_CLICKED,(wxObjectEventFunction)&MultiRNGFrame::OnOkButtonClick);
    Connect(wxID_ANY,wxEVT_CLOSE_WINDOW,(wxObjectEventFunction)&MultiRNGFrame::OnClose);
    //*)
    //Fill the algorithm and distribution combo boxes
    ///Set Algorithm choices
    algorithmChoice->Append(_("Mersenne Twister 19937"));   ///1
    algorithmChoice->Append(_("MInstdRand (LC Variant)"));  ///2
    algorithmChoice->Append(_("Ecuyer 1988"));              ///3
    algorithmChoice->Append(_("Kreutzer 1986"));            ///4
    algorithmChoice->Append(_("Hellekalek 1995"));          ///5
    algorithmChoice->Append(_("Mersenne Twister 11213b"));  ///6
    algorithmChoice->Append(_("Lagged Fibonacci 607"));     ///7
    algorithmChoice->Append(_("Lagged Fibonacci 1279"));    ///8
    algorithmChoice->Append(_("Lagged Fibonacci 2281"));    ///9
    algorithmChoice->Append(_("Lagged Fibonacci 3217"));    ///10
    algorithmChoice->Append(_("Lagged Fibonacci 4423"));    ///11
    algorithmChoice->Append(_("Lagged Fibonacci 9689"));    ///12
    algorithmChoice->Append(_("Lagged Fibonacci 19937"));   ///13
    algorithmChoice->Append(_("Lagged Fibonacci 23209"));   ///14
    algorithmChoice->Append(_("Lagged Fibonacci 44497"));   ///15
    algorithmChoice->SetSelection(0);
    ///Set Distribution choices
    distributionChoice->Append(wxT("Uniform Small Integer"));
    distributionChoice->Append(wxT("Uniform Integer"));
    distributionChoice->Append(wxT("Uniform Real"));
    distributionChoice->Append(wxT("Triangle"));
    distributionChoice->Append(wxT("Bernoulli"));
    distributionChoice->Append(wxT("Cauchy"));
    distributionChoice->Append(wxT("Exponential"));
    distributionChoice->Append(wxT("Geometric"));
    distributionChoice->Append(wxT("Normal"));
    distributionChoice->Append(wxT("Lognormal"));
    distributionChoice->Append(wxT("Uniform on Sphere"));
    distributionChoice->SetSelection(0);
}

MultiRNGFrame::~MultiRNGFrame()
{
    //(*Destroy(MultiRNGFrame)
    //*)
}

void MultiRNGFrame::OnQuit(wxCommandEvent& event)
{
    Close();
}

void MultiRNGFrame::OnAbout(wxCommandEvent& event)
{
    wxString msg = wxbuildinfo(long_f);
    wxMessageBox(msg, _("Welcome to..."));
}

void MultiRNGFrame::OnOkButtonClick(wxCommandEvent& event)
{
    ///Init static variables
    amountParam = lexical_cast<unsigned long long>(amountField->GetValue().mb_str());
    seedParam = generateSeed();
    ulParam = 100000; //TODO
    llParam = 1; //TODO
    distributionSelectionParam = distributionChoice->GetCurrentSelection();
    algorithmSelectionParam = algorithmChoice->GetCurrentSelection();
    filenameParam = lexical_cast<string>(filenameField->GetValue().mb_str());
    distParam1 = lexical_cast<string>(distParam1Field->GetValue().mb_str());
    ///Create a new thread if Multithreading is enabled or call function directly else
    //boost::thread boostThread(&GenRandBoost);
    GenRandBoost();
}

unsigned long MultiRNGFrame::generateSeed()
{
    return 1000100;
    unsigned long seed;
    ///Read from /dev/random if we are using linux or use time(0) on windows
    #ifdef __linux__
    ifstream f;
    ///Dependent on the state of highEntropyCheckbox open either /dev/random or /dev/urandom for read
    f.open("/dev/urandom", ifstream::in);
    f.read(reinterpret_cast<char*>(&seed), sizeof(unsigned long));
    f.close();
    #else
    MTRand mtr(time(0)); ///Create random state variable and seed with time(0)
    seed = mtr.randInt();
    #endif
    return seed;
}

void MultiRNGFrame::OnClose(wxCloseEvent& event)
{
    exit(0);
}


#ifndef STAN_MATH_PRIM_PROB_HMM_HIDDEN_STATE_PROB_HPP
#define STAN_MATH_PRIM_PROB_HMM_HIDDEN_STATE_PROB_HPP

#include <stan/math/prim/core.hpp>
#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err/hmm_check.hpp>
#include <stan/math/prim/fun/Eigen.hpp>
#include <boost/random.hpp>
#include <vector>

namespace stan {
namespace math {

/**
 * For a hidden Markov model with observation y, hidden state x,
 * and parameters theta, compute the marginal probability
 * vector for each x, given y and theta, p(x_i | y, theta).
 * In this setting, the hidden states are discrete
 * and take values over the finite space {1, ..., K}.
 * Hence for each hidden variable x, we compute a simplex with K elements.
 * The final result is stored in a K by N matrix, where N is the length of x.
 * log_omegas is a matrix of observational densities, where
 * the (i, j)th entry corresponds to the density of the ith observation, y_i,
 * given x_i = j.
 * The transition matrix Gamma is such that the (i, j)th entry is the
 * probability that x_n = j given x_{n - 1} = i. The rows of Gamma are
 * simplexes.
 * This function cannot be used to reconstruct the marginal distributon
 * of a state sequence given parameters and an observation sequence,
 * p(x | y, theta),
 * because it only computes marginals on a state-by-state basis.
 *
 * @tparam T_omega type of the log likelihood matrix
 * @tparam T_Gamma type of the transition matrix
 * @tparam T_rho type of the initial guess vector
 * @param[in] log_omegas log matrix of observational densities
 * @param[in] Gamma transition density between hidden states
 * @param[in] rho initial state
 * @return the posterior probability for each latent state
 * @throw `std::invalid_argument` if Gamma is not square
 *         or if the size of rho is not the number of rows of log_omegas
 * @throw `std::domain_error` if rho is not a simplex and of the rows
 *         of Gamma are not a simplex
 */
template <typename T_omega, typename T_Gamma, typename T_rho,
          require_all_eigen_t<T_omega, T_Gamma>* = nullptr,
          require_eigen_col_vector_t<T_rho>* = nullptr>
inline Eigen::MatrixXd hmm_hidden_state_prob(const T_omega& log_omegas,
                                             const T_Gamma& Gamma,
                                             const T_rho& rho) {
  int n_states = log_omegas.rows();
  int n_transitions = log_omegas.cols() - 1;

  Eigen::MatrixXd omegas = value_of(log_omegas).array().exp();
  ref_type_t<decltype(value_of(rho))> rho_dbl = value_of(rho);
  ref_type_t<decltype(value_of(Gamma))> Gamma_dbl = value_of(Gamma);
  hmm_check(log_omegas, Gamma_dbl, rho_dbl, "hmm_hidden_state_prob");

  Eigen::MatrixXd alphas(n_states, n_transitions + 1);
  alphas.col(0) = omegas.col(0).cwiseProduct(rho_dbl);
  alphas.col(0) /= alphas.col(0).maxCoeff();

  for (int n = 0; n < n_transitions; ++n)
    alphas.col(n + 1)
        = omegas.col(n + 1).cwiseProduct(Gamma_dbl.transpose() * alphas.col(n));

  // Backward pass with running normalization
  Eigen::VectorXd beta = Eigen::VectorXd::Ones(n_states);

  alphas.col(n_transitions) /= alphas.col(n_transitions).sum();

  for (int n = n_transitions; n-- > 0;) {
    beta = Gamma_dbl * omegas.col(n + 1).cwiseProduct(beta);
    beta /= beta.maxCoeff();

    // Reuse alphas to store probabilities
    alphas.col(n) = alphas.col(n).cwiseProduct(beta);
    alphas.col(n) /= alphas.col(n).sum();
  }

  return alphas;
}

}  // namespace math
}  // namespace stan
#endif


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

#ifndef MFMG_LAPLACE_HPP
#define MFMG_LAPLACE_HPP

#include <deal.II/base/function.h>
#include <deal.II/base/index_set.h>
#include <deal.II/base/quadrature.h>
#include <deal.II/distributed/tria.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/grid/filtered_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/grid/manifold_lib.h>
#include <deal.II/lac/affine_constraints.h>
#include <deal.II/lac/affine_constraints.templates.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/trilinos_solver.h>
#include <deal.II/lac/trilinos_sparse_matrix.h>
#include <deal.II/lac/trilinos_sparsity_pattern.h>
#include <deal.II/lac/vector.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/vector_tools.h>

#include <boost/property_tree/ptree.hpp>

#include <string>

template <int dim, typename VectorType>
class Laplace
{
public:
  Laplace(MPI_Comm const &comm, unsigned int fe_degree);

  void setup_system(boost::property_tree::ptree const &ptree);

  void assemble_system(dealii::Function<dim> const &source,
                       dealii::Function<dim> const &material_property);

  template <typename PreconditionerType>
  VectorType solve(PreconditionerType &preconditioner);

  double compute_error(dealii::Function<dim> const &exact_solution);

  void output_results() const;

  template <typename PreconditionerType>
  void run(PreconditionerType &preconditioner,
           dealii::Function<dim> const &source,
           dealii::Function<dim> const &material_property);

  // The following variable should be private but there are public for
  // simplicity
  MPI_Comm _comm;
  dealii::parallel::distributed::Triangulation<dim> _triangulation;
  dealii::FE_Q<dim> _fe;
  dealii::DoFHandler<dim> _dof_handler;
  dealii::IndexSet _locally_owned_dofs;
  dealii::IndexSet _locally_relevant_dofs;
  dealii::AffineConstraints<double> _constraints;
  dealii::TrilinosWrappers::SparseMatrix _system_matrix;
  VectorType _locally_relevant_solution;
  VectorType _system_rhs;
};

template <int dim, typename VectorType>
Laplace<dim, VectorType>::Laplace(MPI_Comm const &comm, unsigned int fe_degree)
    : _comm(comm), _triangulation(_comm), _fe(fe_degree),
      _dof_handler(_triangulation)
{
}

template <int dim, typename VectorType>
void Laplace<dim, VectorType>::setup_system(
    boost::property_tree::ptree const &ptree)
{
  std::string const mesh = ptree.get("mesh", "hyper_cube");
  if (mesh == "hyper_ball")
    dealii::GridGenerator::hyper_ball(_triangulation);
  else
    dealii::GridGenerator::hyper_cube(_triangulation);

  _triangulation.refine_global(ptree.get("n_refinements", 3));

  // Set the boundary id to one
  auto boundary_cells =
      dealii::filter_iterators(_triangulation.active_cell_iterators(),
                               dealii::IteratorFilters::AtBoundary());
  for (auto &cell : boundary_cells)
  {
    for (unsigned int f = 0; f < dealii::GeometryInfo<dim>::faces_per_cell; ++f)
      if (cell->face(f)->at_boundary())
        cell->face(f)->set_boundary_id(1);
  }

  if (ptree.get("distort_random", false))
    dealii::GridTools::distort_random(0.1, _triangulation);

  _dof_handler.distribute_dofs(_fe);

  std::string const reordering = ptree.get("reordering", "None");
  if (reordering == "Reverse Cuthill-McKee")
    dealii::DoFRenumbering::Cuthill_McKee(_dof_handler, true);
  else if (reordering == "King")
    dealii::DoFRenumbering::boost::king_ordering(_dof_handler);
  else if (reordering == "Reverse minimum degree")
    dealii::DoFRenumbering::boost::minimum_degree(_dof_handler, true);
  else if (reordering == "Hierarchical")
    dealii::DoFRenumbering::hierarchical(_dof_handler);

  // Get the IndexSets
  _locally_owned_dofs = _dof_handler.locally_owned_dofs();
  dealii::DoFTools::extract_locally_relevant_dofs(_dof_handler,
                                                  _locally_relevant_dofs);

  // Resize the vectors
  _locally_relevant_solution.reinit(_locally_owned_dofs, _locally_relevant_dofs,
                                    _comm);
  _system_rhs.reinit(_locally_owned_dofs, _comm);

  // Compute the constraints
  _constraints.clear();
  _constraints.reinit(_locally_relevant_dofs);
  dealii::DoFTools::make_hanging_node_constraints(_dof_handler, _constraints);
  dealii::VectorTools::interpolate_boundary_values(
      _dof_handler, 1, dealii::Functions::ZeroFunction<dim>(), _constraints);
  _constraints.close();

  // Build the sparsity pattern
  dealii::TrilinosWrappers::SparsityPattern sparsity_pattern(
      _locally_owned_dofs, _comm);
  dealii::DoFTools::make_sparsity_pattern(_dof_handler, sparsity_pattern,
                                          _constraints);
  sparsity_pattern.compress();

  // Reinitialize the sparse matrix with the sparsity pattern
  _system_matrix.reinit(sparsity_pattern);
}

template <int dim, typename VectorType>
void Laplace<dim, VectorType>::assemble_system(
    dealii::Function<dim> const &source,
    dealii::Function<dim> const &material_property)
{
  unsigned int const fe_degree = _fe.degree;
  dealii::QGauss<dim> const quadrature(fe_degree + 1);
  dealii::FEValues<dim> fe_values(
      _fe, quadrature,
      dealii::update_values | dealii::update_gradients |
          dealii::update_quadrature_points | dealii::update_JxW_values);
  unsigned int const dofs_per_cell = _fe.dofs_per_cell;
  unsigned int const n_q_points = quadrature.size();
  dealii::FullMatrix<double> cell_matrix(dofs_per_cell, dofs_per_cell);
  dealii::Vector<double> cell_rhs(dofs_per_cell);

  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);

  for (auto cell :
       dealii::filter_iterators(_dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    cell_matrix = 0;
    cell_rhs = 0;
    fe_values.reinit(cell);

    for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
    {
      double const rhs_value =
          source.value(fe_values.quadrature_point(q_point));
      double const diffusion_coefficient =
          material_property.value(fe_values.quadrature_point(q_point));
      for (unsigned int i = 0; i < dofs_per_cell; ++i)
      {
        for (unsigned int j = 0; j < dofs_per_cell; ++j)
          cell_matrix(i, j) +=
              diffusion_coefficient * fe_values.shape_grad(i, q_point) *
              fe_values.shape_grad(j, q_point) * fe_values.JxW(q_point);
        cell_rhs(i) += rhs_value * fe_values.shape_value(i, q_point) *
                       fe_values.JxW(q_point);
      }
    }

    cell->get_dof_indices(local_dof_indices);
    _constraints.distribute_local_to_global(
        cell_matrix, cell_rhs, local_dof_indices, _system_matrix, _system_rhs);
  }

  _system_matrix.compress(dealii::VectorOperation::add);
  _system_rhs.compress(dealii::VectorOperation::add);
}

template <int dim, typename VectorType>
template <typename PreconditionerType>
VectorType Laplace<dim, VectorType>::solve(PreconditionerType &preconditioner)
{
  VectorType solution(_locally_owned_dofs, _comm);
  dealii::SolverControl solver_control(_dof_handler.n_dofs(),
                                       1e-12 * _system_rhs.l2_norm());
  dealii::TrilinosWrappers::SolverCG solver(solver_control);
  preconditioner.initialize(_system_matrix);
  solver.solve(_system_matrix, solution, _system_rhs, preconditioner);

  if (dealii::Utilities::MPI::this_mpi_process(_comm) == 0)
    std::cout << "Solved in " << solver_control.last_step() << " iterations."
              << std::endl;

  _constraints.distribute(solution);
  _locally_relevant_solution = solution;

  return solution;
}

template <int dim, typename VectorType>
double Laplace<dim, VectorType>::compute_error(
    dealii::Function<dim> const &exact_solution)
{
  unsigned int const fe_degree = _fe.degree;
  dealii::QGauss<dim> const quadrature(fe_degree + 1);

  dealii::Vector<double> difference(
      _dof_handler.get_triangulation().n_active_cells());
  dealii::VectorTools::integrate_difference(
      _dof_handler, _locally_relevant_solution, exact_solution, difference,
      quadrature, dealii::VectorTools::L2_norm);

  return dealii::VectorTools::compute_global_error(
      _dof_handler.get_triangulation(), difference,
      dealii::VectorTools::L2_norm);
}

template <int dim, typename VectorType>
void Laplace<dim, VectorType>::output_results() const
{
  dealii::DataOut<dim> data_out;
  data_out.attach_dof_handler(_dof_handler);
  data_out.add_data_vector(_locally_relevant_solution, "u");

  dealii::Vector<float> subdomain(_triangulation.n_active_cells());
  for (unsigned int i = 0; i < subdomain.size(); ++i)
    subdomain(i) = _triangulation.locally_owned_subdomain();
  data_out.add_data_vector(subdomain, "subdomain");

  data_out.build_patches();

  unsigned int comm_size = dealii::Utilities::MPI::n_mpi_processes(_comm);

  std::string filename =
      "solution-" + std::to_string(_triangulation.locally_owned_subdomain()) +
      "-" + std::to_string(comm_size);
  std::ofstream output((filename + ".vtu").c_str());
  data_out.write_vtu(output);

  if (dealii::Utilities::MPI::this_mpi_process(_comm) == 0)
  {
    std::vector<std::string> filenames;
    for (unsigned int i = 0; i < comm_size; ++i)
      filenames.push_back("solution-" + std::to_string(i) + "-" +
                          std::to_string(comm_size) + ".vtu");
    std::ofstream master_output("solution-" + std::to_string(comm_size) +
                                ".pvtu");
    data_out.write_pvtu_record(master_output, filenames);
  }
  MPI_Barrier(_comm);
}

template <int dim, typename VectorType>
template <typename PreconditionerType>
void Laplace<dim, VectorType>::run(
    PreconditionerType &preconditioner, dealii::Function<dim> const &source,
    dealii::Function<dim> const &material_property)
{
  setup_system(boost::property_tree::ptree());
  assemble_system(source, material_property);
  solve(preconditioner);
  output_results();
}

#endif // #ifdef MFMG_LAPLACE_HPP


//==============================================================================
//         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 arithmetic toolbox - idivround/scalar Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 01/12/2010
///
#include <nt2/arithmetic/include/functions/idivround.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

#include <nt2/constant/constant.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/two.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/five.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/valmin.hpp>

NT2_TEST_CASE_TPL ( idivround_real__2_0,  NT2_REAL_TYPES)
{

  using nt2::idivround;
  using nt2::tag::idivround_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<idivround_(T,T)>::type r_t;
  typedef iT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_ULP_EQUAL(idivround(nt2::Five<T>(),nt2::Three<T>()), nt2::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Four<T>(),nt2::Three<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Four<T>(),nt2::Zero<T>()), nt2::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Inf<T>(), nt2::Inf<T>()), nt2::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Minf<T>(), nt2::Minf<T>()), nt2::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Mone<T>(), nt2::Mone<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Nan<T>(), nt2::Nan<T>()), nt2::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::One<T>(), nt2::One<T>()), nt2::One<r_t>(), 0);
} // end of test for floating_

NT2_TEST_CASE_TPL ( idivround_unsigned_int__2_0,  NT2_UNSIGNED_TYPES)
{

  using nt2::idivround;
  using nt2::tag::idivround_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<idivround_(T,T)>::type r_t;
  typedef iT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_ULP_EQUAL(idivround(nt2::Five<T>(),nt2::Three<T>()), nt2::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Four<T>(),nt2::Three<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Four<T>(),nt2::Zero<T>()), nt2::Valmax<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::One<T>(), nt2::One<T>()), nt2::One<r_t>(), 0);
} // end of test for unsigned_int_

NT2_TEST_CASE_TPL ( idivround_signed_int__2_0,  NT2_INTEGRAL_SIGNED_TYPES)
{

  using nt2::idivround;
  using nt2::tag::idivround_;
  typedef typename nt2::meta::as_integer<T>::type iT;
  typedef typename nt2::meta::call<idivround_(T,T)>::type r_t;
  typedef iT wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS( r_t, wished_r_t );

  // specific values tests
  NT2_TEST_ULP_EQUAL(idivround(nt2::Five<T>(),nt2::Three<T>()), nt2::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Four<T>(),nt2::Three<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Four<T>(),nt2::Zero<T>()), nt2::Valmax<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Mfour<T>(),nt2::Zero<T>()), nt2::Valmin<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::Mone<T>(), nt2::Mone<T>()), nt2::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(idivround(nt2::One<T>(), nt2::One<T>()), nt2::One<r_t>(), 0);
} // end of test for signed_int_


#ifndef CAFFE_NHWC_BATCHNORM_LAYER_HPP_
#define CAFFE_NHWC_BATCHNORM_LAYER_HPP_

#include <vector>

#include "caffe/blob.hpp"
#include "caffe/layer.hpp"
#include "caffe/proto/caffe.pb.h"

#include <Eigen/Dense>

namespace caffe {

template <typename Dtype>
class BatchNormNHWCLayer : public Layer<Dtype> {
 public:
  explicit BatchNormNHWCLayer(const LayerParameter& param)
      : Layer<Dtype>(param) {}
  virtual void LayerSetUp(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  virtual void Reshape(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);

  virtual inline const char* type() const { return "BatchNormNHWC"; }
  virtual inline int ExactNumBottomBlobs() const { return 1; }
  virtual inline int ExactNumTopBlobs() const { return 1; }

 protected:
  //  NOTE: we only implement the forward_cpu method.
  virtual void Forward_cpu(const vector<Blob<Dtype>*>& bottom,
      const vector<Blob<Dtype>*>& top);
  // virtual void Forward_gpu(const vector<Blob<Dtype>*>& bottom,
  //     const vector<Blob<Dtype>*>& top);
  virtual void Backward_cpu(const vector<Blob<Dtype>*>& top,
      const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom) {
    NOT_IMPLEMENTED;
  }
  // virtual void Backward_gpu(const vector<Blob<Dtype>*>& top,
  //    const vector<bool>& propagate_down, const vector<Blob<Dtype>*>& bottom);

  Blob<Dtype> mean_, variance_;
  bool use_global_stats_;
  int channels_;
  Dtype eps_;
};

}

#endif


/* Copyright (c) 2017, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The Astrobee platform is 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 <Eigen/Dense>
#include <Eigen/Jacobi>
#include <Eigen/SVD>

#include <vector>
#include <iostream>
#include <limits>
//#include <ros/ros.h>

using Eigen::MatrixXd;
using namespace Eigen;
//using namespace std;

void apply_givens(MatrixXf & H_f_j, MatrixXf & H_x_j, VectorXf & r_j, int row1, int row2, int col,
    int H_x_j_col_start, int H_x_j_num_cols) {
  JacobiRotation<float> g;
  g.makeGivens(H_f_j(row1, col), H_f_j(row2, col));
  H_f_j.applyOnTheLeft(row1, row2, g.adjoint());
  H_x_j.block(0, H_x_j_col_start, H_x_j.rows(), H_x_j_num_cols).applyOnTheLeft(row1, row2, g.adjoint());
  r_j.applyOnTheLeft(row1, row2, g.adjoint());
}

void apply_givens(MatrixXf & H, VectorXf & r, int row1, int row2, int col) {
  JacobiRotation<float> g;
  g.makeGivens(H(row1, col), H(row2, col));
  H.block(0, col, H.rows(), H.cols() - col).applyOnTheLeft(row1, row2, g.adjoint());
  r.applyOnTheLeft(row1, row2, g.adjoint());
}

Matrix3f skew(const Vector3f & v) {
  Matrix3f ret;
  ret << 0, -v(2), v(1),
         v(2), 0, -v(0),
         -v(1), v(0), 0;
  return ret;
}

// S is lower triangular
int pinv(const MatrixXf & S, MatrixXf & out) {
  LDLT<MatrixXf,Lower> chol(S);
  if (chol.info() != Success) {
    return 1;
  }
  out = MatrixXf::Identity(out.rows(), out.cols());
  chol.solveInPlace(out);
  return 0;
}

int of_residual_and_h(float* of_measured_p, float* global_points_p, float* camera_tf_global_p,
          int* valid_p, int num_points, int ase_of_num_aug, int ase_of_num_features,
          float tun_ase_mahal_distance_max, float ase_of_r_mag, float ase_inv_focal_length, float ase_distortion, float* P_p,
          float* r_out_p, float* H_out_p, unsigned int* augs_bitmask, unsigned char* num_of_tracks_out, float* mahal_dists_out_p, float* R_out_p)
{
  //struct timeval secs1, secs2;
  //gettimeofday(&secs1, 0);
  int covariance_size = 21 + 6 * ase_of_num_aug;
  Map<VectorXf> of_measured(of_measured_p, ase_of_num_features * 2 * ase_of_num_aug);
  Map<MatrixXf> global_points(global_points_p, 4, ase_of_num_features);
  Map<MatrixXf> camera_tf_global(camera_tf_global_p, 4 * ase_of_num_aug, 4);
  Map<MatrixXi> valid(valid_p, ase_of_num_features, ase_of_num_aug);
  Map<MatrixXf> P(P_p, covariance_size, covariance_size);
  Map<VectorXf> r_out(r_out_p, 6 * ase_of_num_aug);
  Map<MatrixXf> H_out(H_out_p, 6 * ase_of_num_aug, covariance_size);
  Map<MatrixXf> R_out(R_out_p, 6 * ase_of_num_aug, 6 * ase_of_num_aug);
  Map<VectorXf> mahal_dists_out(mahal_dists_out_p, ase_of_num_features);

  r_out.setZero();
  H_out.setZero();
  R_out.setZero();
  *num_of_tracks_out = 0;
  *augs_bitmask = 0;

  // figure out which augmentations were used (this helps with speed)
  std::vector<int> used_augs;
  for (int i = 0; i < ase_of_num_aug; i++) {
    if ((valid.col(i).array() > 0).any()) {
      used_augs.push_back(i);
      *augs_bitmask |= 2 << i;
    }
  }
  if (used_augs.size() < 3) {
    r_out(1) = 1;
    return 1;
  }

  MatrixXf reduced_P(used_augs.size() * 6, used_augs.size() * 6);
  for (int i = 0; i < used_augs.size(); i++)
    for (int j = 0; j < used_augs.size(); j++)
      reduced_P.block<6, 6>(6 * i, 6 * j) = P.block<6, 6>(21 + 6 * i, 21 + 6 * j);

  VectorXf r((2 * used_augs.size() - 3) * ase_of_num_features);
  MatrixXf H((2 * used_augs.size() - 3) * ase_of_num_features, 6 * used_augs.size());
  H.setZero();
  //gettimeofday(&secs2, 0);
  //ROS_INFO("of_residual_and_h %d cast", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  int cur_row = 0;
  int num_tracks = 0;
  for (int j = 0; j < num_points; j++) {
    int valid_augs = valid.row(j).count();
    if (valid_augs < 2)
      continue;
    int rows = valid_augs * 2;
    VectorXf r_j(rows);
    MatrixXf H_f_j(rows, 3);
    MatrixXf H_x_j(rows, 6 * used_augs.size());
    H_x_j.setZero();
    int aug_ind = 0;
    // construct r_j, H_x_j, and H_f_j
    for (int i = 0; i < valid_augs; i++) {
      while (!valid(j, used_augs[aug_ind]))
        aug_ind++;
      assert(aug_ind < used_augs.size());

      Matrix<float, 3, 4> c_tf_g = camera_tf_global.block<3, 4>(4 * used_augs[aug_ind], 0);
      Vector3f camera_landmark = c_tf_g * global_points.col(j);
      Vector2f z_est(camera_landmark(0), camera_landmark(1));
      z_est *= 1.0 / camera_landmark(2);

      Matrix<float, 2, 3> prefix;
      prefix << 1, 0, -z_est(0), 0, 1, -z_est(1);
      prefix *= 1.0 / camera_landmark(2);
      Matrix<float, 2, 3> H_theta_ji = prefix * skew(camera_landmark);
      Matrix<float, 2, 3> H_p_ji = -prefix * c_tf_g.block<3, 3>(0, 0);

      r_j.segment<2>(2 * i) = of_measured.segment<2>(2 * ase_of_num_aug * j + 2 * used_augs[aug_ind]) - z_est;
      H_x_j.block<2, 3>(2 * i, aug_ind * 6)     = H_theta_ji;
      H_x_j.block<2, 3>(2 * i, aug_ind * 6 + 3) = H_p_ji;
      H_f_j.block<2, 3>(2 * i, 0) = -H_p_ji;
      aug_ind++;
    }

    // do givens rotations to make H_f_j upper triangular
    // optimized version only if we observed all 4 augmentations
    if (used_augs.size() == valid_augs && valid_augs == 4) {
      // first make each block of 2 rows x x x; 0 x x
      for (int i = 0; i < rows; i+=2)
        apply_givens(H_f_j, H_x_j, r_j, i, i + 1, 0, 6 * (i/2), 6);
      // now make each block of 4 rows x x x; 0 x x; 0 0 x; 0 0 0
      for (int i = 0; i < rows; i+=4) {
        apply_givens(H_f_j, H_x_j, r_j, i    , i + 2, 0, 6 * (i/4), 12);
        apply_givens(H_f_j, H_x_j, r_j, i + 1, i + 2, 1, 6 * (i/4), 12);
        apply_givens(H_f_j, H_x_j, r_j, i + 1, i + 3, 1, 6 * (i/4), 12);
        apply_givens(H_f_j, H_x_j, r_j, i + 2, i + 3, 2, 6 * (i/4), 12);
      }
      // finally eliminate the second block of four
      apply_givens(H_f_j, H_x_j, r_j, 0, 4, 0, 0, 24);
      apply_givens(H_f_j, H_x_j, r_j, 1, 4, 1, 0, 24);
      apply_givens(H_f_j, H_x_j, r_j, 1, 5, 1, 0, 24);
      apply_givens(H_f_j, H_x_j, r_j, 2, 4, 2, 0, 24);
      apply_givens(H_f_j, H_x_j, r_j, 2, 5, 2, 0, 24);
      apply_givens(H_f_j, H_x_j, r_j, 2, 6, 2, 0, 24);
    } else {
      // normal version
      for (int col = 0; col < 3; col++)
       for (int row = rows - 1; row > col; row--)
         apply_givens(H_f_j, H_x_j, r_j, row - 1, row, col, 0, H_x_j.cols());
    }

    // then delete the top three rows which are nonzero in H_f_j
    Block<MatrixXf> H_x_j_reduced = H_x_j.block(3, 0, rows - 3, H_x_j.cols());
    VectorBlock<VectorXf> r_j_reduced = r_j.segment(3, rows - 3);

    // now check the mahalanobis distance
    MatrixXf S(H_x_j_reduced.rows(), H_x_j_reduced.rows());
    S.triangularView<Lower>() = H_x_j_reduced * reduced_P * H_x_j_reduced.transpose();
    for (int i = 0; i < S.rows(); i++)
      S(i, i) += ase_of_r_mag * ase_of_r_mag;
    MatrixXf S_inv(S.rows(), S.cols());
    if (pinv(S, S_inv)) {
      fprintf(stderr, "Failed to take inverse for Mahalanobis distance.\n");
      continue;
    }
    float mahal_dist = sqrt(r_j_reduced.transpose() * S_inv * r_j_reduced);
    mahal_dists_out(j) = mahal_dist;
    if (mahal_dist > tun_ase_mahal_distance_max)
      continue;

    // add to full r and H
    r.segment(cur_row, r_j_reduced.rows()) = r_j_reduced;
    H.block(cur_row, 0, r_j_reduced.rows(), H_x_j_reduced.cols()) = H_x_j_reduced;
    cur_row += r_j_reduced.rows();
    num_tracks++;
  }
  //gettimeofday(&secs2, 0);
  //ROS_INFO("of_residual_and_h %d r and h", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  // set the rest to uninitialized
  for (int j = num_points; j < ase_of_num_features; j++)
    mahal_dists_out(j) = std::numeric_limits<double>::quiet_NaN();

  r.conservativeResize(cur_row);
  H.conservativeResize(cur_row, NoChange);
  if (r.rows() < 6 * used_augs.size()) {
    r_out(1) = 1;
    //ROS_INFO("of_residual_and_h end 1");
    return 1;
  }

  *num_of_tracks_out = static_cast<unsigned char>(num_tracks);
  //gettimeofday(&secs2, 0);
  //ROS_INFO("of_residual_and_h %d output", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  // now we do the compression
  // do givens rotations to make H upper triangular and remove as many rows as possible
  for (int col = 0; col < H.cols(); col++)
    for (int row = H.rows() - 1; row > col; row--)
      apply_givens(H, r, col, row, col);
  //gettimeofday(&secs2, 0);
  //ROS_INFO("of_residual_and_h %d compress", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));
  //secs1 = secs2;

  r_out.segment(0, 6 * used_augs.size()) = r.segment(0, 6 * used_augs.size());
  H_out.block(0, 0, 6 * used_augs.size(), 6 * used_augs.size()) = H.block(0, 0, 6 * used_augs.size(), 6 * used_augs.size());
  for (int i = 0; i < 6 * ase_of_num_aug; i++)
    R_out(i, i) = (ase_of_r_mag * ase_inv_focal_length) * (ase_of_r_mag * ase_inv_focal_length);
  //gettimeofday(&secs2, 0);
  //ROS_INFO("of_residual_and_h %d end 0", (((secs2.tv_sec - secs1.tv_sec) * 1000) + (secs2.tv_usec - secs1.tv_usec)));

  return 0;
}


#include <boost/test/unit_test.hpp>

#include "logs.h"
#include "route.h"
#include "track.h"

using namespace GPS;

const bool isFileName = true;
// Tests the minimum latitude for NorthYorkMoor.gpx this one is done first since is more likely all the others test will pass if this one does
BOOST_AUTO_TEST_SUITE( premadeGPX_Test )
// Will get the minimum latitdue inside NorthYorkMoor.gpx file
BOOST_AUTO_TEST_CASE( rNorthYorkMoors_Test){
   Route route = Route(LogFiles::GPXRoutesDir + "NorthYorkMoors.gpx", isFileName);
   BOOST_CHECK_EQUAL(route.minLatitude(), 54.40498352050781);
}

BOOST_AUTO_TEST_SUITE_END()

////////////////////////////////////////////////////////////////////////////////////////////

// Will test for different min lat being positive, negative, and out of range
BOOST_AUTO_TEST_SUITE( InsideLatRange )

// Testing with positive latitudes
BOOST_AUTO_TEST_CASE(positiveLat){
   Route route = Route(LogFiles::GPXRoutesDir + "N0720717_positiveMinLat.gpx", isFileName);
   BOOST_CHECK_GT( route.minLatitude(), 0);
}

// Test for the correct minimum latitude in a file with only positive ones
BOOST_AUTO_TEST_CASE(espPositiveLat){
   Route route = Route(LogFiles::GPXRoutesDir + "N0720717_positiveMinLat.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minLatitude(), 46.42201554775238, .001);
}

// Testing with negative latitudes
BOOST_AUTO_TEST_CASE(negativeLat){
   Route route = Route(LogFiles::GPXRoutesDir + "N0720717_negativeMinLat.gpx", isFileName);
   BOOST_CHECK_LT( route.minLatitude(), 0);
}

// Test for the correct minimum latitude in a file with only negative ones
BOOST_AUTO_TEST_CASE(espNegativeLat){
   Route route = Route(LogFiles::GPXRoutesDir + "N0720717_negativeMinLat.gpx", isFileName);
   BOOST_CHECK_CLOSE( route.minLatitude(), -54.42204773426058, .001);
}

// Testing with latitude as 0
BOOST_AUTO_TEST_CASE(zeroLat){
   Route route = Route(LogFiles::GPXRoutesDir + "N0720717_zeroMinLat.gpx", isFileName);
   BOOST_CHECK_EQUAL( route.minLatitude(), 0);
}
// Testing with latitude over 90deg
BOOST_AUTO_TEST_CASE(overLat){
   Route route = Route(LogFiles::GPXRoutesDir + "N0720717_overtopedgeMinLat.gpx", isFileName);
   BOOST_CHECK_THROW( route.minLatitude(), std::out_of_range);
}
// Testing with latitude under -90deg
BOOST_AUTO_TEST_CASE(underLat){
   Route route = Route(LogFiles::GPXRoutesDir + "N0720717_underbotedgeMinLat.gpx", isFileName);
   BOOST_CHECK_THROW( route.minLatitude(),std::out_of_range);
}

BOOST_AUTO_TEST_SUITE_END()

////////////////////////////////////////////////////////////////////////////////////////////


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl_ros/transforms.h>
#include <pcl_ros/point_cloud.h>
#include <boost/asio.hpp>
#include <std_msgs/UInt16.h>
#include <cld1a0_pcl.h>
#include <cmath>
#include <thread>

#define PAYLOAD_SIZE            6

#define DATABUFFER_SIZE_2D      249
#define DATASET_SIZE_2D         121

#define DATABUFFER_SIZE_3D      14407
#define DATASET_SIZE_3D         9600

#define COLOR_MIN               0
#define COLOR_MAX               255

#define MATH_PI                 3.14159265

const float BASE_DEPTH_3D = 3000.0;
const float BASE_DEPTH_2D = 16000.0;
const float BASE_ANGLE_2D = 120.0;
const float HALF_ANGLE = 180.0;

typedef pcl::PointCloud<pcl::PointXYZRGBA> PointXYZRGBA;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr scan_2D;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr scan_3D;

ros::Publisher pub;
int version_num;
bool version_changed;

uint8_t *cloudBuffer = new uint8_t[DATABUFFER_SIZE_3D - PAYLOAD_SIZE];
float *cloudSetBuffer = new float[DATASET_SIZE_3D];
uint8_t *cloudBuffer_2D = new uint8_t[DATABUFFER_SIZE_2D - PAYLOAD_SIZE];
float *cloudSetBuffer_2D = new float[DATASET_SIZE_2D];

uint8_t* bufferPtr_2D;
uint8_t* bufferPtr01;
uint8_t* bufferPtr02;

int order, dataCnt;
uint8_t FIRST, SECOND, THIRD;
uint16_t LSB, MSB;

float centerX, centerY, focalLength, width, height;
float xAxis = 0.0, yAxis = 0.0, zAsix = 0.0;
float actualX = 0.0, actualY = 0.0;

int distanceIndex;
float data1, data2, actualDistance, length;
uint8_t currentBuffer;

static std::vector<uint32_t> colorArray;
void colorBuffer()
{
    int colors = 0;
    uint8_t r = 255;
    uint8_t g = 0;
    uint8_t b = 0;

    for (int colors = 0; colors < 2; colors++)
    {
        for (int i = 0; i < 3; i++)
        {
            for (int colorCnt = 0; colorCnt < COLOR_MAX + 1; colorCnt++)
            {
                switch (colors)
                {
                    // RED -> YELLOW
                    case 0:
                        g++;
                        break;
                    // YELLOW -> BLUE
                    case 1:
                        r--;
                        g--;
                        b++;
                        break;
                    // BLUE -> RED
                    case 2:
                        r++;
                        b--;
                        break;
                }

                uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
                colorArray.push_back(rgb);

                if (colorCnt == COLOR_MAX)
                {
                    colors++;
                }
            }
        }
    }

    ROS_INFO("COLORS STORED (%d)", colorArray.size());
}

bool drawing = false;
uint8_t cloudScatter_2D()
{
    if (!drawing)
    {
        drawing = true;
        memcpy(cloudBuffer_2D, &bufferPtr_2D[PAYLOAD_SIZE], sizeof(uint8_t) * DATABUFFER_SIZE_2D);

        int distanceCnt = 0;
        float angleStep = 0;

        for (int dataLength = 0; dataLength < (DATABUFFER_SIZE_2D - PAYLOAD_SIZE) - 1; dataLength+=2)
        {
            LSB = cloudBuffer_2D[dataLength];
            MSB = cloudBuffer_2D[dataLength + 1];

            data1 = (float)((MSB << 8) | LSB);

            cloudSetBuffer_2D[distanceCnt++] = data1;
        }

        float point_angle_var = 0.0;
        angleStep = (BASE_ANGLE_2D / DATASET_SIZE_2D);

        for (int idx = 0; idx < DATASET_SIZE_2D; idx++)
        {
            float point_angle = (float)(((BASE_ANGLE_2D / 2 * -1) + point_angle_var) * MATH_PI / HALF_ANGLE);
            point_angle_var += angleStep;

            actualDistance = (cloudSetBuffer_2D[idx] * 0.1);
            actualX = (sin(point_angle) * actualDistance);
            actualY = (cos(point_angle) * actualDistance); // depth

            scan_2D.get()->points[idx].x = actualX;
            scan_2D.get()->points[idx].y = actualY;
            scan_2D.get()->points[idx].z = 0.0;

            if (cloudSetBuffer_2D[idx] < (float)BASE_DEPTH_2D)
            {
                scan_2D.get()->points[idx].r = 255;
                scan_2D.get()->points[idx].g = 255;
                scan_2D.get()->points[idx].b = 0;
                scan_2D.get()->points[idx].a = 255;
            }
            else
            {
                scan_2D.get()->points[idx].a = 0;
            }
        }

        drawing = false;
    }
}

uint8_t cloudScatter_3D()
{
    if (!drawing)
    {
        drawing = true;

        switch (currentBuffer)
        {
            case 0x00:
                memcpy(cloudBuffer, &bufferPtr02[PAYLOAD_SIZE], sizeof(uint8_t) * DATABUFFER_SIZE_3D);
                break;
            case 0x01:
                memcpy(cloudBuffer, &bufferPtr01[PAYLOAD_SIZE], sizeof(uint8_t) * DATABUFFER_SIZE_3D);
                break;
        }

        int distanceCnt = 0;
        for (int dataLength = 0; dataLength < (DATABUFFER_SIZE_3D - PAYLOAD_SIZE) - 1; dataLength+=3)
        {
            FIRST = cloudBuffer[dataLength];
            SECOND = cloudBuffer[dataLength + 1];
            THIRD = cloudBuffer[dataLength + 2];

            data1 = (float)((FIRST << 4) | (SECOND >> 4));
            data2 = (float)(((SECOND & 0xf) << 8) | THIRD);

            cloudSetBuffer[distanceCnt++] = data1;
            cloudSetBuffer[distanceCnt++] = data2;
        }

        int index = 0;
        for (int yy = 0; yy < height; yy++)
        {
            for (int xx = 0; xx < width; xx++)
            {
                index = (xx + (yy * width));

                float h = sqrt(pow((centerX - xx), 2) + pow((centerY - yy), 2));
                float fh = sqrt(pow(h, 2) + pow(focalLength, 2));
                float dRatio = cloudSetBuffer[index] / fh;

                actualDistance = focalLength * dRatio;
                actualX = (xx - (width / 2)) * actualDistance / focalLength;
                actualY = ((height - yy) - (height / 2)) * actualDistance / focalLength;

                if (cloudSetBuffer[index] < (float)BASE_DEPTH_3D)
                {
                    scan_3D.get()->points[index].x = actualX;
                    scan_3D.get()->points[index].y = actualDistance;
                    scan_3D.get()->points[index].z = actualY;

                    uint32_t rgb = colorArray[(int)actualDistance % colorArray.size()];
                    scan_3D.get()->points[index].rgb = *reinterpret_cast<float*>(&rgb);
                    scan_3D.get()->points[index].a = 255;
                }
                else
                {
                    scan_3D.get()->points[index].x = 0.0;
                    scan_3D.get()->points[index].y = 0.0;
                    scan_3D.get()->points[index].z = 0.0;
                    scan_3D.get()->points[index].a = 0;
                }
            }
        }

        drawing = false;
    }
}

std::string getVersion()
{
    std::string input;
    std::cout << ">>>>> PLEASE ENTER THE TYPE OF DATA YOU WANT TO RECEIVE FROM CYGLIDAR: ";
    std::getline(std::cin, input);

    return input;
}

void running()
{
    ros::NodeHandle nh;
    ros::NodeHandle priv_nh("~");

    std::string port;
    int baud_rate;
    std::string frame_id;

    priv_nh.param("port", port, std::string("/dev/ttyUSB0"));
    priv_nh.param("baud_rate", baud_rate, 3000000);
    priv_nh.param("frame_id", frame_id, std::string("laser_link"));

    ROS_INFO("READY");

    colorBuffer();

    pub = nh.advertise<sensor_msgs::PointCloud2>("scan", 1);
    scan_2D = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);
    scan_3D = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>);

    // PointCloud2 for 2D
    scan_2D.get()->is_dense = false;
    scan_2D.get()->header.frame_id = frame_id;

    scan_2D.get()->points.resize(DATABUFFER_SIZE_2D);
    for (int point = 0; point < DATABUFFER_SIZE_2D; point++)
    {
        scan_2D.get()->points[point].a = 0;
    }

    // PointCloud2 for 3D
    scan_3D.get()->width = 160;
    scan_3D.get()->height = 60;
    scan_3D.get()->is_dense = false;
    scan_3D.get()->header.frame_id = frame_id;

    width = (float)(scan_3D.get()->width);
    height = (float)(scan_3D.get()->height);
    centerX = width / 2.0;
    centerY = height / 2.0;
    focalLength = 40.5;

    scan_3D.get()->points.resize(DATABUFFER_SIZE_3D);
    for (int point = 0; point < DATABUFFER_SIZE_3D; point++)
    {
        scan_3D.get()->points[point].a = 0;
    }

    dataCnt = 0;
    distanceIndex = 0;
    actualDistance = 0.0;

    length = 0.0;
    order = 0;

    drawing = false;
    currentBuffer = 0x00;

    version_changed = false;
    boost::asio::io_service io;
    try
    {
        cld1a0_pcl_driver::cld1a0_pcl laser(port, baud_rate, io);

        version_num = std::stoi(getVersion());
        laser.packet(version_num);

        while (ros::ok())
        {
            uint8_t* result = laser.poll(version_num);
            if (result != false)
            {/*
                ROS_INFO(">>>>>>> TEST: %x, %x, %x, %x, %x, %x",
                result[0], result[1], result[2], result[3], result[4], result[5]);*/

                switch (result[5])
                {
                    case 0x01: // 2D
                        bufferPtr_2D = &result[0];
                        cloudScatter_2D();

                        pcl_conversions::toPCL(ros::Time::now(), scan_2D->header.stamp);
                        pub.publish(scan_2D);
                        break;
                    case 0x08: // 3D
                        switch (currentBuffer)
                        {
                            case 0x00:
                                bufferPtr01 = &result[0];
                                currentBuffer = 0x01;
                                break;
                            case 0x01:
                                bufferPtr02 = &result[0];
                                currentBuffer = 0x00;
                                break;
                        }
                        cloudScatter_3D();

                        pcl_conversions::toPCL(ros::Time::now(), scan_3D->header.stamp);
                        pub.publish(scan_3D);
                        break;
                }
            }
        }
        laser.close();
    }
    catch (boost::system::system_error ex)
    {
        ROS_ERROR("[Error] instantiating laser object. \
        Are you sure you have the correct port and baud rate? \
        Error was %s", ex.what());
    }
}

int main(int argc, char **argv)
{
    // Initialize ROS
    ros::init(argc, argv, "line_laser");

    //std::thread first(getVersion);
    std::thread second(running);

    //first.join();
    second.join();

    return 0;
}


// Copyright (C) 2013 Massachusetts Institute of Technology, Lincoln Laboratory
// License: Boost Software License   See LICENSE.txt for the full license.
// Authors: Davis E. King (davis@dlib.net)
#include "doc_vects.h"
#include <dlib/matrix.h>
#include <dlib/statistics.h>
#include <map>
#include <mitie/group_tokenizer.h>
#include <mitie/gigaword_reader.h>
#include <mitie/unigram_tokenizer.h>

using namespace std;
using namespace dlib;
using namespace mitie;

typedef std::vector<std::pair<dlib::uint32, float> > sparse_vector_type;

// ----------------------------------------------------------------------------------------

static unsigned long get_word_id (
    const std::map<std::string, unsigned long>& vocab,
    const std::string& word
)
{
    std::map<std::string, unsigned long>::const_iterator i;
    i = vocab.find(word);
    if (i != vocab.end())
        return i->second;
    else
        return vocab.size();
}

// ----------------------------------------------------------------------------------------

static std::map<std::string, unsigned long> make_word_to_int_mapping (
    const std::map<std::string, unsigned long>& words,
    unsigned long num
)
{
    std::vector<std::pair<unsigned long, std::string> > temp;
    temp.reserve(words.size());
    for (std::map<std::string, unsigned long>::const_iterator i = words.begin(); i != words.end(); ++i)
    {
        temp.push_back(make_pair(i->second, i->first));
    }

    std::sort(temp.rbegin(), temp.rend());

    // pick the most common words for our result set
    std::map<std::string, unsigned long> res;
    for (unsigned long i = 0; i < temp.size() && res.size() < num; ++i)
    {
        res[temp[i].second] = i;
    }
    return res;
}

// ----------------------------------------------------------------------------------------

void gigaword_doc_to_vects (
    const std::map<std::string, unsigned long>& words,
    const gigaword_document& doc,
    sparse_vector_type& lhs,
    sparse_vector_type& rhs,
    dlib::rand& rnd
)
{
    lhs.clear();
    rhs.clear();
    istringstream sin(doc.text + " " + doc.headline);
    unigram_tokenizer tok(sin);
    string word;
    while (tok(word))
    {
        const unsigned long id = get_word_id(words, word);
        if (rnd.get_random_float() > 0.5)
        {
            lhs.push_back(make_pair(id, 1));
        }
        else
        {
            rhs.push_back(make_pair(id, 1));
        }
    }

    make_sparse_vector_inplace(lhs);
    make_sparse_vector_inplace(rhs);
}

// ----------------------------------------------------------------------------------------

void make_doc_vects(const dlib::command_line_parser& parser)
{
    const long vocab_size = 300000;
    const long num_contexts = 40000000;
    const long num_correlations = get_option(parser, "dims", 500);

    ifstream fin("top_word_counts.dat", ios::binary);
    std::map<std::string, unsigned long> words;
    deserialize(words, fin);
    cout << "num words in dictionary: " << words.size() << endl;

    words = make_word_to_int_mapping(words,vocab_size);

    std::vector<dlib::file> files = get_files_in_directory_tree(directory(parser[0]), match_all());
    cout << "number of gigaword XML files found: " << files.size() << endl;
    gigaword_reader reader(files);

    random_subset_selector<sparse_vector_type> L, R;
    L.set_max_size(num_contexts);
    R.set_max_size(num_contexts);

    // read a bunch of document vectors out of the gigaword corpus
    gigaword_document doc;
    sparse_vector_type lhs, rhs;
    dlib::rand rnd;
    while (reader(doc))
    {
        gigaword_doc_to_vects(words, doc, lhs, rhs, rnd);
        L.add(lhs);
        R.add(rhs);
    }

    cout << "Number of document vectors collected: " << L.size() << endl;
    cout << "Done gathering data, now running CCA." << endl;

    matrix<float> Ltrans, Rtrans;
    matrix<float> cors = cca(L, R, Ltrans, Rtrans, num_correlations, 30);
    cout << "CCA correlations: "<< trans(cors) << endl;

    // now build a map of words to their CCA reduced representation
    std::map<string, matrix<float,0,1> > word_vectors;
    for (std::map<string,unsigned long>::const_iterator i = words.begin(); i != words.end(); ++i)
    {
        // Ltrans and Rtrans should really contain the same basic information.  So we just
        // use Ltrans and ignore the other since it should be redundant.
        word_vectors[i->first] = trans(rowm(Ltrans, get_word_id(words, i->first)));
    }

    cout << "Saving word to vector map to doc_vects.dat file..." << endl;
    std::ofstream fout("doc_vects.dat", ios::binary);
    serialize(word_vectors, fout);
}

// ----------------------------------------------------------------------------------------


// Boost.Geometry

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

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGY_CARTESIAN_PRECISE_AREA_HPP
#define BOOST_GEOMETRY_STRATEGY_CARTESIAN_PRECISE_AREA_HPP

#include <boost/mpl/if.hpp>

//#include <boost/geometry/arithmetic/determinant.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_type.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/strategy/area.hpp>
#include <boost/geometry/util/select_most_precise.hpp>
#include <boost/geometry/util/precise_math.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Cartesian area calculation
\ingroup strategies
\details Calculates cartesian area using the trapezoidal rule and precise
         summation (useful to increase precision with floating point arithmetic)
\tparam CalculationType \tparam_calculation

\qbk{
[heading See also]
[link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
}

*/
template
<
    typename CalculationType = void
>
class precise_cartesian
{
public :
    template <typename Geometry>
    struct result_type
        : strategy::area::detail::result_type
            <
                Geometry,
                CalculationType
            >
    {};

    template <typename Geometry>
    class state
    {
        friend class precise_cartesian;

        typedef typename result_type<Geometry>::type return_type;

    public:
        inline state()
            : sum1(0)
            , sum2(0)
        {
            // Strategy supports only 2D areas
            assert_dimension<Geometry, 2>();
        }

    private:
        inline return_type area() const
        {
            return_type const two = 2;
            return (sum1 + sum2) / two;
        }

        return_type sum1;
        return_type sum2;
    };

    template <typename PointOfSegment, typename Geometry>
    static inline void apply(PointOfSegment const& p1,
                             PointOfSegment const& p2,
                             state<Geometry>& st)
    {
        typedef typename state<Geometry>::return_type return_type;

        auto const det = (return_type(get<0>(p1)) + return_type(get<0>(p2)))
                * (return_type(get<1>(p1)) - return_type(get<1>(p2)));

        auto const res = boost::geometry::detail::precise_math::two_sum(st.sum1, det);

        st.sum1 = res[0];
        st.sum2 += res[1];
    }

    template <typename Geometry>
    static inline auto result(state<Geometry>& st)
    {
        return st.area();
    }

};

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGY_CARTESIAN_PRECISE_AREA_HPP


// Boost.Geometry

// Copyright (c) 2018 Oracle and/or its affiliates.

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

// 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_FORMULAS_MERIDIAN_DIRECT_HPP
#define BOOST_GEOMETRY_FORMULAS_MERIDIAN_DIRECT_HPP

#include <boost/math/constants/constants.hpp>

#include <boost/geometry/core/radius.hpp>

#include <boost/geometry/formulas/differential_quantities.hpp>
#include <boost/geometry/formulas/flattening.hpp>
#include <boost/geometry/formulas/meridian_inverse.hpp>
#include <boost/geometry/formulas/quarter_meridian.hpp>
#include <boost/geometry/formulas/result_direct.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry { namespace formula
{

/*!
\brief Compute the direct geodesic problem on a meridian
*/

template <
    typename CT,
    bool EnableCoordinates = true,
    bool EnableReverseAzimuth = false,
    bool EnableReducedLength = false,
    bool EnableGeodesicScale = false,
    unsigned int Order = 4
>
class meridian_direct
{
    static const bool CalcQuantities = EnableReducedLength || EnableGeodesicScale;
    static const bool CalcRevAzimuth = EnableReverseAzimuth || CalcQuantities;
    static const bool CalcCoordinates = EnableCoordinates || CalcRevAzimuth;

public:
    typedef result_direct<CT> result_type;

    template <typename T, typename Dist, typename Spheroid>
    static inline result_type apply(T const& lo1,
                                    T const& la1,
                                    Dist const& distance,
                                    bool north,
                                    Spheroid const& spheroid)
    {
        result_type result;

        CT const half_pi = math::half_pi<CT>();
        CT const pi = math::pi<CT>();
        CT const one_and_a_half_pi = pi + half_pi;
        CT const c0 = 0;

        CT azimuth = north ? c0 : pi;

        if (BOOST_GEOMETRY_CONDITION(CalcCoordinates))
        {
            CT s0 = meridian_inverse<CT, Order>::apply(la1, spheroid);
            int signed_distance = north ? distance : -distance;
            result.lon2 = lo1;
            result.lat2 = apply(s0 + signed_distance, spheroid);
        }

        if (BOOST_GEOMETRY_CONDITION(CalcRevAzimuth))
        {
            result.reverse_azimuth = azimuth;

            if (result.lat2 > half_pi &&
                result.lat2 < one_and_a_half_pi)
            {
                result.reverse_azimuth =  pi;
            }
            else if (result.lat2 < -half_pi &&
                     result.lat2 >  -one_and_a_half_pi)
            {
                result.reverse_azimuth =  c0;
            }

        }

        if (BOOST_GEOMETRY_CONDITION(CalcQuantities))
        {
            CT const b = CT(get_radius<2>(spheroid));
            CT const f = formula::flattening<CT>(spheroid);

            boost::geometry::math::normalize_spheroidal_coordinates
                <
                    boost::geometry::radian,
                    double
                >(result.lon2, result.lat2);

            typedef differential_quantities
            <
                CT,
                EnableReducedLength,
                EnableGeodesicScale,
                Order
            > quantities;
            quantities::apply(lo1, la1, result.lon2, result.lat2,
                              azimuth, result.reverse_azimuth,
                              b, f,
                              result.reduced_length, result.geodesic_scale);
        }
        return result;
    }

    // https://en.wikipedia.org/wiki/Meridian_arc#The_inverse_meridian_problem_for_the_ellipsoid
    // latitudes are assumed to be in radians and in [-pi/2,pi/2]
    template <typename T, typename Spheroid>
    static CT apply(T m, Spheroid const& spheroid)
    {
        CT const f = formula::flattening<CT>(spheroid);
        CT n = f / (CT(2) - f);
        CT mp = formula::quarter_meridian<CT>(spheroid);
        CT mu = geometry::math::pi<CT>()/CT(2) * m / mp;

        if (Order == 0)
        {
            return mu;
        }

        CT H2 = 1.5 * n;

        if (Order == 1)
        {
            return mu + H2 * sin(2*mu);
        }

        CT n2 = n * n;
        CT H4 = 1.3125 * n2;

        if (Order == 2)
        {
            return mu + H2 * sin(2*mu) + H4 * sin(4*mu);
        }

        CT n3 = n2 * n;
        H2 -= 0.84375 * n3;
        CT H6 = 1.572916667 * n3;

        if (Order == 3)
        {
            return mu + H2 * sin(2*mu) + H4 * sin(4*mu) + H6 * sin(6*mu);
        }

        CT n4 = n2 * n2;
        H4 -= 1.71875 * n4;
        CT H8 = 2.142578125 * n4;

        // Order 4 or higher
        return mu + H2 * sin(2*mu) + H4 * sin(4*mu) + H6 * sin(6*mu) + H8 * sin(8*mu);
    }
};

}}} // namespace boost::geometry::formula

#endif // BOOST_GEOMETRY_FORMULAS_MERIDIAN_DIRECT_HPP


#include "legged_motion_planner/CentroidalNLP.hpp"
#include "legged_motion_planner/Agent.hpp"

#include "IpIpoptApplication.hpp"
#include <Eigen/Dense>
#include <iostream>
#include "legged_motion_planner/class_csv.h"
#include "ros/ros.h"

int main(int argc, char** argv)
{
  bool SOLVE_IPOPT = true;

  // 3D world
  int WORLD_DIM = 3;
  //define legs
  int NUM_LEGS = 2;

  int CONTACT_LEGS = 1;
  // dt
  double dt = 0.001; // in sec
  // frequency
  double freq = 1/dt;
  // read data
  vector<labels> desired = pass_csv("/home/despargy/master_g15_ws/src/legged_motion_planner/records/DesiredTrajectory100.csv", NUM_LEGS);
  // vector<labels> desired = pass_csv("/home/atom/master_ws/src/legged_motion_planner/records/DesiredTrajectory01.csv", NUM_LEGS);

  // points to solved
  int POINTS = desired.size(); // all data - same as desired
  std::cout<<POINTS<<std::endl;

  double IND_STEP_TIME = 2.0 ; // double sup. -> 0.5sec + single sup. -> 1.5sec

  // time to achieve goal
  double HORIZON_TIME = POINTS*dt;

  //define goal for CoM
  Eigen::Vector3d TARGET_XYZ;
  TARGET_XYZ = desired[POINTS-1].CoM_positions;

  Agent *p_agent = new Agent();

  p_agent->setDim(WORLD_DIM);
  p_agent->setLegs(NUM_LEGS, CONTACT_LEGS);
  p_agent->setDt(dt);

  p_agent->setHorizonTime(HORIZON_TIME);

  p_agent->setTargetCoM_xyz(TARGET_XYZ);
  // p_agent->storeStartingCoM_PosVel(); //TODO

  std::cout<<"pass"<<std::endl;

  if(SOLVE_IPOPT){

    /* set problem - IPOPT throw IpoptApplication */

    // Create a new instance of our nlp
    // Problem for this Agent, pass pointer

    SmartPtr<CentroidalNLP> mynlp  = new CentroidalNLP(p_agent);

    mynlp->dt = dt;
    mynlp->n_points = POINTS-1; // USE THE FIRST AS INIT POINT - NOT SOLVE ANYTHING FOR THAT
    mynlp->desired = desired;
    bool init_state = mynlp->init_before_start();

    std::cout<<init_state<<std::endl;

    // Create an instance of the IpoptApplication
    SmartPtr<IpoptApplication> app = IpoptApplicationFactory();

    // The following overwrites the default name (ipopt.opt) of the
    // options file
    app->Options()->SetStringValue("option_file_name", "/home/despargy/master_g15_ws/src/legged_motion_planner/src/ipopt.opt");

    app->Options()->SetStringValue("output_file", "/home/despargy/master_g15_ws/src/legged_motion_planner/src/ipopt.out");

    // Initialize the IpoptApplication and process the options
    ApplicationReturnStatus status;
    status = app->Initialize();
    if( status != Solve_Succeeded )
    {
      std::cout << std::endl << std::endl << "*** Error during initialization!" << std::endl;
      return (int) status;
    }

    // Ask Ipopt to solve the problem
    status = app->OptimizeTNLP(mynlp);

    if (status == Solve_Succeeded) {
      std::cout << std::endl << std::endl << "*** The problem solved!" << std::endl;
    }
    else {
      std::cout << std::endl << std::endl << "*** The problem FAILED!" << std::endl;
    }

  }

  return 0;
}


/*
// Copyright 2018 University of Liege
//
// 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.
//
// Authors:
// - Adrien Crovato
*/

//// Sub-panel identification
// Identify which field points are to close to which panel and prepare AIC matrices.
//
// I/O:
// - bPan: body panels (structure)
// - fPan: field panel (structure)
// - sp: sub-panels (structure)
// - spAIC: sub-panels AIC (structure)
//
// CONSIDER USING vector of structs instead of struct of vectors
// CONSIDER USING fixed size of boolean vector instead of spP
// CONSIDER USING octree (tree) to find points instead of double loop

#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include "id_subpanel.h"

#define NDIM 3

using namespace std;
using namespace Eigen;

void id_subpanel(Network &bPan, Field &fPan, Subpanel &sp, Subpanel_AIC &spAIC) {

    // Temporary variables
    bool FLAG; // reset flag
    int ii; // index
    MatrixXd pV; // current panel vertices
    unsigned long size; // vector size
    pV.resize(4, NDIM);
    RowVector3d pC; // current field center
    Vector4d eL, d, dC, h, hC; // edge length, distance to edge & cut-off, distance to mid-edge & cut-off
    RowVector3d r1, r2, eXL, v; // distance to vertices, unit longitudinal edge vector, field point to mid-edge vector
    vector<int> vTemp; // temporary vector
    MatrixXd mTemp; // temporary matrix

    //// Begin
    cout << "Identifying sub-panels... " << flush;

    //// Identification of nearfield points
    for (int j = 0; j < bPan.nP; j++) {
        // Vertices coordinates & edges length & cut-off distance
        pV.row(0) = bPan.v0.row(j);
        pV.row(1) = bPan.v1.row(j);
        pV.row(2) = bPan.v2.row(j);
        pV.row(3) = bPan.v3.row(j);
        for (int k = 1; k <= 4; ++k) {
            int l = k % 4 + 1;
            eL(k-1) = (pV.row(l-1)-pV.row(k-1)).norm();
        }
        dC = sp.NC * eL;
        hC = sp.LC * eL;
        // Loop reset
        FLAG = 0;
        vTemp = {};
        for (int i = 0; i < fPan.nE; ++i) {
            ii = fPan.eIdx(i);
            // Target coordinates
            pC = fPan.CG.row(ii);
            // Distance to edges and mid-edges
            for (int k = 1; k <= 4; ++k) {
                int l = k % 4 + 1;
                r1 = pC - pV.row(k-1);
                r2 = pC - pV.row(l-1);
                eXL = (r2 - r1) / (r1 - r2).norm();
                v = (r1 + r2)/2;
                h(k-1) = abs(eXL.dot(v));
                d(k-1) = (r1.cross(r2)).norm() / (r1 - r2).norm();
            }
            // if target point is closer to edge than tolerance in perpendicular and longitudinal directions
            if (((d(0)-dC(0) <= 0) && (h(0)-hC(0) <= 0)) || ((d(1)-dC(1) <= 0) && (h(1)-hC(1) <= 0))
                || ((d(2)-dC(2) <= 0) && (h(2)-hC(2) <= 0)) || ((d(3)-dC(3) <= 0) && (h(3)-hC(3) <= 0))) {
                if (!FLAG) {
                    sp.sI.push_back(j);
                    FLAG = 1;
                }
                vTemp.push_back(ii);
            }
            if (i == fPan.nE-1 && FLAG)
                sp.fI.push_back(vTemp);
        }
    }

    //// Initialization of AIC matrices to 0
    for(int i = 0; i < sp.sI.size(); i++) {
        size = sp.fI[i].size();
        mTemp = MatrixXd::Zero(size, sp.NS);
        // Field
        spAIC.A.push_back(mTemp);
        spAIC.B.push_back(mTemp);
    }

    //// Control display
    cout << "Done!" << endl;
    cout << "Number of panels to split: " << sp.sI.size() << endl;
    cout << "Number of sub-panels per panel: " << sp.NS << endl;
    #ifdef VERBOSE
        for(int i = 0; i < sp.sI.size(); i++) {
            cout << sp.sI[i] << ": ";
            for(int j = 0; j < sp.fI[i].size(); j++) {
                cout << sp.fI[i][j] << ',';
            }
            cout << endl;
        }
    #endif
    cout << endl;
}


#include <boost/test/unit_test.hpp>
#include "test_utils.hpp"

#include <moreorg/OrganizationModel.hpp>
#include <moreorg/metrics/Redundancy.hpp>
#include <moreorg/vocabularies/OM.hpp>
#include <owlapi/model/OWLOntologyAsk.hpp>
#include <owlapi/model/OWLOntologyTell.hpp>
#include <owlapi/model/OWLObjectExactCardinality.hpp>
#include <moreorg/vocabularies/OM.hpp>

using namespace moreorg;
using namespace owlapi;
using namespace owlapi::model;

struct RedundancyFixture
{
    RedundancyFixture()
        : has("http://property/has")
    {
        om = make_shared<OrganizationModel>(getOMSchema());

        OWLOntologyTell tell(om->ontology());

        a = tell.klass("http://klass/base");
        b = tell.klass("http://klass/base-derived");
        c = tell.klass("http://klass/base-derived-derived");

        property = tell.objectProperty( has );

        tell.subClassOf(c,b);
        tell.subClassOf(b,a);
        om->ontology()->refresh();
    }

    IRI has;

    OWLClass::Ptr a;
    OWLClass::Ptr b;
    OWLClass::Ptr c;

    OWLObjectProperty::Ptr property;
    OrganizationModel::Ptr om;
};

BOOST_AUTO_TEST_SUITE(model_metrics)

BOOST_FIXTURE_TEST_CASE(redundancy, RedundancyFixture )
{
    // query =2.has.A and =2.has.C
    std::vector<OWLCardinalityRestriction::Ptr> query, resourcePool;
    {
        OWLCardinalityRestriction::Ptr restriction =
            make_shared<OWLObjectExactCardinality>(property, 2, a);
        query.push_back(restriction);
    }
    {
        OWLCardinalityRestriction::Ptr restriction =
            make_shared<OWLObjectExactCardinality>(property, 2, c);
        query.push_back(restriction);
    }

    // available =2.has.B and 2.has.C
    {
        OWLCardinalityRestriction::Ptr restriction =
            make_shared<OWLObjectExactCardinality>(property, 2, b);
        resourcePool.push_back(restriction);
    }
    {
        OWLCardinalityRestriction::Ptr restriction =
            make_shared<OWLObjectExactCardinality>(property, 2, c);
        resourcePool.push_back(restriction);
    }

    OrganizationModelAsk ask(om);
    metrics::Redundancy redundancy(ask, 0.5, has);
    // Serial connection of two parallel a 0.5
    // [ [ 0.5 ] --- [ 0.5 ] ]  --- [ [ 0.5 ] --- [ 0.5 ] ]
    //
    // each serial system has a survivability of: 0.5*0.5 = 0.25
    // overall serial system: 0.25*0.25 = 0.0625
    double expected = 0.0625;
    double redundancyVal = redundancy.computeMetric(query, resourcePool);
    BOOST_REQUIRE_MESSAGE(redundancyVal == expected, "Redundancy expected: " << expected << " but got " << redundancyVal);
}

BOOST_FIXTURE_TEST_CASE(redundancy_same_parallel_in_series, RedundancyFixture)
{
    std::vector<OWLCardinalityRestriction::Ptr> query, resourcePool;
    {
        OWLCardinalityRestriction::Ptr restriction =
            make_shared<OWLObjectExactCardinality>(property, 2, a);
        query.push_back(restriction);
    }

    // Resources
    {
        {
            OWLCardinalityRestriction::Ptr restriction =
                make_shared<OWLObjectExactCardinality>(property, 2, a);
            resourcePool.push_back(restriction);
        }
        {
            OWLCardinalityRestriction::Ptr restriction =
                make_shared<OWLObjectExactCardinality>(property, 2, a);
            resourcePool.push_back(restriction);
        }

        OrganizationModelAsk ask(om);
        metrics::Redundancy redundancy(ask, 0.5, has);
        // when the structure be considered then: (1 -pow( 1-pow(0.5,2),2))*pow(0.5,2);
        double expected = 0.25;
        double redundancyVal = redundancy.computeMetric(query, resourcePool);
        BOOST_REQUIRE_MESSAGE(redundancyVal == expected, "Redundancy expected: " << expected << " but got " << redundancyVal);
    }
    {
        {
            OWLCardinalityRestriction::Ptr restriction =
                make_shared<OWLObjectExactCardinality>(property, 4, a);
            resourcePool.push_back(restriction);
        }

        OrganizationModelAsk ask(om);
        metrics::Redundancy redundancy(ask, 0.5, has);
        // --> (1 - (1-0.25)^2)
        double expected = 0.4375;
        double redundancyVal = redundancy.computeMetric(query, resourcePool);
        BOOST_REQUIRE_MESSAGE(redundancyVal == expected, "Redundancy expected: " << expected << " but got " << redundancyVal);
    }
}

BOOST_AUTO_TEST_CASE(redundancy_computation)
{
    OrganizationModel om(getRootDir() + "/test/data/om-project-transterra.owl");
    OrganizationModelAsk omAsk(OrganizationModel::Ptr(new OrganizationModel(om)) );
    metrics::Redundancy redundancy(omAsk);

    IRI sherpa = moreorg::vocabulary::OM::resolve("Sherpa");
    IRI payloadCamera = moreorg::vocabulary::OM::resolve("PayloadCamera");
    IRI payloadBattery = moreorg::vocabulary::OM::resolve("PayloadBattery");
    IRI transportProvider = moreorg::vocabulary::OM::resolve("TransportProvider");

    double r_1, r_2, r_3;
    {
        ModelPool availableModelPool;
        availableModelPool[sherpa] = 1;

        IRIList functionalities;
        functionalities.push_back(transportProvider);
        r_1 = redundancy.computeSequential(functionalities, availableModelPool);
        BOOST_REQUIRE_MESSAGE(r_1 > 0.7, "Redundancy should be > 0.7, was: " << r_1);
    }

    {
        ModelPool availableModelPool;
        availableModelPool[sherpa] = 1;
        availableModelPool[payloadCamera] = 1;

        IRIList functionalities;
        functionalities.push_back(transportProvider);
        r_2 = redundancy.computeSequential(functionalities, availableModelPool);
        BOOST_REQUIRE_MESSAGE(r_2 > r_1, "Redundancy should be > " << r_1 << " was:" << r_2);
    }

    IRI stereoCameraProvider = moreorg::vocabulary::OM::resolve("StereoImageProvider");
    {
        ModelPool availableModelPool;
        availableModelPool[sherpa] = 1;
        availableModelPool[payloadCamera] = 1;

        IRIList functionalities;
        functionalities.push_back(transportProvider);
        functionalities.push_back(stereoCameraProvider);
        r_3 = redundancy.computeSequential(functionalities, availableModelPool);
        BOOST_REQUIRE_MESSAGE(r_3 > 0.7, "Redundancy should be > 0.7 was: " << r_3);
    }

    {
        ModelPool availableModelPool;
        availableModelPool[sherpa] = 1;

        ModelPool requiredModelPool;
        requiredModelPool[sherpa] = 1;
        requiredModelPool[transportProvider] = 1;

        // todo: redundancy when nothing is required
        BOOST_REQUIRE_NO_THROW( redundancy.computeExclusiveUse(requiredModelPool, availableModelPool));

        availableModelPool[sherpa] = 2;
        double r =  redundancy.computeExclusiveUse(requiredModelPool, availableModelPool);

        BOOST_TEST_MESSAGE("Test show required: " << OWLCardinalityRestriction::toString( omAsk.getCardinalityRestrictions(requiredModelPool, moreorg::vocabulary::OM::has(), OWLCardinalityRestriction::SUM_OP) ) );
        BOOST_TEST_MESSAGE("Test show available: " << OWLCardinalityRestriction::toString( omAsk.getCardinalityRestrictions(availableModelPool, moreorg::vocabulary::OM::has(), OWLCardinalityRestriction::SUM_OP) ) );
        BOOST_REQUIRE_MESSAGE(r > 0.4, "Redundancy exclusive use is:" << r);
    }

    {
        ModelPool availableModelPool;
        availableModelPool[sherpa] = 1;

        ModelPool requiredModelPool;
        requiredModelPool[sherpa] = 1;

        owlapi::model::IRI image_provider = moreorg::vocabulary::OM::resolve("ImageProvider");

    //    // todo: redundancy when nothing is required
    //    double r = redundancy.computeSharedUse(requiredModelPool, availableModelPool);
    //    BOOST_REQUIRE_MESSAGE(true, "Redundancy shared use is:" << r);
    }
}

BOOST_AUTO_TEST_CASE(function_redundancy)
{
    std::string filename = getRootDir() + "/test/data/om-project-transterra.owl";
    OrganizationModel::Ptr om = make_shared<OrganizationModel>(filename);

    {
        ModelPool modelPool;
        modelPool[moreorg::vocabulary::OM::resolve("Sherpa")] = 5;
        modelPool[moreorg::vocabulary::OM::resolve("CREX")] = 3;
        modelPool[moreorg::vocabulary::OM::resolve("BaseCamp")] = 3;
        modelPool[moreorg::vocabulary::OM::resolve("Payload")] = 10;

        OrganizationModelAsk ask(om, modelPool, true);

        {
            IRISet functionSet;
            functionSet.insert(moreorg::vocabulary::OM::resolve("Mapping"));
            Metric::Ptr metrics = Metric::getInstance(metrics::REDUNDANCY, ask);
            double value = metrics->computeSharedUse(functionSet, modelPool);
            BOOST_REQUIRE_MESSAGE(value != 0, "Mapping has redundancy: " << value);
        }

        {
            IRISet functionSet;
            functionSet.insert(moreorg::vocabulary::OM::resolve("TransportProvider"));
            Metric::Ptr metrics = Metric::getInstance(metrics::REDUNDANCY, ask);
            double value = metrics->computeSharedUse(functionSet, modelPool);

            BOOST_REQUIRE_MESSAGE(value != 0, "TransportProvider has redundancy: " << value);
        }
    }

    {
        ModelPool minRequired;
        minRequired[moreorg::vocabulary::OM::resolve("ImageProvider")] = 1;

        ModelPool minAvailable;
        minAvailable[moreorg::vocabulary::OM::resolve("SherpaTT")] = 1;

        OrganizationModelAsk ask(om, minAvailable, true);

        {
            Metric::Ptr metrics = Metric::getInstance(metrics::REDUNDANCY, ask);
            double value = metrics->computeSharedUse(minRequired, minAvailable);
            BOOST_REQUIRE_MESSAGE(value > 0.0, "Shared use should be > 0, was "
                    << value);
        }
    }

    {
        ModelPool minRequired;
        minRequired[moreorg::vocabulary::OM::resolve("SherpaTT")] = 1;

        ModelPool minAvailable;
        minAvailable[moreorg::vocabulary::OM::resolve("SherpaTT")] = 1;

        OrganizationModelAsk ask(om, minAvailable, true);

        {
            Metric::Ptr metrics = Metric::getInstance(metrics::REDUNDANCY, ask);
            double value = metrics->computeSharedUse(minRequired, minAvailable);
            BOOST_REQUIRE_MESSAGE(value > 0.0, "Shared use should be > 0, was "
                    << value);
        }
    }
}

BOOST_AUTO_TEST_CASE(metric_map_computation)
{
//    OrganizationModel om( getOMSchema() );
//    om.ontology()->refresh();
//    metrics::Redundancy redundancy(om);
//
//    MetricMap survivability = redundancy.getMetricMap();
//
//    //BOOST_TEST_MESSAGE("Survivability: \n" << metrics::Redundancy::toString(survivability));
//
//    {
//        IRI locationImageProvider("http://www.rock-robotics.org/2014/01/om-schema#ImageProvider");
//        IRI sherpa("http://www.rock-robotics.org/2014/01/om-schema#Sherpa");
//
//        ModelPool modelPool;
//        for(int i = 1; i <= 3; ++i)
//        {
//            modelPool[sherpa] = i;
//            double pSurvivability = redundancy.compute(locationImageProvider, modelPool);
//            BOOST_TEST_MESSAGE("Survivability for #" << i << " " << sherpa << ": \n" << pSurvivability);
//        }
//    }
//    {
//        IRI locationImageProvider("http://www.rock-robotics.org/2014/01/om-schema#LocationImageProvider");
//        IRI crex("http://www.rock-robotics.org/2014/01/om-schema#CREX");
//
//        ModelPool modelPool;
//        for(int i = 1; i <= 3; ++i)
//        {
//            modelPool[crex] = i;
//
//            //    required:
//            //            ModelBound::List
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Camera' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Localization' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Locomotion' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Mapping' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#PowerSource' (1, 1000000)
//            //
//            //    available:
//            //            ModelBound::List
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Camera' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#EmiPassive' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#LaserScanner' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Localization' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Locomotion' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Mapping' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#PowerSource' (0, 3)
//
//            double redundancyLevel = i;
//            double parallel = 1 - pow((1-0.5),redundancyLevel);
//            double expected = pow(parallel,5);
//            double pSurvivability = redundancy.compute(locationImageProvider, modelPool);
//            BOOST_REQUIRE_MESSAGE(pSurvivability == expected, "Survivability for #" << i << " " << crex << ": \n" << pSurvivability << " (expected: " << expected << ")");
//        }
//    }
//    {
//        IRI locationImageProvider("http://www.rock-robotics.org/2014/01/om-schema#LocationImageProvider");
//        IRI stereoCamera("http://www.rock-robotics.org/2014/01/om-schema#StereoImageProvider");
//
//        IRISet services;
//        services.insert(locationImageProvider);
//        services.insert(stereoCamera);
//
//        IRI robot("http://www.rock-robotics.org/2014/01/om-schema#Sherpa");
//        ModelPool modelPool;
//        for(int i = 1; i <= 3; ++i)
//        {
//            modelPool[robot] = i;
//
//            //    required:
//            //            ModelBound::List
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Camera' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Localization' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Locomotion' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Mapping' (1, 1000000)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#PowerSource' (1, 1000000)
//            //
//            //    available:
//            //            ModelBound::List
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Camera' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#EmiPassive' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#LaserScanner' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Localization' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Locomotion' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#Mapping' (0, 3)
//            //            ModelBound: 'http://www.rock-robotics.org/2014/01/om-schema#PowerSource' (0, 3)
//
//            double redundancyLevel = i;
//            double parallel = 1 - pow((1-0.5),redundancyLevel);
//            double expected = pow(parallel,5);
//
//            double pSurvivability = redundancy.computeSharedUse(services, modelPool);
//            BOOST_TEST_MESSAGE("Survivability for #" << i << " " << robot << ": \n" << pSurvivability);
//            //BOOST_REQUIRE_MESSAGE(pSurvivability == expected, "Survivability for #" << i << " " << crex << ": \n" << pSurvivability << " (expected: " << expected << ")");
//        }
//    }
}

BOOST_AUTO_TEST_SUITE_END()


// The MIT License (MIT)

// Copyright (c) 2019 Caili Li

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

/* code */

#include <cmath>

#include <Eigen/LU>

#include "Transform.hpp"

namespace roboflow
{
namespace navigation_function
{
namespace math
{

Transform::Transform(Eigen::Vector2d s, double r, Eigen::Vector2d t)
{
    scale_ = s;
    rotate_ = r;
    translate_ = t;

    construct_affine();
}

Transform::Transform(double sx, double sy, double r, double x, double y)
    : Transform({sx, sy}, r, {x, y}) {}

Transform::Transform(const Transform &other)
{
    scale_ = other.scale_;
    rotate_ = other.rotate_;
    translate_ = other.translate_;
    affine_ = other.affine_;
}

void Transform::operator=(const Transform &other)
{
    scale_ = other.scale_;
    rotate_ = other.rotate_;
    translate_ = other.translate_;
    affine_ = other.affine_;
}

Transform::Transform(const Transform &&other)
{
    scale_ = std::move(other.scale_);
    rotate_ = std::move(other.rotate_);
    translate_ = std::move(other.translate_);
    affine_ = std::move(other.affine_);
}

void Transform::operator=(const Transform &&other)
{
    scale_ = std::move(other.scale_);
    rotate_ = std::move(other.rotate_);
    translate_ = std::move(other.translate_);
    affine_ = std::move(other.affine_);
}

void Transform::construct_affine()
{
    Eigen::Matrix2d S = (Eigen::Matrix2d() << scale_(0), 0, 0, scale_(1)).finished();
    Eigen::Matrix2d R = (Eigen::Matrix2d() << std::cos(rotate_), -std::sin(rotate_),
                         std::sin(rotate_), std::cos(rotate_))
                            .finished();
    affine_ = R * S;
}

Eigen::Vector2d Transform::operator()(Eigen::Vector2d p) const
{
    return transformFrom(p);
}

Eigen::Vector2d Transform::transformFrom(Eigen::Vector2d p) const
{
    return affine_.inverse() * (p - translate_);
}

Eigen::Vector2d Transform::transformTo(Eigen::Vector2d p) const
{
    return affine_ * p + translate_;
}

} // namespace math
} // namespace navigation_function
} // namespace roboflow