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

#include <mfmg/common/instantiation.hpp>
#include <mfmg/common/operator.hpp>
#include <mfmg/dealii/dealii_hierarchy_helpers.hpp>
#include <mfmg/dealii/dealii_smoother.hpp>
#include <mfmg/dealii/dealii_solver.hpp>
#include <mfmg/dealii/dealii_trilinos_matrix_operator.hpp>
#include <mfmg/dealii/dealii_utils.hpp>

#include <deal.II/base/work_stream.h>
#include <deal.II/dofs/dof_accessor.h>

#include <EpetraExt_MatrixMatrix.h>
#include <Epetra_Map.h>

#include <boost/container_hash/hash.hpp>
#include <boost/smart_ptr/make_unique.hpp>

#include <unordered_map>

namespace mfmg
{
template <int dim, typename VectorType>
std::shared_ptr<Operator<VectorType>>
DealIIHierarchyHelpers<dim, VectorType>::get_global_operator(
    std::shared_ptr<MeshEvaluator> mesh_evaluator)
{
  // Downcast to DealIIMeshEvaluator
  auto dealii_mesh_evaluator =
      std::dynamic_pointer_cast<DealIIMeshEvaluator<dim>>(mesh_evaluator);

  auto system_matrix =
      std::make_shared<dealii::TrilinosWrappers::SparseMatrix>();

  // Call user function to fill in the system matrix
  dealii_mesh_evaluator->evaluate_global(
      dealii_mesh_evaluator->get_dof_handler(),
      dealii_mesh_evaluator->get_constraints(), *system_matrix);

  std::shared_ptr<Operator<VectorType>> global_operator =
      std::make_shared<DealIITrilinosMatrixOperator<VectorType>>(system_matrix);

  return global_operator;
}

template <int dim, typename VectorType>
std::shared_ptr<Operator<VectorType>>
DealIIHierarchyHelpers<dim, VectorType>::build_restrictor(
    MPI_Comm comm, std::shared_ptr<MeshEvaluator> mesh_evaluator,
    std::shared_ptr<boost::property_tree::ptree const> params)
{
  // Downcast to DealIIMeshEvaluator
  auto dealii_mesh_evaluator =
      std::dynamic_pointer_cast<DealIIMeshEvaluator<dim>>(mesh_evaluator);

  auto eigensolver_params = params->get_child("eigensolver");
  int n_eigenvectors = eigensolver_params.get("number of eigenvectors", 1);
  double tolerance = eigensolver_params.get("tolerance", 1e-14);

  auto restrictor_matrix =
      std::make_shared<dealii::TrilinosWrappers::SparseMatrix>();

  auto locally_relevant_global_diag = dealii_mesh_evaluator->get_diagonal();

  bool fast_ap = params->get("fast_ap", false);
  auto agglomerate_params = params->get_child("agglomeration");
  if (fast_ap)
  {
    AMGe_host<dim, DealIIMeshEvaluator<dim>, VectorType> amge(
        comm, dealii_mesh_evaluator->get_dof_handler(), eigensolver_params);
    std::vector<double> eigenvalues;
    // We use unique_ptr because we want to use a special constructor when we
    // fill the matrix, i.e., we don't want to provide a SparsityPattern. All
    // the reinit functions require a SparsityPattern.
    std::unique_ptr<dealii::TrilinosWrappers::SparseMatrix> eigenvector_matrix;
    std::unique_ptr<dealii::TrilinosWrappers::SparseMatrix>
        delta_eigenvector_matrix;
    amge.setup_restrictor(agglomerate_params, n_eigenvectors, tolerance,
                          *dealii_mesh_evaluator, locally_relevant_global_diag,
                          restrictor_matrix, eigenvector_matrix,
                          delta_eigenvector_matrix, eigenvalues);

    dealii::TrilinosWrappers::SparseMatrix delta_correction_matrix(
        eigenvector_matrix->locally_owned_range_indices(),
        eigenvector_matrix->locally_owned_domain_indices(),
        eigenvector_matrix->get_mpi_communicator());

    // Need to apply delta_eigenvector_matrix
    std::vector<std::vector<unsigned int>> interior_agglomerates;
    std::vector<std::vector<unsigned int>> halo_agglomerates;
    std::tie(interior_agglomerates, halo_agglomerates) =
        amge.build_boundary_agglomerates();
    std::unordered_map<std::pair<unsigned int, unsigned int>, double,
                       boost::hash<std::pair<unsigned int, unsigned int>>>
        delta_correction_acc;
    bool is_halo_agglomerate = false;

    // In case there are no patches we own, we still need to construct the
    // restriction operators. We set the number of eigenvalues to zero instead
    // of returning early and duplicating the setup.
    unsigned int const n_local_eigenvectors =
        interior_agglomerates.empty()
            ? 0
            : eigenvector_matrix->locally_owned_range_indices().n_elements() /
                  interior_agglomerates.size();

    for (auto const &agglomerates_vector :
         {interior_agglomerates, halo_agglomerates})
    {
      struct ScratchData
      {
      } scratch_data;
      struct CopyData
      {
        std::vector<dealii::types::global_dof_index> rows;
        std::vector<dealii::types::global_dof_index> cols;
        std::vector<std::vector<dealii::TrilinosScalar>> values_per_row;
      } copy_data;

      auto worker =
          [&](const std::vector<std::vector<unsigned int>>::const_iterator
                  &agglomerate_it,
              ScratchData &, CopyData &local_copy_data) {
            dealii::Triangulation<dim> agglomerate_triangulation;
            std::map<typename dealii::Triangulation<dim>::active_cell_iterator,
                     typename dealii::DoFHandler<dim>::active_cell_iterator>
                patch_to_global_map;
            amge.build_agglomerate_triangulation(*agglomerate_it,
                                                 agglomerate_triangulation,
                                                 patch_to_global_map);
            if (patch_to_global_map.empty())
            {
              return;
            }

            // Now that we have the triangulation, we can do the evaluation on
            // the agglomerate
            dealii::DoFHandler<dim> agglomerate_dof_handler(
                agglomerate_triangulation);
            dealii::AffineConstraints<double> agglomerate_constraints;
            dealii::SparsityPattern agglomerate_sparsity_pattern;
            dealii::SparseMatrix<ScalarType> agglomerate_system_matrix;
            // Call user function to build the system matrix
            dealii_mesh_evaluator->evaluate_agglomerate(
                agglomerate_dof_handler, agglomerate_constraints,
                agglomerate_sparsity_pattern, agglomerate_system_matrix);

            // Put the result in the matrix
            // Compute the map between the local and the global dof indices.
            local_copy_data.rows.resize(n_local_eigenvectors);

            local_copy_data.cols = amge.compute_dof_index_map(
                patch_to_global_map, agglomerate_dof_handler);
            auto const &dof_indices_map = local_copy_data.cols;
            unsigned int const n_elem = dof_indices_map.size();

            // We need a clean reset for the values we are going to store.
            // Otherwise, we would accumulate values across patches
            // corresponding to different degrees of freedom.
            local_copy_data.values_per_row.resize(n_local_eigenvectors);
            std::fill(local_copy_data.values_per_row.begin(),
                      local_copy_data.values_per_row.end(),
                      std::vector<dealii::TrilinosScalar>(n_elem));

            unsigned int const i = agglomerate_it - agglomerates_vector.begin();

            for (unsigned int j = 0; j < n_local_eigenvectors; ++j)
            {
              unsigned int const local_row = i * n_local_eigenvectors + j;
              unsigned int const global_row =
                  eigenvector_matrix->locally_owned_range_indices()
                      .nth_index_in_set(local_row);
              // Get the vector used for the matrix-vector multiplication
              dealii::Vector<ScalarType> delta_eig(n_elem);
              if (is_halo_agglomerate)
              {
                for (unsigned int k = 0; k < n_elem; ++k)
                {
                  delta_eig[k] =
                      delta_eigenvector_matrix->el(global_row,
                                                   dof_indices_map[k]) +
                      eigenvector_matrix->el(global_row, dof_indices_map[k]);
                }
              }
              else
              {
                for (unsigned int k = 0; k < n_elem; ++k)
                {
                  delta_eig[k] = delta_eigenvector_matrix->el(
                      global_row, dof_indices_map[k]);
                }
              }

              // Perform the matrix-vector multiplication
              dealii::Vector<ScalarType> correction(n_elem);
              agglomerate_system_matrix.vmult(correction, delta_eig);

              // Store the values the delta correction matrix is to be filled
              // with.
              local_copy_data.rows[j] = global_row;
              std::transform(correction.begin(), correction.end(),
                             local_copy_data.values_per_row[j].begin(),
                             local_copy_data.values_per_row[j].begin(),
                             std::plus<double>());
            }
          };

      auto copier = [&](const CopyData &local_copy_data) {
        for (unsigned int i = 0; i < local_copy_data.rows.size(); ++i)
        {
          // Since we didn't provide a sparsity pattern this "set" operation
          // actually acts as an "add" operation until "compress" is called.
          delta_correction_matrix.set(
              local_copy_data.rows[i], local_copy_data.cols.size(),
              local_copy_data.cols.data(),
              local_copy_data.values_per_row[i].data(), true);
        }
      };

      dealii::WorkStream::run(agglomerates_vector.begin(),
                              agglomerates_vector.end(), worker, copier,
                              scratch_data, copy_data);

      is_halo_agglomerate = true;
    }

    // Fill delta_correction_matrix
    delta_correction_matrix.compress(dealii::VectorOperation::insert);

    // Scale the eigenvectors by their corresponding eigenvalues.
    // It turned out that accessing the individual matrix entries through the
    // deal.II TrilinosWrapper::SparseMatrix::begin() interface is much slower
    // than constructing a representation of the eigenvalues as
    // Epetra_MultiVector and calling a Epetra_CrsMatrix member function.
    auto const range_start = eigenvector_matrix->local_range().first;
    auto const range_end = eigenvector_matrix->local_range().second;

    dealii::IndexSet locally_owned_elements(eigenvector_matrix->m());
    locally_owned_elements.add_range(range_start, range_end);
    dealii::TrilinosWrappers::MPI::Vector vector_eigenvalues(
        locally_owned_elements, comm);
    std::vector<dealii::types::global_dof_index> local_indices(
        eigenvalues.size());
    std::iota(local_indices.begin(), local_indices.end(), range_start);
    vector_eigenvalues.set(local_indices, eigenvalues);
    vector_eigenvalues.compress(dealii::VectorOperation::insert);

    Epetra_MultiVector &dummy_multi_vector =
        vector_eigenvalues.trilinos_vector();
    auto &dummy_matrix =
        const_cast<Epetra_CrsMatrix &>(eigenvector_matrix->trilinos_matrix());
    dummy_matrix.LeftScale(*dummy_multi_vector(0));

    eigenvector_matrix->compress(dealii::VectorOperation::insert);

    // Add the eigenvector matrix and the delta correction matrix to create ap
    Epetra_CrsMatrix *ap = nullptr;
    // We want to use functions that have been deprecated in deal.II but they
    // won't be removed in the foreseeable future
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
    Epetra_Map range_map = eigenvector_matrix->domain_partitioner();
    Epetra_Map domain_map = eigenvector_matrix->range_partitioner();
#pragma GCC diagnostic pop
    bool const transpose = true;
    int error_code = EpetraExt::MatrixMatrix::Add(
        eigenvector_matrix->trilinos_matrix(), transpose, 1.,
        delta_correction_matrix.trilinos_matrix(), transpose, 1., ap);
    ap->FillComplete(domain_map, range_map);
    ASSERT(error_code == 0, "Problem when adding matrices");

    // Copy the Epetra_CrsMatrix to a dealii::TrilinosWrappers::SparseMatrix
    auto dealii_ap = std::make_shared<dealii::TrilinosWrappers::SparseMatrix>();
    dealii_ap->reinit(*ap);
    delete ap;

    _ap_operator.reset(new DealIITrilinosMatrixOperator<VectorType>(dealii_ap));
  }
  else
  {
    AMGe_host<dim, DealIIMeshEvaluator<dim>, VectorType> amge(
        comm, dealii_mesh_evaluator->get_dof_handler(), eigensolver_params);

    amge.setup_restrictor(agglomerate_params, n_eigenvectors, tolerance,
                          *dealii_mesh_evaluator, locally_relevant_global_diag,
                          *restrictor_matrix);
  }

  std::shared_ptr<Operator<VectorType>> op(
      new DealIITrilinosMatrixOperator<VectorType>(restrictor_matrix));

  return op;
}

template <int dim, typename VectorType>
std::shared_ptr<Smoother<VectorType>>
DealIIHierarchyHelpers<dim, VectorType>::build_smoother(
    std::shared_ptr<Operator<VectorType> const> op,
    std::shared_ptr<boost::property_tree::ptree const> params)
{
  return std::make_shared<DealIISmoother<VectorType>>(op, params);
}

template <int dim, typename VectorType>
std::shared_ptr<Solver<VectorType>>
DealIIHierarchyHelpers<dim, VectorType>::build_coarse_solver(
    std::shared_ptr<Operator<VectorType> const> op,
    std::shared_ptr<boost::property_tree::ptree const> params)
{
  return std::make_shared<DealIISolver<VectorType>>(op, params);
}

template <int dim, typename VectorType>
std::shared_ptr<Operator<VectorType>>
DealIIHierarchyHelpers<dim, VectorType>::fast_multiply_transpose()
{
  return _ap_operator;
}

} // namespace mfmg

// Explicit Instantiation
INSTANTIATE_DIM_VECTORTYPE(TUPLE(DealIIHierarchyHelpers))


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <fstream>
#include <iostream>
#include <random>
#include <vector>
#include "catch.hpp"
#include "netket.hpp"

#include "optimizer_input_tests.hpp"
const Complex im(0, 1);
// const Complex I(0.0, 1.0);
const double pi = 3.14159265358979323846;

// Check that optimizer steps correctly. This means parameters initialized
// correctly, real and imag parts not getting mixed, etc.
TEST_CASE("optimizers step twice correctly", "[optimizer]") {
  auto input_tests = GetOptimizerInputs();
  std::cout << "# Correct output tests size = " << 7 << std::endl;

  for (std::size_t it = 0; it < 7; it++) {
    std::string name = input_tests[it].dump();

    if (input_tests[it]["Optimizer"]["Name"] == "Sgd") {
      SECTION("Stepper test (" + std::to_string(it) + ") on " + name) {
        netket::Optimizer optimizer(input_tests[it]);

        Eigen::VectorXcd grad(2);
        grad << 1.0 + 4.0 * im, 2.0 + 5.0 * im;
        Eigen::VectorXcd params(2);
        params << 5.0, 5.0;
        Eigen::VectorXcd sol(2);
        sol << 4.9 - 0.4 * im, 4.8 - 0.5 * im;
        Eigen::VectorXcd sol2(2);
        sol2 << 4.8 - 0.8 * im, 4.6 - 1.0 * im;

        optimizer.Init(params);
        optimizer.Update(grad, params);
        REQUIRE((params - sol).norm() < 1.0e-7);  // update once correctly
        optimizer.Update(grad, params);
        REQUIRE((params - sol2).norm() < 1.0e-7);  // update twice correctly
      }
    }

    else if (input_tests[it]["Optimizer"]["Name"] == "AdaDelta") {
      SECTION("Stepper test (" + std::to_string(it) + ") on " + name) {
        netket::Optimizer optimizer(input_tests[it]);

        double eps = input_tests[it]["Optimizer"]["Epscut"];
        double rho = input_tests[it]["Optimizer"]["Rho"];

        Eigen::VectorXcd grad(2);
        grad << 1.0 + 4.0 * im, 2.0 + 5.0 * im;
        Eigen::VectorXcd params(2);
        params << 5.0, 5.0;
        Eigen::VectorXcd diff(2);
        diff << -grad(0).real() *
                        std::sqrt(eps / (grad(0).real() * (1. - rho) + eps)) -
                    grad(0).imag() *
                        std::sqrt(
                            eps /
                            (std::pow(grad(0).imag(), 2) * (1. - rho) + eps)) *
                        im,
            -grad(1).real() *
                    std::sqrt(eps / (std::pow(grad(1).real(), 2) * (1. - rho) +
                                     eps)) -
                grad(1).imag() *
                    std::sqrt(eps / (std::pow(grad(1).imag(), 2) * (1. - rho) +
                                     eps)) *
                    im;
        Eigen::VectorXcd sol(2);
        sol << params + diff;
        Eigen::VectorXcd diff2(2);
        diff2 << -grad(0).real() *
                         std::sqrt(
                             ((1. - rho) * std::pow(diff(0).real(), 2) + eps) /
                             ((1. + rho) * (1. - rho) *
                                  std::pow(grad(0).real(), 2) +
                              eps)) -
                     grad(0).imag() *
                         std::sqrt(
                             ((1. - rho) * std::pow(diff(0).imag(), 2) + eps) /
                             ((1. + rho) * (1. - rho) *
                                  std::pow(grad(0).imag(), 2) +
                              eps)) *
                         im,
            -grad(1).real() *
                    std::sqrt(
                        ((1. - rho) * std::pow(diff(1).real(), 2) + eps) /
                        ((1. + rho) * (1. - rho) * std::pow(grad(1).real(), 2) +
                         eps)) -
                grad(1).imag() *
                    std::sqrt(
                        ((1. - rho) * std::pow(diff(1).imag(), 2) + eps) /
                        ((1. + rho) * (1. - rho) * std::pow(grad(1).imag(), 2) +
                         eps)) *
                    im;
        Eigen::VectorXcd sol2(2);
        sol2 << params + diff + diff2;

        optimizer.Init(params);
        optimizer.Update(grad, params);
        REQUIRE((params - sol).norm() < 1.0e-7);  // update once correctly
        optimizer.Update(grad, params);
        REQUIRE((params - sol2).norm() < 1.0e-7);  // update twice correctly*/
      }
    }

    else if (input_tests[it]["Optimizer"]["Name"] == "AdaGrad") {
      SECTION("Stepper test (" + std::to_string(it) + ") on " + name) {
        netket::Optimizer optimizer(input_tests[it]);

        double eta = input_tests[it]["Optimizer"]["LearningRate"];
        double eps = input_tests[it]["Optimizer"]["Epscut"];

        Eigen::VectorXcd grad(2);
        grad << 1.0 + 4.0 * im, 2.0 + 5.0 * im;
        Eigen::VectorXcd params(2);
        params << 5.0, 5.0;
        Eigen::VectorXcd diff(2);
        diff << -eta * grad(0).real() /
                        std::sqrt(std::pow(grad(0).real(), 2) + eps) -
                    eta * grad(0).imag() /
                        std::sqrt(std::pow(grad(0).imag(), 2) + eps) * im,
            -eta * grad(1).real() /
                    std::sqrt(std::pow(grad(1).real(), 2) + eps) -
                eta * grad(1).imag() /
                    std::sqrt(std::pow(grad(1).imag(), 2) + eps) * im;
        Eigen::VectorXcd sol(2);
        sol << params + diff;
        Eigen::VectorXcd diff2(2);
        diff2 << -eta * grad(0).real() /
                         std::sqrt(2. * std::pow(grad(0).real(), 2) + eps) -
                     eta * grad(0).imag() /
                         std::sqrt(2. * std::pow(grad(0).imag(), 2) + eps) * im,
            -eta * grad(1).real() /
                    std::sqrt(2. * std::pow(grad(1).real(), 2) + eps) -
                eta * grad(1).imag() /
                    std::sqrt(2. * std::pow(grad(1).imag(), 2) + eps) * im;
        Eigen::VectorXcd sol2(2);
        sol2 << params + diff + diff2;

        optimizer.Init(params);
        optimizer.Update(grad, params);
        REQUIRE((params - sol).norm() < 1.0e-7);  // update once correctly
        optimizer.Update(grad, params);
        REQUIRE((params - sol2).norm() < 1.0e-7);  // update twice correctly
      }
    }

    else if (input_tests[it]["Optimizer"]["Name"] == "AdaMax") {
      SECTION("Stepper test (" + std::to_string(it) + ") on " + name) {
        netket::Optimizer optimizer(input_tests[it]);

        double alpha = input_tests[it]["Optimizer"]["Alpha"];

        Eigen::VectorXcd grad(2);
        grad << 1.0 + 4.0 * im, 2.0 + 5.0 * im;
        Eigen::VectorXcd params(2);
        params << 5.0, 5.0;
        Eigen::VectorXcd diff(2);
        diff << -alpha - alpha * im, -alpha - alpha * im;
        Eigen::VectorXcd sol(2);
        sol << params + diff;
        Eigen::VectorXcd sol2(2);
        sol2 << params + 2 * diff;

        optimizer.Init(params);
        optimizer.Update(grad, params);
        REQUIRE((params - sol).norm() < 1.0e-7);  // update once correctly
        optimizer.Update(grad, params);
        REQUIRE((params - sol2).norm() < 1.0e-7);  // update twice correctly
      }
    }

    else if (input_tests[it]["Optimizer"]["Name"] == "AMSGrad") {
      SECTION("Stepper test (" + std::to_string(it) + ") on " + name) {
        netket::Optimizer optimizer(input_tests[it]);

        double eta = input_tests[it]["Optimizer"]["LearningRate"];
        double beta1 = input_tests[it]["Optimizer"]["Beta1"];
        double beta2 = input_tests[it]["Optimizer"]["Beta2"];
        double eps = input_tests[it]["Optimizer"]["Epscut"];

        Eigen::VectorXcd grad(2);
        grad << 1.0 + 4.0 * im, 2.0 + 5.0 * im;
        Eigen::VectorXcd params(2);
        params << 5.0, 5.0;
        Eigen::VectorXcd diff(2);
        diff << -eta * (1 - beta1) * grad(0).real() /
                        std::sqrt((1 - beta2) * std::pow(grad(0).real(), 2) +
                                  eps) -
                    eta * (1 - beta1) * grad(0).imag() /
                        std::sqrt((1 - beta2) * std::pow(grad(0).imag(), 2) +
                                  eps) *
                        im,
            -eta * (1 - beta1) * grad(1).real() /
                    std::sqrt((1 - beta2) * std::pow(grad(1).real(), 2) + eps) -
                eta * (1 - beta1) * grad(1).imag() /
                    std::sqrt((1 - beta2) * std::pow(grad(1).imag(), 2) + eps) *
                    im;
        Eigen::VectorXcd sol(2);
        sol << params + diff;
        Eigen::VectorXcd diff2(2);
        diff2 << -eta * (1 + beta1) * (1 - beta1) * grad(0).real() /
                         std::sqrt((1 - beta2) * (1 + beta2) *
                                       std::pow(grad(0).real(), 2) +
                                   eps) -
                     eta * (1 + beta1) * (1 - beta1) * grad(0).imag() /
                         std::sqrt((1 - beta2) * (1 + beta2) *
                                       std::pow(grad(0).imag(), 2) +
                                   eps) *
                         im,
            -eta * (1 + beta1) * (1 - beta1) * grad(1).real() /
                    std::sqrt((1 - beta2) * (1 + beta2) *
                                  std::pow(grad(1).real(), 2) +
                              eps) -
                eta * (1 + beta1) * (1 - beta1) * grad(1).imag() /
                    std::sqrt((1 - beta2) * (1 + beta2) *
                                  std::pow(grad(1).imag(), 2) +
                              eps) *
                    im;
        Eigen::VectorXcd sol2(2);
        sol2 << params + diff + diff2;

        optimizer.Init(params);
        optimizer.Update(grad, params);
        REQUIRE((params - sol).norm() < 1.0e-5);  // update once correctly
        optimizer.Update(grad, params);
        REQUIRE((params - sol2).norm() < 1.0e-5);  // update twice correctly
      }
    }

    else if (input_tests[it]["Optimizer"]["Name"] == "RMSProp") {
      SECTION("Stepper test (" + std::to_string(it) + ") on " + name) {
        netket::Optimizer optimizer(input_tests[it]);

        double eta = input_tests[it]["Optimizer"]["LearningRate"];
        double beta = input_tests[it]["Optimizer"]["Beta"];
        double eps = input_tests[it]["Optimizer"]["Epscut"];

        Eigen::VectorXcd grad(2);
        grad << 1.0 + 4.0 * im, 2.0 + 5.0 * im;
        Eigen::VectorXcd params(2);
        params << 5.0, 5.0;
        Eigen::VectorXcd diff(2);
        diff << -eta * grad(0).real() /
                        std::sqrt((1 - beta) * std::pow(grad(0).real(), 2) +
                                  eps) -
                    eta * grad(0).imag() /
                        std::sqrt((1 - beta) * std::pow(grad(0).imag(), 2) +
                                  eps) *
                        im,
            -eta * grad(1).real() /
                    std::sqrt((1 - beta) * std::pow(grad(1).real(), 2) + eps) -
                eta * grad(1).imag() /
                    std::sqrt((1 - beta) * std::pow(grad(1).imag(), 2) + eps) *
                    im;
        Eigen::VectorXcd sol(2);
        sol << params + diff;
        Eigen::VectorXcd diff2(2);
        diff2 << -eta * grad(0).real() /
                         std::sqrt((1 + beta) * (1 - beta) *
                                       std::pow(grad(0).real(), 2) +
                                   eps) -
                     eta * grad(0).imag() /
                         std::sqrt((1 + beta) * (1 - beta) *
                                       std::pow(grad(0).imag(), 2) +
                                   eps) *
                         im,
            -eta * grad(1).real() /
                    std::sqrt((1 + beta) * (1 - beta) *
                                  std::pow(grad(1).real(), 2) +
                              eps) -
                eta * grad(1).imag() /
                    std::sqrt((1 + beta) * (1 - beta) *
                                  std::pow(grad(1).imag(), 2) +
                              eps) *
                    im;
        Eigen::VectorXcd sol2(2);
        sol2 << params + diff + diff2;

        optimizer.Init(params);
        optimizer.Update(grad, params);
        REQUIRE((params - sol).norm() < 1.0e-7);  // update once correctly
        optimizer.Update(grad, params);
        REQUIRE((params - sol2).norm() < 1.0e-7);  // update twice correctly
      }
    } else if (input_tests[it]["Optimizer"]["Name"] == "Momentum") {
      SECTION("Stepper test (" + std::to_string(it) + ") on " + name) {
        netket::Optimizer optimizer(input_tests[it]);

        double eta = input_tests[it]["Optimizer"]["LearningRate"];
        double beta = input_tests[it]["Optimizer"]["Beta"];

        Eigen::VectorXcd grad(2);
        grad << 1.0 + 4.0 * im, 2.0 + 5.0 * im;
        Eigen::VectorXcd params(2);
        params << 5.0, 5.0;
        Eigen::VectorXcd sol(2);
        sol << params - eta * (1. - beta) * grad;
        Eigen::VectorXcd sol2(2);
        sol2 << params - eta * (1. - beta) * (2. + beta) * grad;

        optimizer.Init(params);
        optimizer.Update(grad, params);
        REQUIRE((params - sol).norm() < 1.0e-7);  // update once correctly
        optimizer.Update(grad, params);
        REQUIRE((params - sol2).norm() < 1.0e-7);  // update twice correctly
      }
    }
  }
}

TEST_CASE("optimizers correctly minimize Matyas function", "[optimizer]") {
  auto input_tests = GetOptimizerInputs();
  std::cout << "# Matyas input tests size = " << 7 << std::endl;

  for (std::size_t it = 0; it < 7; it++) {
    std::string name = input_tests[it].dump();

    SECTION("Optimizer test (" + std::to_string(it) + ") on " + name) {
      float err = 1.0e8;
      float tol = 0.1;
      int iter = 0;

      const double mean = 0.0;
      const double stddev = 0.1;
      std::default_random_engine generator;
      std::normal_distribution<double> dist(mean, stddev);

      Eigen::VectorXd sol(2);
      sol << 0, 0;
      Eigen::VectorXd grad(2);
      Eigen::VectorXd params(2);
      params << 5.0, 5.0;

      netket::Optimizer optimizer(input_tests[it]);
      optimizer.Init(params);

      while (err > tol and iter < 5e4) {
        grad(0) = 0.52 * params(0) - 0.48 * params(1) + dist(generator);
        grad(1) = 0.52 * params(1) - 0.48 * params(0) + dist(generator);

        optimizer.Update(grad, params);
        err = (params - sol).norm();
        iter += 1;
      }

      REQUIRE(err <= tol);
    }
  }
}

TEST_CASE("optimizers correctly minimize Beale function", "[optimizer]") {
  auto input_tests = GetOptimizerInputs();
  std::cout << "# Beale input tests size = " << 7 << std::endl;

  for (std::size_t it = 7; it < 14; it++) {
    std::string name = input_tests[it].dump();

    SECTION("Optimizer test (" + std::to_string(it) + ") on " + name) {
      float err = 1.0e8;
      float tol = 0.1;
      int iter = 0;

      const double mean = 0.0;
      const double stddev = 0.1;
      std::default_random_engine generator;
      std::normal_distribution<double> dist(mean, stddev);

      Eigen::VectorXd sol(2);
      sol << 3, 0.5;
      Eigen::VectorXd grad(2);
      Eigen::VectorXd params(2);
      params << 4.0, 3.0;

      netket::Optimizer optimizer(input_tests[it]);
      optimizer.Init(params);

      while (err > tol and iter < 1e5) {
        grad(0) = 2. * (1.5 + params(0) * (params(1) - 1.)) * (params(1) - 1.) +
                  2. * (2.25 + params(0) * (std::pow(params(1), 2) - 1.)) *
                      (std::pow(params(1), 2) - 1.) +
                  2. * (2.625 + params(0) * (std::pow(params(1), 3) - 1.)) *
                      (std::pow(params(1), 3) - 1.) +
                  dist(generator);
        grad(1) = 2. * (1.5 + params(0) * (params(1) - 1.)) * params(0) +
                  2. * (2.25 + params(0) * (std::pow(params(1), 2) - 1.)) * 2. *
                      params(0) * params(0) +
                  2. * (2.625 + params(0) * (std::pow(params(1), 3) - 1.)) *
                      3. * params(0) * std::pow(params(1), 2) +
                  dist(generator);

        optimizer.Update(grad, params);
        err = (params - sol).norm();
        iter += 1;
      }
      REQUIRE(err <= tol);
    }
  }
}

TEST_CASE("optimizers correctly minimize Rosenbrock function", "[optimizer]") {
  auto input_tests = GetOptimizerInputs();
  std::cout << "# Rosenbrock input tests size = " << 7 << std::endl;

  for (std::size_t it = 14; it < 21; it++) {
    std::string name = input_tests[it].dump();

    SECTION("Optimizer test (" + std::to_string(it) + ") on " + name) {
      float err = 1e8;
      float tol = 0.1;
      int iter = 0;

      const double mean = 0.0;
      const double stddev = 1;
      std::default_random_engine generator;
      std::normal_distribution<double> dist(mean, stddev);

      Eigen::VectorXd sol(4);
      sol << 1., 1., 1., 1.;
      Eigen::VectorXd grad(4);
      Eigen::VectorXd params(4);
      params << 4.0, 3.0, 10.0, -5.0;

      netket::Optimizer optimizer(input_tests[it]);
      optimizer.Init(params);

      while (err > tol and iter < 5e5) {
        grad(0) = -400. * params(0) * (params(1) - std::pow(params(0), 2)) -
                  2. * (1 - params(0)) + dist(generator);
        grad(1) = 200. * (params(1) - std::pow(params(0), 2)) -
                  400. * params(1) * (params(2) - std::pow(params(1), 2)) -
                  2. * (1 - params(1)) + dist(generator);
        grad(2) = 200. * (params(2) - std::pow(params(1), 2)) -
                  400. * params(2) * (params(3) - std::pow(params(2), 2)) -
                  2. * (1 - params(2)) + dist(generator);
        grad(3) = 200. * (params(3) - std::pow(params(2), 2)) + dist(generator);

        optimizer.Update(grad, params);
        err = (params - sol).norm();
        iter += 1;
      }
      REQUIRE(err <= tol);
    }
  }
}

TEST_CASE("optimizers correctly minimize Ackley function", "[optimizer]") {
  auto input_tests = GetOptimizerInputs();
  std::cout << "# Ackley input tests size = " << 7 << std::endl;

  for (std::size_t it = 21; it < 28; it++) {
    std::string name = input_tests[it].dump();
    std::string optName = input_tests[it]["Optimizer"]["Name"];
    SECTION("Optimizer test (" + std::to_string(it) + ") on " + name) {
      float err = 1.0e8;
      float tol = 0.1;
      int iter = 0;
      double stddev;

      const double mean = 0.0;
      if ((optName == "RMSProp") || (optName == "AdaDelta")) {
        stddev = 90;
      } else if ((optName == "Sgd") || (optName == "AMSGrad") ||
                 (optName == "Momentum")) {
        stddev = 10;
      } else {
        stddev = 1;
      }
      std::default_random_engine generator;
      std::normal_distribution<double> dist(mean, stddev);

      Eigen::VectorXd sol(2);
      sol << 0, 0;
      Eigen::VectorXd grad(2);
      Eigen::VectorXd params(2);
      params << 2.5, 2.5;

      netket::Optimizer optimizer(input_tests[it]);
      optimizer.Init(params);

      while (err > tol and iter < 5e5) {
        grad(0) =
            2. * std::sqrt(2) * params(0) *
                std::exp(-0.2 * std::sqrt(0.5 * (std::pow(params(0), 2) +
                                                 std::pow(params(1), 2)))) /
                std::sqrt((std::pow(params(0), 2) + std::pow(params(1), 2))) +
            pi * std::sin(2 * pi * params(0)) *
                std::exp(0.5 * (std::cos(2 * pi * params(0)) +
                                std::cos(2 * pi * params(1)))) +
            dist(generator);
        grad(1) =
            2. * std::sqrt(2) * params(1) *
                std::exp(-0.2 * std::sqrt(0.5 * (std::pow(params(0), 2) +
                                                 std::pow(params(1), 2)))) /
                std::sqrt((std::pow(params(0), 2) + std::pow(params(1), 2))) +
            pi * std::sin(2 * pi * params(1)) *
                std::exp(0.5 * (std::cos(2 * pi * params(0)) +
                                std::cos(2 * pi * params(1)))) +
            dist(generator);

        optimizer.Update(grad, params);
        err = (params - sol).norm();
        iter += 1;
      }
      REQUIRE(err <= tol);
    }
  }
}

TEST_CASE("optimizers correctly minimize complex Ackley function",
          "[optimizer]") {
  auto input_tests = GetOptimizerInputs();
  std::cout << "# complex Ackley input tests size = " << 7 << std::endl;

  for (std::size_t it = 28; it < 35; it++) {
    std::string name = input_tests[it].dump();
    std::string optName = input_tests[it]["Optimizer"]["Name"];
    SECTION("Optimizer test (" + std::to_string(it) + ") on " + name) {
      float err = 1.0e8;
      float tol = 0.1;
      int iter = 0;
      double stddev;

      const double mean = 0.0;
      if ((optName == "RMSProp") || (optName == "AdaDelta")) {
        stddev = 90;
      } else if ((optName == "Sgd") || (optName == "AMSGrad") ||
                 (optName == "Momentum")) {
        stddev = 10;
      } else {
        stddev = 1;
      }
      std::default_random_engine generator;
      std::normal_distribution<double> dist(mean, stddev);

      Eigen::VectorXcd sol(2);
      sol << 0.0 + 0.0 * im, 0.0 + 0.0 * im;
      Eigen::VectorXcd grad(2);
      Eigen::VectorXcd params(2);
      params << 0.0 + 2.5 * im, 0.0 + 2.5 * im;

      netket::Optimizer optimizer(input_tests[it]);
      optimizer.Init(params);

      while (err > tol and iter < 5e5) {
        grad(0) =
            0.0 +
            im * (2. * std::sqrt(2) * params(0).imag() *
                      std::exp(
                          -0.2 *
                          std::sqrt(0.5 * (std::pow(params(0).imag(), 2) +
                                           std::pow(params(1).imag(), 2)))) /
                      std::sqrt((std::pow(params(0).imag(), 2) +
                                 std::pow(params(1).imag(), 2))) +
                  pi * std::sin(2 * pi * params(0).imag()) *
                      std::exp(0.5 * (std::cos(2 * pi * params(0).imag()) +
                                      std::cos(2 * pi * params(1).imag()))) +
                  dist(generator));
        grad(1) =
            0.0 +
            im * (2. * std::sqrt(2) * params(1).imag() *
                      std::exp(
                          -0.2 *
                          std::sqrt(0.5 * (std::pow(params(0).imag(), 2) +
                                           std::pow(params(1).imag(), 2)))) /
                      std::sqrt((std::pow(params(0).imag(), 2) +
                                 std::pow(params(1).imag(), 2))) +
                  pi * std::sin(2 * pi * params(1).imag()) *
                      std::exp(0.5 * (std::cos(2 * pi * params(0).imag()) +
                                      std::cos(2 * pi * params(1).imag()))) +
                  dist(generator));

        optimizer.Update(grad, params);
        err = (params - sol).norm();
        iter += 1;
      }
      REQUIRE(err <= tol);
    }
  }
}


#include <algorithm>
#include <cmath>
#include <dlfcn.h>
#include <pthread.h>

#include <sys/stat.h>

#include <ros/ros.h>
#include "std_msgs/Float64.h"
#include "nav_msgs/Odometry.h"
#include "tf2/utils.h"
#include <boost/bind.hpp>

const double rad2deg = 180 / M_PI;

void CallbackOdom(const nav_msgs::Odometry::ConstPtr& msg, ros::Publisher & odom_ori_yaw_pub){
  double yaw, pitch, roll;
  tf2::getEulerYPR(msg->pose.pose.orientation, yaw, pitch, roll);
  std_msgs::Float64 odom_ori_yaw;
  odom_ori_yaw.data = -(yaw * rad2deg);
  odom_ori_yaw_pub.publish(odom_ori_yaw);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "odom_converter_node");
  ros::NodeHandle nh;

  std::string odom_topic_name;
  if (!ros::param::get("~odom_topic_name", odom_topic_name)){
    ROS_ERROR("odom_topic_name doesn't exist");
    std::exit(1);
  }

  ros::Publisher odom_ori_yaw_pub = nh.advertise<std_msgs::Float64>("/odom_ori_yaw", 100);
  ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(odom_topic_name,100,boost::bind(CallbackOdom, _1, odom_ori_yaw_pub));

  ros::spin();
  ROS_INFO("wait for shutdown");
  ros::waitForShutdown();
  return 0;
}


// NrField.cpp
// created by Kuangdai on 13-May-2016
// base class of nr integer field

#include "NrField.h"
#include "ConstNrField.h"
#include "EmpNrField.h"
#include "WisdomNrField.h"
#include "UserNrField.h"

#include "Parameters.h"
#include "XMPI.h"
#include <boost/algorithm/string.hpp>

void NrField::buildInparam(NrField *&nrf, const Parameters &par, int verbose) {
    if (nrf) {
        delete nrf;
    }

    std::string type = par.getValue<std::string>("NU_TYPE");
    bool useLucky = par.getValue<bool>("FFTW_LUCKY_NUMBER");

    if (boost::iequals(type, "constant")) {
        int nu = par.getValue<int>("NU_CONST");
        nrf = new ConstNrField(useLucky, nu);
    } else if (boost::iequals(type, "empirical")) {
        int nu_ref = par.getValue<int>("NU_EMP_REF");
        int nu_min = par.getValue<int>("NU_EMP_MIN");
        bool scaleS = par.getValue<bool>("NU_EMP_SCALE_AXIS");
        bool scaleT = par.getValue<bool>("NU_EMP_SCALE_THETA");
        bool scaleD = par.getValue<bool>("NU_EMP_SCALE_DEPTH");
        double powS = par.getValue<double>("NU_EMP_POW_AXIS");
        double factPI = par.getValue<double>("NU_EMP_FACTOR_PI");
        double startT = par.getValue<double>("NU_EMP_THETA_START") * degree;
        double powT = par.getValue<double>("NU_EMP_POW_THETA");
        double factD0 = par.getValue<double>("NU_EMP_FACTOR_SURF");
        double startD = par.getValue<double>("NU_EMP_DEPTH_START") * 1e3;
        double endD = par.getValue<double>("NU_EMP_DEPTH_END") * 1e3;
        nrf = new EmpNrField(useLucky, nu_ref, nu_min, scaleS, scaleT, scaleD,
            powS, factPI, startT, powT, factD0, startD, endD);
    } else if (boost::iequals(type, "wisdom")) {
        std::string fname = Parameters::sInputDirectory + "/" + par.getValue<std::string>("NU_WISDOM_REUSE_INPUT");
        double factor = par.getValue<double>("NU_WISDOM_REUSE_FACTOR");
        if (factor <= tinyDouble) {
            factor = 1.0;
        }
        nrf = new WisdomNrField(useLucky, fname, factor);
    } else if (boost::iequals(type, "user-defined")) {
        int nsize = par.getSize("NU_USER_PARAMETER_LIST");
        std::vector<double> params;
        for (int ipar = 0; ipar < nsize; ipar++) {
            double param = par.getValue<double>("NU_USER_PARAMETER_LIST", ipar);
            params.push_back(param);
        }
        nrf = new UserNrField(useLucky, params);
    } else {
        throw std::runtime_error("NrField::build || "
            "Invalid parameter, keyword = NU_TYPE.");
    }

    if (verbose) {
        XMPI::cout << nrf->verbose();
    }
}


#pragma once
#ifndef CANNON_RAY_RAY_H
#define CANNON_RAY_RAY_H

/*!
 * \file cannon/ray/ray.hpp
 * \brief File containing Ray class definition.
 */

#include <Eigen/Dense>

using namespace Eigen;

namespace cannon {
  namespace ray {

    /*!
     * \brief Class representing a ray for raytracing.
     */
    class Ray {
      public:
        /*!
         * Default constructor.
         */
        Ray() {}

        /*!
         * Constructor taking an origin, direction, and time for this ray.
         */
        Ray(const Vector3d& origin, const Vector3d& direction, double time =
            0.0) : orig_(origin), dir_(direction), time_(time) {}

        /*!
         * Method computing the location in 3D space that is the input distance
         * along this ray.
         *
         * \param t Distance along ray to compute.
         *
         * \returns Point at distance t along this ray.
         */
        Vector3d at(double t) const;

      public:
        Vector3d orig_; //!< Origin of this ray.
        Vector3d dir_; //!< Direction of this ray.
        double time_; //!< Time that this ray was sent

    };

  }
}

#endif /* ifndef CANNON_RAY_RAY_H */


/******************************************************************************
 * Author:   Laurent Kneip                                                    *
 * Contact:  kneip.laurent@gmail.com                                          *
 * License:  Copyright (c) 2013 Laurent Kneip, ANU. 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 ANU 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 ANU OR THE 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.                                                               *
 ******************************************************************************/

/**
 * \file types.hpp
 * \brief A collection of variables used in geometric vision for the
 *        computation of calibrated absolute and relative pose.
 */

#ifndef OPENGV_TYPES_HPP_
#define OPENGV_TYPES_HPP_

#include <stdlib.h>
#include <vector>
#include <Eigen/Eigen>

/**
 * \brief The namespace of this library.
 */
namespace opengv
{

/** A 3-vector of unit length used to describe landmark observations/bearings
 *  in camera frames (always expressed in camera frames)
 */
typedef Eigen::Vector3d
    bearingVector_t;

/** An array of bearing-vectors */
typedef std::vector<bearingVector_t, Eigen::aligned_allocator<bearingVector_t> >
    bearingVectors_t;

/** A 3-vector describing a translation/camera position */
typedef Eigen::Vector3d
    translation_t;

/** An array of translations */
typedef std::vector<translation_t, Eigen::aligned_allocator<translation_t> >
    translations_t;

/** A rotation matrix */
typedef Eigen::Matrix3d
    rotation_t;

/** An array of rotation matrices as returned by fivept_kneip [7] */
typedef std::vector<rotation_t, Eigen::aligned_allocator<rotation_t> >
    rotations_t;

/** A 3x4 transformation matrix containing rotation \f$ \mathbf{R} \f$ and
 *  translation \f$ \mathbf{t} \f$ as follows:
 *  \f$ \left( \begin{array}{cc} \mathbf{R} & \mathbf{t} \end{array} \right) \f$
 */
typedef Eigen::Matrix<double,3,4>
    transformation_t;

/** An array of transformations */
typedef std::vector<transformation_t, Eigen::aligned_allocator<transformation_t> >
    transformations_t;

/** A 3-vector containing the cayley parameters of a rotation matrix */
typedef Eigen::Vector3d
    cayley_t;

/** A 4-vector containing the quaternion parameters of rotation matrix */
typedef Eigen::Vector4d
    quaternion_t;

/** Essential matrix \f$ \mathbf{E} \f$ between two viewpoints:
 *
 *  \f$ \mathbf{E} = \f$ skew(\f$\mathbf{t}\f$) \f$ \mathbf{R} \f$,
 *
 *  where \f$ \mathbf{t} \f$ describes the position of viewpoint 2 seen from
 *  viewpoint 1, and \f$\mathbf{R}\f$ describes the rotation from viewpoint 2
 *  to viewpoint 1.
 */
typedef Eigen::Matrix3d
    essential_t;

/** An array of essential matrices */
typedef std::vector<essential_t, Eigen::aligned_allocator<essential_t> >
    essentials_t;

/** An essential matrix with complex entires (as returned from
 *  fivept_stewenius [5])
 */
typedef Eigen::Matrix3cd
    complexEssential_t;

/** An array of complex-type essential matrices */
typedef std::vector< complexEssential_t, Eigen::aligned_allocator< complexEssential_t> >
    complexEssentials_t;

/** A 3-vector describing a point in 3D-space */
typedef Eigen::Vector3d
    point_t;

/** An array of 3D-points */
typedef std::vector<point_t, Eigen::aligned_allocator<point_t> >
    points_t;

/** A 3-vector containing the Eigenvalues of matrix \f$ \mathbf{M} \f$ in the
 *  eigensolver-algorithm (described in [11])
 */
typedef Eigen::Vector3d
    eigenvalues_t;

/** A 3x3 matrix containing the eigenvectors of matrix \f$ \mathbf{M} \f$ in the
 *  eigensolver-algorithm (described in [11])
 */
typedef Eigen::Matrix3d
    eigenvectors_t;

/** EigensolverOutput holds the output-parameters of the eigensolver-algorithm
 *  (described in [11])
 */
typedef struct EigensolverOutput
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /** Position of viewpoint 2 seen from viewpoint 1 (unscaled) */
  translation_t   translation;
  /** Rotation from viewpoint 2 back to viewpoint 1 */
  rotation_t      rotation;
  /** The eigenvalues of matrix \f$ \mathbf{M} \f$ */
  eigenvalues_t   eigenvalues;
  /** The eigenvectors of matrix matrix \f$ \mathbf{M} \f$ */
  eigenvectors_t  eigenvectors;
} eigensolverOutput_t;

/** GeOutput holds the output-parameters of ge
 */
typedef struct GeOutput
{
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  /** Homogeneous position of viewpoint 2 seen from viewpoint 1 */
  Eigen::Vector4d   translation;
  /** Rotation from viewpoint 2 back to viewpoint 1 */
  rotation_t        rotation;
  /** The eigenvalues of matrix \f$ \mathbf{G} \f$ */
  Eigen::Vector4d   eigenvalues;
  /** The eigenvectors of matrix matrix \f$ \mathbf{G} \f$ */
  Eigen::Matrix4d   eigenvectors;
} geOutput_t;

}

#endif /* OPENGV_TYPES_HPP_ */


/* CirKit: A circuit toolkit
 * Copyright (C) 2009-2015  University of Bremen
 * Copyright (C) 2015-2017  EPFL
 *
 * 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 "embed_truth_table.hpp"

#include <boost/assign/std/vector.hpp>
#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>

#include <core/utils/timer.hpp>

#include <reversible/functions/extend_truth_table.hpp>
#include <reversible/functions/truth_table_from_bitset.hpp>

#include "synthesis_utils_p.hpp"

using namespace boost::assign;

// NOTE use unsigned long long instead of unsigned
// now this approach works only for embedding with 32 extra lines
namespace cirkit
{

unsigned additional_garbage( const binary_truth_table& base, std::vector<unsigned>& values )
{
  std::map<unsigned, unsigned> output_value_count;

  for ( binary_truth_table::const_iterator it = base.begin(); it != base.end(); ++it )
  {
    binary_truth_table::cube_type out( it->second.first, it->second.second );
    unsigned number = truth_table_cube_to_number( out );

    if ( output_value_count.find( number ) == output_value_count.end() )
    {
      output_value_count.insert( std::make_pair( number, 1 ) );
    }
    else
    {
      ++output_value_count[number];
    }
  }

  // copy the highest count to mu, e.g. ([0,4], [1,3]) -> 4
  unsigned mu = boost::max_element( output_value_count,
                                  []( const std::map<unsigned,unsigned>::value_type& a, const std::map<unsigned,unsigned>::value_type& b ) {
                                    return a.second < b.second;
                                  } )->second;

  using boost::adaptors::map_keys;
  boost::push_back( values, output_value_count | map_keys ); // TODO map_values here instead?

  return (unsigned)ceil( log( (double)mu ) / log( 2.0 ) );
}

struct minimal_hamming_distance
{
  explicit minimal_hamming_distance( unsigned compare_to ) : compare_to( compare_to ) {}

  bool operator()( unsigned x, unsigned y ) const
  {
    return hamming_distance( compare_to, x ) < hamming_distance( compare_to, y );
  }

private:
  unsigned compare_to;
};

bool embed_truth_table( binary_truth_table& spec, const binary_truth_table& base, const properties::ptr& settings, const properties::ptr& statistics )
{
  std::string garbage_name           = get<std::string>( settings, "garbage_name", "g" );
  std::vector<unsigned> output_order = get<std::vector<unsigned> >( settings, "output_order", std::vector<unsigned>() );

  properties_timer t( statistics );

  /* get number of additional garbage lines needed */
  std::vector<unsigned> values;
  unsigned ag = additional_garbage( base, values );
  ag = (unsigned)std::max( (int)ag, (int)base.num_inputs() - (int)base.num_outputs() );
  unsigned cons = base.num_outputs() + ag - base.num_inputs();

  /* we don't need more than that */
  assert( base.num_inputs() <= base.num_outputs() + ag );

  /* new number of bits */
  std::map<unsigned, unsigned> new_spec;
  unsigned new_bw = base.num_outputs() + ag;

  /* all outputs which are left */
  std::vector<unsigned> all_outputs( 1u << new_bw );
  std::copy( boost::make_counting_iterator( 0u ), boost::make_counting_iterator( 1u << new_bw ), all_outputs.begin() );

  {
    /* greedy method */
    std::map<unsigned,std::vector<unsigned> > output_assignments;

    /* output order */
    if ( output_order.size() != base.num_outputs() )
    {
      output_order.clear();
      std::copy( boost::make_counting_iterator( 0u ), boost::make_counting_iterator( base.num_outputs() ), std::back_inserter( output_order ) );
    }

    /* not in output order for filling up */
    std::vector<unsigned> left_positions;
    for ( unsigned order = 0u; order < new_bw; ++order )
    {
      if ( std::find( output_order.begin(), output_order.end(), order ) == output_order.end() )
      {
        left_positions += order;
      }
    }

    /* since you append the garbage to the end, just count the numbers */
    for ( std::vector<unsigned>::const_iterator itValue = values.begin(); itValue != values.end(); ++itValue )
    {
      unsigned base = 0u;
      for ( unsigned j = 0u; j < output_order.size(); ++j )
      {
        unsigned bit_in_value = !!( *itValue & ( 1u << ( output_order.size() - 1u - j ) ) );
        unsigned bit_pos_in_base = new_bw - 1u - output_order.at( j );
        base |= ( bit_in_value << bit_pos_in_base );
      }

      /* create the values from (value)0..0 to (value)1..1 with rebaset to output_order */
      std::vector<unsigned> assignments;

      for ( unsigned j = 0u; j < ( 1u << ag ); ++j )
      {
        unsigned assignment = base;
        for ( unsigned k = 0; k < ag; ++k )
        {
          unsigned bit_in_value = !!( j & ( 1u << ( ag - 1u - k ) ) );
          unsigned bit_pos_in_base = new_bw - 1u - left_positions.at( k );
          assignment |= ( bit_in_value << bit_pos_in_base );
        }
        assignments += assignment;
      }

      output_assignments[*itValue] = assignments;
    }

    /* truth table is in order */
    for ( binary_truth_table::const_iterator it = base.begin(); it != base.end(); ++it )
    {
      /* input value */
      binary_truth_table::cube_type in( it->first.first, it->first.second );
      unsigned number_in = truth_table_cube_to_number( in );

      /* output value */
      binary_truth_table::cube_type out( it->second.first, it->second.second );
      unsigned number = truth_table_cube_to_number( out );

      /* best suiting element */
      std::vector<unsigned>::iterator bestFit = std::min_element( output_assignments[number].begin(), output_assignments[number].end(), minimal_hamming_distance( number_in ) );
      new_spec.insert( std::make_pair( number_in, *bestFit ) );

      /* remove element from list */
      all_outputs.erase( std::find( all_outputs.begin(), all_outputs.end(), *bestFit ) );
      output_assignments[number].erase( bestFit );
    }
  }

  for ( unsigned i = 0u; i < ( 1u << new_bw ); ++i )
  {
    if ( new_spec.find( i ) == new_spec.end() )
    {
      std::vector<unsigned>::iterator bestFit = std::min_element( all_outputs.begin(), all_outputs.end(), minimal_hamming_distance( i ) );
      new_spec.insert( std::make_pair( i, *bestFit ) );
      all_outputs.erase( bestFit );
    }
  }

  spec.clear();

  for ( std::map<unsigned, unsigned>::const_iterator it = new_spec.begin(); it != new_spec.end(); ++it )
  {
    spec.add_entry( number_to_truth_table_cube( it->first, new_bw ),
                    number_to_truth_table_cube( it->second, new_bw ) );
  }

  /* meta-data */
  std::vector<std::string> inputs( new_bw, "i" );
  std::fill( inputs.begin(), inputs.begin() + cons, "0" );
  std::copy( base.inputs().begin(), base.inputs().end(), inputs.begin() + cons );
  spec.set_inputs( inputs );

  std::vector<std::string> outputs( new_bw, garbage_name );
  for ( std::vector<unsigned>::iterator it = output_order.begin(); it != output_order.end(); ++it ) {
    unsigned index = std::distance( output_order.begin(), it );
    if ( index < base.outputs().size() )
    {
      outputs.at( *it ) = base.outputs().at( index );
    }
  }
  spec.set_outputs( outputs );

  std::vector<constant> constants( new_bw );
  std::fill( constants.begin(), constants.begin() + cons, false );
  std::fill( constants.begin() + cons, constants.end(), constant() );
  spec.set_constants( constants );

  std::vector<bool> garbage( new_bw, true );
  for ( std::vector<unsigned>::const_iterator it = output_order.begin(); it != output_order.end(); ++it ) {
    garbage.at( *it ) = false;
  }
  spec.set_garbage( garbage );

  return true;
}

bool embed_truth_table( binary_truth_table& spec, const tt& base, const properties::ptr& settings, const properties::ptr& statistics )
{
  const auto rbase = truth_table_from_bitset_direct( base );
  return embed_truth_table( spec, rbase, settings, statistics );
}

embedding_func embed_truth_table_func( const properties::ptr& settings, const properties::ptr& statistics )
{
  embedding_func f = [&settings, &statistics]( binary_truth_table& spec, const binary_truth_table& base ) {
    return embed_truth_table( spec, base, settings, statistics );
  };
  f.init( settings, statistics );
  return f;
}

}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#include "Day13-TransparentOrigami.h"

#include "TransparentPaperFolder.h"

#include <AdventOfCodeCommon/Utils.h>

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2021
{
namespace Day13
{

using TextSection = std::vector<std::string>;

Coordinates parseCoordinatesLine(const std::string& coordinatesLine)
{
    std::vector<int> coordinates;
    Utils::splitStringIntoTypedVector(coordinatesLine, coordinates, ',');
    return Coordinates{coordinates.at(0), coordinates.at(1)};
}

Axis parseAxisString(const std::string& axisString)
{
    if (!(axisString == "x" || axisString == "y"))
    {
        throw std::runtime_error("Cannot parse axis: " + axisString);
    }
    return axisString == "x" ? Axis::X : Axis::Y;
}

FoldInstruction parseFoldInstructionLine(const std::string& foldInstructionLine)
{
    std::vector<std::string> tokens;
    boost::split(tokens, foldInstructionLine, boost::is_any_of(" ="));

    if (tokens.size() != 4)
    {
        throw std::runtime_error("Invalid number of tokens in line: " + foldInstructionLine);
    }

    const Axis axis = parseAxisString(tokens.at(2));
    const int position = std::stoi(tokens.at(3));

    return FoldInstruction{axis, position};
}

CoordinatesSet parseCoordinatesTextSection(const TextSection& coordinatesTextSection)
{
    CoordinatesSet coordinatesSet;

    for (const auto& line : coordinatesTextSection)
    {
        Coordinates coordinates = parseCoordinatesLine(line);
        coordinatesSet.insert(std::move(coordinates));
    }

    return coordinatesSet;
}

FoldInstructions parseFoldInstructionTextSection(const std::vector<std::string>& foldInstructionTextSection)
{
    FoldInstructions foldInstructions;

    for (const auto& line : foldInstructionTextSection)
    {
        FoldInstruction foldInstruction = parseFoldInstructionLine(line);
        foldInstructions.push_back(std::move(foldInstruction));
    }

    return foldInstructions;
}

TransparentPaperFolder parseInstructionManualLines(const std::vector<std::string>& instructionManualLines)
{
    std::vector<TextSection> textSections;
    boost::split(textSections, instructionManualLines, [](const auto& elem) { return elem.empty(); });

    CoordinatesSet dotCoordinatesSet = parseCoordinatesTextSection(textSections.at(0));
    FoldInstructions foldInstructions = parseFoldInstructionTextSection(textSections.at(1));

    return TransparentPaperFolder{std::move(dotCoordinatesSet), std::move(foldInstructions)};
}

unsigned numVisibleDotsAfterFirstFold(const std::vector<std::string>& instructionManualLines)
{
    TransparentPaperFolder transparentPaperFolder = parseInstructionManualLines(instructionManualLines);
    transparentPaperFolder.foldOnce();
    return transparentPaperFolder.getNumVisibleDots();
}

std::string thermalCameraActivationCode(const std::vector<std::string>& instructionManualLines)
{
    TransparentPaperFolder transparentPaperFolder = parseInstructionManualLines(instructionManualLines);
    transparentPaperFolder.foldCompletely();
    return transparentPaperFolder.getSerializedMessage();
}

}
}
}


/** A simple trajectory controller.
    It receives a joint destination, a duration and publishes the trajectory
    in joint space at 200Hz
**/
#include "ros/ros.h"
#include <boost/lexical_cast.hpp>
#include <boost/thread/thread.hpp>
#include <trajectory_msgs/JointTrajectory.h>
#include "bosch_arm_srvs/GetJointAngles.h"
int main ( int argc, char **argv )
{
  ros::init ( argc, argv, "traj_gen" );
  ros::NodeHandle n;
  ros::Publisher traj_pub =  n.advertise<trajectory_msgs::JointTrajectory> ( "/traj_cmd",1 );
  //boost::thread t2 = boost::thread::thread ( boost::bind ( &pubTrajectory ) );
  ros::ServiceClient client = n.serviceClient<bosch_arm_srvs::GetJointAngles>("get_joint_angles");
  bosch_arm_srvs::GetJointAngles srv;
  client.call(srv);
  double des[4];
  for(int i=0;i<4;i++)
    des[i]=boost::lexical_cast<double>(argv[i+1]);
  double t=boost::lexical_cast<double>(argv[5]);
  int npts=ceil(t*200);
  trajectory_msgs::JointTrajectory traj;
  traj.header.stamp=ros::Time::now();
  traj.header.frame_id="";
  traj.header.seq=0;
  traj.points.resize(npts+1);
  traj.joint_names.resize ( 4 );
  traj.joint_names[0]="joint1";
  traj.joint_names[1]="joint2";
  traj.joint_names[2]="joint3";
  traj.joint_names[3]="joint4";
  double dq[4];
  for(int i=0;i<4;i++)
    dq[i]=(des[i]-srv.response.joint_angles[i])/npts;
  for(int i=0;i<=npts;i++)
  {
    traj.points[i].positions.resize(4);
    for(int j=0;j<4;j++)
      traj.points[i].positions[j]=srv.response.joint_angles[j]+i*dq[j];
    traj.points[i].time_from_start=ros::Duration(i*t/npts);
  }
  //The first message is always lost
  for(int i=0;i<2;i++){
  traj_pub.publish(traj);
  sleep(1);
  }
  ros::spin();
  return 0;
}


#include <iostream>
#include <cmath>
#include <string>
#include <vector>
#include <variant>

// #include <boost/variant.hpp>

using namespace std;

struct record
{
    int counted_ints= 0,
	counted_doubles= 0,
	counted_strings= 0,
	total_symbols= 0;
};

struct symbol_counter
{
    symbol_counter(record& r) : r(r) {}

    void operator()(int i) const
    {
	r.counted_ints++;
	r.total_symbols += static_cast<int>(floor(log10(i))) + 1;
    }

    void operator()(double d) const
    {
	r.counted_doubles++;
	r.total_symbols+= 15;
    }

    void operator()(string s) const
    {
    	r.counted_strings++;
    	r.total_symbols+= static_cast<int>(s.size());
    }
  private:
    record& r;
};

int main(int argc, char** argv)
{
    using my_variant= variant<int, double, string>;

    my_variant var;
    var= 3;
    var= 4.2;
    var= "Text";

    string s= get<2>(var);
    s= get<string>(var);
    // int i= get<int>(var); // throws std::bad_variant_access

    vector<my_variant> v= {33, 2.3, "Huhu"s, "Dings"s, 11111, 3.809, "Soso"s};

    record r;
    for (const auto& mv : v)
	visit(symbol_counter{r}, mv);

    cout << "We had " << r.counted_ints << " ints, " << r.counted_doubles
	 << " doubles, and " << r.counted_strings << " strings. "
	 << "They have " << r.total_symbols << " symbols in total.\n";

    return 0;
}


/******************************************************************************

  Helper types related to the tangential component of the complex propagation.

  Copyright (c) 2013
  Alexander Rukletsov <rukletsov@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  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.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR 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 AUTHOR 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.

*******************************************************************************/

#ifndef TANGENTIAL_PROPAGATION_HPP_96CE789E_EA28_11E2_A507_AB62D8EF6274
#define TANGENTIAL_PROPAGATION_HPP_96CE789E_EA28_11E2_A507_AB62D8EF6274

#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/assert.hpp>

#include "bo/core/vector.hpp"
#include "bo/math/pca.hpp"

namespace bo {
namespace surfaces {
namespace detail {

// Base class for any tangential propagation.
template <typename RealType>
class BaseTangentialPropagation
{
public:
    typedef BaseTangentialPropagation<RealType> SelfType;
    typedef boost::shared_ptr<SelfType> Ptr;
    typedef Vector<RealType, 3> Point3D;

    BaseTangentialPropagation(RealType weight): weight_(weight)
    { }

    virtual Point3D get(const Point3D& current, const Point3D& inertial) const = 0;

protected:
    RealType weight_;
};

// Standard tangential propagation. Uses the provided k-d tree to fetch neighbours of
// the given point and then takes the greatest vector from the PCA launched on
// neighbours.
template <typename RealType, typename Tree>
class TangentialPropagation: public BaseTangentialPropagation<RealType>
{
public:
    typedef TangentialPropagation<RealType, Tree> SelfType;
    typedef boost::shared_ptr<SelfType> Ptr;
    typedef Vector<RealType, 3> Point3D;
    typedef std::vector<Point3D> Points3D;
    typedef boost::shared_ptr<Tree> TreePtr;

    TangentialPropagation(TreePtr tree_ptr, RealType radius, RealType weight):
        BaseTangentialPropagation<RealType>(weight), tree_ptr_(tree_ptr), radius_(radius)
    { }

    virtual Point3D get(const Point3D& current, const Point3D& inertial) const
    {
        // Initialize collection for neigbours. Reserve gives slightly better performance,
        // but uses more memory.
        Points3D neighbours;
        neighbours.reserve(tree_ptr_->size());

        // Search for nearby points.
        tree_ptr_->find_within_range(current, radius_, std::back_inserter(neighbours));

        // Employ PCA to extract the tangential propagation vector from the set of
        // nearby points.
        typedef math::PCA<RealType, 3> PCAEngine;
        PCAEngine pca;
        typename PCAEngine::Result result = pca(neighbours);
        Point3D tangential = result.template get<1>()[2];

        // Ensure that tangential and inertial components are codirectional.
        if (tangential * inertial < 0)
            tangential = - tangential;

        // TODO: remove this block by refactoring bo::Vector class.
        // Normalize vector.
        Point3D tangential_normalized(tangential.normalized(), 3);

        return this->weight_ * tangential_normalized;
    }

protected:
    TreePtr tree_ptr_;
    RealType radius_;
};

// Tangential propagation that uses neighbouring planes. Calculates and return the
// weighted sum of standard tangential propagations for the main plane and provided
// neighbours (which are projected onto the main plane). Note that main plane is assumed
// to have weight 1.
template <typename RealType, typename Tree>
class NeighbourTangentialPropagation: public BaseTangentialPropagation<RealType>
{
public:
    typedef TangentialPropagation<RealType, Tree> Tangential;
    typedef NeighbourTangentialPropagation<RealType, Tree> SelfType;
    typedef boost::shared_ptr<SelfType> Ptr;
    typedef Vector<RealType, 3> Point3D;

    typedef boost::shared_ptr<Tree> TreePtr;
    typedef std::vector<TreePtr> TreePtrs;
    typedef std::vector<RealType> Weights;
    typedef std::vector<Tangential> TangentialPropagations;

    NeighbourTangentialPropagation(TreePtr tree_ptr, RealType radius, RealType weight,
            const TreePtrs& neighbour_trees, const Weights& neighbour_weights):
        BaseTangentialPropagation<RealType>(weight)
    {
        // Make sure neighbour data is consistent.
        BOOST_ASSERT((neighbour_weights.size() == neighbour_trees.size()) &&
                     "Number of provided neighbour planes doesn't correspond to the "
                     "number of weights.");

        // Cache container sizes and reserve memory.
        std::size_t neighbour_size = neighbour_weights.size();
        total_size_ = neighbour_size + 1;
        tangentials_.reserve(total_size_);

        // Add standard tangential propagation for the main plane.
        tangentials_.push_back(Tangential(tree_ptr, radius, RealType(1)));

        // Add standard tangential propagations for neighbouring planes.
        for (std::size_t idx = 0; idx < neighbour_size; ++idx)
            tangentials_.push_back(Tangential(neighbour_trees[idx], radius,
                                              neighbour_weights[idx]));
    }

    virtual Point3D get(const Point3D& current, const Point3D& inertial) const
    {
        // Compute weighted tangential propagations for the main and neighbouring planes
        // and sum them up.
        Point3D total_tangential;
        for (std::size_t idx = 0; idx < total_size_; ++idx)
            total_tangential += tangentials_[idx].get(current, inertial);

        // TODO: remove this block by refactoring bo::Vector class.
        // Normalize vector.
        Point3D total_tangential_normalized(total_tangential.normalized(), 3);

        return this->weight_ * total_tangential_normalized;
    }

protected:
    TangentialPropagations tangentials_;
    std::size_t total_size_;
};

} // namespace detail
} // namespace surfaces
} // namespace bo

#endif // TANGENTIAL_PROPAGATION_HPP_96CE789E_EA28_11E2_A507_AB62D8EF6274