/* Authors: Andrea Bonito, Sebastian Pauletti. */

/*    $Id: step-38.cc 27657 2012-11-21 13:19:08Z bangerth $       */
/*                                                                */
/*    Copyright (C) 2010-2012 by the deal.II authors */
/*                                                                */
/*    This file is subject to QPL and may not be  distributed     */
/*    without copyright and license information. Please refer     */
/*    to the file deal.II/doc/license.html for the  text  and     */
/*    further information on this license.                        */

// @sect3{Include files}

// If you've read through step-4 and step-7, you will recognize that we have
// used all of the following include files there already. Consequently, we
// will not explain their meaning here again.
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_boundary_lib.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_tools.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/solver_control.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/compressed_sparsity_pattern.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/numerics/data_out.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>

#include <fstream>
#include <iostream>

namespace Step38
{
  using namespace dealii;

  // @sect3{The LaplaceBeltramiProblem class template}

  // This class is almost exactly similar to the LaplaceProblem
  // class in step-4.

  // The essential differences are these:
  //
  // - The template parameter now denotes the dimensionality of the embedding
  //   space, which is no longer the same as the dimensionality of the domain
  //   and the triangulation on which we compute. We indicate this by calling
  //   the parameter @p spacedim , and introducing a constant @p dim equal to
  //   the dimensionality of the domain -- here equal to
  //   spacedim-1.
  // - All member variables that have geometric aspects now need to know about
  //   both their own dimensionality as well as that of the embedding
  //   space. Consequently, we need to specify both of their template
  //   parameters one for the dimension of the mesh @p dim, and the other for
  //   the dimension of the embedding space, @p spacedim. This is exactly what
  //   we did in step-34, take a look there for a deeper explanation.
  // - We need an object that describes which kind of mapping to use from the
  //   reference cell to the cells that the triangulation is composed of. The
  //   classes derived from the Mapping base class do exactly this. Throughout
  //   most of deal.II, if you don't do anything at all, the library assumes
  //   that you want an object of kind MappingQ1 that uses a (bi-, tri-)linear
  //   mapping. In many cases, this is quite sufficient, which is why the use
  //   of these objects is mostly optional: for example, if you have a
  //   polygonal two-dimensional domain in two-dimensional space, a bilinear
  //   mapping of the reference cell to the cells of the triangulation yields
  //   an exact representation of the domain. If you have a curved domain, one
  //   may want to use a higher order mapping for those cells that lie at the
  //   boundary of the domain -- this is what we did in step-11, for
  //   example. However, here we have a curved domain, not just a curved
  //   boundary, and while we can approximate it with bilinearly mapped cells,
  //   it is really only prodent to use a higher order mapping for all
  //   cells. Consequently, this class has a member variable of type MappingQ;
  //   we will choose the polynomial degree of the mapping equal to the
  //   polynomial degree of the finite element used in the computations to
  //   ensure optimal approximation, though this iso-parametricity is not
  //   required.
  template <int spacedim>
  class LaplaceBeltramiProblem
  {
  public:
    LaplaceBeltramiProblem (const unsigned degree = 2);
    void run ();

  private:
    static const unsigned int dim = spacedim-1;

    void make_grid_and_dofs ();
    void assemble_system ();
    void solve ();
    void output_results () const;
    void compute_error () const;

    Triangulation<dim,spacedim>   triangulation;
    FE_Q<dim,spacedim>            fe;
    DoFHandler<dim,spacedim>      dof_handler;
    MappingQ<dim, spacedim>       mapping;

    SparsityPattern               sparsity_pattern;
    SparseMatrix<double>          system_matrix;

    Vector<double>                solution;
    Vector<double>                system_rhs;
  };

  // @sect3{Equation data}

  // Next, let us define the classes that describe the exact solution and the
  // right hand sides of the problem. This is in analogy to step-4 and step-7
  // where we also defined such objects. Given the discussion in the
  // introduction, the actual formulas should be self-explanatory. A point of
  // interest may be how we define the value and gradient functions for the 2d
  // and 3d cases separately, using explicit specializations of the general
  // template. An alternative to doing it this way might have been to define
  // the general template and have a switch statement (or a
  // sequence of ifs) for each possible value of the spatial
  // dimension.
  template <int dim>
  class Solution  : public Function<dim>
  {
  public:
    Solution () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;

    virtual Tensor<1,dim> gradient (const Point<dim>   &p,
                                    const unsigned int  component = 0) const;

  };

  template <>
  double
  Solution<2>::value (const Point<2> &p,
                      const unsigned int) const
  {
    return ( -2. * p(0) * p(1) );
  }

  template <>
  Tensor<1,2>
  Solution<2>::gradient (const Point<2>   &p,
                         const unsigned int) const
  {
    Tensor<1,2> return_value;
    return_value[0] = -2. * p(1) * (1 - 2. * p(0) * p(0));
    return_value[1] = -2. * p(0) * (1 - 2. * p(1) * p(1));

    return return_value;
  }

  template <>
  double
  Solution<3>::value (const Point<3> &p,
                      const unsigned int) const
  {
    return (std::sin(numbers::PI * p(0)) *
            std::cos(numbers::PI * p(1))*exp(p(2)));
  }

  template <>
  Tensor<1,3>
  Solution<3>::gradient (const Point<3>   &p,
                         const unsigned int) const
  {
    using numbers::PI;

    Tensor<1,3> return_value;

    return_value[0] = PI *cos(PI * p(0))*cos(PI * p(1))*exp(p(2));
    return_value[1] = -PI *sin(PI * p(0))*sin(PI * p(1))*exp(p(2));
    return_value[2] = sin(PI * p(0))*cos(PI * p(1))*exp(p(2));

    return return_value;
  }

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    RightHandSide () : Function<dim>() {}

    virtual double value (const Point<dim>   &p,
                          const unsigned int  component = 0) const;
  };

  template <>
  double
  RightHandSide<2>::value (const Point<2> &p,
                           const unsigned int /*component*/) const
  {
    return ( -8. * p(0) * p(1) );
  }

  template <>
  double
  RightHandSide<3>::value (const Point<3> &p,
                           const unsigned int /*component*/) const
  {
    using numbers::PI;

    Tensor<2,3> hessian;

    hessian[0][0] = -PI*PI*sin(PI*p(0))*cos(PI*p(1))*exp(p(2));
    hessian[1][1] = -PI*PI*sin(PI*p(0))*cos(PI*p(1))*exp(p(2));
    hessian[2][2] = sin(PI*p(0))*cos(PI*p(1))*exp(p(2));

    hessian[0][1] = -PI*PI*cos(PI*p(0))*sin(PI*p(1))*exp(p(2));
    hessian[1][0] = -PI*PI*cos(PI*p(0))*sin(PI*p(1))*exp(p(2));

    hessian[0][2] = PI*cos(PI*p(0))*cos(PI*p(1))*exp(p(2));
    hessian[2][0] = PI*cos(PI*p(0))*cos(PI*p(1))*exp(p(2));

    hessian[1][2] = -PI*sin(PI*p(0))*sin(PI*p(1))*exp(p(2));
    hessian[2][1] = -PI*sin(PI*p(0))*sin(PI*p(1))*exp(p(2));

    Tensor<1,3> gradient;
    gradient[0] = PI * cos(PI*p(0))*cos(PI*p(1))*exp(p(2));
    gradient[1] = - PI * sin(PI*p(0))*sin(PI*p(1))*exp(p(2));
    gradient[2] = sin(PI*p(0))*cos(PI*p(1))*exp(p(2));

    Point<3> normal = p;
    normal /= p.norm();

    return (- trace(hessian)
            + 2 * (gradient * normal)
            + (hessian * normal) * normal);
  }

  // @sect3{Implementation of the LaplaceBeltramiProblem class}

  // The rest of the program is actually quite unspectacular if you know
  // step-4. Our first step is to define the constructor, setting the
  // polynomial degree of the finite element and mapping, and associating the
  // DoF handler to the triangulation:
  template <int spacedim>
  LaplaceBeltramiProblem<spacedim>::
  LaplaceBeltramiProblem (const unsigned degree)
    :
    fe (degree),
    dof_handler(triangulation),
    mapping (degree)
  {}

  // @sect4{LaplaceBeltramiProblem::make_grid_and_dofs}

  // The next step is to create the mesh, distribute degrees of freedom, and
  // set up the various variables that describe the linear system. All of
  // these steps are standard with the exception of how to create a mesh that
  // describes a surface. We could generate a mesh for the domain we are
  // interested in, generate a triangulation using a mesh generator, and read
  // it in using the GridIn class. Or, as we do here, we generate the mesh
  // using the facilities in the GridGenerator namespace.
  //
  // In particular, what we're going to do is this (enclosed between the set
  // of braces below): we generate a spacedim dimensional mesh
  // for the half disk (in 2d) or half ball (in 3d), using the
  // GridGenerator::half_hyper_ball function. This function sets the boundary
  // indicators of all faces on the outside of the boundary to zero for the
  // ones located on the perimeter of the disk/ball, and one on the straight
  // part that splits the full disk/ball into two halves. The next step is the
  // main point: The GridTools::extract_boundary_mesh function creates a mesh
  // that consists of those cells that are the faces of the previous mesh,
  // i.e. it describes the surface cells of the original (volume)
  // mesh. However, we do not want all faces: only those on the perimeter of
  // the disk or ball which carry boundary indicator zero; we can select these
  // cells using a set of boundary indicators that we pass to
  // GridTools::extract_boundary_mesh.
  //
  // There is one point that needs to be mentioned. In order to refine a
  // surface mesh appropriately if the manifold is curved (similarly to
  // refining the faces of cells that are adjacent to a curved boundary), the
  // triangulation has to have an object attached to it that describes where
  // new vertices should be located. If you don't attach such a boundary
  // object, they will be located halfway between existing vertices; this is
  // appropriate if you have a domain with straight boundaries (e.g. a
  // polygon) but not when, as here, the manifold has curvature. So for things
  // to work properly, we need to attach a manifold object to our (surface)
  // triangulation, in much the same way as we've already done in 1d for the
  // boundary. We create such an object (with indefinite, static,
  // lifetime) at the top of the function and attach it to the triangulation
  // for all cells with boundary indicator zero that will be created
  // henceforth.
  //
  // The final step in creating the mesh is to refine it a number of
  // times. The rest of the function is the same as in previous tutorial
  // programs.
  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::make_grid_and_dofs ()
  {
    static HyperBallBoundary<dim,spacedim> surface_description;
    triangulation.set_boundary (0, surface_description);

    {
      Triangulation<spacedim> volume_mesh;
      GridGenerator::half_hyper_ball(volume_mesh);

      std::set<types::boundary_id> boundary_ids;
      boundary_ids.insert (0);

      GridTools::extract_boundary_mesh (volume_mesh, triangulation,
                                        boundary_ids);
    }
    triangulation.refine_global(4);

    std::cout << "Surface mesh has " << triangulation.n_active_cells()
              << " cells."
              << std::endl;

    dof_handler.distribute_dofs (fe);

    std::cout << "Surface mesh has " << dof_handler.n_dofs()
              << " degrees of freedom."
              << std::endl;

    CompressedSparsityPattern csp (dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, csp);
    sparsity_pattern.copy_from (csp);

    system_matrix.reinit (sparsity_pattern);

    solution.reinit (dof_handler.n_dofs());
    system_rhs.reinit (dof_handler.n_dofs());
  }

  // @sect4{LaplaceBeltramiProblem::assemble_system}

  // The following is the central function of this program, assembling the
  // matrix that corresponds to the surface Laplacian (Laplace-Beltrami
  // operator). Maybe surprisingly, it actually looks exactly the same as for
  // the regular Laplace operator discussed in, for example, step-4. The key
  // is that the FEValues::shape_gradient function does the magic: It returns
  // the surface gradient $\nabla_K \phi_i(x_q)$ of the $i$th shape function
  // at the $q$th quadrature point. The rest then does not need any changes
  // either:
  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::assemble_system ()
  {
    system_matrix = 0;
    system_rhs = 0;

    const QGauss<dim>  quadrature_formula(2*fe.degree);
    FEValues<dim,spacedim> fe_values (mapping, fe, quadrature_formula,
                                      update_values              |
                                      update_gradients           |
                                      update_quadrature_points   |
                                      update_JxW_values);

    const unsigned int        dofs_per_cell = fe.dofs_per_cell;
    const unsigned int        n_q_points    = quadrature_formula.size();

    FullMatrix<double>        cell_matrix (dofs_per_cell, dofs_per_cell);
    Vector<double>            cell_rhs (dofs_per_cell);

    std::vector<double>       rhs_values(n_q_points);
    std::vector<unsigned int> local_dof_indices (dofs_per_cell);

    const RightHandSide<spacedim> rhs;

    for (typename DoFHandler<dim,spacedim>::active_cell_iterator
         cell = dof_handler.begin_active(),
         endc = dof_handler.end();
         cell!=endc; ++cell)
      {
        cell_matrix = 0;
        cell_rhs = 0;

        fe_values.reinit (cell);

        rhs.value_list (fe_values.get_quadrature_points(), rhs_values);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
              cell_matrix(i,j) += fe_values.shape_grad(i,q_point) *
                                  fe_values.shape_grad(j,q_point) *
                                  fe_values.JxW(q_point);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
            cell_rhs(i) += fe_values.shape_value(i,q_point) *
                           rhs_values[q_point]*
                           fe_values.JxW(q_point);

        cell->get_dof_indices (local_dof_indices);
        for (unsigned int i=0; i<dofs_per_cell; ++i)
          {
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              system_matrix.add (local_dof_indices[i],
                                 local_dof_indices[j],
                                 cell_matrix(i,j));

            system_rhs(local_dof_indices[i]) += cell_rhs(i);
          }
      }

    std::map<unsigned int,double> boundary_values;
    VectorTools::interpolate_boundary_values (mapping,
                                              dof_handler,
                                              0,
                                              Solution<spacedim>(),
                                              boundary_values);

    MatrixTools::apply_boundary_values (boundary_values,
                                        system_matrix,
                                        solution,
                                        system_rhs,false);
  }

  // @sect4{LaplaceBeltramiProblem::solve}

  // The next function is the one that solves the linear system. Here, too, no
  // changes are necessary:
  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::solve ()
  {
    SolverControl solver_control (solution.size(),
                                  1e-7 * system_rhs.l2_norm());
    SolverCG<>    cg (solver_control);

    PreconditionSSOR<> preconditioner;
    preconditioner.initialize(system_matrix, 1.2);

    cg.solve (system_matrix, solution, system_rhs,
              preconditioner);
  }

  // @sect4{LaplaceBeltramiProblem::output_result}

  // This is the function that generates graphical output from the
  // solution. Most of it is boilerplate code, but there are two points worth
  // pointing out:
  //
  // - The DataOut::add_data_vector function can take two kinds of vectors:
  //   Either vectors that have one value per degree of freedom defined by the
  //   DoFHandler object previously attached via DataOut::attach_dof_handler;
  //   and vectors that have one value for each cell of the triangulation, for
  //   example to output estimated errors for each cell. Typically, the
  //   DataOut class knows to tell these two kinds of vectors apart: there are
  //   almost always more degrees of freedom than cells, so we can
  //   differentiate by the two kinds looking at the length of a vector. We
  //   could do the same here, but only because we got lucky: we use a half
  //   sphere. If we had used the whole sphere as domain and $Q_1$ elements,
  //   we would have the same number of cells as vertices and consequently the
  //   two kinds of vectors would have the same number of elements. To avoid
  //   the resulting confusion, we have to tell the DataOut::add_data_vector
  //   function which kind of vector we have: DoF data. This is what the third
  //   argument to the function does.
  // - The DataOut::build_patches function can generate output that subdivides
  //   each cell so that visualization programs can resolve curved manifolds
  //   or higher polynomial degree shape functions better. We here subdivide
  //   each element in each coordinate direction as many times as the
  //   polynomial degree of the finite element in use.
  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::output_results () const
  {
    DataOut<dim,DoFHandler<dim,spacedim> > data_out;
    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution,
                              "solution",
                              DataOut<dim,DoFHandler<dim,spacedim> >::type_dof_data);
    data_out.build_patches (mapping,
                            mapping.get_degree());

    std::string filename ("solution-");
    filename += static_cast<char>('0'+spacedim);
    filename += "d.vtk";
    std::ofstream output (filename.c_str());
    data_out.write_vtk (output);
  }

  // @sect4{LaplaceBeltramiProblem::compute_error}

  // This is the last piece of functionality: we want to compute the error in
  // the numerical solution. It is a verbatim copy of the code previously
  // shown and discussed in step-7. As mentioned in the introduction, the
  // Solution class provides the (tangential) gradient of the
  // solution. To avoid evaluating the error only a superconvergence points,
  // we choose a quadrature rule of sufficiently high order.
  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::compute_error () const
  {
    Vector<float> difference_per_cell (triangulation.n_active_cells());
    VectorTools::integrate_difference (mapping, dof_handler, solution,
                                       Solution<spacedim>(),
                                       difference_per_cell,
                                       QGauss<dim>(2*fe.degree+1),
                                       VectorTools::H1_norm);

    std::cout << "H1 error = "
              << difference_per_cell.l2_norm()
              << std::endl;
  }

  // @sect4{LaplaceBeltramiProblem::run}

  // The last function provides the top-level logic. Its contents are
  // self-explanatory:
  template <int spacedim>
  void LaplaceBeltramiProblem<spacedim>::run ()
  {
    make_grid_and_dofs();
    assemble_system ();
    solve ();
    output_results ();
    compute_error ();
  }
}

// @sect3{The main() function}

// The remainder of the program is taken up by the main()
// function. It follows exactly the general layout first introduced in step-6
// and used in all following tutorial programs:
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step38;

      deallog.depth_console (0);

      LaplaceBeltramiProblem<3> laplace_beltrami;
      laplace_beltrami.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;
}


// Copyright 2019, Nawin

#include "Workspace.h"
#include <algorithm>

#include <boost/container_hash/hash.hpp>

std::size_t hash_value(const Point& obj) {
  std::size_t seed = 0x725c686f;
  boost::hash_combine(seed, obj.x_);
  boost::hash_combine(seed, obj.y_);
  return seed;
}

std::size_t hash_value(const Line& obj) {
  std::size_t seed = 0x719e6b16;
  boost::hash_combine(seed, obj.begin_);
  boost::hash_combine(seed, obj.end_);
  return seed;
}

VectorRectangle::VectorRectangle(Point p, int width, int height) {
  lines_.emplace_back(Line{ p,
      { p.x_ + width, p.y_ } });
  lines_.emplace_back(Line{ { p.x_ + width, p.y_ },
      { p.x_ + width, p.y_ + height } });
  lines_.emplace_back(Line{ { p.x_ + width, p.y_ + height },
      { p.x_, p.y_ + height } });
  lines_.emplace_back(Line{ { p.x_, p.y_ + height },
      { p.x_, p.y_ } });
}

std::vector<Line>::iterator VectorRectangle::begin() {
  return lines_.begin();
}

std::vector<Line>::iterator VectorRectangle::end() {
  return lines_.end();
}

void CPaintDC::SetPixel(int x, int y) {
  std::cout << '(' << x << '/' << y << ") ";
}
void DrawPoints(CPaintDC& dc, std::vector<Point>::iterator start, std::vector<Point>::iterator end) {
  for (auto i = start; i != end; ++i) {
    dc.SetPixel(i->x_, i->y_);
  }
}

LineToPointAdapter::LineToPointAdapter(Line& line) {
  int left = std::min(line.begin_.x_, line.end_.x_);
  int right = std::max(line.begin_.x_, line.end_.x_);
  int top = std::min(line.begin_.y_, line.end_.y_);
  int bottom = std::max(line.begin_.y_, line.end_.y_);
  int dx = right - left;
  int dy = bottom - top;

  //only vertical
  if (!dx) {
    for (int y = top; y <= bottom; ++y)
      points_.emplace_back(Point{ left, y });
  }
  else if (!dy) {
    for (int x = left; x <= right; ++x)
      points_.emplace_back(Point{ x, top });
  }
}

std::map<size_t, LineToPointAdapter::Points> LineToPointAdapter::cache{};
size_t LineToPointAdapter::line_hash{};

LineToPointAdapter::Points::iterator LineToPointAdapter::begin() {
  return cache[line_hash].begin();
}
LineToPointAdapter::Points::iterator LineToPointAdapter::end() {
  return cache[line_hash].end();
}
void LineToPointAdapter::LineToPointCachingAdapter(Line& line) {
  static boost::hash<Line> hash;
  line_hash = hash(line);
  if (cache.find(line_hash) != cache.end())
    return;

  //.. TL;DR
}


#include <gtest/gtest.h>
#include "pitts_multivector_transform.hpp"
#include "pitts_multivector_random.hpp"
#include "pitts_multivector_eigen_adaptor.hpp"
#include "pitts_tensor2.hpp"
#include "pitts_tensor2_random.hpp"
#include "pitts_tensor2_eigen_adaptor.hpp"
#include <Eigen/Dense>
#include "eigen_test_helper.hpp"

TEST(PITTS_MultiVector_transform, invalid_args)
{
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(50,3), Y;
  Tensor2_double M(1,1);

  EXPECT_THROW(transform(X, M, Y), std::invalid_argument);

  M.resize(3,2);

  EXPECT_NO_THROW(transform(X, M, Y));

  EXPECT_THROW(transform(X, M, Y, {10,2}), std::invalid_argument);

  EXPECT_NO_THROW(transform(X, M, Y, {10,10}));
}

TEST(PITTS_MultiVector_transform, single_col_no_reshape)
{
  const auto eps = 1.e-8;
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(50,1), Y;
  Tensor2_double M(1,1);

  randomize(X);
  randomize(M);

  transform(X, M, Y);

  const Eigen::MatrixXd Y_ref = ConstEigenMap(X) * ConstEigenMap(M);
  ASSERT_NEAR(Y_ref, ConstEigenMap(Y), eps);
}

TEST(PITTS_MultiVector_transform, single_col_reshape)
{
  const auto eps = 1.e-8;
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(100,1), Y;
  Tensor2_double M(1,1);

  randomize(X);
  randomize(M);

  transform(X, M, Y, {50, 2});

  ASSERT_EQ(50, Y.rows());
  ASSERT_EQ(2, Y.cols());

  const Eigen::MatrixXd Ytmp = ConstEigenMap(X) * ConstEigenMap(M);
  const Eigen::MatrixXd Y_ref = Eigen::Map<const Eigen::MatrixXd>(Ytmp.data(), 50, 2);
  ASSERT_NEAR(Y_ref, ConstEigenMap(Y), eps);
}

TEST(PITTS_MultiVector_transform, multi_col_no_reshape)
{
  const auto eps = 1.e-8;
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(50,3), Y;
  Tensor2_double M(3,2);

  randomize(X);
  randomize(M);

  transform(X, M, Y);

  const Eigen::MatrixXd Y_ref = ConstEigenMap(X) * ConstEigenMap(M);
  ASSERT_NEAR(Y_ref, ConstEigenMap(Y), eps);
}

TEST(PITTS_MultiVector_transform, multi_col_reshape)
{
  const auto eps = 1.e-8;
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(100,3), Y;
  Tensor2_double M(3,2);

  randomize(X);
  randomize(M);

  transform(X, M, Y, {50, 4});

  ASSERT_EQ(50, Y.rows());
  ASSERT_EQ(4, Y.cols());

  const Eigen::MatrixXd Ytmp = ConstEigenMap(X) * ConstEigenMap(M);
  const Eigen::MatrixXd Y_ref = Eigen::Map<const Eigen::MatrixXd>(Ytmp.data(), 50, 4);
  ASSERT_NEAR(Y_ref, ConstEigenMap(Y), eps);
}

TEST(PITTS_MultiVector_transform, large_no_reshape)
{
  const auto eps = 1.e-8;
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(117,4), Y;
  Tensor2_double M(4,5);

  randomize(X);
  randomize(M);

  transform(X, M, Y);

  const Eigen::MatrixXd Y_ref = ConstEigenMap(X) * ConstEigenMap(M);
  ASSERT_NEAR(Y_ref, ConstEigenMap(Y), eps);
}

TEST(PITTS_MultiVector_transform, large_reshape)
{
  const auto eps = 1.e-8;
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(117,4), Y;
  Tensor2_double M(4,5);

  randomize(X);
  randomize(M);

  transform(X, M, Y, {195, 3});

  ASSERT_EQ(195, Y.rows());
  ASSERT_EQ(3, Y.cols());

  const Eigen::MatrixXd Ytmp = ConstEigenMap(X) * ConstEigenMap(M);
  const Eigen::MatrixXd Y_ref = Eigen::Map<const Eigen::MatrixXd>(Ytmp.data(), 195, 3);
  ASSERT_NEAR(Y_ref, ConstEigenMap(Y), eps);
}

TEST(PITTS_MultiVector_transform, large_reshape2)
{
  const auto eps = 1.e-8;
  using MultiVector_double = PITTS::MultiVector<double>;
  using Tensor2_double = PITTS::Tensor2<double>;

  MultiVector_double X(117,4), Y;
  Tensor2_double M(4,5);

  randomize(X);
  randomize(M);

  transform(X, M, Y, {39, 15});

  ASSERT_EQ(39, Y.rows());
  ASSERT_EQ(15, Y.cols());

  const Eigen::MatrixXd Ytmp = ConstEigenMap(X) * ConstEigenMap(M);
  const Eigen::MatrixXd Y_ref = Eigen::Map<const Eigen::MatrixXd>(Ytmp.data(), 39, 15);
  ASSERT_NEAR(Y_ref, ConstEigenMap(Y), eps);
}

// anonymous namespace
namespace
{
  // helper function that sets all date to an arbitrary value (including padding)
  // used to check that zero padding works fine in all cases
  void prepareOutputWithRubbish(PITTS::MultiVector<double>& M, long long n, long long m)
  {
    // set to´some arbitrary number, then reduce the size to the desired size
    M.resize((n+32)*m,1);
    for(long long i = 0; i < M.rows(); i++)
      M(i,0) = 7.;
  }

  // helper function to check with random data of given size
  void checkWithRandomData(long long n, long long m, long long k, long long n_, long long m_)
  {
    const auto eps = 1.e-8;

    PITTS::MultiVector<double> X(n,m);
    randomize(X);

    PITTS::Tensor2<double> M(m,k);
    randomize(M);

    PITTS::MultiVector<double> Y;
    prepareOutputWithRubbish(Y, n_, m_);

    transform(X, M, Y, {n_, m_});

    using mat = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
    const mat tmp = ConstEigenMap(X) * ConstEigenMap(M);
    ASSERT_EQ(n_*m_, tmp.size());
    Eigen::Map<const mat> Yref(tmp.data(), n_, m_);
    EXPECT_NEAR(Yref, ConstEigenMap(Y), eps);

    // check padding
    for(long long j = 0; j < Y.cols(); j++)
      for(long long i = Y.rows(); i < Y.rowChunks()*PITTS::Chunk<double>::size; i++)
      {
        EXPECT_EQ(0., Y(i,j));
      }
  }

  // output test parameters
  std::string outputParams(const testing::TestParamInfo<std::tuple<int,int,int,int,int>>& info)
  {
    std::string name;
    name += std::to_string(std::get<0>(info.param));
    name += "_" + std::to_string(std::get<1>(info.param));
    name += "_" + std::to_string(std::get<2>(info.param));
    name += "_" + std::to_string(std::get<3>(info.param));
    name += "_" + std::to_string(std::get<4>(info.param));
    return name;
  }
}

class PITTS_MultiVector_transform_param : public testing::TestWithParam<std::tuple<int,int,int,int,int>> {};

TEST_P(PITTS_MultiVector_transform_param, check_result_and_padding)
{
  const auto& [n, m, k, n_, m_] = GetParam();

  checkWithRandomData(n, m, k, n_, m_);
}

INSTANTIATE_TEST_CASE_P(SmallNoReshape, PITTS_MultiVector_transform_param, testing::Values(
    std::make_tuple(1, 1, 1, 1, 1),
    std::make_tuple(1, 5, 1, 1, 1),
    std::make_tuple(5, 1, 1, 5, 1),
    std::make_tuple(5, 3, 1, 5, 1),
    std::make_tuple(5, 7, 4, 5, 4),
    std::make_tuple(7, 3, 4, 7, 4),
    std::make_tuple(3, 5, 7, 3, 7)),
    &outputParams );

INSTANTIATE_TEST_CASE_P(SmallReshape, PITTS_MultiVector_transform_param, testing::Values(
    std::make_tuple(5, 1, 1, 1, 5),
    std::make_tuple(5, 3, 1, 1, 5),
    std::make_tuple(5, 7, 4, 10, 2),
    std::make_tuple(5, 7, 4, 20, 1),
    std::make_tuple(5, 7, 4, 4, 5),
    std::make_tuple(7, 3, 4, 4, 7),
    std::make_tuple(3, 5, 7, 7, 3)),
    &outputParams );

INSTANTIATE_TEST_CASE_P(LargeNoReshape, PITTS_MultiVector_transform_param, testing::Values(
    std::make_tuple(32, 32, 32, 32, 32),
    std::make_tuple(64, 7, 8, 64, 8),
    std::make_tuple(71, 3, 5, 71, 5),
    std::make_tuple(5, 120, 113, 5, 113),
    std::make_tuple(50, 3, 50, 50, 50),
    std::make_tuple(99, 2, 1, 99, 1),
    std::make_tuple(150, 2, 3, 150, 3)),
    &outputParams );

INSTANTIATE_TEST_CASE_P(LargeReshape, PITTS_MultiVector_transform_param, testing::Values(
    std::make_tuple(32, 32, 32, 64, 16),
    std::make_tuple(32, 32, 32, 16, 64),
    std::make_tuple(64, 7, 8, 32, 16),
    std::make_tuple(71, 3, 5, 5, 71),
    std::make_tuple(5, 120, 113, 113, 5),
    std::make_tuple(50, 3, 50, 100, 25),
    std::make_tuple(50, 3, 50, 25, 100),
    std::make_tuple(99, 2, 1, 1, 99),
    std::make_tuple(150, 2, 3, 30, 15)),
    &outputParams );


// (C) Copyright David James Pearce 2005. Permission to copy, use,
// modify, sell and distribute this software is granted provided this
// copyright notice appears in all copies. This software is provided
// "as is" without express or implied warranty, and with no claim as
// to its suitability for any purpose.
//
// Email: d.pearce@mcs.vuw.ac.nz

// Basically, these algorithms are used to
// generate random graphs ...

#ifndef RANDOM_GRAPH_H
#define RANDOM_GRAPH_H

#include <algorithm>
#if __GNUC__ >= 3
#include <ext/algorithm>
#define RANDOM_SAMPLE_N __gnu_cxx::random_sample_n
#else
#define RANDOM_SAMPLE_N random_sample_n
#endif
#include <vector>
#include <stdexcept>
#include <assert.h>
#include <limits.h>
#include <iterator>

#include <boost/iterator/counting_iterator.hpp>
#include <boost/random/geometric_distribution.hpp>
#include <boost/random/random_number_generator.hpp>

// =========================================
// Some Configuration Parameters
//
// #define FAST_RANDOM_DAG_GENERATOR
// #define PRECISE_FAST_RANDOM_DAG_GENERATOR
// =========================================

class random_dag_iterator {
public:
  typedef std::forward_iterator_tag iterator_category;
  typedef std::pair<unsigned int, unsigned int> value_type;
  typedef size_t difference_type;
  typedef value_type& reference;
  typedef value_type* pointer;
private:
  unsigned int _node;
  unsigned int _edge;
  unsigned int _V;
public:
  random_dag_iterator(unsigned int n,
		      unsigned int V)
    : _node(n), _edge(0), _V(V) {
  }
  bool operator==(random_dag_iterator const &src) {
    return _node == src._node && _edge == src._edge;
  }

  bool operator!=(random_dag_iterator const &src) {
    return _node != src._node || _edge != src._edge;
  }

  random_dag_iterator const &operator++(void) {
    ++_edge;
    if(_edge >= (_V-(_node+1))) {
      _edge=0;
      ++_node;
    }
    return *this;
  }

  std::pair<unsigned int,unsigned int> operator*(void) const {
    return std::make_pair(_node,
			  _node+_edge+1);
  }
};

template<class EdgeList, class UniformRandomNumberGenerator>
void random_acyclic_edgelist(unsigned int V, unsigned int E,
		     EdgeList &edges,
		     UniformRandomNumberGenerator &urng) {

  boost::random_number_generator<UniformRandomNumberGenerator,unsigned int> rng(urng);

  // the algorithm doesn't make any guarantee about the
  // distribution of edges in the edgelist. so, using random_shuffle
  // might be a good idea, if that's the desired result

  unsigned int V_VM1_D2((V*(V-1)) / 2);

  assert(V != 0);
  assert(E < V_VM1_D2);

  // first, generate a random topological order by
  // first initialising the vector such rtorder[i]=i
  // and then shuffling it around.
  std::vector<unsigned int> rtorder;
  std::copy(boost::make_counting_iterator<unsigned int>(0),
	    boost::make_counting_iterator<unsigned int>(V),
	    std::back_inserter(rtorder));

  random_shuffle(rtorder.begin(),rtorder.end(),rng);

  // obtain E random samples from the enumeration of edges
  std::vector<unsigned int> sample;
  RANDOM_SAMPLE_N(random_dag_iterator(0,V),
		  random_dag_iterator(V-1,V),
		  std::back_inserter(edges),E,rng);

  // now convert indices into actual nodes according
  // to the order

  for(typename EdgeList::iterator i(edges.begin());
      i!=edges.end();++i) {
    i->first = rtorder[i->first];
    i->second = rtorder[i->second];
  }
}

class random_digraph_iterator {
public:
  typedef std::forward_iterator_tag iterator_category;
  typedef std::pair<unsigned int, unsigned int> value_type;
  typedef size_t difference_type;
  typedef value_type& reference;
  typedef value_type* pointer;
private:
  unsigned int _node;
  unsigned int _edge;
  unsigned int _V;
public:
  random_digraph_iterator(unsigned int n, unsigned int V)
    : _node(n), _edge(0), _V(V) {
  }

  bool operator==(random_digraph_iterator const &src) {
    return _node == src._node && _edge == src._edge;
  }

  bool operator!=(random_digraph_iterator const &src) {
    return _node != src._node || _edge != src._edge;
  }

  random_digraph_iterator const &operator++(void) {
    if(++_edge >= _V) {
      _edge=0;
      ++_node;
    }
    return *this;
  }

  std::pair<unsigned int,unsigned int> operator*(void) const {
    return std::make_pair(_node,_edge);
  }
};

template<class EdgeList, class UniformRandomNumberGenerator>
void random_edgelist(unsigned int V, unsigned int E,
		     EdgeList &edges,
		     UniformRandomNumberGenerator &urng) {

  boost::random_number_generator<UniformRandomNumberGenerator,unsigned int> rng(urng);

  // the algorithm doesn't make any guarantee about the
  // distribution of edges in the edgelist. so, using random_shuffle
  // might be a good idea, if that's the desired result

  assert(V != 0);
  assert(E < (V*V));

  // obtain E random samples from by enumeration of edges
  std::vector<unsigned int> sample;
  RANDOM_SAMPLE_N(random_digraph_iterator(0,V),
		  random_digraph_iterator(V,V),
		  std::back_inserter(edges),E,rng);
}

#endif


//////////////////////////////////////////////////////////////////////////////////////////////
/// \file LineSearchGaussNewtonSolver.hpp
///
/// \author Sean Anderson, ASRL
//////////////////////////////////////////////////////////////////////////////////////////////

#ifndef STEAM_LINE_SEARCH_GAUSS_NEWTON_SOLVER_HPP
#define STEAM_LINE_SEARCH_GAUSS_NEWTON_SOLVER_HPP

#include <Eigen/Core>

#include <steam/solver/GaussNewtonSolverBase.hpp>

namespace steam {

//////////////////////////////////////////////////////////////////////////////////////////////
/// \brief Solver using a backtracking line-search to make sure cost doesn't increase
//////////////////////////////////////////////////////////////////////////////////////////////
class LineSearchGaussNewtonSolver : public GaussNewtonSolverBase
{
public:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Line-search parameters
  //////////////////////////////////////////////////////////////////////////////////////////////
  struct Params : public SolverBase::Params {

    //////////////////////////////////////////////////////////////////////////////////////////////
    /// \brief Default constructor
    //////////////////////////////////////////////////////////////////////////////////////////////
    Params() : SolverBase::Params(), backtrackMultiplier(0.5), maxBacktrackSteps(10) {
    }

    /// Amount to decrease step after each backtrack
    double backtrackMultiplier;

    /// Maximimum number of times to backtrack before giving up
    unsigned int maxBacktrackSteps;
  };

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Constructor
  //////////////////////////////////////////////////////////////////////////////////////////////
  LineSearchGaussNewtonSolver(OptimizationProblem* problem, const Params& params = Params());

private:

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Build the system, solve for a step size and direction, and update the state
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual bool linearizeSolveAndUpdate(double* newCost, double* gradNorm);

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Casts parameters to base type (for SolverBase class)
  //////////////////////////////////////////////////////////////////////////////////////////////
  virtual const SolverBase::Params& getSolverBaseParams() const;

  //////////////////////////////////////////////////////////////////////////////////////////////
  /// \brief Parameters
  //////////////////////////////////////////////////////////////////////////////////////////////
  Params params_;

};

} // steam

#endif // STEAM_LINE_SEARCH_GAUSS_NEWTON_SOLVER_HPP


#ifndef UTILS_HPP
#define UTILS_HPP

#include "graph.hpp"
#include "units.hpp"

#include <boost/graph/connected_components.hpp>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <cassert>
#include <iostream>
#include <list>
#include <map>
#include <random>
#include <set>
#include <string>
#include <vector>

namespace ba = boost::accumulators;

// The accumulator with double values.
typedef ba::accumulator_set<double, ba::stats<ba::tag::min,
                                              ba::tag::mean,
                                              ba::tag::max,
                                              ba::tag::variance> >
dbl_acc;

/**
 * Generate a random number from min to max, including both min and
 * max.
 */
template<typename E>
int
get_random_int(int min, int max, E &eng)
{
  std::uniform_int_distribution<> d(min, max);
  return d(eng);
}

// Calculate the cost of the path.
template<typename Graph, typename Container>
typename boost::property_map<Graph, boost::edge_weight_t>::value_type
get_path_length(const Graph &g, const Container &con)
{
  using ew = boost::edge_weight_t;
  using w = typename boost::property_map<Graph, ew>::value_type;

  auto c = std::accumulate(con.begin(), con.end(), w{},
                           [g](const auto &c, const auto &e)
                           {
                             auto o = boost::get(boost::edge_weight,
                                                 g, e);
                             return c + o;
                           });
  return c;
}

// Calculate the cost of a path.
template<typename Graph>
auto
get_cost(const Graph &g, const CU_Path<Graph> &p)
{
  return p.first.count() * get_path_length(g, p.second);
}

/**
 * Interpret a string, and return the matched enum.
 */
template <typename T>
T
interpret(const std::string &name, const std::string &text,
          const std::map <std::string, T> &m)
{
  auto i = m.find (text);

  if (i == m.end ())
    {
      std::cerr << "Wrong value of " << name << ".  Choose one of:";
      for (auto &p: m)
        std::cerr << " " << p.first;
      std::cerr << std::endl;
      exit (1);
    }

  return i->second;
}

/**
 * Find the SU that is available along the path.
 */
SU
find_path_su(const graph &g, const path &p);

/**
 * This is the << operator for a pair.
 */
template <typename A, typename B>
std::ostream &operator << (std::ostream &os, const std::pair<A, B> &p)
{
  os << "pair(" << p.first << ", " << p.second << ")";
  return os;
}

/**
 * This is the << operator for a cupath.
 */
std::ostream &operator << (std::ostream &os, const cupath &p);

/**
 * This is the << operator for a vector.
 */
template <typename T>
std::ostream &operator << (std::ostream &os, const std::vector<T> &v)
{
  typename std::vector<T>::const_iterator i = v.begin();

  os << "[";

  while(i != v.end())
    {
      os << *i;
      if (++i != v.end())
        os << ", ";
    }

  os << "]";

  return os;
}

/**
 * This is the << operator for a set.
 */
template <typename T>
std::ostream &operator << (std::ostream &os, const std::set<T> &v)
{
  typename std::set<T>::const_iterator i = v.begin();

  os << "{";

  while(i != v.end())
    {
      os << *i;
      if (++i != v.end())
        os << ", ";
    }

  os << "}";

  return os;
}

/**
 * This is the << operator for a list.
 */
template <typename T>
std::ostream &operator << (std::ostream &os, const std::list<T> &v)
{
  typename std::list<T>::const_iterator i = v.begin();

  os << "(";

  while(i != v.end())
    {
      os << *i;
      if (++i != v.end())
        os << ", ";
    }

  os << ")";

  return os;
}

/**
 * This is the << operator for a map.
 */
template <typename K, typename V>
std::ostream &operator << (std::ostream &os, const std::map<K, V> &m)
{
  for(auto const &e: m)
    os << e.first << " -> " << e.second << std::endl;

  return os;
}

/**
 * True if the graph is connected.
 *
 * @return: true if the graph is connected, false otherwise.
 */
template<typename G>
bool
is_connected(const G &g)
{
  int c[boost::num_vertices(g)];

  // "num" is the number of connected components.
  int num = boost::connected_components(g, c);

  return num == 1;
}

/**
 * Get a random element from a container.
 */
template <typename C, typename E>
auto
get_random_element(const C &c, E &eng)
{
  assert(!c.empty());

  typename C::const_iterator i = c.begin();
  std::advance(i, get_random_int(0, c.size() - 1, eng));

  return *i;
}

/**
 * Generate a random node pair.
 */
template<typename G, typename E>
std::pair<typename G::vertex_descriptor,
	  typename G::vertex_descriptor>
random_node_pair(const G &g, E &eng)
{
  typedef typename G::vertex_descriptor vertex;
  typedef typename G::vertex_iterator vertex_i;

  vertex_i begin = vertices(g).first;
  int n = num_vertices(g);

  int src_n = get_random_int(0, n - 1, eng);
  vertex_i src_i = begin;
  std::advance(src_i, src_n);
  vertex src = *src_i;

  // Choose any number between 0 and n - 1, but not src_n.
  int dst_n = get_random_int(0, n - 2, eng);
  if (dst_n == src_n)
    dst_n = n - 1;
  vertex_i dst_i = begin;
  std::advance(dst_i, dst_n);
  vertex dst = *dst_i;

  return std::make_pair(src, dst);
}

template<typename G>
double
calculate_utilization(const G &g)
{
  ba::accumulator_set<double, ba::stats<ba::tag::mean> > load_acc;

  typename G::edge_iterator ei, ee;
  for (tie(ei, ee) = edges(g); ei != ee; ++ei)
    {
      // Total available units.
      int tsc = boost::get(boost::edge_nou, g, *ei);
      // Currenlty available units.
      int asc = boost::get(boost::edge_su, g, *ei).count();
      // The link load.
      double load = double(tsc - asc) / tsc;
      load_acc(load);
    }

  return ba::mean(load_acc);
}

bool
vertex_in_path(const graph &g, const path &p, vertex v);

/**
 * Sets the units property on edges.
 */
template<typename G>
void
set_units(G &g, unsigned units)
{
  SU su = {{0, units}};

  // Total units property map.
  typename boost::property_map<G, boost::edge_nou_t>::type
    tpm = get(boost::edge_nou_t(), g);
  // Available units property map.
  typename boost::property_map<G, boost::edge_su_t>::type
    apm = get(boost::edge_su_t(), g);

  typename G::edge_iterator ei, ee;
  for (tie(ei, ee) = edges(g); ei != ee; ++ei)
    {
      tpm[*ei] = units;
      apm[*ei] = su;
    }
}

// For the shortest paths between all node pairs, calculate the
// statistics for hops and lengths.
void
calc_sp_stats(const graph &g, dbl_acc &hop_acc, dbl_acc &len_acc);

// Calculate the mean connection arrival time for the given mean
// number of hops of a shortest path (mnh), the mean holding time
// (mht) of a connection, the mean number of units of a connection,
// and the offered load (ol).
double
calc_mcat(const graph &g, double mnh, double mht, double mnu,
          double ol);

bool
load_graphviz(const std::string &file_name, graph &g);

#endif /* UTILS_HPP */


#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <pybind11/iostream.h>
namespace py = pybind11;

#include <Eigen/Dense>
#include <MeshFEM/BoundaryConditions.hh>
#include <MeshFEM/FEMMesh.hh>
#include <MeshFEM/ElasticityTensor.hh>
#include <MeshFEM/SymmetricMatrix.hh>
#include <MeshFEM/PeriodicHomogenization.hh>
#include <MeshFEM/OrthotropicHomogenization.hh>
#include <MeshFEM/LinearElasticity.hh>
#include <MeshFEM/Utilities/MeshConversion.hh>
#include <MeshFEM/Utilities/NameMangling.hh>

template<typename Mesh>
using ETensor = ElasticityTensor<typename Mesh::Real, Mesh::EmbeddingDimension>;

template<typename Mesh>
struct HomogenizationResult {
    static constexpr size_t Dim = Mesh::EmbeddingDimension;
    using Real    = typename Mesh::Real;
    using VField  = Eigen::Matrix<Real, Eigen::Dynamic, Dim>;
    using SMField = SymmetricMatrixField<Real, Dim>;

    ETensor<Mesh>        Ch;
    std::vector<VField>  w_ij;
    std::vector<SMField> strain_w_ij;
};

template<size_t _N>
using HMG = LinearElasticity::HomogenousMaterialGetter<Materials::Constant>::template Getter<_N>;

template<typename _Mesh>
HomogenizationResult<_Mesh> runHomogenization(
        const _Mesh &mesh, const ETensor<_Mesh> &Cbase, bool orthotropicCell,
        const std::string &manualPeriodicVerticesFile, bool centerFluctuationDisplacements,
        bool ignorePeriodicMismatch) {
    using Real = typename _Mesh::Real;
    static constexpr size_t N = _Mesh::EmbeddingDimension;
    using LEMesh = LinearElasticity::Mesh<N, _Mesh::Deg, HMG>;
    HMG<N>::material.setTensor(Cbase);
    LinearElasticity::Simulator<LEMesh> sim(getF(mesh), getV(mesh));

    HomogenizationResult<_Mesh> result;
    std::vector<VectorField<Real, N>> w_ij;

    // Compute fluctuation displacements and homogenized elasticity tensor
    std::unique_ptr<PeriodicCondition<N>> pc;
    if (!manualPeriodicVerticesFile.empty())
        pc = Future::make_unique<PeriodicCondition<N>>(sim.mesh(), manualPeriodicVerticesFile);
    if (orthotropicCell) {
        auto systems = PeriodicHomogenization::Orthotropic::solveCellProblems(w_ij, sim);
        result.Ch = PeriodicHomogenization::Orthotropic::homogenizedElasticityTensorDisplacementForm(w_ij, sim);
    }
    else {
        PeriodicHomogenization::solveCellProblems(w_ij, sim, 1e-7, ignorePeriodicMismatch, std::move(pc));
        result.Ch = PeriodicHomogenization::homogenizedElasticityTensorDisplacementForm(w_ij, sim);
    }

    const size_t numCellProblems = w_ij.size();

    if (centerFluctuationDisplacements) {
        for (size_t i = 0; i < numCellProblems; ++i) {
            auto &w = w_ij[i];
            VectorND<N> total(VectorND<N>::Zero());
            for (size_t ii = 0; ii < w.domainSize(); ++ii) total += w(ii);
            total *= 1.0 / w.domainSize();
            for (size_t ii = 0; ii < w.domainSize(); ++ii) w(ii) -= total;
        }
    }

    // Convert to numpy-compatible output
    result.w_ij.resize(numCellProblems);
    for (size_t i = 0; i < numCellProblems; ++i) {
        const auto &w = w_ij[i];
        result.w_ij[i].resize(w.domainSize(), N);
        for (size_t ii = 0; ii < w.domainSize(); ++ii)
            result.w_ij[i].row(ii) = w(ii).transpose();
    }

    // Compute fluctuation strains
    result.strain_w_ij.resize(numCellProblems);
    for (size_t i = 0; i < numCellProblems; ++i)
        result.strain_w_ij[i] = sim.averageStrainField(w_ij[i]);

    return result;
}

template<class _Mesh, class HR, class SMValue>
std::tuple<typename HR::VField, typename HR::SMField>
getProbeResult(const _Mesh &mesh, const HR &homogenizationResult, const SMValue &macroStrain) {
    const HR &hr = homogenizationResult;
    const size_t numCellProblems = hr.w_ij.size();
    constexpr size_t N = SMValue::N;
    using Vec = VectorND<N>;

    typename HR::VField         w = macroStrain[0] * hr.w_ij[0];
    typename HR::SMField strain_w = macroStrain[0] * hr.strain_w_ij[0];

    for (size_t i = 1; i < numCellProblems; ++i) {
        typename HR::Real shearDoubler = (i < N) ? 1.0 : 2.0;
        w        += shearDoubler * macroStrain[i] * hr.w_ij[i];
        strain_w += shearDoubler * macroStrain[i] * hr.strain_w_ij[i];
    }

    // Remove rigid translation of fluctuation displacement relative to the
    // base cell (i.e. try to keep the fluctuation-displaced microstructure
    // "within" the base cell):
    // We need to ensure vertices on periodic boundary do not move off the
    // boundary. We enforce this in an average sense for each cell face by
    // translating so that the corresponding displacement component's average
    // over all vertices on the face is zero. (Note: this is different from
    // preventing the center of mass from moving.)
    {
        auto bbox = mesh.boundingBox();
        Vec translation(Vec::Zero());
        Vec numAveraged(Vec::Zero());

        for (auto bn : mesh.boundaryNodes()) {
            auto n = bn.volumeNode();
            for (size_t d = 0; d < N; ++d) {
                if (std::abs(n->p[d] - bbox.minCorner[d]) < 1e-9) {
                    translation[d] += w(n.index(), d);
                    numAveraged[d] += 1.0;
                }
            }
        }
        translation.array() /= numAveraged.array();
        w.rowwise() -= translation.transpose();
    }

    // Add in the linear term
    auto        u = w;
    auto strain_u = strain_w;
    for (auto n : mesh.nodes())
        u.row(n.index()) += macroStrain.contract(n->p);
    for (size_t i = 0; i < strain_u.domainSize(); ++i)
        strain_u(i) += macroStrain;

    return std::make_tuple(u, strain_u);
}

template<typename _Mesh>
void bindHomogenization(py::module &m, py::module &detail_module) {
    using Real = typename _Mesh::Real;
    static constexpr size_t N = _Mesh::EmbeddingDimension;
    using HR = HomogenizationResult<_Mesh>;
    using SMValue = SymmetricMatrixValue<Real, N>;

    py::class_<HR>(detail_module, ("HomogenizationResult" + NameMangler<_Mesh>::name()).c_str())
        .def_readonly("Ch",          &HR::Ch)
        .def_readonly("w_ij",        &HR::w_ij)
        .def_readonly("strain_w_ij", &HR::strain_w_ij)
        ;

    m.def("homogenize", runHomogenization<_Mesh>,
          py::arg("mesh"), py::arg("Cbase"), py::arg("orthotropicCell") = false, py::arg("manualPeriodicVerticesFile") = std::string(),
          py::arg("centerFluctuationDisplacements") = true, py::arg("ignorePeriodicMismatch") = false)
     .def("probe", getProbeResult<_Mesh, HR, SMValue>, py::arg("mesh"), py::arg("homogenizationResult"), py::arg("macroStrain"))
     .def("probe", [](const _Mesh &mesh, const ETensor<_Mesh> &Cbase, const SMValue &macroStrain,
                      bool orthotropicCell, const std::string &manualPeriodicVerticesFile,
                      bool ignorePeriodicMismatch) {
                auto hr = runHomogenization(mesh, Cbase, orthotropicCell, manualPeriodicVerticesFile, false,
                                            ignorePeriodicMismatch);
                return getProbeResult(mesh, hr, macroStrain);
            }, py::arg("mesh"), py::arg("Cbase"), py::arg("macroStrain"), py::arg("orthotropicCell") = false, py::arg("manualPeriodicVerticesFile") = std::string(),
               py::arg("ignorePeriodicMismatch") = false)
     ;
}

template<typename _Real>
void addBindings(py::module &m, py::module &detail_module) {
    using V3d = Eigen::Matrix<_Real, 3, 1>;
    using V2d = Eigen::Matrix<_Real, 2, 1>;

    bindHomogenization<FEMMesh<3, 1, V3d>>(m, detail_module); // linear    tet mesh in 3d
    bindHomogenization<FEMMesh<3, 2, V3d>>(m, detail_module); // quadratic tet mesh in 3d
    bindHomogenization<FEMMesh<2, 1, V2d>>(m, detail_module); // linear    tri mesh in 2d
    bindHomogenization<FEMMesh<2, 2, V2d>>(m, detail_module); // quadratic tri mesh in 2d
}

PYBIND11_MODULE(periodic_homogenization, m) {
    m.doc() = "Periodic Homogenization";

    py::module detail_module = m.def_submodule("detail");

    py::module::import("mesh");
    py::module::import("tensors");

    addBindings<double>(m, detail_module);
// #if MESHFEM_BIND_LONG_DOUBLE
//     addBindings<long double>(m, detail_module);
// #endif
}


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

#include <kernel/equip/coil_biot_savart.h>
//#include <boost/math/special_functions/ellint_1.hpp>
//#include <boost/math/special_functions/ellint_2.hpp>

namespace ssl {
namespace equip {

CoilBiotSavart::CoilBiotSavart()
    : radius_(0.06),
      azimuth_(0),
      elevation_(0),
      segment_(20),
      current_direction_(1) {
  // TODO Auto-generated constructor stub

}

CoilBiotSavart::~CoilBiotSavart() {
  // TODO Auto-generated destructor stub
}
void CoilBiotSavart::assign() {
  Coil::assign();
  //std::string str = getAttribute("azimuth");
  /*if (!str.empty())
	azimuth_ = boost::lexical_cast<double>(str);

  str = getAttribute("elevation");
  if (!str.empty())
	elevation_ = boost::lexical_cast<double>(str);*/

  azimuth_ *= _pi / 180;
  elevation_ *= _pi / 180;

  //str = getAttribute("pos");
  //if (!str.empty()) {
  // std::vector<double> pars;// = str_cast<double>(str);
  //  pos_(0) = pars[0];
  //  pos_(1) = pars[1];
  //  pos_(2) = pars[2];
  //}
  segments_ = get_sampe_points();
}
cd CoilBiotSavart::get_sensitivity(const vec3 &pos) const {
  cd B1s;
  vec3 B1 = vec3::Zero();
  //for (size_t i = 0; i < segments_.size() - 1; i++) {
  //   vec3 dl = segments_[i + 1] - segments_[i];  // components of segment std::vector dl.
  //   vec3 cp = (segments_[i + 1] + segments_[i]) / 2;  // the location of the midpoint of current segment.
  //   vec r = pos - cp;  // segment on loop and observation point.
  //  double r3 = pow(norm(r, 2), 3);  // r^3 from r std::vector.
  //   vec3 dl_r = cross(dl, r);  // cross product between dl and r.
  //  B1 += (radius_ / k_2pi / r3) * dl_r;  // increment sum of B1 magnetic field.
  //}
  double magn = sqrt(pow(B1(cx), 2) + pow(B1(cy), 2));
  double phase = atan2(B1(cy), B1(cx));
  return cd(magn, phase);
  //double mu0 = 4 * _pi * 1e-7;  // permeability of free space (T*m/A).
  //double I = 1;  // current in the loop (A).
  //double constant = mu0 / (4 * _pi) * I;   // Useful constant.
  // the magnetic field at the center of the coil B0 = Imu0/2*radius.
  // ref from http://www.netdenizen.com/emagnettest/offaxis/?offaxisloop
}
std::vector<vec3> CoilBiotSavart::get_sampe_points() const {
  // vec theta =  linspace< vec>(
  //    0, k_2pi * (double) current_direction_, segment_);
  // rowvec3 N;
  //N << cos(elevation_) * cos(azimuth_) << cos(elevation_) * sin(azimuth_)
  //  << sin(elevation_) <<  endr;
  //N /= norm(N);
  // mat V = null(N);
  //double ang = atan2(dot(cross(V.col(0), V.col(1)), N),
  //                   dot(cross(V.col(0), N), cross(V.col(1), N)));  // determine angle direction.
  //V.col(0) = (ang / abs(ang)) * V.col(0);  // match angle direction.
  std::vector<vec3> points;
  /* for (size_t i = 0; i < theta.size(); i++) {
	  vec3 p = pos_
		 + radius_ * (cos(theta(i)) * V.col(0) + sin(theta(i)) * V.col(1));
	 points.push_back(p);
   }*/
  return points;
}
} /* namespace equip */
} /* namespace ssl */


#define BOOST_TEST_MODULE SparseMatrixVectorProduct
#include <boost/test/unit_test.hpp>
#include <vexcl/vector.hpp>
#include <vexcl/multivector.hpp>
#include <vexcl/spmat.hpp>
#include <vexcl/function.hpp>
#include "context_setup.hpp"
#include "random_matrix.hpp"

BOOST_AUTO_TEST_CASE(vector_product)
{
    const size_t n = 1024;

    std::vector<size_t> row;
    std::vector<size_t> col;
    std::vector<double> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n);

    vex::SpMat <double> A(ctx, n, n, row.data(), col.data(), val.data());
    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });

    Y -= A * X;

    check_sample(Y, [&](size_t, double a) { BOOST_CHECK_SMALL(a, 1e-8); });

    Y += 42 * (A * X);

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, 42 * sum, 1e-8);
            });

    Y = X + A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, x[idx] + sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(non_square_matrix)
{
    const size_t n = 1024;
    const size_t m = 2 * n;

    std::vector<size_t> row;
    std::vector<size_t> col;
    std::vector<double> val;

    random_matrix(n, m, 16, row, col, val);

    std::vector<double> x = random_vector<double>(m);

    vex::SpMat <double> A(ctx, n, m, row.data(), col.data(), val.data());
    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(non_default_types)
{
    const size_t n = 1024;

    std::vector<unsigned> row;
    std::vector<int>      col;
    std::vector<double>   val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n);

    vex::SpMat <double, int, unsigned> A(ctx, n, n, row.data(), col.data(), val.data());
    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(empty_rows)
{
    const size_t n = 1024;
    const size_t non_empty_part = 256;

    std::vector<size_t> row;
    std::vector<size_t> col;
    std::vector<double> val;

    row.reserve(n + 1);

    random_matrix(non_empty_part, n, 16, row, col, val);

    while(row.size() < n + 1) row.push_back(col.size());

    std::vector<double> x = random_vector<double>(n);

    vex::SpMat <double> A(ctx, n, n, row.data(), col.data(), val.data());
    vex::vector<double> X(ctx, x);
    vex::vector<double> Y(ctx, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(ccsr_vector_product)
{
    const size_t n = 32;
    const double h2i = (n - 1) * (n - 1);

    std::vector<size_t> idx;
    std::vector<size_t> row(3);
    std::vector<int>    col(8);
    std::vector<double> val(8);

    idx.reserve(n * n * n);

    row[0] = 0;
    row[1] = 1;
    row[2] = 8;

    col[0] = 0;
    val[0] = 1;

    col[1] = -static_cast<int>(n * n);
    col[2] = -static_cast<int>(n);
    col[3] =    -1;
    col[4] =     0;
    col[5] =     1;
    col[6] =     n;
    col[7] =  (n * n);

    val[1] = -h2i;
    val[2] = -h2i;
    val[3] = -h2i;
    val[4] =  h2i * 6;
    val[5] = -h2i;
    val[6] = -h2i;
    val[7] = -h2i;

    for(size_t k = 0; k < n; k++) {
        for(size_t j = 0; j < n; j++) {
            for(size_t i = 0; i < n; i++) {
                if (
                        i == 0 || i == (n - 1) ||
                        j == 0 || j == (n - 1) ||
                        k == 0 || k == (n - 1)
                   )
                {
                    idx.push_back(0);
                } else {
                    idx.push_back(1);
                }
            }
        }
    }

    std::vector<double> x = random_vector<double>(n * n * n);

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    vex::SpMatCCSR<double,int> A(queue[0], x.size(), row.size() - 1,
            idx.data(), row.data(), col.data(), val.data());

    vex::vector<double> X(queue, x);
    vex::vector<double> Y(queue, x.size());

    Y = A * X;

    check_sample(Y, [&](size_t ii, double a) {
            double sum = 0;
            size_t i = idx[ii];
            for(size_t j = row[i]; j < row[i + 1]; j++)
                sum += val[j] * x[ii + col[j]];

            BOOST_CHECK_CLOSE(a, sum, 1e-8);
            });

    Y = X + A * X;

    check_sample(Y, [&](size_t ii, double a) {
            double sum = 0;
            size_t i = idx[ii];
            for(size_t j = row[i]; j < row[i + 1]; j++)
                sum += val[j] * x[ii + col[j]];

            BOOST_CHECK_CLOSE(a, x[ii] + sum, 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(inline_spmv)
{
    const size_t n = 1024;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    std::vector<size_t> row;
    std::vector<size_t> col;
    std::vector<double> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n);

    vex::SpMat <double> A(queue, n, n, row.data(), col.data(), val.data());
    vex::vector<double> X(queue, x);
    vex::vector<double> Y(queue, n);

    Y = sin(vex::make_inline(A * X));

    check_sample(Y, [&](size_t idx, double a) {
            double sum = 0;
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a, sin(sum), 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(multivector_product)
{
    const size_t n = 1024;
    const size_t m = 2;

    typedef std::array<double, m> elem_t;

    std::vector<size_t> row;
    std::vector<size_t> col;
    std::vector<double> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n * m);

    vex::SpMat <double> A(ctx, n, n, row.data(), col.data(), val.data());

    vex::multivector<double,m> X(ctx, x);
    vex::multivector<double,m> Y(ctx, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, elem_t a) {
            double sum[] = {0, 0};
            for(size_t j = row[idx]; j < row[idx + 1]; j++) {
                sum[0] += val[j] * x[0 + col[j]];
                sum[1] += val[j] * x[n + col[j]];
            }

            BOOST_CHECK_CLOSE(a[0], sum[0], 1e-8);
            BOOST_CHECK_CLOSE(a[1], sum[1], 1e-8);
            });

    Y = X + A * X;

    check_sample(Y, [&](size_t idx, elem_t a) {
            double sum[] = {0, 0};
            for(size_t j = row[idx]; j < row[idx + 1]; j++) {
                sum[0] += val[j] * x[0 + col[j]];
                sum[1] += val[j] * x[n + col[j]];
            }

            BOOST_CHECK_CLOSE(a[0], x[0 + idx] + sum[0], 1e-8);
            BOOST_CHECK_CLOSE(a[1], x[n + idx] + sum[1], 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(inline_multivector_product)
{
    const size_t n = 1024;
    const size_t m = 2;

    typedef std::array<double, m> elem_t;

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    std::vector<size_t> row;
    std::vector<size_t> col;
    std::vector<double> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<double> x = random_vector<double>(n * m);

    vex::SpMat <double> A(queue, n, n, row.data(), col.data(), val.data());

    vex::multivector<double,m> X(queue, x);
    vex::multivector<double,m> Y(queue, n);

    Y = cos(vex::make_inline(A * X));

    check_sample(Y, [&](size_t idx, elem_t a) {
            double sum[] = {0, 0};
            for(size_t j = row[idx]; j < row[idx + 1]; j++) {
                sum[0] += val[j] * x[0 + col[j]];
                sum[1] += val[j] * x[n + col[j]];
            }

            BOOST_CHECK_CLOSE(a[0], cos(sum[0]), 1e-8);
            BOOST_CHECK_CLOSE(a[1], cos(sum[1]), 1e-8);
            });
}

BOOST_AUTO_TEST_CASE(ccsr_multivector_product)
{
    const size_t n = 32;
    const size_t N = n * n * n;
    const double h2i = (n - 1) * (n - 1);

    typedef std::array<double, 2> elem_t;

    std::vector<size_t> idx;
    std::vector<size_t> row(3);
    std::vector<int>    col(8);
    std::vector<double> val(8);

    idx.reserve(N);

    row[0] = 0;
    row[1] = 1;
    row[2] = 8;

    col[0] = 0;
    val[0] = 1;

    col[1] = -static_cast<int>(n * n);
    col[2] = -static_cast<int>(n);
    col[3] =    -1;
    col[4] =     0;
    col[5] =     1;
    col[6] =     n;
    col[7] =  (n * n);

    val[1] = -h2i;
    val[2] = -h2i;
    val[3] = -h2i;
    val[4] =  h2i * 6;
    val[5] = -h2i;
    val[6] = -h2i;
    val[7] = -h2i;

    for(size_t k = 0; k < n; k++) {
        for(size_t j = 0; j < n; j++) {
            for(size_t i = 0; i < n; i++) {
                if (
                        i == 0 || i + 1 == n ||
                        j == 0 || j + 1 == n ||
                        k == 0 || k + 1 == n
                   )
                {
                    idx.push_back(0);
                } else {
                    idx.push_back(1);
                }
            }
        }
    }

    std::vector<double> x = random_vector<double>(N * 2);

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    vex::SpMatCCSR<double,int> A(queue[0], N, row.size() - 1,
            idx.data(), row.data(), col.data(), val.data());

    vex::multivector<double,2> X(queue, x);
    vex::multivector<double,2> Y(queue, N);

    Y = A * X;

    check_sample(Y, [&](size_t ii, elem_t a) {
            double sum[] = {0, 0};
            size_t i = idx[ii];
            for(size_t j = row[i]; j < row[i + 1]; j++) {
                sum[0] += val[j] * x[0 + ii + col[j]];
                sum[1] += val[j] * x[N + ii + col[j]];
            }

            BOOST_CHECK_CLOSE(a[0], sum[0], 1e-8);
            BOOST_CHECK_CLOSE(a[1], sum[1], 1e-8);
            });

    Y = X + A * X;

    check_sample(Y, [&](size_t ii, elem_t a) {
            double sum[] = {0, 0};
            size_t i = idx[ii];
            for(size_t j = row[i]; j < row[i + 1]; j++) {
                sum[0] += val[j] * x[0 + ii + col[j]];
                sum[1] += val[j] * x[N + ii + col[j]];
            }

            BOOST_CHECK_CLOSE(a[0], x[0 + ii] + sum[0], 1e-8);
            BOOST_CHECK_CLOSE(a[1], x[N + ii] + sum[1], 1e-8);
            });
}

#if defined(VEXCL_BACKEND_OPENCL) || defined(VEXCL_BACKEND_COMPUTE)
BOOST_AUTO_TEST_CASE(vector_valued_matrix)
{
    const size_t n = 1024;

    std::vector<size_t>     row;
    std::vector<size_t>     col;
    std::vector<cl_double2> val;

    random_matrix(n, n, 16, row, col, val);

    std::vector<cl_double2> x = random_vector<cl_double2>(n);

    vex::SpMat <cl_double2> A(ctx, n, n, row.data(), col.data(), val.data());
    vex::vector<cl_double2> X(ctx, x);
    vex::vector<cl_double2> Y(ctx, n);

    Y = A * X;

    check_sample(Y, [&](size_t idx, cl_double2 a) {
            cl_double2 sum = {{0, 0}};
            for(size_t j = row[idx]; j < row[idx + 1]; j++)
                sum += val[j] * x[col[j]];

            BOOST_CHECK_CLOSE(a.s[0], sum.s[0], 1e-8);
            BOOST_CHECK_CLOSE(a.s[1], sum.s[1], 1e-8);
            });
}

cl_double2 make_d2(double v) {
    cl_double2 d2 = {{v, v}};
    return d2;
}

BOOST_AUTO_TEST_CASE(vector_valued_ccsr_matrix)
{
    const size_t n = 32;
    const double h2i = (n - 1) * (n - 1);

    std::vector<size_t> idx;
    std::vector<size_t> row(3);
    std::vector<int>    col(8);
    std::vector<cl_double2> val(8);

    idx.reserve(n * n * n);

    row[0] = 0;
    row[1] = 1;
    row[2] = 8;

    col[0] = 0;
    val[0] = make_d2(1);

    col[1] = -static_cast<int>(n * n);
    col[2] = -static_cast<int>(n);
    col[3] =    -1;
    col[4] =     0;
    col[5] =     1;
    col[6] =     n;
    col[7] =  (n * n);

    val[1] = make_d2(-h2i);
    val[2] = make_d2(-h2i);
    val[3] = make_d2(-h2i);
    val[4] = make_d2( h2i * 6);
    val[5] = make_d2(-h2i);
    val[6] = make_d2(-h2i);
    val[7] = make_d2(-h2i);

    for(size_t k = 0; k < n; k++) {
        for(size_t j = 0; j < n; j++) {
            for(size_t i = 0; i < n; i++) {
                if (
                        i == 0 || i == (n - 1) ||
                        j == 0 || j == (n - 1) ||
                        k == 0 || k == (n - 1)
                   )
                {
                    idx.push_back(0);
                } else {
                    idx.push_back(1);
                }
            }
        }
    }

    std::vector<cl_double2> x = random_vector<cl_double2>(n * n * n);

    std::vector<vex::command_queue> queue(1, ctx.queue(0));

    vex::SpMatCCSR<cl_double2,int> A(queue[0], x.size(), row.size() - 1,
            idx.data(), row.data(), col.data(), val.data());

    vex::vector<cl_double2> X(queue, x);
    vex::vector<cl_double2> Y(queue, x.size());

    Y = A * X;

    check_sample(Y, [&](size_t ii, cl_double2 a) {
            cl_double2 sum = {{0, 0}};
            size_t i = idx[ii];
            for(size_t j = row[i]; j < row[i + 1]; j++)
                sum += val[j] * x[ii + col[j]];

            BOOST_CHECK_CLOSE(a.s[0], sum.s[0], 1e-8);
            BOOST_CHECK_CLOSE(a.s[1], sum.s[1], 1e-8);
            });
}
#endif

BOOST_AUTO_TEST_SUITE_END()


#include "marginal_algorithm.h"

#include <Eigen/Dense>
#include <stan/math/prim/fun.hpp>

#include "lib/progressbar/progressbar.h"
#include "marginal_state.pb.h"
#include "src/algorithms/base_algorithm.h"
#include "src/collectors/base_collector.h"
#include "src/mixings/marginal_mixing.h"
#include "src/utils/eigen_utils.h"

void MarginalAlgorithm::initialize() {
  BaseAlgorithm::initialize();
  marg_mixing = std::dynamic_pointer_cast<MarginalMixing>(mixing);
}

//! \param grid      Grid of points in matrix form to evaluate the density on
//! \param collector Collector containing the algorithm chain
//! \return          Matrix whose i-th column is the lpdf at i-th iteration
Eigen::MatrixXd MarginalAlgorithm::eval_lpdf(
    BaseCollector *const collector, const Eigen::MatrixXd &grid,
    const Eigen::MatrixXd &hier_covariates /*= Eigen::MatrixXd(0, 0)*/,
    const Eigen::MatrixXd &mix_covariates /*= Eigen::MatrixXd(0, 0)*/) {
  std::deque<Eigen::VectorXd> lpdf;
  bool keep = true;
  progresscpp::ProgressBar bar(collector->get_size(), 60);
  while (keep) {
    keep = update_state_from_collector(collector);
    if (!keep) {
      break;
    }
    lpdf.push_back(lpdf_from_state(grid, hier_covariates, mix_covariates));
    ++bar;
    bar.display();
  }
  collector->reset();
  bar.done();
  return bayesmix::stack_vectors(lpdf);
}

Eigen::VectorXd MarginalAlgorithm::lpdf_from_state(
    const Eigen::MatrixXd &grid, const Eigen::MatrixXd &hier_covariates,
    const Eigen::MatrixXd &mix_covariates) {
  Eigen::VectorXd lpdf(grid.rows());
  unsigned int n_data = curr_state.cluster_allocs_size();
  unsigned int n_clust = curr_state.cluster_states_size();
  marg_mixing->set_state_from_proto(curr_state.mixing_state());
  Eigen::MatrixXd lpdf_local(grid.rows(), n_clust + 1);
  auto temp_hier = unique_values[0]->clone();
  for (size_t j = 0; j < n_clust; j++) {
    temp_hier->set_state_from_proto(curr_state.cluster_states(j));
    lpdf_local.col(j) =
        marg_mixing->mass_existing_cluster(n_data, true, false, temp_hier) +
        temp_hier->like_lpdf_grid(grid, hier_covariates).array();
    // TODO add mixing covariate
  }
  lpdf_local.col(n_clust) =
      marg_mixing->mass_new_cluster(n_data, true, false, n_clust) +
      lpdf_marginal_component(temp_hier, grid, hier_covariates).array();
  // TODO add mixing covariate

  for (size_t j = 0; j < grid.rows(); j++) {
    lpdf(j) = stan::math::log_sum_exp(lpdf_local.row(j));
  }
  return lpdf;
}

bool MarginalAlgorithm::update_state_from_collector(BaseCollector *coll) {
  bool success = coll->get_next_state(&curr_state);
  return success;
}


/**
 * \file
 * \copyright
 * Copyright (c) 2012-2020, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 */

#include "LevelSetFunction.h"

#include <boost/math/special_functions/sign.hpp>
#include <numeric>

#include "BaseLib/Algorithm.h"
#include "BranchProperty.h"
#include "FractureProperty.h"
#include "JunctionProperty.h"

namespace
{
// Heaviside step function
inline double Heaviside(double v)
{
    return (v < 0.0) ? 0.0 : 1.0;
}

}  // namespace

namespace ProcessLib
{
namespace LIE
{
double levelsetFracture(FractureProperty const& frac, Eigen::Vector3d const& x)
{
    return boost::math::sign(
        frac.normal_vector.dot(x - frac.point_on_fracture));
}

double levelsetBranch(BranchProperty const& branch, Eigen::Vector3d const& x)
{
    return boost::math::sign(
        branch.normal_vector_branch.dot(x - branch.coords));
}

std::vector<double> uGlobalEnrichments(
    std::vector<FractureProperty*> const& frac_props,
    std::vector<JunctionProperty*> const& junction_props,
    std::unordered_map<int, int> const& fracID_to_local,
    Eigen::Vector3d const& x)
{
    // pre-calculate levelsets for all fractures
    std::vector<double> levelsets(frac_props.size());
    for (std::size_t i = 0; i < frac_props.size(); i++)
    {
        levelsets[i] = Heaviside(levelsetFracture(*frac_props[i], x));
    }

    std::vector<double> enrichments(frac_props.size() + junction_props.size());
    // fractures possibly with branches
    for (std::size_t i = 0; i < frac_props.size(); i++)
    {
        auto const* frac = frac_props[i];
        enrichments[i] = std::accumulate(
            cbegin(frac->branches_slave), cend(frac->branches_slave),
            levelsets[i], [&](double const& enrich, auto const& branch) {
                return enrich * Heaviside(levelsetBranch(branch, x));
            });
    }

    // junctions
    for (std::size_t i = 0; i < junction_props.size(); i++)
    {
        auto const* junction = junction_props[i];
        auto fid1 = fracID_to_local.at(junction->fracture_ids[0]);
        auto fid2 = fracID_to_local.at(junction->fracture_ids[1]);
        double enrich = levelsets[fid1] * levelsets[fid2];
        enrichments[i + frac_props.size()] = enrich;
    }

    return enrichments;
}

std::vector<double> duGlobalEnrichments(
    std::size_t this_frac_id,
    std::vector<FractureProperty*> const& frac_props,
    std::vector<JunctionProperty*> const& junction_props,
    std::unordered_map<int, int> const& fracID_to_local,
    Eigen::Vector3d const& x)
{
    auto this_frac_local_index = fracID_to_local.at(this_frac_id);
    auto const& this_frac = *frac_props[this_frac_local_index];
    // pre-calculate levelsets for all fractures
    std::vector<double> levelsets(frac_props.size());
    for (std::size_t i = 0; i < frac_props.size(); i++)
    {
        levelsets[i] = Heaviside(levelsetFracture(*frac_props[i], x));
    }

    std::vector<double> enrichments(frac_props.size() + junction_props.size());
    enrichments[this_frac_local_index] = 1.0;

    // fractures possibly with branches
    if (frac_props.size() > 1)
    {
        for (auto const& branch : this_frac.branches_master)
        {
            if (branch.master_fracture_id != this_frac.fracture_id)
            {
                continue;
            }

            if (fracID_to_local.find(branch.slave_fracture_id) ==
                fracID_to_local.end())
            {
                continue;
            }

            double singned = boost::math::sign(
                this_frac.normal_vector.dot(branch.normal_vector_branch));
            auto slave_fid = fracID_to_local.at(branch.slave_fracture_id);
            double enrich = singned * levelsets[slave_fid];
            enrichments[slave_fid] = enrich;
        }
    }

    // junctions
    for (unsigned i = 0; i < junction_props.size(); i++)
    {
        auto const* junction = junction_props[i];
        if (!BaseLib::contains(junction->fracture_ids, this_frac.fracture_id))
        {
            continue;
        }

        auto another_frac_id =
            (junction->fracture_ids[0] == this_frac.fracture_id)
                ? junction->fracture_ids[1]
                : junction->fracture_ids[0];
        auto fid = fracID_to_local.at(another_frac_id);
        double enrich = levelsets[fid];
        enrichments[i + frac_props.size()] = enrich;
    }

    return enrichments;
}

}  // namespace LIE
}  // namespace ProcessLib


/******************************************************************************
 * Copyright (C) 2014 by Jerome Maye                                          *
 * jerome.maye@gmail.com                                                      *
 ******************************************************************************/

#include "aslam/calibration/car/error-terms/ErrorTermSteering.h"

#include <cmath>

#include <Eigen/Dense>

#include <sm/kinematics/rotations.hpp>

#include <aslam/calibration/data-structures/VectorDesignVariable.h>

namespace aslam {
  namespace calibration {

/******************************************************************************/
/* Constructors and Destructor                                                */
/******************************************************************************/

    ErrorTermSteering::ErrorTermSteering(const
        aslam::backend::EuclideanExpression& v_v_mw, double measurement, double
        sigma2, VectorDesignVariable<4>* params) :
        _v_v_mw(v_v_mw),
        _measurement(measurement),
        _sigma2(sigma2),
        _params(params) {
      Eigen::Matrix<double, 1, 1> sigma2_mat;
      sigma2_mat << sigma2;
      setInvR(sigma2_mat.inverse());
      aslam::backend::DesignVariable::set_t dv;
      v_v_mw.getDesignVariables(dv);
      dv.insert(params);
      setDesignVariablesIterator(dv.begin(), dv.end());
    }

    ErrorTermSteering::ErrorTermSteering(const ErrorTermSteering& other) :
        ErrorTermFs<1>(other),
        _v_v_mw(other._v_v_mw),
        _measurement(other._measurement),
        _sigma2(other._sigma2),
        _params(other._params) {
    }

    ErrorTermSteering& ErrorTermSteering::operator = (const ErrorTermSteering&
        other) {
      if (this != &other) {
        ErrorTermFs<1>::operator=(other);
       _v_v_mw = other._v_v_mw;
       _measurement = other._measurement;
       _sigma2 = other._sigma2;
       _params = other._params;
      }
      return *this;
    }

    ErrorTermSteering::~ErrorTermSteering() {
    }

/******************************************************************************/
/* Accessors                                                                  */
/******************************************************************************/

    double ErrorTermSteering::getMeasurement() const {
      return _measurement;
    }

    void ErrorTermSteering::setMeasurement(double measurement) {
      _measurement = measurement;
    }

    double ErrorTermSteering::getVariance() const {
      return _sigma2;
    }

    void ErrorTermSteering::setVariance(double sigma2) {
      _sigma2 = sigma2;
    }

/******************************************************************************/
/* Methods                                                                    */
/******************************************************************************/

    double ErrorTermSteering::evaluateErrorImplementation() {
      error_t error;
      const double a0 = _params->getValue()(0);
      const double a1 = _params->getValue()(1);
      const double a2 = _params->getValue()(2);
      const double a3 = _params->getValue()(3);
      const double v0 = _v_v_mw.toValue()(0);
      const double v1 = _v_v_mw.toValue()(1);
      const double phi = std::atan2(v1, v0);
      error(0) = a0 + a1 * _measurement + a2 * _measurement * _measurement +
        a3 * _measurement * _measurement * _measurement - phi;
      error(0) = sm::kinematics::angleMod(error(0));
      setError(error);
      return evaluateChiSquaredError();
    }

    void ErrorTermSteering::evaluateJacobiansImplementation(
        aslam::backend::JacobianContainer& jacobians) {
      const double v0 = _v_v_mw.toValue()(0);
      const double v1 = _v_v_mw.toValue()(1);
      Eigen::Matrix<double, 1, 4> J_a = Eigen::Matrix<double, 1, 4>::Zero();
      J_a(0, 0) = 1;
      J_a(0, 1) = _measurement;
      J_a(0, 2) = _measurement * _measurement;
      J_a(0, 3) = _measurement * _measurement * _measurement;
      jacobians.add(_params, J_a);
      Eigen::Matrix<double, 1, 3> J_v_v_mw =
        Eigen::Matrix<double, 1, 3>::Zero();
      J_v_v_mw(0, 0) = -v1 / (v0 * v0 * (v1 * v1 / (v0 * v0) + 1));
      J_v_v_mw(0, 1) = 1 / (v0 * (v1 * v1 / (v0 * v0) + 1));
      _v_v_mw.evaluateJacobians(jacobians, -J_v_v_mw);
    }

  }
}


// This is main.
// This is MD simulation of particles interacting with a LJ potential in a periodic box

using namespace std;

#include<iostream>
#include<iomanip>
#include<fstream>
#include<cmath>
#include<vector>
#include "simulationbox.h"
#include "particle.h"
#include "functions.h"
#include <iostream>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_matrix.h>
#include <boost/program_options.hpp>

using namespace std;
using namespace boost::program_options;

void update_forces(vector<PARTICLE>&, SIMULATIONBOX&, double);
double update_energies(vector<PARTICLE>&, SIMULATIONBOX&, double);

int main(int argc, char* argv[])

{
  // we begin with some fundamental numbers/physical constants
  const double pi = 3.141593;	// Pi
  const double kB = 1.38e-23;	// Joules per Kelvin
  const double mol = 6.0e23;	// Avogadro number

  // reduced units
  double unitlength = 0.3405e-9; // m; unit of length is these many meters (diameter of a typical atom)
  double unitenergy = 119.8 * kB;// Joules; unit of energy is these many Joules (typical interaction strength)
  double unitmass = 0.03994 / mol; // kg; unit of mass (mass of a typical atom)
  double unittime = sqrt(unitmass * unitlength * unitlength / unitenergy); // Unit of time
  double unittemperature = unitenergy/kB;			// unit of temperature

  // essential parameters needed for particles in a box simulation
  /**********************/
  double ljatom_density=0.8442 ;	// this is taken in as mass density (in reduced units)
  int number_ljatom=8;		// total number of particles in your system
  double ljatom_diameter;
  double ljatom_mass;
  double bx, by, bz;		// box edge lengths
  double temperature;
  double dcut;			// cutoff distance for potential in reduced units
  /**********************/

    options_description desc("Usage:\n Manyparticle <options>");
    desc.add_options()
            ("help,h", "print usage message")
            ("density,d", value<double>(&ljatom_density)->default_value(0.8442),
             "enter density (in LJ reduced units: recall unit of length is diameter of your lj particle); tested for rho = 0.8442 ")                // enter in nanometers
            ("number_ljatom,n", value<int>(&number_ljatom)->default_value(108),
             "enter total number of ljatoms (108 is a typical value; tested for this value) ");

    variables_map vm;
    store(parse_command_line(argc, argv, desc), vm);
    notify(vm);
    if (vm.count("help")) {
        std::cout << desc << "\n";
        return 0;
    }

  cout << "\nProgram starts\n";
  cout << "units of length, energy, mass are given near the top of the main function" << endl;
  cout << "unit of length is " << unitlength << " meters" << endl;
  cout << "unit of mass is " << unitmass << " kilograms" << endl;
  cout << "unit of energy is " << unitenergy << " Joules" << endl;
  cout << "unit of time is " << unittime << " seconds" << endl;
  cout << "enter density (in LJ reduced units: recall unit of length is diameter of your lj particle); tested for rho = 0.8442  :" << ljatom_density <<endl;
  //cin >> ljatom_density;
  cout << "enter total number of ljatoms (108 is a typical value; tested for this value) :" << number_ljatom << endl;
  //cin >> number_ljatom;
  dcut = 2.5;

  ljatom_diameter = 1.0;	// in reduced units
  ljatom_mass = 1.0;

  double edge_length = pow(number_ljatom/ljatom_density,1.0/3.0);
  cout << "edge length calculated to be " << edge_length << endl;

#pragma omp parallel default(shared)
  {
    if (omp_get_thread_num() == 0) {
     printf("The app comes with OpenMP parallelization)\n");
     printf("Number of OpenMP threads %d\n", omp_get_num_threads());
     printf("Make sure that number of ions is greater than %d\n", omp_get_num_threads());
   }
  }

  // Different parts of the system
  vector<PARTICLE> ljatom;		// all particles in the system

  bx = edge_length;
  by = edge_length;
  bz = edge_length;
  SIMULATIONBOX simulation_box = SIMULATIONBOX(VECTOR3D(0,0,0),bx,by,bz);		// place where the action happens, periodic boundaries
  simulation_box.put_ljatoms_inside(ljatom, number_ljatom, ljatom_diameter, ljatom_mass, ljatom_density);

  // output to screen the parameters of the problem
  cout << "Box dimensions x | y | z " << setw(15) << simulation_box.lx << setw(15) << simulation_box.ly << setw(15) << simulation_box.lz << endl;
  cout << "Number of LJ atoms inside the box " << ljatom.size() << endl;
  cout << "LJ atom diameter (in SI units; meter) " << ljatom_diameter*unitlength << endl;
  cout << "LJ atom mass (in SI units; kg) " << ljatom_mass*unitmass << endl;
  cout << "cut off distance (in reduced units) " << dcut << endl;

  initialize_particle_velocities(ljatom);

  update_forces(ljatom, simulation_box, dcut);	// expensive step

  double delta_t, totaltime;
  totaltime = 200;
  int steps;		// number of time discretizations (slices)
  steps = 200000;

  delta_t = totaltime/steps;	// the code deterimes the time-step delta_t. choose steps carefully, make sure you have a fine discretization
  cout << "timestep (reduced units) " << delta_t << endl;
  cout << "timestep (in SI units; second) " << delta_t*unittime << endl;
  cout << "total simulation time (in real units) " << totaltime*unittime << endl;

  char filename_movie[200];
  sprintf(filename_movie, "propagation_rho=%f_N=%d.lammpstrj", ljatom_density, number_ljatom);

  ofstream list_propagation(filename_movie, ios::out); // create a file to store data; first line displays the labels

  // filing the data with naming within the code. -- sometimes could be handy -- uncomment next three lines and comment the 4th if you want that.
  char filename[200];
  sprintf(filename, "energy_rho=%f_N=%d.out", ljatom_density, number_ljatom);
  ofstream output_energy(filename, ios::out);
  //ofstream output_energy("energy.out", ios::out);

  double totalpe = update_energies(ljatom, simulation_box, dcut);

  double totalke = 0.0;
  for (unsigned int i = 0; i < ljatom.size(); i++)
  {
    ljatom[i].kinetic_energy();
    totalke += ljatom[i].ke;
  }
  output_energy << 0 << "  " << totalke/ljatom.size() << "  " << totalpe/ljatom.size() << "  " << (totalke+totalpe)/ljatom.size() << endl;
  cout << "initial total energy per lj particle is " << (totalke+totalpe)/ljatom.size() << endl;

  int movie_frequency = 100;

  double average_pe = 0.0;
  double average_ke = 0.0;
  int data_collect_frequency = 1000;
  int samples = 0;

  int hit_eqm = 3000; // this is your choice of where you think the system hit equilibrium

  make_movie(0,ljatom,simulation_box.lx,simulation_box.ly,simulation_box.lz,list_propagation);

  // Molecular Dynamics
  cout << "progress..." << endl;
  for (int num = 1; num <= steps; num++)
  {
    // velocity-Verlet
    for (unsigned int i = 0; i < ljatom.size(); i++)
      ljatom[i].update_velocity(delta_t);// update velocity half timestep
    for (unsigned int i = 0; i < ljatom.size(); i++)
      ljatom[i].update_position(delta_t);// update position full timestep

    update_forces(ljatom, simulation_box, dcut);	// expensive step
    double totalpe;
    if (num%data_collect_frequency == 0)
      totalpe = update_energies(ljatom, simulation_box, dcut);

    for (unsigned int i = 0; i < ljatom.size(); i++)
      ljatom[i].update_velocity(delta_t);// update velocity half timestep

    double totalke = 0.0;
    for (unsigned int i = 0; i < ljatom.size(); i++)
    {
      ljatom[i].kinetic_energy();
      totalke += ljatom[i].ke;
    }

    // calling a movie function to get a movie of the simulation
    if (num%movie_frequency == 0)
      make_movie(num,ljatom,simulation_box.lx,simulation_box.ly,simulation_box.lz,list_propagation);

      // outputting the energy to make sure simulation can be trusted
    if (num%data_collect_frequency == 0)
        output_energy << num << "  " << totalke/ljatom.size() << "  " << totalpe/ljatom.size() << "  " << (totalke+totalpe)/ljatom.size() << endl;

    if (num > hit_eqm && num%data_collect_frequency == 0)
    {
      average_pe = average_pe + totalpe;
      average_ke = average_ke + totalke;
      samples++;
    }
    double fraction_completed = ((num)/(double)steps);
    ProgressBar(fraction_completed);
  }

  cout << endl;
  cout << "average pe per particle is " << (average_pe/samples)/ljatom.size() << endl;
  cout << "average ke per particle is " << (average_ke/samples)/ljatom.size() << endl;
  cout << "average T per particle is " << 2*(average_ke/samples)/(3*number_ljatom) << endl;

  return 0;
}
// End of main