#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/table_handler.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/petsc_parallel_sparse_matrix.h>
#include <deal.II/lac/petsc_parallel_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/lac/vector.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/distributed/grid_refinement.h>
#include <deal.II/distributed/tria.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_q.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <mandy/elastic_tensor.h>
#include <mandy/lattice_tensor.h>
#include <mandy/matrix_creator.h>
#include <mandy/vector_creator.h>

#include <mandy/crystal_symmetry_group.h>

#include <fstream>
#include <iostream>

#include <algorithm>    // std::transform
#include <functional>   // std::plus

namespace mandy
{

  /**
   * Solve the system Mx=b, where M is the mass matrix and b is a
   * linear function.
   */
  template <int dim>
  class MaterialID
  {
  public:

    /**
     * Class constructor.
     */
    MaterialID (const std::string &prm);

    /**
     * Class destructor.
     */
    ~MaterialID ();

    /**
     * Wrapper function, that controls the order of excecution.
     */
    void run ();

  private:

    /**
     * Make intial coarse grid.
     */
    void make_coarse_grid ();

    /**
     * Setup system matrices and vectors.
     */
    void setup_system ();

    /**
     * Assemble system matrices and vectors.
     */
    void assemble_system();

    /**
     * Solve the linear algebra system.
     */
    unsigned int solve ();

    /**
     * Output results, ie., finite element functions and derived
     * quantitites for this cycle.
     */
    void output_results (const unsigned int cycle);

    /**
     * Refine grid based on Kelly's error estimator working on the
     * material id (solution vector).
     */
    void refine_grid ();

    /**
     * MPI communicator.
     */
    MPI_Comm mpi_communicator;

    /**
     * A distributed grid on which all computations are done.
     */
    dealii::parallel::distributed::Triangulation<dim> triangulation;

    /**
     * Scalar DoF handler primarily used for interpolating material
     * identification.
     */
    dealii::DoFHandler<dim> dof_handler;

    /**
     * Scalar valued finite element primarily used for interpolating
     * material iudentification.
     */
    dealii::FESystem<dim> fe;

    /**
     * Index set of locally owned DoFs.
     */
    dealii::IndexSet locally_owned_dofs;

    /**
     * Index set of locally relevant DoFs.
     */
    dealii::IndexSet locally_relevant_dofs;

    /**
     * A list of (hanging node) constraints.
     */
    dealii::ConstraintMatrix constraints;

    /**
     * System matrix - a mass matrix.
     */
    dealii::PETScWrappers::MPI::SparseMatrix system_matrix;

    /**
     * Locally relevant solution vector.
     */
    dealii::PETScWrappers::MPI::Vector locally_relevant_solution;

    /**
     * System right hand side function - interpolated function.
     */
    dealii::PETScWrappers::MPI::Vector system_rhs;

    /**
     * Parallel iostream.
     */
    dealii::ConditionalOStream pcout;

    /**
     * Stop clock.
     */
    dealii::TimerOutput timer;

    /**
     * Input parameter file.
     */
    dealii::ParameterHandler parameters;

  }; // MaterialID

  /**
   * Class constructor.
   */
  template <int dim>
  MaterialID<dim>::MaterialID (const std::string &prm)
    :
    mpi_communicator (MPI_COMM_WORLD),
    triangulation (mpi_communicator,
                   typename dealii::Triangulation<dim>::MeshSmoothing
                   (dealii::Triangulation<dim>::smoothing_on_refinement |
                    dealii::Triangulation<dim>::smoothing_on_coarsening)),
    dof_handler (triangulation),
    fe (dealii::FE_Q<dim> (2), 1),
    // ---
    pcout (std::cout, (dealii::Utilities::MPI::this_mpi_process (mpi_communicator) == 0)),
    timer (mpi_communicator, pcout,
	   dealii::TimerOutput::summary,
	   dealii::TimerOutput::wall_times)
  {
    parameters.declare_entry ("Global mesh refinement steps", "5",
                              dealii::Patterns::Integer (0, 20),
                              "The number of times the 1-cell coarse mesh should "
                              "be refined globally for our computations.");

    parameters.declare_entry ("MaterialID", "0",
                              dealii::Patterns::Anything (),
                              "A functional description of the material ID.");

    parameters.parse_input (prm);
  }

  /**
   * Class destructor.
   */
  template <int dim>
  MaterialID<dim>::~MaterialID ()
  {
    // Wipe DoF handlers.
    dof_handler.clear ();
  }

  /**
   * Make initial coarse grid.
   */
  template <int dim>
  void
  MaterialID<dim>::make_coarse_grid ()
  {
    dealii::TimerOutput::Scope time (timer, "make coarse grid");

    // Create a coarse grid according to the parameters given in the
    // input file.
    dealii::GridGenerator::hyper_cube (triangulation, -10, 10);

    triangulation.refine_global (parameters.get_integer ("Global mesh refinement steps"));
  }

  /**
   * Setup system matrices and vectors.
   */
  template <int dim>
  void MaterialID<dim>::setup_system ()
  {
    dealii::TimerOutput::Scope time (timer, "setup system");

    // Determine locally relevant DoFs.
    dof_handler.distribute_dofs (fe);
    locally_owned_dofs = dof_handler.locally_owned_dofs ();
    dealii::DoFTools::extract_locally_relevant_dofs (dof_handler, locally_relevant_dofs);

    // Initialise distributed vectors.
    locally_relevant_solution.reinit (locally_owned_dofs, locally_relevant_dofs,
				      mpi_communicator);
    system_rhs.reinit (locally_owned_dofs,
		       mpi_communicator);

    // Setup hanging node constraints.
    constraints.clear ();
    constraints.reinit (locally_relevant_dofs);
    dealii::DoFTools::make_hanging_node_constraints (dof_handler, constraints);
    constraints.close ();

    // Finally, create a distributed sparsity pattern and initialise
    // the system matrix from that.
    dealii::DynamicSparsityPattern dsp (locally_relevant_dofs);
    dealii::DoFTools::make_sparsity_pattern (dof_handler, dsp, constraints, false);
    dealii::SparsityTools::distribute_sparsity_pattern (dsp,
							dof_handler.n_locally_owned_dofs_per_processor (),
							mpi_communicator,
							locally_relevant_dofs);

    system_matrix.reinit (locally_owned_dofs, locally_owned_dofs,
                          dsp, mpi_communicator);

  }

  /**
   * Assemble system matrices and vectors.
   *
   * TODO Ideally, we would use a function like this:
   *
   * dealii::MatrixCreator::create_mass_matrix (dof_handler, quadrature_rule, system_matrix, 1, constraints);
   *
   * however no such thing currently exists in the deal.II library for
   * parallel matrices and vectors. Instead, the mass matrix and right
   * hand side vector are assembled by hand in functions defined in
   * the namepsaces, mandy::MatrixCreator and mandy::VectorCreator,
   * respectively.
   */
  template <int dim>
  void
  MaterialID<dim>::assemble_system ()
  {
    dealii::TimerOutput::Scope time (timer, "assemble system");

    // Define quadrature rule to be used.
    const dealii::QGauss<dim> quadrature_formula (3);

    // Initialise the function parser.
    dealii::FunctionParser<dim> material_identification;
    material_identification.initialize (dealii::FunctionParser<dim>::default_variable_names (),
					parameters.get ("MaterialID"),
					typename dealii::FunctionParser<dim>::ConstMap ());

    mandy::MatrixCreator::create_mass_matrix<dim> (fe, dof_handler, quadrature_formula,
						   system_matrix, constraints,
						   mpi_communicator);

    mandy::VectorCreator::create_right_hand_side_vector<dim> (fe, dof_handler, quadrature_formula,
     							      system_rhs, constraints,
							      material_identification,
							      mpi_communicator);
  }

  /**
   * Solve the linear algebra system.
   */
  template <int dim>
  unsigned int
  MaterialID<dim>::solve ()
  {
    dealii::TimerOutput::Scope time (timer, "solve");

    dealii::PETScWrappers::MPI::Vector completely_distributed_solution (locally_owned_dofs, mpi_communicator);

    // Solve using conjugate gradient method with no preconditioner
    // (ie., system_matrix is ignored).
    dealii::SolverControl solver_control (dof_handler.n_dofs (), 1e-06);
    dealii::PETScWrappers::SolverCG solver (solver_control, mpi_communicator);
    dealii::PETScWrappers::PreconditionNone preconditioner (system_matrix);

    solver.solve (system_matrix, completely_distributed_solution, system_rhs,
		  preconditioner);

    // Ensure that all ghost elements are also copied as necessary.
    constraints.distribute (completely_distributed_solution);
    locally_relevant_solution = completely_distributed_solution;

    // Return the number of iterations (last step) of the solve.
    return solver_control.last_step ();
  }

  /**
   * Output results, ie., finite element functions and derived
   * quantitites for this cycle..
   */
  template <int dim>
  void
  MaterialID<dim>::output_results (const unsigned int cycle)
  {
    dealii::TimerOutput::Scope time (timer, "output_results");

    dealii::DataOut<dim> data_out;
    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (locally_relevant_solution, "material_id");

    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 ();

    const std::string filename = ("material_id-" +
                                  dealii::Utilities::int_to_string (cycle, 2) +
                                  "." +
                                  dealii::Utilities::int_to_string
                                  (triangulation.locally_owned_subdomain (), 4));

    std::ofstream output ((filename + ".vtu").c_str ());
    data_out.write_vtu (output);

    if (dealii::Utilities::MPI::this_mpi_process(mpi_communicator) == 1)
      {
	std::vector<std::string> filenames;

	for (unsigned int i=0;
	     i<dealii::Utilities::MPI::n_mpi_processes (mpi_communicator);
	     ++i)
	  filenames.push_back ("material_id-" +
			       dealii::Utilities::int_to_string (cycle, 2) +
			       "." +
			       dealii::Utilities::int_to_string (i, 4) +
			       ".vtu");
	std::ofstream master_output (("material_id-" +
				      dealii::Utilities::int_to_string (cycle, 2) +
				      ".pvtu").c_str ());

	data_out.write_pvtu_record (master_output, filenames);
      }
  }

  /**
   * Refine grid based on Kelly's error estimator working on the
   * material id (solution vector).
   */
  template <int dim>
  void MaterialID<dim>::refine_grid ()
  {
    dealii::TimerOutput::Scope time (timer, "refine grid");

    dealii::Vector<float> estimated_error_per_cell (triangulation.n_active_cells());

    dealii::KellyErrorEstimator<dim>::estimate (dof_handler, dealii::QGauss<dim-1>(4),
						typename dealii::FunctionMap<dim>::type (),
						locally_relevant_solution,
						estimated_error_per_cell);

    dealii::parallel::distributed::GridRefinement::
      refine_and_coarsen_fixed_number (triangulation,
				       estimated_error_per_cell,
				       0.250, 0.025);

    triangulation.execute_coarsening_and_refinement ();
  }

  /**
   * Run the application in the order specified.
   */
  template <int dim>
  void
  MaterialID<dim>::run ()
  {
    const unsigned int n_cycles = 5;

    for (unsigned int cycle=0; cycle<n_cycles; ++cycle)
      {
        pcout << "MaterialID:: Cycle " << cycle << ':'
	      << std::endl;

	if (cycle==0)
	  make_coarse_grid ();

	else
	  refine_grid ();

	pcout << "   Number of active cells:       "
	      << triangulation.n_global_active_cells ()
	      << std::endl;

	setup_system ();

	pcout << "   Number of degrees of freedom: "
	      << dof_handler.n_dofs ()
	      << std::endl;

	assemble_system ();

	const unsigned int n_iterations = solve ();

	pcout << "   Solved in " << n_iterations
	      << " iterations."
	      << std::endl;

	pcout << "   Linfty-norm:                  "
	      << locally_relevant_solution.linfty_norm ()
	      << std::endl;

	// Output results if the number of processes is less than or
	// equal to 32.
	if (dealii::Utilities::MPI::n_mpi_processes (mpi_communicator) <= 32)
	  output_results (cycle);

	// timer.print_summary ();
        pcout << std::endl;

      } // for cycle<n_cycles
  }

} // namespace mandy

/**
 * Main function: Initialise problem and run it.
 */
int main (int argc, char *argv[])
{
  // Initialise MPI
  dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);

  try
    {
      mandy::MaterialID<3> material_id ("step-0-material.prm");
      material_id.run ();
    }

  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }

  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include <benchmark/benchmark.h>

#include <Eigen/StdVector>
#include <kdtreepp.hpp>
#include <random>

using Vector3 = Eigen::Vector3d;
using AlignedBox3 = Eigen::AlignedBox3d;

static void BM_Points3dConstruction(benchmark::State& state) {
  std::vector<Vector3, Eigen::aligned_allocator<Vector3>> points;
  std::mt19937_64 randGen{size_t(state.thread_index)};
  std::uniform_real_distribution<double> dist{-1000.0, 1000.0};

  // Make random points
  const auto count = size_t(state.range(0));
  points.resize(count);
  for (auto& point : points) {
    point << dist(randGen), dist(randGen), dist(randGen);
  }

  while (state.KeepRunning()) {
    const auto node = kdtreepp::MakeEigenKdTreeNode<double, 3>(
        points.begin(), points.end(), [](const Vector3& p) { return p; },
        [](const Vector3& p) { return p; });
    benchmark::DoNotOptimize(node);
    benchmark::ClobberMemory();
  }
}

static void BM_Points3dClosest(benchmark::State& state) {
  std::vector<Vector3, Eigen::aligned_allocator<Vector3>> points;
  std::mt19937_64 randGen{size_t(state.thread_index)};
  std::uniform_real_distribution<double> dist{-1000.0, 1000.0};

  // Make random points
  points.resize(size_t(state.range(0)));
  for (auto& point : points) {
    point << dist(randGen), dist(randGen), dist(randGen);
  }

  const auto node = kdtreepp::MakeEigenKdTreeNode<double, 3>(
      points.begin(), points.end(), [](const Vector3& p) { return p; },
      [](const Vector3& p) { return p; });

  const auto iterations = size_t(state.range(1));

  while (state.KeepRunning()) {
    for (size_t i = 0; i < iterations; i++) {
      const Vector3 checkPoint{dist(randGen), dist(randGen), dist(randGen)};

      double minDistSq = std::numeric_limits<double>::max();
      Vector3 closestPoint;
      node.visit(
          [&minDistSq, checkPoint](const AlignedBox3& bounds) {
            return bounds.squaredExteriorDistance(checkPoint) < minDistSq;
          },
          [&minDistSq, &closestPoint, checkPoint](const Vector3& point) {
            const double rSq = (point - checkPoint).squaredNorm();
            if (rSq < minDistSq) {
              minDistSq = rSq;
              closestPoint = point;
            }
          });
      benchmark::DoNotOptimize(closestPoint);
      benchmark::ClobberMemory();
    }
  }
}

int main(int argc, char* argv[]) {
  // NOLINTNEXTLINE(clang-analyzer-cplusplus.NewDeleteLeaks)
  benchmark::RegisterBenchmark("BM_Points3dConstruction", BM_Points3dConstruction)
      ->Arg(1)
      ->Arg(10)
      ->Arg(100)
      ->Arg(1000)
      ->Arg(10000)
      ->Arg(100000)
      ->Arg(1000000);
  benchmark::RegisterBenchmark("BM_Points3dClosest", BM_Points3dClosest)->Args({100000, 10000});
  benchmark::Initialize(&argc, argv);
  benchmark::RunSpecifiedBenchmarks();

  return 0;
}


#include "mpi_fsi.h"
#include "mpi_scnsim.h"
#include "mpi_shared_hypo_elasticity.h"
#include "parameters.h"
#include "utilities.h"
#include <deal.II/grid/grid_in.h>
#include <deal.II/grid/grid_out.h>

extern template class Fluid::MPI::SCnsIM<2>;
extern template class Solid::MPI::SharedHypoElasticity<2>;
extern template class MPI::FSI<2>;

using namespace dealii;

template <int dim>
class SigmaPMLField : public Function<dim>
{
public:
  SigmaPMLField(double sig, double l)
    : Function<dim>(), SigmaPMLMax(sig), PMLLength(l)
  {
  }
  virtual double value(const Point<dim> &p,
                       const unsigned int component = 0) const;
  virtual void value_list(const std::vector<Point<dim>> &points,
                          std::vector<double> &values,
                          const unsigned int component = 0) const;

private:
  double SigmaPMLMax;
  double PMLLength;
};

template <int dim>
class ArtificialBF : public TensorFunction<1, dim>
{
public:
  ArtificialBF() : TensorFunction<1, dim>() {}
  virtual Tensor<1, dim> value(const Point<dim> &p) const;
  virtual void value_list(const std::vector<Point<dim>> &points,
                          std::vector<Tensor<1, dim>> &values) const;
};

template <int dim>
double SigmaPMLField<dim>::value(const Point<dim> &p,
                                 const unsigned int component) const
{
  (void)component;
  (void)p;
  double SigmaPML = 0.0;
  double boundary = 0.0;
  // For tube acoustics
  if (p[0] < PMLLength + boundary)
    // A quadratic increasing function from boundary-PMLlength to the boundary
    SigmaPML = SigmaPMLMax * pow((PMLLength + boundary - p[0]) / PMLLength, 4);
  return SigmaPML;
}

template <int dim>
void SigmaPMLField<dim>::value_list(const std::vector<Point<dim>> &points,
                                    std::vector<double> &values,
                                    const unsigned int component) const
{
  (void)component;
  for (unsigned int i = 0; i < points.size(); ++i)
    values[i] = this->value(points[i]);
}

template <int dim>
Tensor<1, dim> ArtificialBF<dim>::value(const Point<dim> &p) const
{
  Tensor<1, dim> value;
  double rho = 1.3e-3;
  double bf = 1.0e4 / rho;
  if (p[0] > 1.0 - 5e-4 && p[0] < 2.0 + 5e-4)
    value[0] = bf;
  return value;
}

template <int dim>
void ArtificialBF<dim>::value_list(const std::vector<Point<dim>> &points,
                                   std::vector<Tensor<1, dim>> &values) const
{
  for (unsigned int i = 0; i < points.size(); ++i)
    values[i] = this->value(points[i]);
}

int main(int argc, char *argv[])
{
  using namespace dealii;

  double PMLlength = 1.0, SigmaMax = 340000;

  try
    {
      Utilities::MPI::MPI_InitFinalize mpi_initialization(argc, argv, 1);

      std::string infile("parameters.prm");
      if (argc > 1)
        {
          infile = argv[1];
        }
      Parameters::AllParameters params(infile);

      if (params.dimension == 2)
        {
          // Read solid mesh
          Triangulation<2> tria_solid;
          dealii::GridGenerator::subdivided_hyper_rectangle(
            tria_solid,
            {static_cast<unsigned int>(10), static_cast<unsigned int>(40)},
            Point<2>(0, 0),
            Point<2>(0.5, 2),
            true);

          // Read fluid mesh
          parallel::distributed::Triangulation<2> tria_fluid(MPI_COMM_WORLD);
          dealii::GridGenerator::subdivided_hyper_rectangle(
            tria_fluid,
            {static_cast<unsigned int>(114), static_cast<unsigned int>(29)},
            Point<2>(0, 0),
            Point<2>(5, 2),
            true);

          // Translate solid mesh
          Tensor<1, 2> offset({2, 0});
          GridTools::shift(offset, tria_solid);

          // placeholder for hard code BC
          std::shared_ptr<Functions::ZeroFunction<2>> ptr =
            std::make_shared<Functions::ZeroFunction<2>>(
              Functions::ZeroFunction<2>(3));
          // initialize the pml field
          auto pml = std::make_shared<SigmaPMLField<2>>(
            SigmaPMLField<2>(SigmaMax, PMLlength));
          // artificial body force
          auto bf_ptr = std::make_shared<ArtificialBF<2>>(ArtificialBF<2>());

          Fluid::MPI::SCnsIM<2> fluid(tria_fluid,
                                      params); //, ptr, pml, bf_ptr);
          Solid::MPI::SharedHypoElasticity<2> solid(
            tria_solid, params, 0.05, 1.3);
          MPI::FSI<2> fsi(fluid, solid, params);
          fsi.run();
        }
      else
        {
          AssertThrow(false, ExcNotImplemented());
        }
    }
  catch (std::exception &exc)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  catch (...)
    {
      std::cerr << std::endl
                << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }
  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 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
//==============================================================================
#ifndef NT2_TOOLBOX_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHCOSH_HPP_INCLUDED
#define NT2_TOOLBOX_HYPERBOLIC_FUNCTIONS_SIMD_COMMON_SINHCOSH_HPP_INCLUDED

#include <nt2/toolbox/hyperbolic/functions/sinhcosh.hpp>
#include <nt2/include/functions/simd/tofloat.hpp>
#include <nt2/include/functions/simd/abs.hpp>
#include <nt2/include/functions/simd/expm1.hpp>
#include <nt2/include/functions/simd/if_else.hpp>
#include <nt2/include/functions/simd/negif.hpp>
#include <nt2/include/functions/simd/is_equal.hpp>
#include <nt2/include/functions/simd/oneplus.hpp>
#include <nt2/include/functions/simd/is_negative.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/multiplies.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION_IF(  nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1)(X)
                                  , (boost::mpl::equal_to < nt2::meta::cardinal_of<A0>
                                                        , nt2::meta::cardinal_of<A1>
                                                        >
                                )
                               , ((simd_<arithmetic_<A0>,X>))
                                 ((simd_<floating_<A1>,X>))
                                 ((simd_<floating_<A1>,X>))
                             )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a0,A1 & a1,A1 & a2) const
    {
      typedef typename meta::as_logical<A1>::type ltype;
      A1 a00 =  nt2::abs(a0);
      ltype test =  eq(a00, Inf<A1>());
      const A1 u = nt2::expm1(a00);
      const A1 up1 = oneplus(u);
      const A1 tmp =u/up1;
      a1 = negif(is_negative(a0), if_else(test, a00, Half<A1>()*tmp*(oneplus(up1))));
      a2 = if_else(test, a00, oneplus(Half<A1>()*tmp*u));
      return 0;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION_IF(nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1)(X),
                                (boost::mpl::equal_to<nt2::meta::cardinal_of<A0>,
                                                 nt2::meta::cardinal_of<A1>
                                        >
                                ),
                                ((simd_ < arithmetic_<A0>,X > ))
                                ((simd_ < floating_<A1>,X > ))
                             )
  {
    typedef A1 result_type;
    inline result_type operator()(A0 const& a0,A1 & a2) const
    {
      A1 a1;
      sinhcosh(tofloat(a0),a1, a2);
      return a1;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinhcosh_, tag::cpu_,
                         (A0)(X),
                         ((simd_<arithmetic_<A0>,X>))
                        )
  {
      typedef typename meta::as_floating<A0>::type  rtype;
      typedef boost::fusion::tuple<rtype, rtype> result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      boost::fusion::at_c<0>(res) = sinhcosh(tofloat(a0),
                                              boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


// Copyright András Vukics 2006–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "Randomized.h"
#include "Range.h"

#include <blitz/array.h>

#include <boost/assign/std/vector.hpp>

#include <boost/progress.hpp>

#include <vector>

using namespace boost::assign;

using blitz::Array;

typedef std::vector<double> Data;
typedef std::vector<int   > Size;

typedef blitz::TinyVector<int,5> Idx;

const size_t nRepeat=100000000;

class RunTimeRanked // In this form works only for Rank=5
{
public:
  static const Size calculateStrides(const Size& sizes)
  {
    Size res(1,1);
    for (Size::const_iterator i=sizes.begin(); i!=sizes.end()-1; ++i) res+=(*i)*(*--res.end());
    return res;
  }

  RunTimeRanked(const Size& sizes, const Data& data)
    : rank_(sizes.size()), data_(data), sizes_(sizes), strides_(calculateStrides(sizes)), stridesRaw_(&strides_[0]) {}

  double operator()(int i0, int i1, int i2, int i3, int i4) const     {return data_[calculateIndex(i0,i1,i2,i3,i4)]  ;}
  void   operator()(int i0, int i1, int i2, int i3, int i4, double v) {       data_[calculateIndex(i0,i1,i2,i3,i4)]=v;}
  // ... similar brute-force implementations for all ranks.

private:
  size_t calculateIndex(int i0, int i1, int i2, int i3, int i4) const {return i0*strides_[0]+i1*strides_[1]+i2*strides_[2]+i3*strides_[3]+i4*strides_[4];}
  // ... similar brute-force implementations for all ranks.

  const size_t rank_;
  Data data_;
  const Size sizes_, strides_;

  const Idx stridesRaw_;

};

template<typename A>
const A& fillRan(A& data)
{
  using namespace randomized;
  Randomized::Ptr Ran(MakerGSL()(1001));
  boost::generate(data,bind(&Randomized::operator(),Ran));
  return data;
}

int main()
{
  /////////
  // RANK=5
  /////////

  double d=0;

  { // Handcrafted
    Size size; size+=16,15,14,17,18 ;
    Data temp       (16*15*14*17*18);
    RunTimeRanked       target(size,fillRan(temp));
    const RunTimeRanked source(size,fillRan(temp));

    {
      boost::progress_timer t;
      for (size_t count=0; count<nRepeat; ++count)
        for (int j=1; j<16; ++j) {
          // target(13,4,11,j,12,source(11,6,10,j+1,11));
          d+=target(13,4,11,j-1,12);
        }
    }
  }
  { // Blitz++
    Array<double,5> target(16,15,14,17,18), source(16,15,14,17,18);
    fillRan(target); fillRan(source);

    {
      boost::progress_timer t;
      for (size_t count=0; count<nRepeat; ++count)
        for (int j=1; j<16; ++j) {
          // target(13,4,11,j,12)=source(11,6,10,j+1,11);
          d+=target(13,4,11,j-1,12);
        }

    }
  }
  std::cout<<d<<std::endl;

}


/*
    This file is part of WARG's computer-vision

    Copyright (c) 2015, Waterloo Aerial Robotics Group (WARG)
    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.
    3. Usage of this code MUST be explicitly referenced to WARG and this code
       cannot be used in any competition against WARG.
    4. Neither the name of the WARG 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 WARG ''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 WARG 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.
*/

#include "k_means_filter.h"
#include <boost/log/trivial.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <algorithm>

using namespace cv;

KMeansFilter::KMeansFilter() {
    (*this->parameters)["reductionFactor"] = 2; //beyond 2 seems to decrease effectiveness of the diff'd image; beyong 1 causes segfaults
    (*this->parameters)["clusters"] = 4;
    (*this->parameters)["attempts"] = 1;
    (*this->parameters)["noiseReduction"] = 5;
}

cv::Mat * KMeansFilter::filter(const Mat & src) {
    int kernelSize = 3;
    int reductionFactor = (*this->parameters)["reductionFactor"];
    BOOST_LOG_TRIVIAL(trace) << "Calculating kmeans...";
    BOOST_LOG_TRIVIAL(debug) << "reductionFactor = " << (*parameters)["reductionFactor"];
    BOOST_LOG_TRIVIAL(debug) << "clusters = " << (*parameters)["clusters"];
    BOOST_LOG_TRIVIAL(debug) << "attempts = " << (*parameters)["attempts"];
    BOOST_LOG_TRIVIAL(debug) << "noiseReduction = " << (*parameters)["noiseReduction"];

    Mat tmp;
    resize(src, tmp, Size(src.cols/reductionFactor, src.rows/reductionFactor));

    // Analyze colour clusters in image
    Mat samples(tmp.rows * tmp.cols, 3, CV_32F);
    for(int i = 0; i < tmp.rows * tmp.cols; i++)
	for(int z = 0; z < 3; z++)
	    samples.at<float>(i, z) = tmp.at<Vec3b>(i)[z];

    int clusterCount = (*this->parameters)["clusters"];
    Mat labels;
    int attempts = (*this->parameters)["attempts"];
    Mat centers;
    kmeans(samples, clusterCount, labels, TermCriteria(TermCriteria::MAX_ITER, 1, 1), attempts, KMEANS_PP_CENTERS, centers );

    BOOST_LOG_TRIVIAL(trace) << "Generating new image...";
    BOOST_LOG_TRIVIAL(debug) << "Src " << src.cols << "x" << src.rows << " tmp " << tmp.cols << "x" << tmp.rows;
    Mat * new_image = new Mat( src.size(), src.type() );
    BOOST_LOG_TRIVIAL(debug) << "new " << new_image->cols << "x" << new_image->rows << " Labels " << labels.rows << " " << labels.cols;

    for(int y = 0; y < src.rows; y++) {
	for(int x = 0; x < src.cols; x++) {
	    int index = x/reductionFactor + (y/reductionFactor) * tmp.cols;
	    int clusterIdx = labels.at<int>(index, 0);

	    if (clusterIdx >= 0 && clusterIdx < centers.rows) {
		new_image->at<Vec3b>(y, x)[0] = abs(src.at<Vec3b>(y, x)[0] - centers.at<float>(clusterIdx, 0));
		new_image->at<Vec3b>(y, x)[1] = abs(src.at<Vec3b>(y, x)[1] - centers.at<float>(clusterIdx, 1));
		new_image->at<Vec3b>(y, x)[2] = abs(src.at<Vec3b>(y, x)[2] - centers.at<float>(clusterIdx, 2));
	    }
	}
    }
    BOOST_LOG_TRIVIAL(trace) << "Reducing Noise...";
    int noiseReduction = (*this->parameters)["noiseReduction"];
    erode(*new_image, *new_image, noiseReduction);
    dilate(*new_image, *new_image, noiseReduction);

    /// Reduce noise with a kernel
    blur(*new_image, *new_image, Size(kernelSize,kernelSize));
    return new_image;
}


#include "particle_slam.h"
#include "robot_configuration.h"
#include "error_handling.h"
#include "occupancy_grid.inl"

#include <boost/range/algorithm/max_element.hpp>
#include <boost/range/adaptor/transformed.hpp>

#include <opencv2/imgproc.hpp>
#include <iostream>
#include <random>
#include <future>

/////////////////////
// SParticle
SParticle::SParticle()
    : m_pose(rbt::pose<double>::zero()),
    m_matLikelihood(c_nMapExtent, c_nMapExtent, CV_32FC1, cv::Scalar(0))
{}

SParticle::SParticle(SParticle const& p)
    : m_pose(p.m_pose),
     m_matLikelihood(p.m_matLikelihood.clone()),
     m_occgrid(p.m_occgrid)
{}

SParticle& SParticle::operator=(SParticle const& p) {
    m_pose = p.m_pose;
    m_matLikelihood = p.m_matLikelihood.clone();
    m_occgrid = p.m_occgrid;
    return *this;
}

void SParticle::update(SScanLine const& scanline) {
    m_pose = sample_motion_model(m_pose, scanline.translation(), scanline.rotation());

    // OPTIMIZE: Match fewer points
    m_fWeight = measurement_model_map(m_pose, scanline,
        [this](rbt::point<double> const& pt) {
            auto const ptn = ToGridCoordinate(pt);
            return static_cast<double>(m_matLikelihood.at<float>(ptn.y, ptn.x));
        });

    // OPTIMIZE: Recalculate occupancy grid after resampling?
    // OPTIMIZE: m_occgrid.update also sets occupancy of robot itself each time
    boost::for_each(scanline.m_vecscan, [&](auto const& scan) {
        m_occgrid.update(m_pose, scan.m_fRadAngle, scan.m_nDistance);
    });
    cv::distanceTransform(m_occgrid.ObstacleMap(), m_matLikelihood, CV_DIST_L2, 3);
}

///////////////////////
// SParticleSLAM
CParticleSlamBase::CParticleSlamBase(int cParticles)
    : m_vecparticle(cParticles), m_itparticleBest(m_vecparticle.end()), m_vecparticleTemp(cParticles)
{}

static std::random_device s_rd;
void CParticleSlamBase::receivedSensorData(SScanLine const& scanline) {
    // TODO: Ignore data when robot is not moving for a long time

    // if scanline full, update all particles,
    LOG("Update particles");
    LOG("t = (" << scanline.translation().x <<
        ";" << scanline.translation().y << ") "
        "r = " << scanline.rotation());

    std::vector<std::future<double>> vecfuture;
    boost::for_each(m_vecparticle, [&](SParticle& p) {
        vecfuture.emplace_back(
            std::async(std::launch::async | std::launch::deferred,
                [&] {
                    p.update(scanline);
                    return p.m_fWeight;
                }
            ));
    });

    double fWeightTotal = 0.0;
    for(int i=0; i<vecfuture.size(); ++i) {
        fWeightTotal += vecfuture[i].get();

#ifdef ENABLE_LOG
        auto const& p = m_vecparticle[i];
        LOG("Particle " << i << " -> " <<
            " pt = (" << p.m_pose.m_pt.x << "; " << p.m_pose.m_pt.y << ") " <<
            " yaw =  " << p.m_pose.m_fYaw <<
            " w = " << p.m_fWeight);
#endif
    }

    // Resampling
    // Thrun, Probabilistic robotics, p. 110
    auto const fStepSize = fWeightTotal/m_vecparticle.size();
    auto const r = std::uniform_real_distribution<double>(0.0, fStepSize)(s_rd);
    auto c = m_vecparticle.front().m_fWeight;

    auto itparticleOut = m_vecparticleTemp.begin();
    for(int i = 0, m = 0; m<m_vecparticle.size(); ++m) {
        auto const u = r + m * fStepSize;
        while(c<u) {
            ++i;
            c += m_vecparticle[i].m_fWeight;
        }
        LOG("Sample particle " << i);
        *itparticleOut = m_vecparticle[i];
        ++itparticleOut;
    }
    std::swap(m_vecparticle, m_vecparticleTemp);

    m_itparticleBest = boost::max_element(
        boost::adaptors::transform(m_vecparticle, std::mem_fn(&SParticle::m_fWeight))
    ).base();
    m_vecpose.emplace_back(m_itparticleBest->m_pose);
}

cv::Mat CParticleSlamBase::getMap() const {
    ASSERT(m_itparticleBest!=m_vecparticle.end());
    return m_itparticleBest->m_occgrid.ObstacleMapWithPoses(m_vecpose);
}


/* ----------------------------------------------------------------------------

 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/*
 * @file Unit3.h
 * @date Feb 02, 2011
 * @author Can Erdogan
 * @author Frank Dellaert
 * @author Alex Trevor
 * @brief The Unit3 class - basically a point on a unit sphere
 */

#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>

#ifdef __clang__
#  pragma clang diagnostic push
#  pragma clang diagnostic ignored "-Wunused-variable"
#endif
#include <boost/random/uniform_on_sphere.hpp>
#ifdef __clang__
#  pragma clang diagnostic pop
#endif

#include <boost/random/variate_generator.hpp>
#include <iostream>

using namespace std;

namespace gtsam {

/* ************************************************************************* */
Unit3 Unit3::FromPoint3(const Point3& point, boost::optional<Matrix&> H) {
  Unit3 direction(point);
  if (H) {
    // 3*3 Derivative of representation with respect to point is 3*3:
    Matrix D_p_point;
    point.normalize(D_p_point); // TODO, this calculates norm a second time :-(
    // Calculate the 2*3 Jacobian
    H->resize(2, 3);
    *H << direction.basis().transpose() * D_p_point;
  }
  return direction;
}

/* ************************************************************************* */
Unit3 Unit3::Random(boost::mt19937 & rng) {
  // TODO allow any engine without including all of boost :-(
  boost::uniform_on_sphere<double> randomDirection(3);
  // This variate_generator object is required for versions of boost somewhere
  // around 1.46, instead of drawing directly using boost::uniform_on_sphere(rng).
  boost::variate_generator<boost::mt19937&, boost::uniform_on_sphere<double> >
      generator(rng, randomDirection);
  vector<double> d = generator();
  Unit3 result;
  result.p_ = Point3(d[0], d[1], d[2]);
  return result;
}

/* ************************************************************************* */
const Unit3::Matrix32& Unit3::basis() const {

  // Return cached version if exists
  if (B_)
    return *B_;

  // Get the axis of rotation with the minimum projected length of the point
  Point3 axis;
  double mx = fabs(p_.x()), my = fabs(p_.y()), mz = fabs(p_.z());
  if ((mx <= my) && (mx <= mz))
    axis = Point3(1.0, 0.0, 0.0);
  else if ((my <= mx) && (my <= mz))
    axis = Point3(0.0, 1.0, 0.0);
  else if ((mz <= mx) && (mz <= my))
    axis = Point3(0.0, 0.0, 1.0);
  else
    assert(false);

  // Create the two basis vectors
  Point3 b1 = p_.cross(axis);
  b1 = b1 / b1.norm();
  Point3 b2 = p_.cross(b1);
  b2 = b2 / b2.norm();

  // Create the basis matrix
  B_.reset(Unit3::Matrix32());
  (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
  return *B_;
}

/* ************************************************************************* */
/// The print fuction
void Unit3::print(const std::string& s) const {
  cout << s << ":" << p_ << endl;
}

void Unit3::print(std::ostream& os, const std::string& s) const {
  os << s << ":" << p_ << endl;
}

/* ************************************************************************* */
Matrix Unit3::skew() const {
  return skewSymmetric(p_.x(), p_.y(), p_.z());
}

/* ************************************************************************* */
Vector Unit3::error(const Unit3& q, boost::optional<Matrix&> H) const {
  // 2D error is equal to B'*q, as B is 3x2 matrix and q is 3x1
  Matrix Bt = basis().transpose();
  Vector xi = Bt * q.p_.vector();
  if (H)
    *H = Bt * q.basis();
  return xi;
}

/* ************************************************************************* */
double Unit3::distance(const Unit3& q, boost::optional<Matrix&> H) const {
  Vector xi = error(q, H);
  double theta = xi.norm();
  if (H)
    *H = (xi.transpose() / theta) * (*H);
  return theta;
}

/* ************************************************************************* */
Unit3 Unit3::retract(const Vector& v) const {

  // Get the vector form of the point and the basis matrix
  Vector p = Point3::Logmap(p_);
  Matrix B = basis();

  // Compute the 3D xi_hat vector
  Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1);

  double xi_hat_norm = xi_hat.norm();

  // Avoid nan
  if (xi_hat_norm == 0.0) {
    if (v.norm() == 0.0)
      return Unit3(point3());
    else
      return Unit3(-point3());
  }

  Vector exp_p_xi_hat = cos(xi_hat_norm) * p
      + sin(xi_hat_norm) * (xi_hat / xi_hat_norm);
  return Unit3(exp_p_xi_hat);

}

/* ************************************************************************* */
Vector Unit3::localCoordinates(const Unit3& y) const {

  Vector p = Point3::Logmap(p_);
  Vector q = Point3::Logmap(y.p_);
  double dot = p.dot(q);

  // Check for special cases
  if (std::abs(dot - 1.0) < 1e-16)
    return (Vector(2) << 0, 0);
  else if (std::abs(dot + 1.0) < 1e-16)
    return (Vector(2) << M_PI, 0);
  else {
    // no special case
    double theta = acos(dot);
    Vector result_hat = (theta / sin(theta)) * (q - p * dot);
    return basis().transpose() * result_hat;
  }
}
/* ************************************************************************* */

}


/*
 * Copyright 2018 James Dyer
 *
 * 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.
 *
 * This is being developed for the TANGO Project: http://tango-project.eu
 */

#include <iostream>
#include <sstream>
#include <NTL/RR.h>
#include <NTL/ZZ_p.h>
#include <json/json.h>
#include "HE1Encrypter.h"

#include "Random.h"

HE1Encrypter::HE1Encrypter(int lambda, int eta)
{
	generateParameters(lambda,eta);
}

HE1Encrypter::HE1Encrypter(int n, int d, int rho)
{
	NTL::RR two(2);
	NTL::RR exp1(rho*d);
	NTL::RR nplusone(n+1);
	NTL::RR exp2(d);
	NTL::RR ploBound;
	ploBound = pow(two,exp1)*pow(nplusone,exp2);
	NTL::ZZ pLowerBound;
	conv(pLowerBound,ploBound);
	long lambda = NumBits(pLowerBound) + 1;
	long eta = ((lambda * lambda / rho) - lambda);
	generateParameters(lambda,eta);
};

NTL::ZZ_p HE1Encrypter::encrypt(NTL::ZZ& plaintext)
{
	NTL::ZZ_p r = to_ZZ_p(rng->nextBigInteger(ONE, q));
	NTL::ZZ_p ptext = to_ZZ_p(plaintext);
	return ptext+r*(*pmod);
};

NTL::ZZ& HE1Encrypter::getKey()
{
	return p;
};

std::string HE1Encrypter::writeSecretsToJSON()
{
	Json::Value root;
	std::stringstream pStr;
	pStr << p;
	root["p"] = pStr.str();
	Json::FastWriter writer;
	return writer.write(root);
};


#ifndef FSTMODELNORMAL_H
#define FSTMODELNORMAL_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    model_normal.hpp
   \brief   Implements normal (gaussian) model
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    October 2010
   \version 3.0.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz).
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <limits>
#include <cmath>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "model.hpp"
#include "indexed_vector.hpp"
#include "indexed_matrix.hpp"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

//! Implements normal (gaussian) model
template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Model_Normal : public Model<SUBSET,DATAACCESSOR> {
public:
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> PSubset;
	Model_Normal() : _classes(0), _n_max(0), _n(0), _d(0) {notify("Model_Normal (empty) constructor");}
	Model_Normal(const Model_Normal& mn); // copy-constructor
	virtual ~Model_Normal() {notify("Model_Normal destructor");}

	virtual void learn(PDataAccessor da); // learn model of full dimensionality
	virtual void learn(PDataAccessor da, const PSubset sub); // learn model of lower dimensionality

	// NOTE: the learn() methods create model structures. narrow() methods allow access to sub-space in existing learned structures
	void narrow_to(const PSubset sub); // fill _index[] -> access to data is then mapped to sub-matrix
	void denarrow(); // reset _index[] -> access is reset to the full matrix

	DIMTYPE get_n_max() const {return _n_max;}
	DIMTYPE get_n() const {return _n;}
	DIMTYPE get_d() const {return _d;}
	DIMTYPE get_classes() const {return _classes;}

	// NOTE: caller must ensure any access takes place only during _mean, _Pc and _cov existence
	REALTYPE get_Pc(const DIMTYPE c) const {assert(c>=0 && c<_classes); return _Pc[c];}
protected:
	template<class, class, class, class, class, class, class> friend class Classifier_Normal_Bayes;
	template<class, class, class, class, class, class, class> friend class Criterion_Normal_Bhattacharyya;
	template<class, class, class, class, class, class, class> friend class Criterion_Normal_GMahalanobis;
	template<class, class, class, class, class, class, class> friend class Criterion_Normal_Divergence;
	const boost::scoped_array<Indexed_Vector<REALTYPE,DIMTYPE,SUBSET> >& get_mean() const {return _mean;}
	const boost::scoped_array<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> >& get_cov() const {return _cov;}
protected:
	DIMTYPE _classes; // WARNING: int sufficient ? note expressions like _classes*_n*_n
	// NOTE: matrix size is defined by:
	//       _n_max - hard constant representing maximum allocated space of size _n_max*_n_max
	//       _n     - "full feature set" dimensionality, _n<=_n_max. If Indexed_Matrix servers as output buffer
	//                to store matrices of various (reduced) dimensionalites, this is needed for correct functionality
	//                of at(), at_raw() etc. (both "raw" and "non-raw" matrix item access methods)
	//       _d     - "virtual subset" dimensionality, _d<=_n. Assuming the matrix holds _n*_n values (_n consecutively stored
	//                rows of _n values each), a virtual sub-matrix can be defined and accessed using at() ("non-raw" methods)
	DIMTYPE _n_max;
	DIMTYPE _n; // WARNING: int sufficient ? note expressions like _classes*_n*_n
	boost::scoped_array<REALTYPE> _Pc;
	boost::scoped_array<Indexed_Vector<REALTYPE,DIMTYPE,SUBSET> > _mean;
	boost::scoped_array<Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET> > _cov;
	// NOTE: all cov[] and mean[] implement redundantly the same subspace indexing logic. It would be
	// more effective to extract the "index" array to standalone object, one instance,
	// to be shared using boost::shared_ptr<> anywhere needed ?
	DIMTYPE _d;
	// NOTE: _n and _d should be always synchronized with those inside _mean and _cov;

	// NOTE: _learn_index here is not used for narrow()ing, but for learn(da,sub)
	boost::scoped_array<DIMTYPE> _learn_index; // maps virtual sub-space _n dimensions to original _n_max dimensions
};

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Model_Normal<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Model_Normal(const Model_Normal& mn) : // copy-constructor
	Model<SUBSET,DATAACCESSOR>(mn),
	_classes(mn._classes),
	_n_max(mn._n_max),
	_n(mn._n),
	_d(mn._d)
{
	notify("Model_Normal copy-constructor.");
	if(_classes>0) {
		_Pc.reset(new REALTYPE[_classes]); memcpy((void *)_Pc.get(),(void *)(mn._Pc).get(),sizeof(REALTYPE)*_classes);
		// yes I know the default constructor calls are superfluous
		_mean.reset(new Indexed_Vector<REALTYPE,DIMTYPE,SUBSET>[_classes]);
		_cov.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>[_classes]);
		for(DIMTYPE c=0;c<_classes;c++) {_mean[c]=mn._mean[c]; _cov[c]=mn._cov[c];}
	}
	if(_n_max>0)
	{
		// to store subspace info for the to-be-learned model
		_learn_index.reset(new DIMTYPE[_n_max]); memcpy((void *)_learn_index.get(),(void *)(mn._learn_index).get(),sizeof(DIMTYPE)*_n_max);
	}
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Normal<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::learn(PDataAccessor da)
{
	assert(da);
	assert(da->getNoOfClasses()>0);
	assert(da->getNoOfFeatures()>0);
	PSubset fullset(new SUBSET(da->getNoOfFeatures()));
	fullset->select_all();
#ifdef DEBUG
	{std::ostringstream sos; sos << "fullset = "<< *fullset << std::endl; syncout::print(std::cout,sos);}
#endif
	learn(da,fullset);
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Normal<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::learn(PDataAccessor da, const PSubset sub)
{
	notify("Model_Normal::learn():");
	assert(da->getNoOfClasses()>0);
	assert(da->getNoOfFeatures()>0);
	assert(sub);
	assert(sub->get_n_raw()==da->getNoOfFeatures());
	assert(sub->get_d_raw()>0);

	if(_classes!=da->getNoOfClasses()) // || _n!=da->getNoOfFeatures())
	{
		_classes=da->getNoOfClasses();
		_mean.reset(new Indexed_Vector<REALTYPE,DIMTYPE,SUBSET>[_classes]);
		_cov.reset(new Indexed_Matrix<REALTYPE,DIMTYPE,SUBSET>[_classes]);
		_Pc.reset(new REALTYPE[_classes]);
		_n_max=0; // request re-allocation in the following
	}
	// NOTE: class priors are inefficiently estimated in each learn() call even if da does not change
	// NOTE: class priors are estimated from complete data available through da.
	//       Note that when only parts of data are processed (i.e., using data splits), the
	//       estimates below would possibly not precisely depict the relative frequency of documents
	//       in stratified class parts, esp. if class sizes are small
	IDXTYPE _allpatterns=da->getClassSizeSum(); // just to estimate class priors
	assert(_allpatterns>0);
	for(DIMTYPE c=0;c<_classes;c++) _Pc[c]=(REALTYPE)da->getClassSize(c)/(REALTYPE)_allpatterns;

	if(sub->get_n_raw()>_n_max) // insufficient buffers - reset constants and reallocate
	{
		_n_max=sub->get_n_raw();
		for(DIMTYPE c=0;c<_classes;c++) {
			_mean[c].reset(_n_max);
			_cov[c].reset(_n_max);
		}
		_learn_index.reset(new DIMTYPE[_n_max]); // to store subspace info for the to-be-learned model
	}

	_n=sub->get_d_raw(); // actual to-be-learned model dimensionality is to be the one defined by the subset defined in sub
	for(DIMTYPE c=0;c<_classes;c++) {
		_mean[c].redim(_n); _mean[c].set_all_raw_to(0.0);
		_cov[c].redim(_n); _cov[c].set_all_raw_to(0.0);
	}
	_d=_n; // access to learned model is initially not narrow()ed
	DIMTYPE f, fi=0;
	for(bool b=sub->getFirstFeature(f);b==true;b=sub->getNextFeature(f)) {_learn_index[fi++]=f;}
	assert(fi==_n);

	typename DATAACCESSOR::PPattern p;
	IDXTYPE cnt;
	assert(da->getSplitIndex()>0); // splitting loop to be maintained externally
	// mean
	for(DIMTYPE c=0;c<_classes;c++) {
		da->setClass(c);
		assert(da->getNoOfPatterns(TRAIN)>0);
#ifdef DEBUG
		{std::ostringstream sos; sos << "da->getNoOfPatterns(TRAIN) [class="<<c<<"] = "<<da->getNoOfPatterns(TRAIN)<<std::endl; syncout::print(std::cout,sos);}
#endif
		for(bool b=da->getFirstBlock(TRAIN,p,cnt);b!=false;b=da->getNextBlock(TRAIN,p,cnt)) {
			for(IDXTYPE i=0;i<cnt;i++)
				for(DIMTYPE f=0;f<_n;f++) (_mean[c])[f]+=p[i*_n_max+_learn_index[f]];
		}
		for(DIMTYPE f=0;f<_n;f++) (_mean[c])[f]/=da->getNoOfPatterns(TRAIN);
	}

#ifdef DEBUG
	{
		std::ostringstream sos; sos << "_Pc:"<<std::endl;
		for(DIMTYPE c=0;c<_classes;c++) sos << _Pc[c] << " "; sos << std::endl;
		sos << "_mean:"<<std::endl;
		for(DIMTYPE c=0;c<_classes;c++) {for(DIMTYPE f=0;f<_n;f++) sos << (_mean[c])[f] << " "; sos << std::endl;}
		syncout::print(std::cout,sos);
	}
#endif
	// cov
	for(DIMTYPE c=0;c<_classes;c++) {
		da->setClass(c);
		for(bool b=da->getFirstBlock(TRAIN,p,cnt);b!=false;b=da->getNextBlock(TRAIN,p,cnt)) {
			for(IDXTYPE i=0;i<cnt;i++) {
				for(DIMTYPE f1=0;f1<_n;f1++)
				for(DIMTYPE f2=f1;f2<_n;f2++){
					_cov[c].at_raw(f1,f2)+=(p[i*_n_max+_learn_index[f1]]-(_mean[c])[f1])*(p[i*_n_max+_learn_index[f2]]-(_mean[c])[f2]);
				}
			}
		}
	}
	// normalize and copy to lower triangle
	for(DIMTYPE c=0;c<_classes;c++) {
		da->setClass(c);
		REALTYPE pats=da->getNoOfPatterns(TRAIN)-1;
		assert(pats>0);
		if(pats<1) pats=1;
		for(DIMTYPE f1=0;f1<_n;f1++)
			for(DIMTYPE f2=0;f2<f1;f2++) {
				_cov[c].at_raw(f1,f2)=_cov[c].at_raw(f2,f1);
			}
		for(DIMTYPE f1=0;f1<_n;f1++)
			for(DIMTYPE f2=0;f2<_n;f2++) {
				_cov[c].at_raw(f1,f2)/=pats;
			}
	}

#ifdef DEBUG
	{
		std::ostringstream sos; sos << "_cov:"<<std::endl;
		for(DIMTYPE c=0;c<_classes;c++) {for(DIMTYPE f1=0;f1<_n;f1++) {for(DIMTYPE f2=0;f2<_n;f2++) sos << _cov[c].at_raw(f1,f2) << " "; sos << std::endl;} sos << std::endl;}
		syncout::print(std::cout,sos);
	}
#endif
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Normal<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::narrow_to(const PSubset sub)
{
	assert(_mean);
	assert(_cov);
	assert(sub->get_n_raw()==_n); // NOTE: sub should represent subspace of the learned subspace ! not of the full space
	assert(sub->get_d_raw()>0);
	for(DIMTYPE c=0;c<_classes;c++) {
		_mean[c].narrow_to(sub);
		_cov[c].narrow_to(sub);
	}
	_d=sub->get_d_raw();
}

template<typename DATATYPE, typename REALTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Model_Normal<DATATYPE,REALTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::denarrow()
{
	assert(_mean);
	assert(_cov);
	for(DIMTYPE c=0;c<_classes;c++) {
		_mean[c].denarrow();
		_cov[c].denarrow();
	}
	_d=_n;
}

} // namespace
#endif // FSTMODELNORMAL_H ///:~


/*=============================================================================
    Copyright (c) 2001-2010 Joel de Guzman

    Distributed under 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)
=============================================================================*/
///////////////////////////////////////////////////////////////////////////////
//
//  Yet another calculator example! This time, we will compile to a simple
//  virtual machine. This is actually one of the very first Spirit example
//  circa 2000. Now, it's ported to Spirit2.
//
//  [ JDG Sometime 2000 ]       pre-boost
//  [ JDG September 18, 2002 ]  spirit1
//  [ JDG April 8, 2007 ]       spirit2
//
///////////////////////////////////////////////////////////////////////////////

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_container.hpp>
#include <boost/spirit/include/phoenix_statement.hpp>
#include <boost/spirit/include/phoenix_object.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>

#include <iostream>
#include <string>
#include <vector>

namespace client
{
    namespace qi = boost::spirit::qi;
    namespace phoenix = boost::phoenix;
    namespace ascii = boost::spirit::ascii;

    ///////////////////////////////////////////////////////////////////////////
    //  The Virtual Machine
    ///////////////////////////////////////////////////////////////////////////
    enum byte_code
    {
        op_neg,     //  negate the top stack entry
        op_add,     //  add top two stack entries
        op_sub,     //  subtract top two stack entries
        op_mul,     //  multiply top two stack entries
        op_div,     //  divide top two stack entries
        op_int,     //  push constant integer into the stack
    };

    class vmachine
    {
    public:

        vmachine(unsigned stackSize = 4096)
          : stack(stackSize)
          , stack_ptr(stack.begin())
        {
        }

        int top() const { return stack_ptr[-1]; };
        void execute(std::vector<int> const& code);

    private:

        std::vector<int> stack;
        std::vector<int>::iterator stack_ptr;
    };

    void vmachine::execute(std::vector<int> const& code)
    {
        std::vector<int>::const_iterator pc = code.begin();
        stack_ptr = stack.begin();

        while (pc != code.end())
        {
            switch (*pc++)
            {
                case op_neg:
                    stack_ptr[-1] = -stack_ptr[-1];
                    break;

                case op_add:
                    --stack_ptr;
                    stack_ptr[-1] += stack_ptr[0];
                    break;

                case op_sub:
                    --stack_ptr;
                    stack_ptr[-1] -= stack_ptr[0];
                    break;

                case op_mul:
                    --stack_ptr;
                    stack_ptr[-1] *= stack_ptr[0];
                    break;

                case op_div:
                    --stack_ptr;
                    stack_ptr[-1] /= stack_ptr[0];
                    break;

                case op_int:
                    *stack_ptr++ = *pc++;
                    break;
            }
        }
    }

    ///////////////////////////////////////////////////////////////////////////
    //  Our calculator grammar and compiler
    ///////////////////////////////////////////////////////////////////////////
    template <typename Iterator>
    struct calculator : qi::grammar<Iterator, ascii::space_type>
    {
        calculator(std::vector<int>& code)
          : calculator::base_type(expression)
          , code(code)
        {
            using namespace qi::labels;
            using qi::uint_;
            using qi::on_error;
            using qi::fail;

            using phoenix::val;
            using phoenix::ref;
            using phoenix::push_back;
            using phoenix::construct;

            expression =
                term
                >> *(   ('+' > term             [push_back(ref(code), op_add)])
                    |   ('-' > term             [push_back(ref(code), op_sub)])
                    )
                ;

            term =
                factor
                >> *(   ('*' > factor           [push_back(ref(code), op_mul)])
                    |   ('/' > factor           [push_back(ref(code), op_div)])
                    )
                ;

            factor =
                uint_                           [
                                                    push_back(ref(code), op_int),
                                                    push_back(ref(code), _1)
                                                ]
                |   '(' > expression > ')'
                |   ('-' > factor               [push_back(ref(code), op_neg)])
                |   ('+' > factor)
                ;

            expression.name("expression");
            term.name("term");
            factor.name("factor");

            on_error<fail>
            (
                expression
              , std::cout
                    << val("Error! Expecting ")
                    << _4                               // what failed?
                    << val(" here: \"")
                    << construct<std::string>(_3, _2)   // iterators to error-pos, end
                    << val("\"")
                    << std::endl
            );
        }

        qi::rule<Iterator, ascii::space_type> expression, term, factor;
        std::vector<int>& code;
    };

    template <typename Grammar>
    bool compile(Grammar const& calc, std::string const& expr)
    {
        std::string::const_iterator iter = expr.begin();
        std::string::const_iterator end = expr.end();
        bool r = phrase_parse(iter, end, calc, ascii::space);

        if (r && iter == end)
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing succeeded\n";
            std::cout << "-------------------------\n";
            return true;
        }
        else
        {
            std::cout << "-------------------------\n";
            std::cout << "Parsing failed\n";
            std::cout << "-------------------------\n";
            return false;
        }
    }
}

///////////////////////////////////////////////////////////////////////////////
//  Main program
///////////////////////////////////////////////////////////////////////////////
int
main()
{
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Expression parser...\n\n";
    std::cout << "/////////////////////////////////////////////////////////\n\n";
    std::cout << "Type an expression...or [q or Q] to quit\n\n";

    typedef std::string::const_iterator iterator_type;
    typedef client::calculator<iterator_type> calculator;

    client::vmachine mach;          //  Our virtual machine
    std::vector<int> code;          //  Our VM code
    calculator calc(code);          //  Our grammar

    std::string str;
    while (std::getline(std::cin, str))
    {
        if (str.empty() || str[0] == 'q' || str[0] == 'Q')
            break;

        code.clear();
        if (client::compile(calc, str))
        {
            mach.execute(code);
            std::cout << "\n\nresult = " << mach.top() << std::endl;
            std::cout << "-------------------------\n\n";
        }
    }

    std::cout << "Bye... :-) \n\n";
    return 0;
}


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

// Copyright (c) 2019 Tinko Bartels, Berlin, Germany.

// 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_EXTENSIONS_RANDOM_STRATEGIES_CARTESIAN_UNIFORM_POINT_DISTRIBUTION_SEGMENT_HPP
#define BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_CARTESIAN_UNIFORM_POINT_DISTRIBUTION_SEGMENT_HPP

#include <random>
#include <iterator>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/iterators/segment_iterator.hpp>
#include <boost/geometry/arithmetic/arithmetic.hpp>

#include <boost/geometry/extensions/random/strategies/uniform_point_distribution.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace uniform_point_distribution {

template
<
    typename Point,
    typename DomainGeometry
>
struct segment_linear_mapping_distribution
{
    segment_linear_mapping_distribution(DomainGeometry const& g) {}
    bool equals(DomainGeometry const& l_domain,
                DomainGeometry const& r_domain,
                segment_linear_mapping_distribution const& r_strategy) const
    {
        return boost::geometry::equals(l_domain.domain(), r_domain.domain());
    }

    template
    <
        typename LengthType,
        typename PointIn
    >
    static Point map(PointIn const& p1, PointIn const& p2, LengthType const& l)
    {
        Point r;
        assign(r, p2);
        subtract_point(r, p1);
        multiply_value(r, l);
        add_point(r, p1);
        return r;
    }

    template<typename Gen>
    Point apply(Gen& g, DomainGeometry const& d)
    {
        typedef typename select_most_precise
            <
                typename coordinate_type<DomainGeometry>::type,
                double
            >::type sample_type;
        std::uniform_real_distribution<sample_type> real_dist(0, 1);
        typedef typename default_length_result<DomainGeometry>::type
            length_type;
        boost::geometry::segment_view<DomainGeometry> view(d);
        return map<length_type>(*view.begin(), *(view.begin() + 1), real_dist(g));
    }
    void reset(DomainGeometry const&) {};
};

namespace services {

template
<
    typename Point,
    typename DomainGeometry,
    int Dim
>
struct default_strategy
<
    Point,
    DomainGeometry,
    segment_tag,
    single_tag,
    Dim,
    cartesian_tag
> : public segment_linear_mapping_distribution<Point, DomainGeometry> {
    typedef segment_linear_mapping_distribution<Point, DomainGeometry> base;
    using base::base;
};

} // namespace services

}} // namespace strategy::uniform_point_distribution

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_RANDOM_STRATEGIES_CARTESIAN_UNIFORM_POINT_DISTRIBUTION_SEGMENT_HPP


/*! EXAMPLE
	shows calculation of ECP integrals and first derivatives for HI
	in a cc-pVDZ(-PP) basis, using ECP28MDF

	USAGE
	./example [LIBECPINT_SHARE_DIR_PATH]
 */

#include "libecpint.hpp"

// if you want to try out the Eigen matrix build
#ifdef _WITH_EIGEN
	#include <Eigen/Dense>
	#include <Eigen/Core>
#endif

#include <iostream>
#include <fstream>
#include <string>
#include <vector>
#include <array>
#include <algorithm>
#include <iterator>

using DVec = std::vector<double>;
using IVec = std::vector<int>;
using Coord = std::array<double, 3>;

void read_basis_file(std::string, DVec&, DVec&, DVec&, IVec&, IVec&, Coord&);

int main(int argc, char* argv[]) {
	using namespace libecpint;

	// this code expects the path to the libecpint/share directory
	// to be passed as an argument to the executable
	std::string share_dir = argv[1];

	// Roughly equilibrium position HI molecule
	// NOTE: coordinate values must be in BOHR
	Coord H_pos = {0.0, 0.0, 0.0};
	Coord I_pos = {0.0, 0.0, 3.0};

	// Let's read in the GTO basis from file
	// store in standard vectors
	DVec g_exps, g_coeffs; // length = total number of primitives in basis
	IVec g_ams, g_lens; // length = total number of shells in basis
	DVec g_coords; // length = 3*total number of shells

	// hydrogen cc-pVDZ
	read_basis_file("hydrogen.bas", g_exps, g_coeffs, g_coords, g_ams, g_lens, H_pos);
	// iodine cc-pVDZ-PP
	read_basis_file("iodine.bas", g_exps, g_coeffs,  g_coords, g_ams, g_lens, I_pos);
	// check sizes, should read 'Basis read: 44, 12, 36'
	std::cout << "Basis read: " << g_exps.size() << ", " << g_ams.size() << ", " << g_coords.size() << std::endl;

	// Now to perform a calculation using the high-level API we do the following:
	ECPIntegrator factory; // object used to build ECP integral matrices

	// Set the orbital basis sets
	factory.set_gaussian_basis(g_ams.size(), g_coords.data(), g_exps.data(), g_coeffs.data(), g_ams.data(), g_lens.data());
	// put an ECP on the iodine (charge = 53, name="ecp28mdf")
	std::vector<std::string> names = {"ecp28mdf"};
	int charges[1] = {53};
	factory.set_ecp_basis_from_library(1, I_pos.data(), charges, names, share_dir);

	// initialise - we want integrals and derivatives, so deriv_order=1
	factory.init(1);

	// and compute both the integrals and derivatives
	std::cout << "Computing integrals..." << std::endl;
	factory.compute_integrals();
	std::cout << "Computing first derivs..." << std::endl;
	factory.compute_first_derivs();
	std::cout << "Done." << std::endl;

	// we can now access the integrals
	// as an example, let's build them into an
	// eigen matrix
#ifdef _WITH_EIGEN
	 // grab the integrals
	 std::shared_ptr<DVec> ints = factory.get_integrals();

	 // map into a square matrix
	 // note that we use ROW-MAJOR ORDERING
	 // 12 basis functions
	 Eigen::MatrixXd ecpints = Eigen::Map<Eigen::Matrix<double, 12, 12, Eigen::RowMajor>>(ints->data());
	 std::cout << "ECP Integrals:" << std::endl;
	 std::cout << ecpints << std::endl;

	 // we could do the same with derivatives
	 // this returns pointers in order [Hx, Hy, Hz, Ix, Iy, Iz]
	 std::vector<std::shared_ptr<DVec>> derivs = factory.get_first_derivs();
	 // e.g. the Iodine y-derivative
	 Eigen::MatrixXd iodine_y_derivs = Eigen::Map<Eigen::Matrix<double, 12, 12, Eigen::RowMajor>>(derivs[4]->data());
	 std::cout << "y-derivs on iodine:" << std::endl;
	 std::cout << iodine_y_derivs << std::endl;
#endif

	return 0;
}

/*! Reads in the Gaussian orbital basis from a file.

	filename - the path/name of the basis file
	exps - vector to place exponents in
	coeffs - vector to place coefficients in
	coords - the vector of xyz coords for each shell
	ams - vector to place angular momenta in
	lens - vector to place shell lengths in
	atom - the position of the atom being read in
 */
void read_basis_file(std::string filename, DVec& exps, DVec& coeffs, DVec& coords, IVec& ams, IVec& lens, Coord& atom) {
	std::ifstream input_file(filename);
	if (input_file.is_open()) {
		// We expect the file to have the format
		// L; x, c; x, c; x, c; x, c;
		// with a line for each shell
		std::string line, token;
		while (!input_file.eof()) {
			std::getline(input_file, line);

			// split the line by semicolon
			size_t sc_pos = line.find(';');
			int len = -1;
			int am;
			while (sc_pos != std::string::npos) {
				token = line.substr(0, sc_pos);
				line.erase(0, sc_pos+1);

				// First bit is L
				if (len == -1) {
					am = std::stoi(token);
					len++;
				} else {
					// subsequent bits are x,c
					size_t c_pos = token.find(',');
					if (c_pos != std::string::npos) {
						exps.push_back(std::stod(token.substr(0, c_pos)));
						coeffs.push_back(std::stod(token.substr(c_pos+1, token.length())));
						len++;
					}
				}
				sc_pos = line.find(';');
			}
			if (len > 0) {
				// non-empty shell found
				ams.push_back(am);
				lens.push_back(len);
				std::copy(atom.begin(), atom.end(), std::back_inserter(coords));
			}
		}
	}
}


#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/array_family/versa_matrix.h>
#include <scitbx/matrix/outer_product.h>
#include <scitbx/matrix/norms.h>
#include <scitbx/matrix/move.h>
#include <scitbx/matrix/matrix_vector_operations.h>
#include <scitbx/array_family/boost_python/packed_to_flex_conversions.h>
#include <boost/python/args.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost_adaptbx/std_pair_conversion.h>

namespace scitbx { namespace af {

namespace {

  void
  exercise_packed_u_accessor()
  {
    versa<double, packed_u_accessor>
      a(packed_u_accessor(5));
    ref<double, packed_u_accessor> r = a.ref();
    SCITBX_ASSERT(a.size() == 5*(5+1)/2);
    SCITBX_ASSERT(r.size() == 5*(5+1)/2);
    SCITBX_ASSERT(a.accessor().n == 5);
    SCITBX_ASSERT(r.accessor().n == 5);
    for(unsigned i=0;i<r.size();i++) r[i] = i+1;
    unsigned v = 1;
    for(unsigned i=0;i<r.accessor().n;i++) {
      for(unsigned j=i;j<r.accessor().n;j++,v++) {
        SCITBX_ASSERT(a(i,j) == v);
        SCITBX_ASSERT(r(i,j) == v);
      }
    }
  }

  versa<double, packed_u_accessor> exercise_versa_packed_u_to_flex() {
    versa<double, packed_u_accessor> result(3);
    for (int i=0; i<3; ++i) for (int j=i; j<3; ++j) result(i,j) = 10*(i+1) + j+1;
    return result;
  }

  bool
  is_square_matrix(
    versa<double, flex_grid<> > const& self)
  {
    return self.accessor().is_square_matrix();
  }

} // namespace <anonymous>

namespace boost_python {

  versa<double, c_grid<2> >
  matrix_multiply_real_matrix_real_matrix(
    const_ref<double, c_grid<2> > const& a,
    const_ref<double, c_grid<2> > const& b)
  {
    return matrix_multiply(a, b);
  }

  versa<std::complex<double>, c_grid<2> >
  matrix_multiply_real_matrix_complex_matrix(
    const_ref<double, c_grid<2> > const& a,
    const_ref<std::complex<double>, c_grid<2> > const& b)
  {
    return matrix_multiply(a, b);
  }

  versa<double, c_grid<2> >
  matrix_transpose_multiply_real_matrix_real_matrix(
    const_ref<double, c_grid<2> > const& a,
    const_ref<double, c_grid<2> > const& b)
  {
    return matrix_transpose_multiply(a, b);
  }

  versa<double, c_grid<2> >
  matrix_multiply_transpose_real_matrix_real_matrix(
    const_ref<double, c_grid<2> > const& a,
    const_ref<double, c_grid<2> > const& b)
  {
    return matrix_multiply_transpose(a, b);
  }

  versa<double, c_grid<2> >
  matrix_multiply_packed_u_real_matrix_real_u(
    const_ref<double, c_grid<2> > const& a,
    const_ref<double> const& b)
  {
    return matrix_multiply_packed_u(a, b);
  }

  versa<std::complex<double>, c_grid<2> >
  matrix_multiply_packed_u_real_matrix_complex_u(
    const_ref<double, c_grid<2> > const& a,
    const_ref<std::complex<double> > const& b)
  {
    return matrix_multiply_packed_u(a, b);
  }

  shared<double>
  matrix_multiply_packed_u_multiply_lhs_transpose_real_matrix_real_u(
    const_ref<double, c_grid<2> > const& a,
    const_ref<double> const& b)
  {
    return matrix_multiply_packed_u_multiply_lhs_transpose(a, b);
  }

  shared<std::complex<double> >
  matrix_multiply_packed_u_multiply_lhs_transpose_real_matrix_complex_u(
    const_ref<double, c_grid<2> > const& a,
    const_ref<std::complex<double> > const& b)
  {
    return matrix_multiply_packed_u_multiply_lhs_transpose(a, b);
  }

  double (*matrix_norm_1)(const_ref<double, mat_grid> const &) = matrix::norm_1;

  double matrix_symmetric_upper_triangle_quadratic_form(
    const_ref<double, packed_u_accessor> const &q,
    const_ref<double> const &x)
  {
    SCITBX_ASSERT(q.n_columns() == x.size());
    return matrix::quadratic_form_packed_u(x.size(), q.begin(), x.begin());
  }

  void
  wrap_flex_double_matrix(
    flex_wrapper<double>::class_f_t& class_f_t)
  {
    exercise_packed_u_accessor();
    default_packed_flex_conversions<double>();
    boost::python::def("exercise_versa_packed_u_to_flex",
                       exercise_versa_packed_u_to_flex);

    using namespace boost::python;
    using boost::python::arg;

    boost_adaptbx::std_pair_conversions::to_tuple<shared<double>,
                                                  shared<double> >();

    class_f_t
      .def("is_square_matrix", is_square_matrix)
      .def("matrix_diagonal",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&)) matrix_diagonal)
      .def("matrix_upper_bidiagonal", matrix_upper_bidiagonal<double>)
      .def("matrix_lower_bidiagonal", matrix_lower_bidiagonal<double>)
      .def("matrix_diagonal_set_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&,
          double const&)) matrix_diagonal_set_in_place,
            arg("value"))
      .def("matrix_diagonal_set_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&,
          const_ref<double> const&)) matrix_diagonal_set_in_place,
            arg("diagonal"))
      .def("matrix_diagonal_add_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&,
          double const&)) matrix_diagonal_add_in_place,
            arg("value"))
      .def("matrix_diagonal_sum",
        (double(*)(
          const_ref<double, c_grid<2> > const&)) matrix_diagonal_sum)
      .def("matrix_trace",
        (double(*)(
          const_ref<double, c_grid<2> > const&)) matrix_diagonal_sum)
      .def("matrix_diagonal_product",
        (double(*)(
          const_ref<double, c_grid<2> > const&)) matrix_diagonal_product)
      .def("matrix_norm_1", matrix_norm_1)
      .def("matrix_norm_inf", matrix::norm_inf<double>)
      .def("matrix_norm_frobenius", matrix::norm_frobenius<double>)
      .def("matrix_multiply", matrix_multiply_real_matrix_real_matrix)
      .def("matrix_multiply", matrix_multiply_real_matrix_complex_matrix)
      .def("matrix_transpose_multiply",
        matrix_transpose_multiply_real_matrix_real_matrix)
      .def("matrix_multiply_transpose",
        matrix_multiply_transpose_real_matrix_real_matrix)
      .def("matrix_multiply",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&,
          const_ref<double> const&)) matrix_multiply)
      .def("matrix_multiply",
        (shared<double>(*)(
          const_ref<double> const&,
          const_ref<double, c_grid<2> > const&)) matrix_multiply)
      .def("matrix_multiply",
        (double(*)(
          const_ref<double> const&,
          const_ref<double> const&)) matrix_multiply)
      .def("dot",
        (double(*)(
          const_ref<double> const&,
          const_ref<double> const&)) matrix_multiply)
      .def("matrix_multiply_packed_u",
        matrix_multiply_packed_u_real_matrix_real_u)
      .def("matrix_multiply_packed_u",
        matrix_multiply_packed_u_real_matrix_complex_u)
      .def("matrix_multiply_packed_u_multiply_lhs_transpose",
        matrix_multiply_packed_u_multiply_lhs_transpose_real_matrix_real_u, (
          arg("packed_u")))
      .def("matrix_multiply_packed_u_multiply_lhs_transpose",
        matrix_multiply_packed_u_multiply_lhs_transpose_real_matrix_complex_u,(
          arg("packed_u")))
      .def("matrix_transpose_multiply_as_packed_u",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&))
            matrix_transpose_multiply_as_packed_u)
      .def("matrix_transpose_multiply_diagonal_multiply_as_packed_u",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&,
          const_ref<double> const&))
            matrix_transpose_multiply_diagonal_multiply_as_packed_u, (
              arg("diagonal_elements")))
      .def("matrix_transpose",
        (versa<double, c_grid<2> >(*)(
           const_ref<double, c_grid<2> > const&)) matrix_transpose)
      .def("matrix_transpose_in_place",
        (void(*)(versa<double, flex_grid<> >&)) matrix_transpose_in_place)
      .def("matrix_rot90",
        (versa<double, c_grid<2> >(*)(
           const_ref<double, c_grid<2> > const&, int)) matrix_rot90)
      .def("matrix_outer_product",
        (versa<double, c_grid<2> >(*)(
           const_ref<double> const&,
           const_ref<double> const&)) matrix::outer_product, (arg("rhs")))
      .def("matrix_outer_product",
        (versa<double, c_grid<2> >(*)(
           const_ref<double> const&,
           double const&,
           int)) matrix::outer_product, (arg("rhs"), arg("rhs_size")=-1))
      .def("matrix_lu_decomposition_in_place",
        (shared<std::size_t>(*)(
          ref<double, c_grid<2> > const&)) matrix_lu_decomposition_in_place)
      .def("matrix_lu_back_substitution",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&,
          const_ref<std::size_t> const&,
          const_ref<double> const&)) matrix_lu_back_substitution, (
        arg("pivot_indices"), arg("b")))
      .def("matrix_forward_substitution", matrix_forward_substitution<double>,
           (arg("l"), arg("b"), arg("unit_diag")=false))
      .def("matrix_back_substitution", matrix_back_substitution<double>,
           (arg("u"), arg("b"), arg("unit_diag")=false))
      .def("matrix_forward_substitution_given_transpose",
           matrix_forward_substitution_given_transpose<double>,
           (arg("u"), arg("b"), arg("unit_diag")=false))
      .def("matrix_back_substitution_given_transpose",
           matrix_back_substitution_given_transpose<double>,
           (arg("l"), arg("b"), arg("unit_diag")=false))
      .def("matrix_determinant_via_lu",
        (double(*)(
          const_ref<double, c_grid<2> > const&,
          const_ref<std::size_t> const&)) matrix_determinant_via_lu, (
        arg("pivot_indices")))
      .def("matrix_determinant_via_lu",
        (double(*)(
          const_ref<double, c_grid<2> > const&)) matrix_determinant_via_lu)
      .def("matrix_inversion_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&,
          ref<double, c_grid<2> > const&)) matrix_inversion_in_place, (
        arg("b")))
      .def("matrix_inversion_in_place",
        (void(*)(ref<double, c_grid<2> > const&)) matrix_inversion_in_place)
      .def("matrix_upper_triangle_as_packed_u",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&))
            matrix::upper_triangle_as_packed_u)
      .def("matrix_packed_u_as_upper_triangle",
        (versa<double, c_grid<2> >(*)(
          const_ref<double> const&))
            matrix::packed_u_as_upper_triangle)
      .def("matrix_lower_triangle_as_packed_l",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&))
            matrix::lower_triangle_as_packed_l)
      .def("matrix_packed_l_as_lower_triangle",
        (versa<double, c_grid<2> >(*)(
          const_ref<double> const&))
            matrix::packed_l_as_lower_triangle)
      .def("matrix_symmetric_as_packed_u",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&, double const&))
            matrix::symmetric_as_packed_u, (
              arg("relative_epsilon")=1e-12))
      .def("matrix_symmetric_as_packed_l",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&, double const&))
            matrix::symmetric_as_packed_l, (
              arg("relative_epsilon")=1e-12))
      .def("matrix_is_symmetric",
        (bool(*)(
          const_ref<double, c_grid<2> > const&, double const&))
            matrix::is_symmetric, ((
              arg("relative_epsilon"))))
      .def("matrix_packed_u_as_symmetric",
        (versa<double, c_grid<2> >(*)(
          const_ref<double> const&))
            matrix::packed_u_as_symmetric)
      .def("matrix_packed_l_as_symmetric",
        (versa<double, c_grid<2> >(*)(
          const_ref<double> const&))
            matrix::packed_l_as_symmetric)
      .def("matrix_packed_u_diagonal",
        (shared<double>(*)(
          const_ref<double> const&))
            matrix::packed_u_diagonal)
      .def("matrix_packed_u_diagonal_add_in_place",
           (void (*)(ref<double> const &, double))
            matrix::packed_u_diagonal_add_in_place)
      .def("matrix_packed_u_diagonal_add_in_place",
           (void (*)(ref<double> const &, const_ref<double> const &))
            matrix::packed_u_diagonal_add_in_place)
      .def("matrix_copy_upper_to_lower_triangle_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&))
            matrix::copy_upper_to_lower_triangle_in_place)
      .def("matrix_copy_lower_to_upper_triangle_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&))
            matrix::copy_lower_to_upper_triangle_in_place)
      .def("matrix_copy_column",
        (shared<double>(*)(
          const_ref<double, c_grid<2> > const&,
          unsigned))
            matrix::copy_column, (
              arg("i_column")))
      .def("matrix_copy_block",
        (versa<double, c_grid<2> >(*)(
          const_ref<double, c_grid<2> > const&,
          unsigned, unsigned, unsigned, unsigned))
            matrix::copy_block, (
              arg("i_row"),
              arg("i_column"),
              arg("n_rows"),
              arg("n_columns")))
      .def("matrix_paste_block_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&,
          const_ref<double, c_grid<2> > const&,
          unsigned, unsigned))
            matrix::paste_block_in_place, (
              arg("block"),
              arg("i_row"),
              arg("i_column")))
      .def("matrix_paste_column_in_place",
           matrix::paste_column_in_place<double>,
           (arg("column"), "i_column"))
      .def("matrix_copy_upper_triangle", matrix::copy_upper_triangle<double>)
      .def("matrix_copy_lower_triangle", matrix::copy_lower_triangle<double>)
      .def("matrix_swap_rows_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&, unsigned, unsigned))
            matrix::swap_rows_in_place, (
              arg("i"),
              arg("j")))
      .def("matrix_swap_columns_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&, unsigned, unsigned))
            matrix::swap_columns_in_place, (
              arg("i"),
              arg("j")))
      .def("matrix_symmetric_upper_triangle_swap_rows_and_columns_in_place",
        (void(*)(
          ref<double, c_grid<2> > const&, unsigned, unsigned))
            matrix::symmetric_upper_triangle_swap_rows_and_columns_in_place, (
              arg("i"),
              arg("j")))
      .def("matrix_symmetric_upper_triangle_quadratic_form",
           matrix_symmetric_upper_triangle_quadratic_form)
      .def("matrix_packed_u_swap_rows_and_columns_in_place",
        (void(*)(
          ref<double> const&, unsigned, unsigned))
            matrix::packed_u_swap_rows_and_columns_in_place, (
              arg("i"),
              arg("j")))
      .def("cos_angle",
        (boost::optional<double>(*)(
          const_ref<double> const&,
          const_ref<double> const&)) cos_angle, (
        arg("b")))
      .def("cos_angle",
        (double(*)(
          const_ref<double> const&,
          const_ref<double> const&,
          const double&)) cos_angle, (
        arg("b"), arg("value_if_undefined")))
      .def("angle",
        (boost::optional<double>(*)(
          const_ref<double> const&,
          const_ref<double> const&)) angle, (
        arg("b")))
      .def("angle",
        (boost::optional<double>(*)(
          const_ref<double> const&,
          const_ref<double> const&,
          bool)) angle, (
        arg("b"), arg("deg")=false))
    ;
  }

}}} // namespace scitbx::af::boost_python


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

#include <boost/simd/ieee/include/functions/ldexp.hpp>
#include <boost/simd/include/functions/dec.hpp>
#include <boost/simd/include/constants/limitexponent.hpp>
#include <boost/simd/include/constants/minexponent.hpp>
#include <boost/simd/include/constants/halfeps.hpp>
#include <boost/simd/include/constants/smallestposval.hpp>
#include <boost/simd/include/constants/valmax.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/four.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL(ldexp_real__2_0, BOOST_SIMD_REAL_TYPES)
{

  using boost::simd::ldexp;
  using boost::simd::tag::ldexp_;
  using boost::simd::dec;
  typedef typename boost::dispatch::meta::as_integer<T>::type iT;
  typedef typename boost::dispatch::meta::call<ldexp_(T,iT)>::type r_t;
  typedef T wished_r_t;

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

#ifndef BOOST_SIMD_NO_INVALIDS
  NT2_TEST_EQUAL(ldexp(boost::simd::Inf<T>(),  2), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Minf<T>(), 2), boost::simd::Minf<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Nan<T>(),  2), boost::simd::Nan<r_t>());
#endif
  NT2_TEST_EQUAL(ldexp(boost::simd::Mone<T>(), 2), -boost::simd::Four<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::One<T>(),  2), boost::simd::Four<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Zero<T>(), 2), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::One <T>(), boost::simd::Minexponent<T>()), boost::simd::Smallestposval<r_t>());
  NT2_TEST_EQUAL(ldexp(boost::simd::One<T>()-boost::simd::Halfeps<T>(),  boost::simd::Maxexponent<T>()), boost::simd::Valmax<T>()/2);
  NT2_TEST_EQUAL(ldexp(boost::simd::One<T>()-boost::simd::Halfeps<T>(),  boost::simd::Limitexponent<T>()), boost::simd::Valmax<T>());
#ifndef BOOST_SIMD_NO_DENORMALS
  NT2_TEST_EQUAL(ldexp(boost::simd::One <T>(), dec(boost::simd::Minexponent<T>())), boost::simd::Smallestposval<T>()/2);
  NT2_TEST_EQUAL(ldexp(boost::simd::Two <T>(), dec(boost::simd::Minexponent<T>())), boost::simd::Smallestposval<T>());
  NT2_TEST_EQUAL(ldexp(boost::simd::Two <T>(), dec(boost::simd::Minexponent<T>()-1)), boost::simd::Smallestposval<T>()/2);
  NT2_TEST_EQUAL(ldexp(boost::simd::One <T>(), boost::simd::Minexponent<T>()-5), boost::simd::Smallestposval<T>()/32);
#endif
}


// https://www.boost.org/doc/libs/1_67_0/doc/html/atomic/usage_examples.html#boost_atomic.usage_examples.example_ringbuffer

#include <boost/atomic.hpp>

template <typename T, size_t Size>
class ringbuffer
{
  public:
    ringbuffer() : head_(0), tail_(0) {}

    bool push(const T &value)
    {
        size_t head = head_.load(boost::memory_order_relaxed);
        size_t next_head = next(head);
        if (next_head == tail_.load(boost::memory_order_acquire))
            return false;
        ring_[head] = value;
        head_.store(next_head, boost::memory_order_release);
        return true;
    }
    bool pop(T &value)
    {
        size_t tail = tail_.load(boost::memory_order_relaxed);
        if (tail == head_.load(boost::memory_order_acquire))
            return false;
        value = ring_[tail];
        tail_.store(next(tail), boost::memory_order_release);
        return true;
    }

  private:
    size_t next(size_t current)
    {
        return (current + 1) % Size;
    }
    T ring_[Size];
    boost::atomic<size_t> head_, tail_;
};

int main()
{
    ringbuffer<int, 32> r;

    // try to insert an element
    if (r.push(42))
    { /* succeeded */
    }
    else
    { /* buffer full */
    }

    // try to retrieve an element
    int value;
    if (r.pop(value))
    { /* succeeded */
    }
    else
    { /* buffer empty */
    }
}


#include <falcon/templates.hpp>
#include "test.hpp"
#include "templates.hpp"

#if __cplusplus >= 201103L
#include <falcon/c++1x/syntax.hpp>
#include <falcon/functional/operators.hpp>

template<typename _T, typename _U>
struct static_value
{
	static const bool value = 1;
};

void templates_test()
{
	STATIC_CHECK_VALUE(true, falcon::template_or<std::is_pointer, std::is_integral>::filter<int>);
	STATIC_CHECK_VALUE(false, falcon::template_and<std::is_pointer, std::is_integral>::filter<int>);
	STATIC_CHECK_VALUE(true, falcon::template_or<static_value>::filter<int, int>);
	STATIC_CHECK_VALUE(false, falcon::template_not<falcon::template_or<static_value>::filter>::filter<int, int>);
	STATIC_CHECK_VALUE(false, falcon::template_binary<falcon::equal_to<>, std::is_pointer, std::is_integral>::value_wrapper<int>);
	STATIC_CHECK_VALUE(true, falcon::template_unary<falcon::logical_not<>, std::is_pointer>::value_wrapper<int>);
	STATIC_CHECK_VALUE(true, falcon::template_left_accumulator<falcon::template_or<static_value>::filter, int>::value_wrapper<int>);
	STATIC_CHECK_VALUE(true, falcon::template_right_accumulator<falcon::template_or<static_value>::filter, int>::value_wrapper<int>);
	STATIC_CHECK_VALUE(6, falcon::template_size<int,int,int,int,int,int>);
	STATIC_CHECK_VALUE(15u, falcon::template_binary<falcon::plus<>, falcon::template_size, falcon::template_val<std::size_t, 10>::filter>::value_wrapper<int,int,int,int,int>);

	STATIC_CHECK_VALUE(1, falcon::template_to_val<std::rank>::filter<int[]>);

	CHECK_EQUAL_VALUE(6u, falcon::template_binary<
		falcon::multiplies<>,
		falcon::template_to_val<
			std::rank
		>::filter,
		falcon::template_val<std::size_t, 3>::filter
	>::value_wrapper<int[4][8]>::value);
}
#else

#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_integral.hpp>

template<typename _T>
struct static_value
{
	static const bool value = 1;
	typedef bool type;
};

template<typename _T,typename _U>
struct static_value2
{
	static const int value = 2;
	typedef int type;
};

void templates_test()
{
	STATIC_CHECK_VALUE(true, falcon::template_or<boost::is_pointer, boost::is_integral>::filter<int>);
	STATIC_CHECK_VALUE(false, falcon::template_and<boost::is_pointer, boost::is_integral>::filter<int>);
	STATIC_CHECK_VALUE(true, falcon::template_or<static_value>::filter<int>);
	STATIC_CHECK_VALUE(false, falcon::template_not<static_value>::filter<int>);
	STATIC_CHECK_VALUE(2, falcon::template_left_accumulator<static_value2, int>::value_wrapper<int>);
	STATIC_CHECK_VALUE(2, falcon::template_right_accumulator<static_value2, int>::value_wrapper<int>);
	STATIC_CHECK_VALUE(1, falcon::template_to_val<static_value>::filter<int[]>);
}
#endif

FALCON_TEST_TO_MAIN(templates_test)