/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2009 - 2021 by the deal.II authors
 *
 * This file is part of the deal.II library.
 *
 * The deal.II library is free software; you can use it, redistribute
 * it, and/or modify it under the terms of the GNU Lesser General
 * Public License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 * The full text of the license can be found in the file LICENSE.md at
 * the top level directory of deal.II.
 *
 * ---------------------------------------------------------------------
 *
 * Author: Guido Kanschat, Texas A&M University, 2009
 *         Timo Heister, Clemson University, 2019
 */

// 前面几个文件已经在前面的例子中讲过了，因此不再做进一步的评论。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/numerics/data_out.h>
#include <deal.II/fe/mapping_q1.h>

// 这里定义了不连续的有限元。它们的使用方式与所有其他有限元相同，不过--正如你在以前的教程程序中所看到的--用户与有限元类的交互根本不多：它们被传递给 DoFHandler 和 FEValues 对象，仅此而已。

#include <deal.II/fe/fe_dgq.h>

// FEInterfaceValues需要这个头来计算界面上的积分。

#include <deal.II/fe/fe_interface_values.h>

// 我们将使用最简单的求解器，称为Richardson迭代，它代表了一个简单的缺陷修正。这与一个块状SSOR预处理器（定义在precondition_block.h中）相结合，该预处理器使用DG离散产生的系统矩阵的特殊块状结构。

#include <deal.II/lac/solver_richardson.h>
#include <deal.II/lac/precondition_block.h>

// 我们将使用梯度作为细化指标。

#include <deal.II/numerics/derivative_approximation.h>

// 最后，新的包含文件用于使用MeshWorker框架中的Mesh_loop。

#include <deal.II/meshworker/mesh_loop.h>

// 像所有的程序一样，我们在完成这一部分时，要包括所需的C++头文件，并声明我们要使用dealii命名空间中的对象，不含前缀。

#include <iostream>
#include <fstream>

namespace Step12
{
  using namespace dealii;
// @sect3{Equation data}

// 首先，我们定义一个描述不均匀边界数据的类。由于只使用它的值，我们实现value_list()，但不定义Function的所有其他函数。

  template <int dim>
  class BoundaryValues : public Function<dim>
  {
  public:
    BoundaryValues() = default;
    virtual void value_list(const std::vector<Point<dim>> &points,
                            std::vector<double> &          values,
                            const unsigned int component = 0) const override;
  };

// 考虑到流动方向，单位方块 $[0,1]^2$ 的流入边界是右边界和下边界。我们在x轴上规定了不连续的边界值1和0，在右边界上规定了值0。该函数在流出边界上的值将不会在DG方案中使用。

  template <int dim>
  void BoundaryValues<dim>::value_list(const std::vector<Point<dim>> &points,
                                       std::vector<double> &          values,
                                       const unsigned int component) const
  {
    (void)component;
    AssertIndexRange(component, 1);
    Assert(values.size() == points.size(),
           ExcDimensionMismatch(values.size(), points.size()));

    for (unsigned int i = 0; i < values.size(); ++i)
      {
        if (points[i](0) < 0.5)
          values[i] = 1.;
        else
          values[i] = 0.;
      }
  }

// 最后，一个计算并返回风场的函数  $\beta=\beta(\mathbf x)$  。正如在介绍中所解释的，在2D中我们将使用一个围绕原点的旋转场。在3D中，我们只需不设置 $z$ 分量（即为零），而这个函数在目前的实现中不能用于1D。

  template <int dim>
  Tensor<1, dim> beta(const Point<dim> &p)
  {
    Assert(dim >= 2, ExcNotImplemented());

    Tensor<1, dim> wind_field;
    wind_field[0] = -p[1];
    wind_field[1] = p[0];

    if (wind_field.norm() > 1e-10)
      wind_field /= wind_field.norm();

    return wind_field;
  }
// @sect3{The ScratchData and CopyData classes}

// 以下对象是我们在调用 MeshWorker::mesh_loop(). 时使用的抓取和复制对象 新对象是FEInterfaceValues对象，它的工作原理类似于FEValues或FEFacesValues，只是它作用于两个单元格之间的接口，并允许我们以我们的弱形式组装接口条款。

  template <int dim>
  struct ScratchData
  {
    ScratchData(const Mapping<dim> &       mapping,
                const FiniteElement<dim> & fe,
                const Quadrature<dim> &    quadrature,
                const Quadrature<dim - 1> &quadrature_face,
                const UpdateFlags          update_flags = update_values |
                                                 update_gradients |
                                                 update_quadrature_points |
                                                 update_JxW_values,
                const UpdateFlags interface_update_flags =
                  update_values | update_gradients | update_quadrature_points |
                  update_JxW_values | update_normal_vectors)
      : fe_values(mapping, fe, quadrature, update_flags)
      , fe_interface_values(mapping,
                            fe,
                            quadrature_face,
                            interface_update_flags)
    {}

    ScratchData(const ScratchData<dim> &scratch_data)
      : fe_values(scratch_data.fe_values.get_mapping(),
                  scratch_data.fe_values.get_fe(), fas
                  scratch_data.fe_values.get_quadrature(),
                  scratch_data.fe_values.get_update_flags())
      , fe_interface_values(scratch_data.fe_interface_values.get_mapping(),
                            scratch_data.fe_interface_values.get_fe(),
                            scratch_data.fe_interface_values.get_quadrature(),
                            scratch_data.fe_interface_values.get_update_flags())
    {}

    FEValues<dim>          fe_values;
    FEInterfaceValues<dim> fe_interface_values;
  };

  struct CopyDataFace
  {
    FullMatrix<double>                   cell_matrix;
    std::vector<types::global_dof_index> joint_dof_indices;
  };

  struct CopyData
  {
    FullMatrix<double>                   cell_matrix;
    Vector<double>                       cell_rhs;
    std::vector<types::global_dof_index> local_dof_indices;
    std::vector<CopyDataFace>            face_data;

    template <class Iterator>
    void reinit(const Iterator &cell, unsigned int dofs_per_cell)
    {
      cell_matrix.reinit(dofs_per_cell, dofs_per_cell);
      cell_rhs.reinit(dofs_per_cell);

      local_dof_indices.resize(dofs_per_cell);
      cell->get_dof_indices(local_dof_indices);
    }
  };
// @sect3{The AdvectionProblem class}

// 在这个准备工作之后，我们继续进行这个程序的主类，称为AdvectionProblem。

// 这对你来说应该是非常熟悉的。有趣的细节只有在实现集合函数的时候才会出现。

  template <int dim>
  class AdvectionProblem
  {
  public:
    AdvectionProblem();
    void run();

  private:
    void setup_system();
    void assemble_system();
    void solve();
    void refine_grid();
    void output_results(const unsigned int cycle) const;

    Triangulation<dim>   triangulation;
    const MappingQ1<dim> mapping;

// 此外，我们要使用DG元素。

    const FE_DGQ<dim> fe;
    DoFHandler<dim>   dof_handler;

    const QGauss<dim>     quadrature;
    const QGauss<dim - 1> quadrature_face;

// 接下来的四个成员代表要解决的线性系统。  system_matrix and right_hand_side 是由 assemble_system(), the solution 产生的，在 solve(). The sparsity_pattern 中计算，用于确定 system_matrix 中非零元素的位置。

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;

    Vector<double> solution;
    Vector<double> right_hand_side;
  };

// 我们从构造函数开始。 fe 的构造器调用中的1是多项式的度数。

  template <int dim>
  AdvectionProblem<dim>::AdvectionProblem()
    : mapping()
    , fe(1)
    , dof_handler(triangulation)
    , quadrature(fe.tensor_degree() + 1)
    , quadrature_face(fe.tensor_degree() + 1)
  {}

  template <int dim>
  void AdvectionProblem<dim>::setup_system()
  {

// 在设置通常的有限元数据结构的函数中，我们首先需要分配DoF。

    dof_handler.distribute_dofs(fe);

// 我们从生成稀疏模式开始。为此，我们首先用系统中出现的耦合物填充一个动态稀疏模式（DynamicSparsityPattern）类型的中间对象。在建立模式之后，这个对象被复制到 sparsity_pattern 并可以被丢弃。

// 为了建立DG离散的稀疏模式，我们可以调用类似于 DoFTools::make_sparsity_pattern, 的函数，该函数被称为 DoFTools::make_flux_sparsity_pattern:  。
    DynamicSparsityPattern dsp(dof_handler.n_dofs());
    DoFTools::make_flux_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

// 最后，我们设置了线性系统的所有组成部分的结构。

    system_matrix.reinit(sparsity_pattern);
    solution.reinit(dof_handler.n_dofs());
    right_hand_side.reinit(dof_handler.n_dofs());
  }
// @sect4{The assemble_system function}

// 这里我们看到了与手工组装的主要区别。我们不需要在单元格和面上写循环，而是在调用 MeshWorker::mesh_loop() 时包含逻辑，我们只需要指定在每个单元格、每个边界面和每个内部面应该发生什么。这三个任务是由下面的函数里面的lambda函数处理的。

  template <int dim>
  void AdvectionProblem<dim>::assemble_system()
  {
    using Iterator = typename DoFHandler<dim>::active_cell_iterator;
    const BoundaryValues<dim> boundary_function;

// 这是将对每个单元格执行的函数。

    const auto cell_worker = [&](const Iterator &  cell,
                                 ScratchData<dim> &scratch_data,
                                 CopyData &        copy_data) {
      const unsigned int n_dofs =
        scratch_data.fe_values.get_fe().n_dofs_per_cell();
      copy_data.reinit(cell, n_dofs);
      scratch_data.fe_values.reinit(cell);

      const auto &q_points = scratch_data.fe_values.get_quadrature_points();

      const FEValues<dim> &      fe_v = scratch_data.fe_values;
      const std::vector<double> &JxW  = fe_v.get_JxW_values();

// 我们解决的是一个同质方程，因此在单元项中没有显示出右手。 剩下的就是整合矩阵条目。

      for (unsigned int point = 0; point < fe_v.n_quadrature_points; ++point)
        {
          auto beta_q = beta(q_points[point]);
          for (unsigned int i = 0; i < n_dofs; ++i)
            for (unsigned int j = 0; j < n_dofs; ++j)
              {
                copy_data.cell_matrix(i, j) +=
                  -beta_q                      // -\beta
                  * fe_v.shape_grad(i, point)  // \nabla \phi_i
                  * fe_v.shape_value(j, point) // \phi_j
                  * JxW[point];                // dx
              }
        }
    };

// 这是为边界面调用的函数，包括使用FEFaceValues的正常积分。新的逻辑是决定该术语是进入系统矩阵（流出）还是进入右手边（流入）。

    const auto boundary_worker = [&](const Iterator &    cell,
                                     const unsigned int &face_no,
                                     ScratchData<dim> &  scratch_data,
                                     CopyData &          copy_data) {
      scratch_data.fe_interface_values.reinit(cell, face_no);
      const FEFaceValuesBase<dim> &fe_face =
        scratch_data.fe_interface_values.get_fe_face_values(0);

      const auto &q_points = fe_face.get_quadrature_points();

      const unsigned int n_facet_dofs = fe_face.get_fe().n_dofs_per_cell();
      const std::vector<double> &        JxW     = fe_face.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals = fe_face.get_normal_vectors();

      std::vector<double> g(q_points.size());
      boundary_function.value_list(q_points, g);

      for (unsigned int point = 0; point < q_points.size(); ++point)
        {
          const double beta_dot_n = beta(q_points[point]) * normals[point];

          if (beta_dot_n > 0)
            {
              for (unsigned int i = 0; i < n_facet_dofs; ++i)
                for (unsigned int j = 0; j < n_facet_dofs; ++j)
                  copy_data.cell_matrix(i, j) +=
                    fe_face.shape_value(i, point)   // \phi_i
                    * fe_face.shape_value(j, point) // \phi_j
                    * beta_dot_n                    // \beta . n
                    * JxW[point];                   // dx
            }
          else
            for (unsigned int i = 0; i < n_facet_dofs; ++i)
              copy_data.cell_rhs(i) += -fe_face.shape_value(i, point) // \phi_i
                                       * g[point]                     // g
                                       * beta_dot_n  // \beta . n
                                       * JxW[point]; // dx
        }
    };

// 这是在内部面调用的函数。参数指定了单元格、面和子面的指数（用于自适应细化）。我们只是将它们传递给FEInterfaceValues的reinit()函数。

    const auto face_worker = [&](const Iterator &    cell,
                                 const unsigned int &f,
                                 const unsigned int &sf,
                                 const Iterator &    ncell,
                                 const unsigned int &nf,
                                 const unsigned int &nsf,
                                 ScratchData<dim> &  scratch_data,
                                 CopyData &          copy_data) {
      FEInterfaceValues<dim> &fe_iv = scratch_data.fe_interface_values;
      fe_iv.reinit(cell, f, sf, ncell, nf, nsf);
      const auto &q_points = fe_iv.get_quadrature_points();

      copy_data.face_data.emplace_back();
      CopyDataFace &copy_data_face = copy_data.face_data.back();

      const unsigned int n_dofs        = fe_iv.n_current_interface_dofs();
      copy_data_face.joint_dof_indices = fe_iv.get_interface_dof_indices();

      copy_data_face.cell_matrix.reinit(n_dofs, n_dofs);

      const std::vector<double> &        JxW     = fe_iv.get_JxW_values();
      const std::vector<Tensor<1, dim>> &normals = fe_iv.get_normal_vectors();

      for (unsigned int qpoint = 0; qpoint < q_points.size(); ++qpoint)
        {
          const double beta_dot_n = beta(q_points[qpoint]) * normals[qpoint];
          for (unsigned int i = 0; i < n_dofs; ++i)
            for (unsigned int j = 0; j < n_dofs; ++j)
              copy_data_face.cell_matrix(i, j) +=
                fe_iv.jump(i, qpoint) // [\phi_i]
                *
                fe_iv.shape_value((beta_dot_n > 0), j, qpoint) // phi_j^{upwind}
                * beta_dot_n                                   // (\beta . n)
                * JxW[qpoint];                                 // dx
        }
    };

// 下面的lambda函数将处理从单元格和面组件中复制数据到全局矩阵和右侧的问题。

// 虽然我们不需要AffineConstraints对象，因为在DG离散中没有悬空节点约束，但我们在这里使用一个空对象，因为这允许我们使用其`copy_local_to_global`功能。

    const AffineConstraints<double> constraints;

    const auto copier = [&](const CopyData &c) {
      constraints.distribute_local_to_global(c.cell_matrix,
                                             c.cell_rhs,
                                             c.local_dof_indices,
                                             system_matrix,
                                             right_hand_side);

      for (auto &cdf : c.face_data)
        {
          constraints.distribute_local_to_global(cdf.cell_matrix,
                                                 cdf.joint_dof_indices,
                                                 system_matrix);
        }
    };

    ScratchData<dim> scratch_data(mapping, fe, quadrature, quadrature_face);
    CopyData         copy_data;

// 在这里，我们最终处理了装配问题。我们传入ScratchData和CopyData对象，以及上面的lambda函数，并指定我们要对内部面进行一次装配。

    MeshWorker::mesh_loop(dof_handler.begin_active(),
                          dof_handler.end(),
                          cell_worker,
                          copier,
                          scratch_data,
                          copy_data,
                          MeshWorker::assemble_own_cells |
                            MeshWorker::assemble_boundary_faces |
                            MeshWorker::assemble_own_interior_faces_once,
                          boundary_worker,
                          face_worker);
  }
// @sect3{All the rest}

// 对于这个简单的问题，我们使用了最简单的求解器，称为Richardson迭代，它代表了简单的缺陷修正。这与一个块状SSOR预处理相结合，该预处理使用DG离散化产生的系统矩阵的特殊块状结构。这些块的大小是每个单元的DoF数量。这里，我们使用SSOR预处理，因为我们没有根据流场对DoFs进行重新编号。如果在流的下游方向对DoFs进行重新编号，那么块状的Gauss-Seidel预处理（见PreconditionBlockSOR类，放松=1）会做得更好。

  template <int dim>
  void AdvectionProblem<dim>::solve()
  {
    SolverControl                    solver_control(1000, 1e-12);
    SolverRichardson<Vector<double>> solver(solver_control);

// 这里我们创建了预处理程序。

    PreconditionBlockSSOR<SparseMatrix<double>> preconditioner;

// 然后将矩阵分配给它，并设置正确的块大小。

    preconditioner.initialize(system_matrix, fe.n_dofs_per_cell());

// 做完这些准备工作后，我们就可以启动线性求解器了。

    solver.solve(system_matrix, solution, right_hand_side, preconditioner);

    std::cout << "  Solver converged in " << solver_control.last_step()
              << " iterations." << std::endl;
  }

// 我们根据一个非常简单的细化标准来细化网格，即对解的梯度的近似。由于这里我们考虑的是DG(1)方法（即我们使用片状双线性形状函数），我们可以简单地计算每个单元的梯度。但是我们并不希望我们的细化指标只建立在每个单元的梯度上，而是希望同时建立在相邻单元之间的不连续解函数的跳跃上。最简单的方法是通过差分商计算近似梯度，包括考虑中的单元和其相邻的单元。这是由 DerivativeApproximation 类完成的，它计算近似梯度的方式类似于本教程 step-9 中描述的 GradientEstimation 。事实上， DerivativeApproximation 类是在 step-9 的 GradientEstimation 类之后开发的。与  step-9  中的讨论相关，这里我们考虑  $h^{1+d/2}|\nabla_h u_h|$  。此外，我们注意到，我们不考虑近似的二次导数，因为线性平流方程的解一般不在 $H^2$ 中，而只在 $H^1$ 中（或者，更准确地说：在 $H^1_\beta$ 中，即在方向 $\beta$ 上的导数是可平方整除的函数空间）。

  template <int dim>
  void AdvectionProblem<dim>::refine_grid()
  {

//  DerivativeApproximation 类将梯度计算为浮点精度。这已经足够了，因为它们是近似的，只作为细化指标。

    Vector<float> gradient_indicator(triangulation.n_active_cells());

// 现在，近似梯度被计算出来了

    DerivativeApproximation::approximate_gradient(mapping,
                                                  dof_handler,
                                                  solution,
                                                  gradient_indicator);

//并且它们的单元格按系数 $h^{1+d/2}$ 进行缩放。
    unsigned int cell_no = 0;
    for (const auto &cell : dof_handler.active_cell_iterators())
      gradient_indicator(cell_no++) *=
        std::pow(cell->diameter(), 1 + 1.0 * dim / 2);

// 最后它们作为细化指标。

    GridRefinement::refine_and_coarsen_fixed_number(triangulation,
                                                    gradient_indicator,
                                                    0.3,
                                                    0.1);

    triangulation.execute_coarsening_and_refinement();
  }

// 这个程序的输出包括一个自适应细化网格的vtk文件和数值解。最后，我们还用 VectorTools::integrate_difference(). 计算了解的L-无穷大规范。
  template <int dim>
  void AdvectionProblem<dim>::output_results(const unsigned int cycle) const
  {
    const std::string filename = "solution-" + std::to_string(cycle) + ".vtk";
    std::cout << "  Writing solution to <" << filename << ">" << std::endl;
    std::ofstream output(filename);

    DataOut<dim> data_out;
    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution, "u", DataOut<dim>::type_dof_data);

    data_out.build_patches(mapping);

    data_out.write_vtk(output);

    {
      Vector<float> values(triangulation.n_active_cells());
      VectorTools::integrate_difference(mapping,
                                        dof_handler,
                                        solution,
                                        Functions::ZeroFunction<dim>(),
                                        values,
                                        quadrature,
                                        VectorTools::Linfty_norm);
      const double l_infty =
        VectorTools::compute_global_error(triangulation,
                                          values,
                                          VectorTools::Linfty_norm);
      std::cout << "  L-infinity norm: " << l_infty << std::endl;
    }
  }

// 下面的 run 函数与前面的例子类似。

  template <int dim>
  void AdvectionProblem<dim>::run()
  {
    for (unsigned int cycle = 0; cycle < 6; ++cycle)
      {
        std::cout << "Cycle " << cycle << std::endl;

        if (cycle == 0)
          {
            GridGenerator::hyper_cube(triangulation);
            triangulation.refine_global(3);
          }
        else
          refine_grid();

        std::cout << "  Number of active cells:       "
                  << triangulation.n_active_cells() << std::endl;

        setup_system();

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

        assemble_system();
        solve();

        output_results(cycle);
      }
  }
} // namespace Step12

// 下面的 main 函数与前面的例子也类似，不需要注释。

int main()
{
  try
    {
      Step12::AdvectionProblem<2> dgmethod;
      dgmethod.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 (C) 2019  Rhys Mainwaring
 *
 * 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 <cmath>
#include <iostream>
#include <string>

#include <Eigen/Dense>
#include <ignition/common/Console.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Vector2.hh>
#include <ignition/math/Vector3.hh>

#include "Wavefield.hh"

using namespace ignition;
using namespace gazebo;

///////////////////////////////////////////////////////////////////////////////
// Utilities
std::ostream& operator<<(std::ostream &_os, const std::vector<double> &_vec)
{
  for (auto&& v : _vec ) // NOLINT
    _os << v << ", ";
  return _os;
}

///////////////////////////////////////////////////////////////////////////////
/// \brief Private data for the WavefieldParameters.
class ignition::gazebo::WavefieldPrivate
{
  /// \brief Constructor.
  public: WavefieldPrivate():
    size({1000, 1000}),
    cellCount({50, 50}),
    model("PMS"),
    number(1),
    scale(2),
    angle(2.0*M_PI/10.0),
    steepness(1.0),
    amplitude(0.0),
    period(1.0),
    phase(0.0),
    direction(1, 0),
    angularFrequency(2.0*M_PI),
    wavelength(2 * M_PI / this->DeepWaterDispersionToWavenumber(2.0 * M_PI)),
    wavenumber(this->DeepWaterDispersionToWavenumber(2.0 * M_PI)),
    tau(1.0),
    gain(1.0)
  {
  }

  /// \brief The size of the wavefield.
  public: ignition::math::Vector2d size;

  /// \brief The number of grid cells in the wavefield.
  public: ignition::math::Vector2d cellCount;

  /// \brief Name of wavefield model to use - must be "PMS" or "CWR"
  public: std::string model;

  /// \brief The number of component waves.
  public: size_t number;

  /// \brief Set the scale of the largest and smallest waves.
  public: double scale;

  /// \brief Set the angle between component waves and the mean direction.
  public: double angle;

  /// \brief Control the wave steepness. 0 is sine waves, 1 is Gerstner waves.
  public: double steepness;

  /// \brief The mean wave amplitude [m].
  public: double amplitude;

  /// \brief The mean wave period [s]
  public: double period;

  /// \brief The mean wave phase (not currently enabled).
  public: double phase;

  /// \brief The mean wave direction.
  public: ignition::math::Vector2d direction;

  /// \brief The time constant for exponential increasing waves on startup
  public: double tau;

  /// \brief The multiplier applied to PM spectra
  public: double gain;

  /// \brief The mean wave angular frequency (derived).
  public: double angularFrequency;

  /// \brief The mean wavelength (derived).
  public: double wavelength;

  /// \brief The mean wavenumber (derived).
  public: double wavenumber;

  /// \brief The component wave angular frequencies (derived).
  public: std::vector<double> angularFrequencies;

  /// \brief The component wave amplitudes (derived).
  public: std::vector<double> amplitudes;

  /// \brief The component wave phases (derived).
  public: std::vector<double> phases;

  /// \brief The component wave steepness factors (derived).
  public: std::vector<double> steepnesses;

  /// \brief The component wavenumbers (derived).
  public: std::vector<double> wavenumbers;

  /// \brief The component wave dirctions (derived).
  public: std::vector<ignition::math::Vector2d> directions;

  /// \brief Recalculate for constant wavelength-amplitude ratio
  public: void RecalculateCwr()
  {
    // Normalize direction
    this->direction.Normalize();

    // Derived mean values
    this->angularFrequency = 2.0 * M_PI / this->period;
    this->wavenumber = \
      this->DeepWaterDispersionToWavenumber(this->angularFrequency);
    this->wavelength = 2.0 * M_PI / this->wavenumber;

    // Update components
    this->angularFrequencies.clear();
    this->amplitudes.clear();
    this->phases.clear();
    this->wavenumbers.clear();
    this->steepnesses.clear();
    this->directions.clear();

    for (size_t i = 0; i < this->number; ++i)
    {
      const int n = i - this->number / 2;
      const double scaleFactor = std::pow(this->scale, n);
      const double a = scaleFactor * this->amplitude;
      const double k = this->wavenumber / scaleFactor;
      const double omega = this->DeepWaterDispersionToOmega(k);
      const double phi = this->phase;
      double q = 0.0;
      if (!ignition::math::equal(a, 0.0))
      {
        q = std::min(1.0, this->steepness / (a * k * this->number));
      }

      this->amplitudes.push_back(a);
      this->angularFrequencies.push_back(omega);
      this->phases.push_back(phi);
      this->steepnesses.push_back(q);
      this->wavenumbers.push_back(k);

      // Direction
      const double c = std::cos(n * this->angle);
      const double s = std::sin(n * this->angle);

      const ignition::math::Vector2d d(
        c * this->direction.X() - s * this->direction.Y(),
        s * this->direction.X() + c * this->direction.Y());
      directions.push_back(d);
    }
  }

  // \brief Pierson-Moskowitz wave spectrum
  public: double pm(double _omega, double _omegaP)
  {
    double alpha = 0.0081;
    double g = 9.81;
    return alpha * std::pow(g, 2.0) / std::pow(_omega, 5.0) * \
      std::exp(-(5.0 / 4.0) * std::pow(_omegaP / _omega, 4.0));
  }

  /// \brief Recalculate for Pierson-Moskowitz spectrum sampling model
  public: void RecalculatePms()
  {
    // Normalize direction
    this->direction.Normalize();

    // Derived mean values
    this->angularFrequency = 2.0 * M_PI / this->period;
    this->wavenumber = \
      this->DeepWaterDispersionToWavenumber(this->angularFrequency);
    this->wavelength = 2.0 * M_PI / this->wavenumber;

    // Update components
    this->angularFrequencies.clear();
    this->amplitudes.clear();
    this->phases.clear();
    this->wavenumbers.clear();
    this->steepnesses.clear();
    this->directions.clear();

    // Vector for spaceing
    std::vector<double> omegaSpacing;
    omegaSpacing.push_back(this->angularFrequency * (1.0 - 1.0 / this->scale));
    omegaSpacing.push_back(this->angularFrequency * \
                            (this->scale - 1.0 / this->scale) / 2.0);
    omegaSpacing.push_back(this->angularFrequency * (this->scale - 1.0));

    for (size_t i = 0; i < this->number; ++i)
    {
      const int n = i - 1;
      const double scaleFactor = std::pow(this->scale, n);
      const double omega = this->angularFrequency * scaleFactor;
      const double pms = pm(omega, this->angularFrequency);
      const double a = this->gain * std::sqrt(2.0 * pms * omegaSpacing[i]);
      const double k = this->DeepWaterDispersionToWavenumber(omega);
      const double phi = this->phase;
      double q = 0.0;
      if (!ignition::math::equal(a, 0.0))
      {
        q = std::min(1.0, this->steepness / (a * k * this->number));
      }

      this->amplitudes.push_back(a);
      this->angularFrequencies.push_back(omega);
      this->phases.push_back(phi);
      this->steepnesses.push_back(q);
      this->wavenumbers.push_back(k);

      // Direction
      const double c = std::cos(n * this->angle);
      const double s = std::sin(n * this->angle);

      const ignition::math::Vector2d d(
        c * this->direction.X() - s * this->direction.Y(),
        s * this->direction.X() + c * this->direction.Y());
      directions.push_back(d);
    }
  }

  /// \brief Recalculate all derived quantities from inputs.
  public: void Recalculate()
  {
    if (!this->model.compare("PMS"))
    {
      ignmsg << "Using Pierson-Moskowitz spectrum sampling wavefield model "
            << std::endl;
      this->RecalculatePms();
    }
    else if (!this->model.compare("CWR"))
    {
      ignmsg << "Using Constant wavelength-ampltude ratio wavefield model "
            << std::endl;
      this->RecalculateCwr();
    }
    else
    {
      ignwarn<< "Wavefield model specified as <" << this->model
            << "> which is not one of the two supported wavefield models: "
            << "PMS or CWR!!!" << std::endl;
    }
  }

  /////////////////////////////////////////////////
  private: double DeepWaterDispersionToOmega(double _wavenumber)
  {
    const double g = std::fabs(-9.8);
    return std::sqrt(g * _wavenumber);
  }

  /////////////////////////////////////////////////
  private: double DeepWaterDispersionToWavenumber(double _omega)
  {
    const double g = std::fabs(-9.8);
    return _omega * _omega / g;
  }
};

/////////////////////////////////////////////////////////////////////////////
Wavefield::Wavefield()
  : data(std::make_unique<WavefieldPrivate>())
{
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
Wavefield::~Wavefield()
{
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::Load(const std::shared_ptr<const sdf::Element> &_sdf)
{
  if (!_sdf->HasElement("wavefield"))
    return;

  auto ptr = const_cast<sdf::Element *>(_sdf.get());
  auto sdfWavefield = ptr->GetElement("wavefield");

  this->data->size = sdfWavefield->Get<ignition::math::Vector2d>("size",
    this->data->size).first;
  this->data->cellCount = sdfWavefield->Get<ignition::math::Vector2d>("cell_count",
    this->data->cellCount).first;
  if (sdfWavefield->HasElement("wave"))
  {
    auto sdfWave = sdfWavefield->GetElement("wave");

    this->data->model = sdfWave->Get<std::string>("model", "PMS").first;
    this->data->number =
      sdfWave->Get<double>("number", this->data->number).first;
    this->data->amplitude =
      sdfWave->Get<double>("amplitude", this->data->amplitude).first;
    this->data->period =
      sdfWave->Get<double>("period", this->data->period).first;
    this->data->phase = sdfWave->Get<double>("phase", this->data->phase).first;
    this->data->direction =
      sdfWave->Get<ignition::math::Vector2d>("direction",
        this->data->direction).first;
    this->data->scale = sdfWave->Get<double>("scale", this->data->scale).first;
    this->data->angle = sdfWave->Get<double>("angle", this->data->angle).first;
    this->data->steepness =
      sdfWave->Get<double>("steepness", this->data->steepness).first;
    this->data->tau = sdfWave->Get<double>("tau", this->data->tau).first;
    this->data->gain = sdfWave->Get<double>("gain", this->data->gain).first;

    this->data->Recalculate();
  }
  this->DebugPrint();
}

///////////////////////////////////////////////////////////////////////////////
size_t Wavefield::Number() const
{
  return this->data->number;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Angle() const
{
  return this->data->angle;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Scale() const
{
  return this->data->scale;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Steepness() const
{
  return this->data->steepness;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::AngularFrequency() const
{
  return this->data->angularFrequency;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Amplitude() const
{
  return this->data->amplitude;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Period() const
{
  return this->data->period;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Phase() const
{
  return this->data->phase;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Wavelength() const
{
  return this->data->wavelength;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::Wavenumber() const
{
  return this->data->wavenumber;
}

///////////////////////////////////////////////////////////////////////////////
float Wavefield::Tau() const
{
  return this->data->tau;
}

///////////////////////////////////////////////////////////////////////////////
float Wavefield::Gain() const
{
  return this->data->gain;
}

///////////////////////////////////////////////////////////////////////////////
ignition::math::Vector2d Wavefield::Direction() const
{
  return this->data->direction;
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetNumber(size_t _number)
{
  this->data->number = _number;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetAngle(double _angle)
{
  this->data->angle = _angle;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetScale(double _scale)
{
  this->data->scale = _scale;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetSteepness(double _steepness)
{
  this->data->steepness = _steepness;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetAmplitude(double _amplitude)
{
  this->data->amplitude = _amplitude;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetPeriod(double _period)
{
  this->data->period = _period;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetPhase(double _phase)
{
  this->data->phase = _phase;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetTau(double _tau)
{
  this->data->tau = _tau;
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetGain(double _gain)
{
  this->data->gain = _gain;
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::SetDirection(const ignition::math::Vector2d &_direction)
{
  this->data->direction = _direction;
  this->data->Recalculate();
}

///////////////////////////////////////////////////////////////////////////////
const std::vector<double> &Wavefield::AngularFrequency_V() const
{
  return this->data->angularFrequencies;
}

///////////////////////////////////////////////////////////////////////////////
const std::vector<double> &Wavefield::Amplitude_V() const
{
  return this->data->amplitudes;
}

///////////////////////////////////////////////////////////////////////////////
const std::vector<double> &Wavefield::Phase_V() const
{
  return this->data->phases;
}

///////////////////////////////////////////////////////////////////////////////
const std::vector<double> &Wavefield::Steepness_V() const
{
  return this->data->steepnesses;
}

///////////////////////////////////////////////////////////////////////////////
const std::vector<double> &Wavefield::Wavenumber_V() const
{
  return this->data->wavenumbers;
}

///////////////////////////////////////////////////////////////////////////////
const std::vector<ignition::math::Vector2d> &Wavefield::Direction_V() const
{
  return this->data->directions;
}

///////////////////////////////////////////////////////////////////////////////
void Wavefield::DebugPrint() const
{
  ignmsg << "Input Parameters:" << std::endl;
  ignmsg << "model:     " << this->data->model << std::endl;
  ignmsg << "number:     " << this->data->number << std::endl;
  ignmsg << "scale:      " << this->data->scale << std::endl;
  ignmsg << "angle:      " << this->data->angle << std::endl;
  ignmsg << "steepness:  " << this->data->steepness << std::endl;
  ignmsg << "amplitude:  " << this->data->amplitude << std::endl;
  ignmsg << "period:     " << this->data->period << std::endl;
  ignmsg << "direction:  " << this->data->direction << std::endl;
  ignmsg << "tau:  " << this->data->tau << std::endl;
  ignmsg << "gain:  " << this->data->gain << std::endl;
  ignmsg << "Derived Parameters:" << std::endl;
  ignmsg << "amplitudes:  " << this->data->amplitudes << std::endl;
  ignmsg << "wavenumbers: " << this->data->wavenumbers << std::endl;
  ignmsg << "omegas:      " << this->data->angularFrequencies << std::endl;
  ignmsg << "periods:     ";
  for (auto&& omega : this->data->angularFrequencies) // NOLINT
  {
    ignmsg << 2.0 * M_PI / omega <<", ";
  }
  ignmsg << std::endl;
  ignmsg << "phases:      " << this->data->phases << std::endl;
  ignmsg << "steepnesses: " << this->data->steepnesses << std::endl;
  ignmsg << "directions:  ";
  for (auto&& d : this->data->directions) // NOLINT
  {
    ignmsg << d << "; ";
  }
  ignmsg << std::endl;
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::ComputeDepthSimply(const ignition::math::Vector3d &_point,
  double _time, double _timeInit)
{
  double h = 0.0;
  for (std::size_t i = 0; i < this->Number(); ++i)
  {
    double k = this->Wavenumber_V()[i];
    double a = this->Amplitude_V()[i];
    double dx =  this->Direction_V()[i].X();
    double dy =  this->Direction_V()[i].Y();
    double dot = _point.X() * dx + _point.Y() * dy;
    double omega = this->AngularFrequency_V()[i];
    double theta = k * dot - omega * _time;
    double c = cos(theta);
    h += a*c;
  }

  // Exponentially grow the waves
  return h * (1 - exp(-1.0 * (_time - _timeInit) / this->Tau()));
}

///////////////////////////////////////////////////////////////////////////////
double Wavefield::ComputeDepthDirectly(const ignition::math::Vector3d &_point,
  double _time, double _timeInit)
{
  // Struture for passing wave parameters to lambdas
  struct WaveParams
  {
    WaveParams(
      const std::vector<double>& _a,
      const std::vector<double>& _k,
      const std::vector<double>& _omega,
      const std::vector<double>& _phi,
      const std::vector<double>& _q,
      const std::vector<ignition::math::Vector2d>& _dir) :
      a(_a), k(_k), omega(_omega), phi(_phi), q(_q), dir(_dir) {}

    const std::vector<double>& a;
    const std::vector<double>& k;
    const std::vector<double>& omega;
    const std::vector<double>& phi;
    const std::vector<double>& q;
    const std::vector<ignition::math::Vector2d>& dir;
  };

  // Compute the target function and Jacobian. Also calculate pz,
  // the z-component of the Gerstner wave, which we essentially get for free.
  // cppcheck-suppress constParameter
  auto wave_fdf = [=](auto x, auto p, auto t, auto &wp, auto &F, auto &J)
  {
    double pz = 0;
    F(0) = p.x() - x.x();
    F(1) = p.y() - x.y();
    J(0, 0) = -1;
    J(0, 1) =  0;
    J(1, 0) =  0;
    J(1, 1) = -1;
    const size_t n = wp.a.size();
    for (auto&& i = 0; i < n; ++i) // NOLINT
    {
      const double dx = wp.dir[i].X();
      const double dy = wp.dir[i].Y();
      const double q = wp.q[i];
      const double a = wp.a[i];
      const double k = wp.k[i];
      const double dot = x.x() * dx + x.y() * dy;
      const double theta = k * dot - wp.omega[i] * t;
      const double s = std::sin(theta);
      const double c = std::cos(theta);
      const double qakc = q * a * k * c;
      const double df1x = qakc * dx * dx;
      const double df1y = qakc * dx * dy;
      const double df2x = df1y;
      const double df2y = qakc * dy * dy;
      pz += a * c;
      F(0) += a * dx * s;
      F(1) += a * dy * s;
      J(0, 0) += df1x;
      J(0, 1) += df1y;
      J(1, 0) += df2x;
      J(1, 1) += df2y;
    }
    // Exponentially grow the waves
    return pz * (1 - exp(-1.0 * (_time - _timeInit) / this->Tau()));
  };

  // Simple multi-variate Newton solver -
  // this version returns the z-component of the
  // wave field at the desired point p.
  // cppcheck-suppress constParameter
  auto solver = [=](auto& fdfunc, auto x0, auto p, auto t, \
                    auto& wp, auto tol, auto nmax)
  {
    int n = 0;
    double err = 1;
    double pz = 0;
    auto xn = x0;
    Eigen::Vector2d F;
    Eigen::Matrix2d J;
    while (std::abs(err) > tol && n < nmax)
    {
      pz = fdfunc(x0, p, t, wp, F, J);
      xn = x0 - J.inverse() * F;
      x0 = xn;
      err = F.norm();
      n++;
    }
    return pz;
  };

  // Set up parameter references
  WaveParams wp(
    this->Amplitude_V(),
    this->Wavenumber_V(),
    this->AngularFrequency_V(),
    this->Phase_V(),
    this->Steepness_V(),
    this->Direction_V());

  // Tolerances etc.
  const double tol = 1.0E-10;
  const double nmax = 30;

  // Use the target point as the initial guess
  // (this is within sum{amplitudes} of the solution)
  Eigen::Vector2d p2(_point.X(), _point.Y());
  const double pz = solver(wave_fdf, p2, p2, _time, wp, tol, nmax);
  // Removed so that height is reported relative to mean water level
  // const double h = pz - _point.Z();
  const double h = pz;
  return h;
}


/*
 * visualization.cpp
 *
 *  Created on: Jan 18, 2012
 *      Author: eba
 */

#include "mhfpython.h"

#include <boost/foreach.hpp>
#include "visualization.h"
#include <numpy/noprefix.h>

// TODO expand the definition of this function to handle higher dimensional cases
//PyObject * actualDistributionAfterPredict(const GaussianHypothesis<dim> & gh,
//										 transformations<dim,procnoisedim,inputdim> &trans,
//										 const Matrix<double,inputdim,1> & input,
//										 const Matrix<double,procnoisedim,procnoisedim> & noisecov,
//										 int samples, double limit, const int steps) {
//    double * zData = new double[steps * steps];
//    Map<Matrix<double, Dynamic, Dynamic> > z(zData, steps,steps);
//    z.fill(0);
//    double cellArea = pow(2 * limit / steps,2);
//    const Matrix<double,dim,1> & mean = gh.mean;
//    Matrix<double,dim,dim> cholState = gh.cov.llt().matrixL();
//    Matrix<double,procnoisedim,procnoisedim> cholNoise = noisecov.llt().matrixL();
//    boost::mt19937 rng;
//    boost::normal_distribution<> nd(0,1);
//    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);
//
//
//    for (int i=0; i<samples; i++) {
//		Matrix<double,dim,1> sample = mean + cholState * randomMatrix<dim,1>(var_nor);
//		Matrix<double,procnoisedim,1> noise = cholNoise * randomMatrix<procnoisedim,1>(var_nor);
//		sample = trans.statetrans(sample,noise,input);
//		if(limit>sample.maxCoeff() && sample.minCoeff()>-limit) {
//			Matrix<int,dim,1> index = (sample.array() * steps/2 / limit + steps/2).matrix().cast<int>();
//			z(index(0,0),index(1,0)) += 1./(samples*cellArea);
//		}
//	}
//    int N[] = {steps,steps};
//    PyArrayObject * result = (PyArrayObject*) PyArray_SimpleNewFromData(2,N, PyArray_DOUBLE, zData);
//    result->flags = result->flags | OWNDATA;
//    return (PyObject *)result;
//}

void propagateSamples(samplevector & samples,
					  transformations<dim,procnoisedim,inputdim> &trans,
					  const Matrix<double,inputdim,1> & input,
					  const Matrix<double,procnoisedim,procnoisedim> & noisecov
	) {
    Matrix<double,procnoisedim,procnoisedim> cholNoise = noisecov.llt().matrixL();
    boost::mt19937 rng;
    boost::normal_distribution<> nd(0,1);
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng, nd);

    BOOST_FOREACH(Sample & sample, samples) {
		Matrix<double,procnoisedim,1> noise = cholNoise * randomMatrix<procnoisedim,1>(var_nor);
		sample.state = trans.statetrans(sample.state,noise,input);
	}
}

void updateSamples(samplevector & samples,
				   transformations<dim,measdim,measdim> &trans,
				   const Matrix<double,measdim,1> & meas,
				   const Matrix<double,measdim,measdim> & noisecov
	) {

	nullclass nullobj;
	double totalWeight = 0;
    BOOST_FOREACH(Sample & sample, samples) {
		sample.weight = trans.measlikelihood(sample.state,noisecov,meas,nullobj);
		totalWeight += sample.weight;
	}
    BOOST_FOREACH(Sample & sample, samples) {
    	sample.weight /= totalWeight;
    }
}

// TODO expand the definition of this function to handle higher dimensional cases
PyObject * layoutSamples(const samplevector & samples, double limit, const int steps) {
    double * zData = new double[steps * steps];
    Map<Matrix<double, Dynamic, Dynamic> > z(zData, steps,steps);
    z.fill(0);
    double cellArea = pow(2 * limit / steps,2);

    typedef Matrix<double,dim,1> SampleMat;

    BOOST_FOREACH(const Sample & sample, samples) {
		if(limit>sample.state.block<2,1>(0,0).maxCoeff() && sample.state.block<2,1>(0,0).minCoeff()>-limit) { // TODO This line is temporary
			Matrix<int,dim,1> index = (sample.state.array() * steps/2 / limit + steps/2).matrix().cast<int>();
			z(index(0,0),index(1,0)) += sample.weight/cellArea;
		}
	}
    npy_intp N[] = {steps,steps};
    PyArrayObject * result = (PyArrayObject*) PyArray_SimpleNewFromData(2,N, PyArray_DOUBLE, zData);
    result->flags = result->flags | OWNDATA;
    return (PyObject *)result;
}

void resample(samplevector & samples) {
	random_shuffle ( samples.begin(), samples.end() );
	int sampleCount = samples.size();
	double sampleWeight = 1.0/sampleCount;
	samplevector oldsamples = samples;
	double weight = (double(rand()) / double(RAND_MAX)) * sampleWeight;
	int sampleNum = 0;
	BOOST_FOREACH(const Sample & sample, oldsamples) {
		while(sample.weight>=weight) {
			samples[sampleNum].state = sample.state;
			samples[sampleNum].weight = sampleWeight;
			weight += sampleWeight;
			sampleNum++;
		}
		weight -= sample.weight;
	}
	if(sampleNum!=sampleCount) std::cerr<<"problem!\n";
}

void export_visualization() {
	import_array();
	class_<samplevector>("samplevector");
	//def("actualDistributionAfterPredict", actualDistributionAfterPredict);
	def("drawSamples", drawSamples<dim>);
	def("propagateSamples", propagateSamples);
	def("updateSamples", updateSamples);
	def("layoutSamples", layoutSamples);
	def("resample", resample);
}


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

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

 * See LICENSE for the license information

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

/*
 * @file testUnit3.cpp
 * @date Feb 03, 2012
 * @author Can Erdogan
 * @author Frank Dellaert
 * @author Alex Trevor
 * @brief Tests the Unit3 class
 */

#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

#include <CppUnitLite/TestHarness.h>

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

#include <cmath>
#include <random>

using namespace boost::assign;
using namespace gtsam;
using namespace std;
using gtsam::symbol_shorthand::U;

GTSAM_CONCEPT_TESTABLE_INST(Unit3)
GTSAM_CONCEPT_MANIFOLD_INST(Unit3)

//*******************************************************************************
Point3 point3_(const Unit3& p) {
  return p.point3();
}

TEST(Unit3, point3) {
  vector<Point3> ps;
  ps += Point3(1, 0, 0), Point3(0, 1, 0), Point3(0, 0, 1), Point3(1, 1, 0)
      / sqrt(2.0);
  Matrix actualH, expectedH;
  for(Point3 p: ps) {
    Unit3 s(p);
    expectedH = numericalDerivative11<Point3, Unit3>(point3_, s);
    EXPECT(assert_equal(p, s.point3(actualH), 1e-8));
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
}

//*******************************************************************************
static Unit3 rotate_(const Rot3& R, const Unit3& p) {
  return R * p;
}

TEST(Unit3, rotate) {
  Rot3 R = Rot3::Yaw(0.5);
  Unit3 p(1, 0, 0);
  Unit3 expected = Unit3(R.column(1));
  Unit3 actual = R * p;
  EXPECT(assert_equal(expected, actual, 1e-8));
  Matrix actualH, expectedH;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expectedH = numericalDerivative21(rotate_, R, p);
    R.rotate(p, actualH, boost::none);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
  {
    expectedH = numericalDerivative22(rotate_, R, p);
    R.rotate(p, boost::none, actualH);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
}

//*******************************************************************************
static Unit3 unrotate_(const Rot3& R, const Unit3& p) {
  return R.unrotate(p);
}

TEST(Unit3, unrotate) {
  Rot3 R = Rot3::Yaw(-M_PI / 4.0);
  Unit3 p(1, 0, 0);
  Unit3 expected = Unit3(1, 1, 0);
  Unit3 actual = R.unrotate(p);
  EXPECT(assert_equal(expected, actual, 1e-8));

  Matrix actualH, expectedH;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expectedH = numericalDerivative21(unrotate_, R, p);
    R.unrotate(p, actualH, boost::none);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
  {
    expectedH = numericalDerivative22(unrotate_, R, p);
    R.unrotate(p, boost::none, actualH);
    EXPECT(assert_equal(expectedH, actualH, 1e-9));
  }
}

TEST(Unit3, dot) {
  Unit3 p(1, 0.2, 0.3);
  Unit3 q = p.retract(Vector2(0.5, 0));
  Unit3 r = p.retract(Vector2(0.8, 0));
  Unit3 t = p.retract(Vector2(0, 0.3));
  EXPECT(assert_equal(1.0, p.dot(p), 1e-8));
  EXPECT(assert_equal(0.877583, p.dot(q), 1e-5));
  EXPECT(assert_equal(0.696707, p.dot(r), 1e-5));
  EXPECT(assert_equal(0.955336, p.dot(t), 1e-5));

  // Use numerical derivatives to calculate the expected Jacobians
  Matrix H1, H2;
  boost::function<double(const Unit3&, const Unit3&)> f = boost::bind(&Unit3::dot, _1, _2,  //
                                                                      boost::none, boost::none);
  {
    p.dot(q, H1, H2);
    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, q), H1, 1e-9));
    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, q), H2, 1e-9));
  }
  {
    p.dot(r, H1, H2);
    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, r), H1, 1e-9));
    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, r), H2, 1e-9));
  }
  {
    p.dot(t, H1, H2);
    EXPECT(assert_equal(numericalDerivative21<double,Unit3>(f, p, t), H1, 1e-9));
    EXPECT(assert_equal(numericalDerivative22<double,Unit3>(f, p, t), H2, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, error) {
  Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
  r = p.retract(Vector2(0.8, 0));
  EXPECT(assert_equal((Vector)(Vector2(0, 0)), p.error(p), 1e-8));
  EXPECT(assert_equal((Vector)(Vector2(0.479426, 0)), p.error(q), 1e-5));
  EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.error(r), 1e-5));

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalDerivative11<Vector2,Unit3>(
        boost::bind(&Unit3::error, &p, _1, boost::none), q);
    p.error(q, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
  {
    expected = numericalDerivative11<Vector2,Unit3>(
        boost::bind(&Unit3::error, &p, _1, boost::none), r);
    p.error(r, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, error2) {
  Unit3 p(0.1, -0.2, 0.8);
  Unit3 q = p.retract(Vector2(0.2, -0.1));
  Unit3 r = p.retract(Vector2(0.8, 0));

  // Hard-coded as simple regression values
  EXPECT(assert_equal((Vector)(Vector2(0.0, 0.0)), p.errorVector(p), 1e-8));
  EXPECT(assert_equal((Vector)(Vector2(0.198337495, -0.0991687475)), p.errorVector(q), 1e-5));
  EXPECT(assert_equal((Vector)(Vector2(0.717356, 0)), p.errorVector(r), 1e-5));

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalDerivative21<Vector2, Unit3, Unit3>(
        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
    p.errorVector(q, actual, boost::none);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
  {
    expected = numericalDerivative21<Vector2, Unit3, Unit3>(
        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
    p.errorVector(r, actual, boost::none);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
  {
    expected = numericalDerivative22<Vector2, Unit3, Unit3>(
        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, q);
    p.errorVector(q, boost::none, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
  {
    expected = numericalDerivative22<Vector2, Unit3, Unit3>(
        boost::bind(&Unit3::errorVector, _1, _2, boost::none, boost::none), p, r);
    p.errorVector(r, boost::none, actual);
    EXPECT(assert_equal(expected, actual, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, distance) {
  Unit3 p(1, 0, 0), q = p.retract(Vector2(0.5, 0)), //
  r = p.retract(Vector2(0.8, 0));
  EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8);
  EXPECT_DOUBLES_EQUAL(0.47942553860420301, p.distance(q), 1e-8);
  EXPECT_DOUBLES_EQUAL(0.71735609089952279, p.distance(r), 1e-8);

  Matrix actual, expected;
  // Use numerical derivatives to calculate the expected Jacobian
  {
    expected = numericalGradient<Unit3>(
        boost::bind(&Unit3::distance, &p, _1, boost::none), q);
    p.distance(q, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
  {
    expected = numericalGradient<Unit3>(
        boost::bind(&Unit3::distance, &p, _1, boost::none), r);
    p.distance(r, actual);
    EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, localCoordinates0) {
  Unit3 p;
  Vector actual = p.localCoordinates(p);
  EXPECT(assert_equal(Z_2x1, actual, 1e-8));
}

TEST(Unit3, localCoordinates) {
  {
    Unit3 p, q;
    Vector2 expected = Vector2::Zero();
    Vector2 actual = p.localCoordinates(q);
    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
  }
  {
    Unit3 p, q(1, 6.12385e-21, 0);
    Vector2 expected = Vector2::Zero();
    Vector2 actual = p.localCoordinates(q);
    EXPECT(assert_equal((Vector) Z_2x1, actual, 1e-8));
    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
  }
  {
    Unit3 p, q(-1, 0, 0);
    Vector2 expected(M_PI, 0);
    Vector2 actual = p.localCoordinates(q);
    EXPECT(assert_equal(expected, actual, 1e-8));
    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
  }
  {
    Unit3 p, q(0, 1, 0);
    Vector2 expected(0,-M_PI_2);
    Vector2 actual = p.localCoordinates(q);
    EXPECT(assert_equal(expected, actual, 1e-8));
    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
  }
  {
    Unit3 p, q(0, -1, 0);
    Vector2 expected(0, M_PI_2);
    Vector2 actual = p.localCoordinates(q);
    EXPECT(assert_equal(expected, actual, 1e-8));
    EXPECT(assert_equal(q, p.retract(expected), 1e-8));
  }
  {
    Unit3 p(0,1,0), q(0,-1,0);
    Vector2 actual = p.localCoordinates(q);
    EXPECT(assert_equal(q, p.retract(actual), 1e-8));
  }
  {
    Unit3 p(0,0,1), q(0,0,-1);
    Vector2 actual = p.localCoordinates(q);
    EXPECT(assert_equal(q, p.retract(actual), 1e-8));
  }

  double twist = 1e-4;
  {
    Unit3 p(0, 1, 0), q(0 - twist, -1 + twist, 0);
    Vector2 actual = p.localCoordinates(q);
    EXPECT(actual(0) < 1e-2);
    EXPECT(actual(1) > M_PI - 1e-2)
  }
  {
    Unit3 p(0, 1, 0), q(0 + twist, -1 - twist, 0);
    Vector2 actual = p.localCoordinates(q);
    EXPECT(actual(0) < 1e-2);
    EXPECT(actual(1) < -M_PI + 1e-2)
  }
}

//*******************************************************************************
// Wrapper to make basis return a Vector6 so we can test numerical derivatives.
Vector6 BasisTest(const Unit3& p, OptionalJacobian<6, 2> H) {
  Matrix32 B = p.basis(H);
  Vector6 B_vec;
  B_vec << B.col(0), B.col(1);
  return B_vec;
}

TEST(Unit3, basis) {
  Unit3 p(0.1, -0.2, 0.9);

  Matrix expected(3, 2);
  expected << 0.0, -0.994169047, 0.97618706, -0.0233922129, 0.216930458, 0.105264958;

  Matrix62 actualH;
  Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
      boost::bind(BasisTest, _1, boost::none), p);

  // without H, first time
  EXPECT(assert_equal(expected, p.basis(), 1e-6));

  // without H, cached
  EXPECT(assert_equal(expected, p.basis(), 1e-6));

  // with H, first time
  EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
  EXPECT(assert_equal(expectedH, actualH, 1e-8));

  // with H, cached
  EXPECT(assert_equal(expected, p.basis(actualH), 1e-6));
  EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

//*******************************************************************************
/// Check the basis derivatives of a bunch of random Unit3s.
TEST(Unit3, basis_derivatives) {
  int num_tests = 100;
  std::mt19937 rng(42);
  for (int i = 0; i < num_tests; i++) {
    Unit3 p = Unit3::Random(rng);

    Matrix62 actualH;
    p.basis(actualH);

    Matrix62 expectedH = numericalDerivative11<Vector6, Unit3>(
                           boost::bind(BasisTest, _1, boost::none), p);
    EXPECT(assert_equal(expectedH, actualH, 1e-8));
  }
}

//*******************************************************************************
TEST(Unit3, retract) {
  {
    Unit3 p;
    Vector2 v(0.5, 0);
    Unit3 expected(0.877583, 0, 0.479426);
    Unit3 actual = p.retract(v);
    EXPECT(assert_equal(expected, actual, 1e-6));
    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
  }
  {
    Unit3 p;
    Vector2 v(0, 0);
    Unit3 actual = p.retract(v);
    EXPECT(assert_equal(p, actual, 1e-6));
    EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
  }
}

//*******************************************************************************
TEST (Unit3, jacobian_retract) {
  Matrix22 H;
  Unit3 p;
  boost::function<Unit3(const Vector2&)> f =
      boost::bind(&Unit3::retract, p, _1, boost::none);
  {
      Vector2 v (-0.2, 0.1);
      p.retract(v, H);
      Matrix H_expected_numerical = numericalDerivative11(f, v);
      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
  }
  {
      Vector2 v (0, 0);
      p.retract(v, H);
      Matrix H_expected_numerical = numericalDerivative11(f, v);
      EXPECT(assert_equal(H_expected_numerical, H, 1e-9));
  }
}

//*******************************************************************************
TEST(Unit3, retract_expmap) {
  Unit3 p;
  Vector2 v((M_PI / 2.0), 0);
  Unit3 expected(Point3(0, 0, 1));
  Unit3 actual = p.retract(v);
  EXPECT(assert_equal(expected, actual, 1e-8));
  EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8));
}

//*******************************************************************************
TEST(Unit3, Random) {
  std::mt19937 rng(42);
  // Check that means are all zero at least
  Point3 expectedMean(0,0,0), actualMean(0,0,0);
  for (size_t i = 0; i < 100; i++)
    actualMean = actualMean + Unit3::Random(rng).point3();
  actualMean = actualMean / 100;
  EXPECT(assert_equal(expectedMean,actualMean,0.1));
}

//*******************************************************************************
// New test that uses Unit3::Random
TEST(Unit3, localCoordinates_retract) {
  std::mt19937 rng(42);
  size_t numIterations = 10000;

  for (size_t i = 0; i < numIterations; i++) {
    // Create two random Unit3s
    const Unit3 s1 = Unit3::Random(rng);
    const Unit3 s2 = Unit3::Random(rng);
    // Check that they are not at opposite ends of the sphere, which is ill defined
    if (s1.unitVector().dot(s2.unitVector())<-0.9) continue;

    // Check if the local coordinates and retract return consistent results.
    Vector v12 = s1.localCoordinates(s2);
    Unit3 actual_s2 = s1.retract(v12);
    EXPECT(assert_equal(s2, actual_s2, 1e-9));
  }
}

//*************************************************************************
TEST (Unit3, FromPoint3) {
  Matrix actualH;
  Point3 point(1, -2, 3); // arbitrary point
  Unit3 expected(point);
  EXPECT(assert_equal(expected, Unit3::FromPoint3(point, actualH), 1e-8));
  Matrix expectedH = numericalDerivative11<Unit3, Point3>(
      boost::bind(Unit3::FromPoint3, _1, boost::none), point);
  EXPECT(assert_equal(expectedH, actualH, 1e-8));
}

//*******************************************************************************
TEST(Unit3, ErrorBetweenFactor) {
  std::vector<Unit3> data;
  data.push_back(Unit3(1.0, 0.0, 0.0));
  data.push_back(Unit3(0.0, 0.0, 1.0));

  NonlinearFactorGraph graph;
  Values initial_values;

  // Add prior factors.
  SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
  for (size_t i = 0; i < data.size(); i++) {
    graph.addPrior(U(i), data[i], R_prior);
  }

  // Add process factors using the dot product error function.
  SharedNoiseModel R_process = noiseModel::Isotropic::Sigma(2, 0.01);
  for (size_t i = 0; i < data.size() - 1; i++) {
    Expression<Vector2> exp(Expression<Unit3>(U(i)), &Unit3::errorVector,
                            Expression<Unit3>(U(i + 1)));
    graph.addExpressionFactor<Vector2>(R_process, Vector2::Zero(), exp);
  }

  // Add initial values. Since there is no identity, just pick something.
  for (size_t i = 0; i < data.size(); i++) {
    initial_values.insert(U(i), Unit3(0.0, 1.0, 0.0));
  }

  Values values = GaussNewtonOptimizer(graph, initial_values).optimize();

  // Check that the y-value is very small for each.
  for (size_t i = 0; i < data.size(); i++) {
    EXPECT(assert_equal(0.0, values.at<Unit3>(U(i)).unitVector().y(), 1e-3));
  }

  // Check that the dot product between variables is close to 1.
  for (size_t i = 0; i < data.size() - 1; i++) {
    EXPECT(assert_equal(1.0, values.at<Unit3>(U(i)).dot(values.at<Unit3>(U(i + 1))), 1e-2));
  }
}

/* ************************************************************************* */
TEST(actualH, Serialization) {
  Unit3 p(0, 1, 0);
  EXPECT(serializationTestHelpers::equalsObj(p));
  EXPECT(serializationTestHelpers::equalsXML(p));
  EXPECT(serializationTestHelpers::equalsBinary(p));
}

/* ************************************************************************* */
int main() {
  srand(time(nullptr));
  TestResult tr;
  return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

//#define NDEBUG
//#define VIENNACL_DEBUG_BUILD

//
// *** System
//
#include <iostream>

// We don't need debug mode in UBLAS:
#define BOOST_UBLAS_NDEBUG

//
// *** Boost
//
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/io.hpp>

//
// *** ViennaCL
//
//#define VIENNACL_DEBUG_ALL
//#define VIENNACL_DEBUG_BUILD
#define VIENNACL_WITH_UBLAS 1
#include "viennacl/scalar.hpp"
#include "viennacl/matrix.hpp"
#include "viennacl/matrix_proxy.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/linalg/prod.hpp"
#include "viennacl/linalg/norm_2.hpp"
#include "viennacl/linalg/direct_solve.hpp"
#include "examples/tutorial/Random.hpp"
//
// -------------------------------------------------------------
//
using namespace boost::numeric;
//
// -------------------------------------------------------------
//
template<typename ScalarType>
ScalarType diff(ScalarType & s1, viennacl::scalar<ScalarType> & s2)
{
   viennacl::backend::finish();
   if (s1 != s2)
      return (s1 - s2) / std::max(fabs(s1), fabs(s2));
   return 0;
}

template<typename ScalarType>
ScalarType diff(ublas::vector<ScalarType> & v1, viennacl::vector<ScalarType> & v2)
{
   ublas::vector<ScalarType> v2_cpu(v2.size());
   viennacl::backend::finish();
   viennacl::copy(v2.begin(), v2.end(), v2_cpu.begin());
   viennacl::backend::finish();

   for (std::size_t i=0;i<v1.size(); ++i)
   {
      if ( std::max( fabs(v2_cpu[i]), fabs(v1[i]) ) > 0 )
         v2_cpu[i] = fabs(v2_cpu[i] - v1[i]) / std::max( fabs(v2_cpu[i]), fabs(v1[i]) );
      else
         v2_cpu[i] = 0.0;
   }

   return norm_inf(v2_cpu);
}

template<typename ScalarType, typename VCLMatrixType>
ScalarType diff(ublas::matrix<ScalarType> & mat1, VCLMatrixType & mat2)
{
   ublas::matrix<ScalarType> mat2_cpu(mat2.size1(), mat2.size2());
   viennacl::backend::finish();  //workaround for a bug in APP SDK 2.7 on Trinity APUs (with Catalyst 12.8)
   viennacl::copy(mat2, mat2_cpu);
   ScalarType ret = 0;
   ScalarType act = 0;

    for (unsigned int i = 0; i < mat2_cpu.size1(); ++i)
    {
      for (unsigned int j = 0; j < mat2_cpu.size2(); ++j)
      {
         act = std::fabs(mat2_cpu(i,j) - mat1(i,j)) / std::max( std::fabs(mat2_cpu(i, j)), std::fabs(mat1(i,j)) );
         if (act > ret)
           ret = act;
      }
    }
   //std::cout << ret << std::endl;
   return ret;
}

//
// Triangular solvers
//

template<typename RHSTypeRef, typename RHSTypeCheck, typename Epsilon >
void run_solver_check(RHSTypeRef & B_ref, RHSTypeCheck & B_check, int & retval, Epsilon const & epsilon)
{
   double act_diff = fabs(diff(B_ref, B_check));
   if ( act_diff > epsilon )
   {
     std::cout << " FAILED!" << std::endl;
     std::cout << "# Error at operation: matrix-matrix solve" << std::endl;
     std::cout << "  diff: " << act_diff << std::endl;
     retval = EXIT_FAILURE;
   }
   else
     std::cout << " passed! " << act_diff << std::endl;

}

template< typename NumericT, typename Epsilon,
          typename ReferenceMatrixTypeA, typename ReferenceMatrixTypeB, typename ReferenceMatrixTypeC,
          typename MatrixTypeA, typename MatrixTypeB, typename MatrixTypeC, typename MatrixTypeResult>
int test_solve(Epsilon const& epsilon,

              ReferenceMatrixTypeA const & A,
              ReferenceMatrixTypeB const & B_start,
              ReferenceMatrixTypeC const & C_start,

              MatrixTypeA const & vcl_A,
              MatrixTypeB & vcl_B,
              MatrixTypeC & vcl_C,
              MatrixTypeResult const &
             )
{
   int retval = EXIT_SUCCESS;

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

   ReferenceMatrixTypeA result;
   ReferenceMatrixTypeC C_trans;

   ReferenceMatrixTypeB B = B_start;
   ReferenceMatrixTypeC C = C_start;

   MatrixTypeResult vcl_result;

   // Test: A \ B with various tags --------------------------------------------------------------------------
   std::cout << "Testing A \\ B: " << std::endl;
   std::cout << " * upper_tag:      ";
   result = ublas::solve(A, B, ublas::upper_tag());
   vcl_result = viennacl::linalg::solve(vcl_A, vcl_B, viennacl::linalg::upper_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   std::cout << " * unit_upper_tag: ";
   result = ublas::solve(A, B, ublas::unit_upper_tag());
   vcl_result = viennacl::linalg::solve(vcl_A, vcl_B, viennacl::linalg::unit_upper_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   std::cout << " * lower_tag:      ";
   result = ublas::solve(A, B, ublas::lower_tag());
   vcl_result = viennacl::linalg::solve(vcl_A, vcl_B, viennacl::linalg::lower_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   std::cout << " * unit_lower_tag: ";
   result = ublas::solve(A, B, ublas::unit_lower_tag());
   vcl_result = viennacl::linalg::solve(vcl_A, vcl_B, viennacl::linalg::unit_lower_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   if (retval == EXIT_SUCCESS)
     std::cout << "Test A \\ B passed!" << std::endl;

   B = B_start;
   C = C_start;

   // Test: A \ B^T --------------------------------------------------------------------------
   std::cout << "Testing A \\ B^T: " << std::endl;
   std::cout << " * upper_tag:      ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   //check solve():
   result = ublas::solve(A, C_trans, ublas::upper_tag());
   vcl_result = viennacl::linalg::solve(vcl_A, trans(vcl_C), viennacl::linalg::upper_tag());
   run_solver_check(result, vcl_result, retval, epsilon);
   //check compute kernels:
   std::cout << " * upper_tag:      ";
   ublas::inplace_solve(A, C_trans, ublas::upper_tag());
   viennacl::linalg::inplace_solve(vcl_A, trans(vcl_C), viennacl::linalg::upper_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   std::cout << " * unit_upper_tag: ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   ublas::inplace_solve(A, C_trans, ublas::unit_upper_tag());
   viennacl::linalg::inplace_solve(vcl_A, trans(vcl_C), viennacl::linalg::unit_upper_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   std::cout << " * lower_tag:      ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   ublas::inplace_solve(A, C_trans, ublas::lower_tag());
   viennacl::linalg::inplace_solve(vcl_A, trans(vcl_C), viennacl::linalg::lower_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   std::cout << " * unit_lower_tag: ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   ublas::inplace_solve(A, C_trans, ublas::unit_lower_tag());
   viennacl::linalg::inplace_solve(vcl_A, trans(vcl_C), viennacl::linalg::unit_lower_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   if (retval == EXIT_SUCCESS)
     std::cout << "Test A \\ B^T passed!" << std::endl;

   B = B_start;
   C = C_start;

   // Test: A \ B with various tags --------------------------------------------------------------------------
   std::cout << "Testing A^T \\ B: " << std::endl;
   std::cout << " * upper_tag:      ";
   viennacl::copy(B, vcl_B);
   result = ublas::solve(trans(A), B, ublas::upper_tag());
   vcl_result = viennacl::linalg::solve(trans(vcl_A), vcl_B, viennacl::linalg::upper_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   std::cout << " * unit_upper_tag: ";
   viennacl::copy(B, vcl_B);
   result = ublas::solve(trans(A), B, ublas::unit_upper_tag());
   vcl_result = viennacl::linalg::solve(trans(vcl_A), vcl_B, viennacl::linalg::unit_upper_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   std::cout << " * lower_tag:      ";
   viennacl::copy(B, vcl_B);
   result = ublas::solve(trans(A), B, ublas::lower_tag());
   vcl_result = viennacl::linalg::solve(trans(vcl_A), vcl_B, viennacl::linalg::lower_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   std::cout << " * unit_lower_tag: ";
   viennacl::copy(B, vcl_B);
   result = ublas::solve(trans(A), B, ublas::unit_lower_tag());
   vcl_result = viennacl::linalg::solve(trans(vcl_A), vcl_B, viennacl::linalg::unit_lower_tag());
   run_solver_check(result, vcl_result, retval, epsilon);

   if (retval == EXIT_SUCCESS)
     std::cout << "Test A^T \\ B passed!" << std::endl;

   B = B_start;
   C = C_start;

   // Test: A^T \ B^T --------------------------------------------------------------------------
   std::cout << "Testing A^T \\ B^T: " << std::endl;
   std::cout << " * upper_tag:      ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   //check solve():
   result = ublas::solve(trans(A), C_trans, ublas::upper_tag());
   vcl_result = viennacl::linalg::solve(trans(vcl_A), trans(vcl_C), viennacl::linalg::upper_tag());
   run_solver_check(result, vcl_result, retval, epsilon);
   //check kernels:
   std::cout << " * upper_tag:      ";
   ublas::inplace_solve(trans(A), C_trans, ublas::upper_tag());
   viennacl::linalg::inplace_solve(trans(vcl_A), trans(vcl_C), viennacl::linalg::upper_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   std::cout << " * unit_upper_tag: ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   ublas::inplace_solve(trans(A), C_trans, ublas::unit_upper_tag());
   viennacl::linalg::inplace_solve(trans(vcl_A), trans(vcl_C), viennacl::linalg::unit_upper_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   std::cout << " * lower_tag:      ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   ublas::inplace_solve(trans(A), C_trans, ublas::lower_tag());
   viennacl::linalg::inplace_solve(trans(vcl_A), trans(vcl_C), viennacl::linalg::lower_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   std::cout << " * unit_lower_tag: ";
   viennacl::copy(C, vcl_C); C_trans = trans(C);
   ublas::inplace_solve(trans(A), C_trans, ublas::unit_lower_tag());
   viennacl::linalg::inplace_solve(trans(vcl_A), trans(vcl_C), viennacl::linalg::unit_lower_tag());
   C = trans(C_trans); run_solver_check(C, vcl_C, retval, epsilon);

   if (retval == EXIT_SUCCESS)
     std::cout << "Test A^T \\ B^T passed!" << std::endl;

   return retval;
}

template< typename NumericT, typename F_A, typename F_B, typename Epsilon >
int test_solve(Epsilon const& epsilon)
{
  int ret = EXIT_SUCCESS;
  std::size_t matrix_size = 135;  //some odd number, not too large
  std::size_t rhs_num = 67;

  std::cout << "--- Part 2: Testing matrix-matrix solver ---" << std::endl;

  ublas::matrix<NumericT> A(matrix_size, matrix_size);
  ublas::matrix<NumericT> B_start(matrix_size, rhs_num);
  ublas::matrix<NumericT> C_start(rhs_num, matrix_size);

  for (std::size_t i = 0; i < A.size1(); ++i)
  {
    for (std::size_t j = 0; j < A.size2(); ++j)
        A(i,j) = static_cast<NumericT>(-0.5) * random<NumericT>();
    A(i,i) = NumericT(1.0) + NumericT(2.0) * random<NumericT>(); //some extra weight on diagonal for stability
  }

  for (std::size_t i = 0; i < B_start.size1(); ++i)
    for (std::size_t j = 0; j < B_start.size2(); ++j)
        B_start(i,j) = random<NumericT>();

  for (std::size_t i = 0; i < C_start.size1(); ++i)
    for (std::size_t j = 0; j < C_start.size2(); ++j)
        C_start(i,j) = random<NumericT>();

  // A
  viennacl::range range1_A(matrix_size, 2*matrix_size);
  viennacl::range range2_A(2*matrix_size, 3*matrix_size);
  viennacl::slice slice1_A(matrix_size, 2, matrix_size);
  viennacl::slice slice2_A(0, 3, matrix_size);

  viennacl::matrix<NumericT, F_A>    vcl_A(matrix_size, matrix_size);
  viennacl::copy(A, vcl_A);

  viennacl::matrix<NumericT, F_A>    vcl_big_range_A(4*matrix_size, 4*matrix_size);
  viennacl::matrix_range<viennacl::matrix<NumericT, F_A> > vcl_range_A(vcl_big_range_A, range1_A, range2_A);
  viennacl::copy(A, vcl_range_A);

  viennacl::matrix<NumericT, F_A>    vcl_big_slice_A(4*matrix_size, 4*matrix_size);
  viennacl::matrix_slice<viennacl::matrix<NumericT, F_A> > vcl_slice_A(vcl_big_slice_A, slice1_A, slice2_A);
  viennacl::copy(A, vcl_slice_A);

  // B
  viennacl::range range1_B(matrix_size, 2*matrix_size);
  viennacl::range range2_B(2*rhs_num, 3*rhs_num);
  viennacl::slice slice1_B(matrix_size, 2, matrix_size);
  viennacl::slice slice2_B(0, 3, rhs_num);

  viennacl::matrix<NumericT, F_B>    vcl_B(matrix_size, rhs_num);
  viennacl::copy(B_start, vcl_B);

  viennacl::matrix<NumericT, F_B>    vcl_big_range_B(4*matrix_size, 4*rhs_num);
  viennacl::matrix_range<viennacl::matrix<NumericT, F_B> > vcl_range_B(vcl_big_range_B, range1_B, range2_B);
  viennacl::copy(B_start, vcl_range_B);

  viennacl::matrix<NumericT, F_B>    vcl_big_slice_B(4*matrix_size, 4*rhs_num);
  viennacl::matrix_slice<viennacl::matrix<NumericT, F_B> > vcl_slice_B(vcl_big_slice_B, slice1_B, slice2_B);
  viennacl::copy(B_start, vcl_slice_B);

  // C
  viennacl::range range1_C(rhs_num, 2*rhs_num);
  viennacl::range range2_C(2*matrix_size, 3*matrix_size);
  viennacl::slice slice1_C(rhs_num, 2, rhs_num);
  viennacl::slice slice2_C(0, 3, matrix_size);

  viennacl::matrix<NumericT, F_B>    vcl_C(rhs_num, matrix_size);
  viennacl::copy(C_start, vcl_C);

  viennacl::matrix<NumericT, F_B>    vcl_big_range_C(4*rhs_num, 4*matrix_size);
  viennacl::matrix_range<viennacl::matrix<NumericT, F_B> > vcl_range_C(vcl_big_range_C, range1_C, range2_C);
  viennacl::copy(C_start, vcl_range_C);

  viennacl::matrix<NumericT, F_B>    vcl_big_slice_C(4*rhs_num, 4*matrix_size);
  viennacl::matrix_slice<viennacl::matrix<NumericT, F_B> > vcl_slice_C(vcl_big_slice_C, slice1_C, slice2_C);
  viennacl::copy(C_start, vcl_slice_C);

  std::cout << "Now using A=matrix, B=matrix" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_A, vcl_B, vcl_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=matrix, B=range" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_A, vcl_range_B, vcl_range_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=matrix, B=slice" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_A, vcl_slice_B, vcl_slice_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=range, B=matrix" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_range_A, vcl_B, vcl_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=range, B=range" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_range_A, vcl_range_B, vcl_range_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=range, B=slice" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_range_A, vcl_slice_B, vcl_slice_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=slice, B=matrix" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_slice_A, vcl_B, vcl_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=slice, B=range" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_slice_A, vcl_range_B, vcl_range_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "Now using A=slice, B=slice" << std::endl;
  ret = test_solve<NumericT>(epsilon,
                             A, B_start, C_start,
                             vcl_slice_A, vcl_slice_B, vcl_slice_C, vcl_B
                            );
  if (ret != EXIT_SUCCESS)
    return ret;

  return ret;

}

//
// Control functions
//

template< typename NumericT, typename Epsilon >
int test(Epsilon const& epsilon)
{
  int ret;

  std::cout << "////////////////////////////////" << std::endl;
  std::cout << "/// Now testing A=row, B=row ///" << std::endl;
  std::cout << "////////////////////////////////" << std::endl;
  ret = test_solve<NumericT, viennacl::row_major, viennacl::row_major>(epsilon);
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "////////////////////////////////" << std::endl;
  std::cout << "/// Now testing A=row, B=col ///" << std::endl;
  std::cout << "////////////////////////////////" << std::endl;
  ret = test_solve<NumericT, viennacl::row_major, viennacl::column_major>(epsilon);
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "////////////////////////////////" << std::endl;
  std::cout << "/// Now testing A=col, B=row ///" << std::endl;
  std::cout << "////////////////////////////////" << std::endl;
  ret = test_solve<NumericT, viennacl::column_major, viennacl::row_major>(epsilon);
  if (ret != EXIT_SUCCESS)
    return ret;

  std::cout << "////////////////////////////////" << std::endl;
  std::cout << "/// Now testing A=col, B=col ///" << std::endl;
  std::cout << "////////////////////////////////" << std::endl;
  ret = test_solve<NumericT, viennacl::column_major, viennacl::column_major>(epsilon);
  if (ret != EXIT_SUCCESS)
    return ret;

  return ret;
}

//
// -------------------------------------------------------------
//
int main()
{
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "## Test :: BLAS 3 routines" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;

   int retval = EXIT_SUCCESS;

   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;
   {
      typedef float NumericT;
      NumericT epsilon = NumericT(1.0E-3);
      std::cout << "# Testing setup:" << std::endl;
      std::cout << "  eps:     " << epsilon << std::endl;
      std::cout << "  numeric: float" << std::endl;
      retval = test<NumericT>(epsilon);
      if ( retval == EXIT_SUCCESS )
        std::cout << "# Test passed" << std::endl;
      else
        return retval;
   }
   std::cout << std::endl;
   std::cout << "----------------------------------------------" << std::endl;
   std::cout << std::endl;
#ifdef VIENNACL_WITH_OPENCL
   if ( viennacl::ocl::current_device().double_support() )
#endif
   {
      {
        typedef double NumericT;
        NumericT epsilon = 1.0E-11;
        std::cout << "# Testing setup:" << std::endl;
        std::cout << "  eps:     " << epsilon << std::endl;
        std::cout << "  numeric: double" << std::endl;
        retval = test<NumericT>(epsilon);
        if ( retval == EXIT_SUCCESS )
          std::cout << "# Test passed" << std::endl;
        else
          return retval;
      }
      std::cout << std::endl;
      std::cout << "----------------------------------------------" << std::endl;
      std::cout << std::endl;
   }

   std::cout << std::endl;
   std::cout << "------- Test completed --------" << std::endl;
   std::cout << std::endl;

   return retval;
}


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersFixture.hpp"
#include "../TfLiteParser.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

struct DepthwiseConvolution2dFixture : public ParserFlatbuffersFixture
{
    explicit DepthwiseConvolution2dFixture(const std::string& inputShape,
                                           const std::string& outputShape,
                                           const std::string& filterShape,
                                           const std::string& filterData,
                                           const std::string& strides,
                                           const std::string& paddingType,
                                           const std::string biasShape = "",
                                           const std::string biasData = "")
    {
        std::string inputTensors = "[ 0, 2 ]";
        std::string biasTensor = "";
        std::string biasBuffer = "";
        if (biasShape.size() > 0 && biasData.size() > 0)
        {
            inputTensors = "[ 0, 2, 3 ]";
            biasTensor = R"(
                        {
                            "shape": )" + biasShape + R"( ,
                            "type": "INT32",
                            "buffer": 3,
                            "name": "biasTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        } )";
            biasBuffer = R"(
                    { "data": )" + biasData + R"(, }, )";
        }
        m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [ { "builtin_code": "DEPTHWISE_CONV_2D" } ],
                "subgraphs": [ {
                    "tensors": [
                        {
                            "shape": )" + inputShape + R"(,
                            "type": "UINT8",
                            "buffer": 0,
                            "name": "inputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + outputShape + R"(,
                            "type": "UINT8",
                            "buffer": 1,
                            "name": "outputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 511.0 ],
                                "scale": [ 2.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + filterShape + R"(,
                            "type": "UINT8",
                            "buffer": 2,
                            "name": "filterTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        }, )" + biasTensor + R"(
                    ],
                    "inputs": [ 0 ],
                    "outputs": [ 1 ],
                    "operators": [
                        {
                            "opcode_index": 0,
                            "inputs": )" + inputTensors + R"(,
                            "outputs": [ 1 ],
                            "builtin_options_type": "DepthwiseConv2DOptions",
                            "builtin_options": {
                                "padding": ")" + paddingType + R"(",
                                "stride_w": )" + strides+ R"(,
                                "stride_h": )" + strides+ R"(,
                                "depth_multiplier": 1,
                                "fused_activation_function": "NONE"
                            },
                            "custom_options_format": "FLEXBUFFERS"
                        }
                    ],
                } ],
                "buffers" : [
                    { },
                    { },
                    { "data": )" + filterData + R"(, }, )"
                    + biasBuffer + R"(
                ]
            }
        )";
        SetupSingleInputSingleOutput("inputTensor", "outputTensor");
    }
};

struct DepthwiseConvolution2dSameFixture : DepthwiseConvolution2dFixture
{
    DepthwiseConvolution2dSameFixture()
    : DepthwiseConvolution2dFixture("[ 1, 3, 3, 1 ]",           // inputShape
                                    "[ 1, 3, 3, 1 ]",           // outputShape
                                    "[ 1, 3, 3, 1 ]",           // filterShape
                                    "[ 9,8,7, 6,5,4, 3,2,1 ]",  // filterData
                                    "1",                        // stride w and h
                                    "SAME")                     // padding type
    {}
};

BOOST_FIXTURE_TEST_CASE(ParseDepthwiseConv2DSame, DepthwiseConvolution2dSameFixture)
{
    RunTest<4, armnn::DataType::QuantisedAsymm8>(
        0,
        { 0, 1, 2,
          3, 4, 5,
          6, 7, 8 },
        // the expected values were generated using the example python implementation at
        // https://eli.thegreenplace.net/2018/depthwise-separable-convolutions-for-machine-learning/
        // divide the expected values by the output scale, as it is not 1.0
        {  14/2,  35/2,  38/2,
           57/2, 120/2, 111/2,
          110/2, 197/2, 158/2 });
}

struct DepthwiseConvolution2dValidFixture : DepthwiseConvolution2dFixture
{
    DepthwiseConvolution2dValidFixture ()
    : DepthwiseConvolution2dFixture("[ 1, 3, 3, 1 ]",           // inputShape
                                    "[ 1, 1, 1, 1 ]",           // outputShape
                                    "[ 1, 3, 3, 1 ]",           // filterShape
                                    "[ 9,8,7, 6,5,4, 3,2,1 ]",  // filterData
                                    "1",                        // stride w and h
                                    "VALID")                    // padding type
    {}
};

BOOST_FIXTURE_TEST_CASE(ParseDepthwiseConv2DValid, DepthwiseConvolution2dValidFixture)
{
    RunTest<4, armnn::DataType::QuantisedAsymm8>(
        0,
        { 0, 1, 2,
          3, 4, 5,
          6, 7, 8 },
        // divide the expected values by the output scale, as it is not 1.0
        { 120/2 });
}

struct DepthwiseConvolution2dSameBiasFixture : DepthwiseConvolution2dFixture
{
    DepthwiseConvolution2dSameBiasFixture()
    : DepthwiseConvolution2dFixture("[ 1, 3, 3, 1 ]",           // inputShape
                                    "[ 1, 3, 3, 1 ]",           // outputShape
                                    "[ 1, 3, 3, 1 ]",           // filterShape
                                    "[ 9,8,7, 6,5,4, 3,2,1 ]",  // filterData
                                    "1",                        // stride w and h
                                    "SAME",                     // padding type
                                    "[ 1 ]",                    // biasShape
                                    "[ 10, 0, 0, 0 ]")          // biasData
    {}
};

BOOST_FIXTURE_TEST_CASE(ParseDepthwiseConv2DSameBias, DepthwiseConvolution2dSameBiasFixture)
{
    RunTest<4, armnn::DataType::QuantisedAsymm8>(
        0,
        { 0, 1, 2,
          3, 4, 5,
          6, 7, 8 },
        // divide the expected values by the output scale, as it is not 1.0
        { ( 14+10)/2, ( 35+10)/2, ( 38+10)/2,
          ( 57+10)/2, (120+10)/2, (111+10)/2,
          (110+10)/2, (197+10)/2, (158+10)/2 });
}

BOOST_AUTO_TEST_SUITE_END()


#include <iostream>

#include <Eigen/Geometry>
#include <Eigen/StdVector>

// contains indices of the mesh vertex that are handle points
Eigen::MatrixXi BE;

int main(int argc, char **argv){

        std::vector<int> CE_ind = {0,28821, 33603};

        BE.resize(2,2);
        BE << 0,1,
            1,2;

}


#include "Outer_Parameters.hxx"
#include "Function.hxx"
#include "../sdp_read.hxx"
#include "../sdp_solve.hxx"

#include "../ostream_vector.hxx"

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/property_tree/json_parser.hpp>

// We convert the optimization problem into a regular linear
// programming problem.
//
// 1) Each polynomial in Block::polys adds two variables.  The
// weights of those polynomials are unbounded, while linear
// programming requires a strictly positive variable.  So we
// substitute W_n = w_n+ - w_n+, where both w_n+ and w_n- are
// strictly positive.  All blocks should have the same number of
// polynomials, so we only need to look at the first one.
//
// 2) Each constraint adds one 'slack' variable s_n.  There is one
// constraint per coordinate 'x', with each block having multiple,
// separate coordinates.
//
// 3) One more global variable delta, which gives the linear
// program something to minimize.
//
// This turns the problem
//
//   A_0 . W > 0
//   A_1 . W > 0
//   ...
//
// into
//
//   min delta
//
// where
//
//   A_0 . (w_+ - w_-) + delta - s_0 = 0
//   A_1 . (w_+ - w_-) + delta - s_1 = 0
//   ...
//   w_n+, w_n-, s_n, delta >= 0
//
// There is a constraint for every point that is sampled.  At the
// beginning, we sample the min and max for each block, so there are
// 2*num_blocks constraints.  For the single correlator example, there
// is an additional constraint on the first block at x=0.  In general
//
// => num_rows == num_constraints
// => num_columns == 2*num_weights + num_constraints + 1

std::vector<El::BigFloat>
load_vector(const boost::filesystem::path &vector_path);

void read_function_blocks(
  const boost::filesystem::path &input_file,
  std::vector<El::BigFloat> &objectives,
  std::vector<El::BigFloat> &normalization,
  std::vector<std::vector<std::vector<std::vector<Function>>>> &functions);

void read_points(const boost::filesystem::path &input_path,
                 std::vector<std::vector<El::BigFloat>> &points);

std::vector<El::BigFloat> compute_optimal(
  const std::vector<std::vector<std::vector<std::vector<Function>>>> &functions,
  const std::vector<std::vector<El::BigFloat>> &initial_points,
  const std::vector<El::BigFloat> &objectives,
  const std::vector<El::BigFloat> &normalization,
  const Outer_Parameters &parameters_in);

int main(int argc, char **argv)
{
  El::Environment env(argc, argv);
  Outer_Parameters parameters(argc, argv);
  if(!parameters.is_valid())
    {
      return 0;
    }

  const int64_t precision(parameters.solver.precision);
  El::gmp::SetPrecision(precision);
  // El::gmp wants base-2 bits, but boost::multiprecision wants
  // base-10 digits.
  Boost_Float::default_precision(precision * log(2) / log(10));

  if(parameters.verbosity >= Verbosity::regular && El::mpi::Rank() == 0)
    {
      std::cout << "Outer_Limits started at "
                << boost::posix_time::second_clock::local_time() << '\n'
                << parameters << '\n'
                << std::flush;
    }

  std::vector<El::BigFloat> objectives, normalization;
  std::vector<std::vector<std::vector<std::vector<Function>>>> functions;
  read_function_blocks(parameters.functions_path, objectives, normalization,
                       functions);

  std::vector<std::vector<El::BigFloat>> initial_points;
  read_points(parameters.points_path, initial_points);

  std::vector<El::BigFloat> weights(compute_optimal(
    functions, initial_points, objectives, normalization, parameters));

  El::BigFloat optimal(0);
  for(size_t index(0); index < objectives.size(); ++index)
    {
      optimal += objectives[index] * weights[index];
    }
  if(parameters.verbosity >= Verbosity::regular && El::mpi::Rank() == 0)
    {
      set_stream_precision(std::cout);
      std::cout << "optimal: " << optimal << "\n";
    }
  if(El::mpi::Rank() == 0)
    {
      if(parameters.verbosity >= Verbosity::regular)
        {
          std::cout << "Saving solution to " << parameters.output_path << "\n";
        }
      boost::filesystem::ofstream output(parameters.output_path);
      set_stream_precision(output);
      output << "{\n  \"optimal\": \"" << optimal << "\",\n"
             << "  \"y\":\n  [\n";
      for(auto weight(weights.begin()); weight != weights.end(); ++weight)
        {
          if(weight != weights.begin())
            {
              output << ",\n";
            }
          output << "    \"" << *weight << "\"";
        }

      output << "\n  ],\n  \"options\": \n";
      boost::property_tree::write_json(output, to_property_tree(parameters));
      output << "}\n";
    }
}