/* Author: Ivan Christov, Wolfgang Bangerth, Texas A&M University, 2006 */

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

// @sect3{Include files and global variables}

// For an explanation of the include files, the reader should refer to the
// example programs step-1 through step-4. They are in the standard order,
// which is base -- lac -- grid --
// dofs -- fe -- numerics (since each
// of these categories roughly builds upon previous ones), then a few C++
// headers for file input/output and string streams.
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/utilities.h>
#include <deal.II/lac/vector.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/data_out.h>

#include <fstream>
#include <iostream>

// The last step is as in all previous programs:
namespace Step25
{
  using namespace dealii;

  // @sect3{The SineGordonProblem class template}

  // The entire algorithm for solving the problem is encapsulated in this
  // class. As in previous example programs, the class is declared with a
  // template parameter, which is the spatial dimension, so that we can solve
  // the sine-Gordon equation in one, two or three spatial dimensions. For
  // more on the dimension-independent class-encapsulation of the problem, the
  // reader should consult step-3 and step-4.
  //
  // Compared to step-23 and step-24, there isn't anything newsworthy in the
  // general structure of the program (though there is of course in the inner
  // workings of the various functions!). The most notable difference is the
  // presence of the two new functions compute_nl_term and
  // compute_nl_matrix that compute the nonlinear contributions
  // to the system matrix and right-hand side of the first equation, as
  // discussed in the Introduction. In addition, we have to have a vector
  // solution_update that contains the nonlinear update to the
  // solution vector in each Newton step.
  //
  // As also mentioned in the introduction, we do not store the velocity
  // variable in this program, but the mass matrix times the velocity. This is
  // done in the M_x_velocity variable (the "x" is intended to
  // stand for "times").
  //
  // Finally, the output_timestep_skip variable stores the number
  // of time steps to be taken each time before graphical output is to be
  // generated. This is of importance when using fine meshes (and consequently
  // small time steps) where we would run lots of time steps and create lots
  // of output files of solutions that look almost the same in subsequent
  // files. This only clogs up our visualization procedures and we should
  // avoid creating more output than we are really interested in. Therefore,
  // if this variable is set to a value $n$ bigger than one, output is
  // generated only every $n$th time step.
  template <int dim>
  class SineGordonProblem
  {
  public:
    SineGordonProblem ();
    void run ();

  private:
    void make_grid_and_dofs ();
    void assemble_system ();
    void compute_nl_term (const Vector<double> &old_data,
                          const Vector<double> &new_data,
                          Vector<double>       &nl_term) const;
    void compute_nl_matrix (const Vector<double> &old_data,
                            const Vector<double> &new_data,
                            SparseMatrix<double> &nl_matrix) const;
    unsigned int solve ();
    void output_results (const unsigned int timestep_number) const;

    Triangulation<dim>   triangulation;
    FE_Q<dim>            fe;
    DoFHandler<dim>      dof_handler;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> system_matrix;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;

    const unsigned int n_global_refinements;

    double time;
    const double final_time, time_step;
    const double theta;

    Vector<double>       solution, solution_update, old_solution;
    Vector<double>       M_x_velocity;
    Vector<double>       system_rhs;

    const unsigned int output_timestep_skip;
  };

  // @sect3{Initial conditions}

  // In the following two classes, we first implement the exact solution for
  // 1D, 2D, and 3D mentioned in the introduction to this program. This
  // space-time solution may be of independent interest if one wanted to test
  // the accuracy of the program by comparing the numerical against the
  // analytic solution (note however that the program uses a finite domain,
  // whereas these are analytic solutions for an unbounded domain). This may,
  // for example, be done using the VectorTools::integrate_difference
  // function. Note, again (as was already discussed in step-23), how we
  // describe space-time functions as spatial functions that depend on a time
  // variable that can be set and queried using the FunctionTime::set_time()
  // and FunctionTime::get_time() member functions of the FunctionTime base
  // class of the Function class.
  template <int dim>
  class ExactSolution : public Function<dim>
  {
  public:
    ExactSolution (const unsigned int n_components = 1,
                   const double time = 0.) : Function<dim>(n_components, time) {}
    virtual double value (const Point<dim> &p,
                          const unsigned int component = 0) const;
  };

  template <int dim>
  double ExactSolution<dim>::value (const Point<dim> &p,
                                    const unsigned int /*component*/) const
  {
    double t = this->get_time ();

    switch (dim)
      {
      case 1:
      {
        const double m = 0.5;
        const double c1 = 0.;
        const double c2 = 0.;
        return -4.*std::atan (m /
                              std::sqrt(1.-m*m) *
                              std::sin(std::sqrt(1.-m*m)*t+c2) /
                              std::cosh(m*p[0]+c1));
      }

      case 2:
      {
        const double theta  = numbers::PI/4.;
        const double lambda  = 1.;
        const double a0  = 1.;
        const double s   = 1.;
        const double arg = p[0] * std::cos(theta) +
                           std::sin(theta) *
                           (p[1] * std::cosh(lambda) +
                            t * std::sinh(lambda));
        return 4.*std::atan(a0*std::exp(s*arg));
      }

      case 3:
      {
        double theta  = numbers::PI/4;
        double phi = numbers::PI/4;
        double tau = 1.;
        double c0  = 1.;
        double s   = 1.;
        double arg = p[0]*std::cos(theta) +
                     p[1]*std::sin(theta) * std::cos(phi) +
                     std::sin(theta) * std::sin(phi) *
                     (p[2]*std::cosh(tau)+t*std::sinh(tau));
        return 4.*std::atan(c0*std::exp(s*arg));
      }

      default:
        Assert (false, ExcNotImplemented());
        return -1e8;
      }
  }

  // In the second part of this section, we provide the initial conditions. We
  // are lazy (and cautious) and don't want to implement the same functions as
  // above a second time. Rather, if we are queried for initial conditions, we
  // create an object ExactSolution, set it to the correct time,
  // and let it compute whatever values the exact solution has at that time:
  template <int dim>
  class InitialValues : public Function<dim>
  {
  public:
    InitialValues (const unsigned int n_components = 1,
                   const double time = 0.)
      :
      Function<dim>(n_components, time)
    {}

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

  template <int dim>
  double InitialValues<dim>::value (const Point<dim> &p,
                                    const unsigned int component) const
  {
    return ExactSolution<dim>(1, this->get_time()).value (p, component);
  }

  // @sect3{Implementation of the SineGordonProblem class}

  // Let's move on to the implementation of the main class, as it implements
  // the algorithm outlined in the introduction.

  // @sect4{SineGordonProblem::SineGordonProblem}

  // This is the constructor of the SineGordonProblem class. It
  // specifies the desired polynomial degree of the finite elements,
  // associates a DoFHandler to the triangulation
  // object (just as in the example programs step-3 and step-4), initializes
  // the current or initial time, the final time, the time step size, and the
  // value of $\theta$ for the time stepping scheme. Since the solutions we
  // compute here are time-periodic, the actual value of the start-time
  // doesn't matter, and we choose it so that we start at an interesting time.
  //
  // Note that if we were to chose the explicit Euler time stepping scheme
  // ($\theta = 0$), then we must pick a time step $k \le h$, otherwise the
  // scheme is not stable and oscillations might arise in the solution. The
  // Crank-Nicolson scheme ($\theta = \frac{1}{2}$) and the implicit Euler
  // scheme ($\theta=1$) do not suffer from this deficiency, since they are
  // unconditionally stable. However, even then the time step should be chosen
  // to be on the order of $h$ in order to obtain a good solution. Since we
  // know that our mesh results from the uniform subdivision of a rectangle,
  // we can compute that time step easily; if we had a different domain, the
  // technique in step-24 using GridTools::minimal_cell_diameter would work as
  // well.
  template <int dim>
  SineGordonProblem<dim>::SineGordonProblem ()
    :
    fe (1),
    dof_handler (triangulation),
    n_global_refinements (6),
    time (-5.4414),
    final_time (2.7207),
    time_step (10*1./std::pow(2.,1.*n_global_refinements)),
    theta (0.5),
    output_timestep_skip (1)
  {}

  // @sect4{SineGordonProblem::make_grid_and_dofs}

  // This function creates a rectangular grid in dim dimensions
  // and refines it several times. Also, all matrix and vector members of the
  // SineGordonProblem class are initialized to their appropriate
  // sizes once the degrees of freedom have been assembled. Like step-24, we
  // use the MatrixCreator class to generate a mass matrix $M$
  // and a Laplace matrix $A$ and store them in the appropriate variables for
  // the remainder of the program's life.
  template <int dim>
  void SineGordonProblem<dim>::make_grid_and_dofs ()
  {
    GridGenerator::hyper_cube (triangulation, -10, 10);
    triangulation.refine_global (n_global_refinements);

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

    dof_handler.distribute_dofs (fe);

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

    sparsity_pattern.reinit (dof_handler.n_dofs(),
                             dof_handler.n_dofs(),
                             dof_handler.max_couplings_between_dofs());
    DoFTools::make_sparsity_pattern (dof_handler, sparsity_pattern);
    sparsity_pattern.compress ();

    system_matrix.reinit  (sparsity_pattern);
    mass_matrix.reinit    (sparsity_pattern);
    laplace_matrix.reinit (sparsity_pattern);

    MatrixCreator::create_mass_matrix (dof_handler,
                                       QGauss<dim>(3),
                                       mass_matrix);
    MatrixCreator::create_laplace_matrix (dof_handler,
                                          QGauss<dim>(3),
                                          laplace_matrix);

    solution.reinit       (dof_handler.n_dofs());
    solution_update.reinit     (dof_handler.n_dofs());
    old_solution.reinit   (dof_handler.n_dofs());
    M_x_velocity.reinit    (dof_handler.n_dofs());
    system_rhs.reinit     (dof_handler.n_dofs());
  }

  // @sect4{SineGordonProblem::assemble_system}

  // This functions assembles the system matrix and right-hand side vector for
  // each iteration of Newton's method. The reader should refer to the
  // Introduction for the explicit formulas for the system matrix and
  // right-hand side.
  //
  // Note that during each time step, we have to add up the various
  // contributions to the matrix and right hand sides. In contrast to step-23
  // and step-24, this requires assembling a few more terms, since they depend
  // on the solution of the previous time step or previous nonlinear step. We
  // use the functions compute_nl_matrix and
  // compute_nl_term to do this, while the present function
  // provides the top-level logic.
  template <int dim>
  void SineGordonProblem<dim>::assemble_system ()
  {
    // First we assemble the Jacobian matrix $F'_h(U^{n,l})$, where $U^{n,l}$
    // is stored in the vector solution for convenience.
    system_matrix = 0;
    system_matrix.copy_from (mass_matrix);
    system_matrix.add (std::pow(time_step*theta,2), laplace_matrix);

    SparseMatrix<double> tmp_matrix (sparsity_pattern);
    compute_nl_matrix (old_solution, solution, tmp_matrix);
    system_matrix.add (-std::pow(time_step*theta,2), tmp_matrix);

    // Then, we compute the right-hand side vector $-F_h(U^{n,l})$.
    system_rhs = 0;

    tmp_matrix = 0;
    tmp_matrix.copy_from (mass_matrix);
    tmp_matrix.add (std::pow(time_step*theta,2), laplace_matrix);

    Vector<double> tmp_vector (solution.size());
    tmp_matrix.vmult (tmp_vector, solution);
    system_rhs += tmp_vector;

    tmp_matrix = 0;
    tmp_matrix.copy_from (mass_matrix);
    tmp_matrix.add (-std::pow(time_step,2)*theta*(1-theta), laplace_matrix);

    tmp_vector = 0;
    tmp_matrix.vmult (tmp_vector, old_solution);
    system_rhs -= tmp_vector;

    system_rhs.add (-time_step, M_x_velocity);

    tmp_vector = 0;
    compute_nl_term (old_solution, solution, tmp_vector);
    system_rhs.add (std::pow(time_step,2)*theta, tmp_vector);

    system_rhs *= -1;
  }

  // @sect4{SineGordonProblem::compute_nl_term}

  // This function computes the vector $S(\cdot,\cdot)$, which appears in the
  // nonlinear term in the both equations of the split formulation. This
  // function not only simplifies the repeated computation of this term, but
  // it is also a fundamental part of the nonlinear iterative solver that we
  // use when the time stepping is implicit (i.e. $\theta\ne 0$). Moreover, we
  // must allow the function to receive as input an "old" and a "new"
  // solution. These may not be the actual solutions of the problem stored in
  // old_solution and solution, but are simply the
  // two functions we linearize about. For the purposes of this function, let
  // us call the first two arguments $w_{\mathrm{old}}$ and $w_{\mathrm{new}}$
  // in the documentation of this class below, respectively.
  //
  // As a side-note, it is perhaps worth investigating what order quadrature
  // formula is best suited for this type of integration. Since $\sin(\cdot)$
  // is not a polynomial, there are probably no quadrature formulas that can
  // integrate these terms exactly. It is usually sufficient to just make sure
  // that the right hand side is integrated up to the same order of accuracy
  // as the discretization scheme is, but it may be possible to improve on the
  // constant in the asympotitic statement of convergence by choosing a more
  // accurate quadrature formula.
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_term (const Vector<double> &old_data,
                                                const Vector<double> &new_data,
                                                Vector<double>       &nl_term) const
  {
    const QGauss<dim> quadrature_formula (3);
    FEValues<dim>     fe_values (fe, quadrature_formula,
                                 update_values |
                                 update_JxW_values |
                                 update_quadrature_points);

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

    Vector<double> local_nl_term (dofs_per_cell);
    std::vector<unsigned int> local_dof_indices (dofs_per_cell);
    std::vector<double> old_data_values (n_q_points);
    std::vector<double> new_data_values (n_q_points);

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();

    for (; cell!=endc; ++cell)
      {
        // Once we re-initialize our FEValues instantiation to
        // the current cell, we make use of the
        // get_function_values routine to get the values of the
        // "old" data (presumably at $t=t_{n-1}$) and the "new" data
        // (presumably at $t=t_n$) at the nodes of the chosen quadrature
        // formula.
        fe_values.reinit (cell);
        fe_values.get_function_values (old_data, old_data_values);
        fe_values.get_function_values (new_data, new_data_values);

        // Now, we can evaluate $\int_K \sin\left[\theta w_{\mathrm{new}} +
        // (1-\theta) w_{\mathrm{old}}\right] \,\varphi_j\,\mathrm{d}x$ using
        // the desired quadrature formula.
        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            local_nl_term(i) += (std::sin(theta * new_data_values[q_point] +
                                          (1-theta) * old_data_values[q_point]) *
                                 fe_values.shape_value (i, q_point) *
                                 fe_values.JxW (q_point));

        // We conclude by adding up the contributions of the integrals over
        // the cells to the global integral.
        cell->get_dof_indices (local_dof_indices);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          nl_term(local_dof_indices[i]) += local_nl_term(i);

        local_nl_term = 0;
      }
  }

  // @sect4{SineGordonProblem::compute_nl_matrix}

  // This is the second function dealing with the nonlinear scheme. It
  // computes the matrix $N(\cdot,\cdot)$, whicih appears in the nonlinear
  // term in the Jacobian of $F(\cdot)$. Just as compute_nl_term,
  // we must allow this function to receive as input an "old" and a "new"
  // solution, which we again call $w_{\mathrm{old}}$ and $w_{\mathrm{new}}$
  // below, respectively.
  template <int dim>
  void SineGordonProblem<dim>::compute_nl_matrix (const Vector<double> &old_data,
                                                  const Vector<double> &new_data,
                                                  SparseMatrix<double> &nl_matrix) const
  {
    QGauss<dim>   quadrature_formula (3);
    FEValues<dim> fe_values (fe, quadrature_formula,
                             update_values | update_JxW_values | update_quadrature_points);

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

    FullMatrix<double> local_nl_matrix (dofs_per_cell, dofs_per_cell);
    std::vector<unsigned int> local_dof_indices (dofs_per_cell);
    std::vector<double> old_data_values (n_q_points);
    std::vector<double> new_data_values (n_q_points);

    typename DoFHandler<dim>::active_cell_iterator
    cell = dof_handler.begin_active(),
    endc = dof_handler.end();

    for (; cell!=endc; ++cell)
      {
        // Again, first we re-initialize our FEValues
        // instantiation to the current cell.
        fe_values.reinit (cell);
        fe_values.get_function_values (old_data, old_data_values);
        fe_values.get_function_values (new_data, new_data_values);

        // Then, we evaluate $\int_K \cos\left[\theta w_{\mathrm{new}} +
        // (1-\theta) w_{\mathrm{old}}\right]\, \varphi_i\,
        // \varphi_j\,\mathrm{d}x$ using the desired quadrature formula.
        for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
          for (unsigned int i=0; i<dofs_per_cell; ++i)
            for (unsigned int j=0; j<dofs_per_cell; ++j)
              local_nl_matrix(i,j) += (std::cos(theta * new_data_values[q_point] +
                                                (1-theta) * old_data_values[q_point]) *
                                       fe_values.shape_value (i, q_point) *
                                       fe_values.shape_value (j, q_point) *
                                       fe_values.JxW (q_point));

        // Finally, we add up the contributions of the integrals over the
        // cells to the global integral.
        cell->get_dof_indices (local_dof_indices);

        for (unsigned int i=0; i<dofs_per_cell; ++i)
          for (unsigned int j=0; j<dofs_per_cell; ++j)
            nl_matrix.add(local_dof_indices[i], local_dof_indices[j],
                          local_nl_matrix(i,j));

        local_nl_matrix = 0;
      }
  }

  // @sect4{SineGordonProblem::solve}

  // As discussed in the Introduction, this function uses the CG iterative
  // solver on the linear system of equations resulting from the finite
  // element spatial discretization of each iteration of Newton's method for
  // the (nonlinear) first equation of the split formulation. The solution to
  // the system is, in fact, $\delta U^{n,l}$ so it is stored in
  // solution_update and used to update solution in
  // the run function.
  //
  // Note that we re-set the solution update to zero before solving for
  // it. This is not necessary: iterative solvers can start from any point and
  // converge to the correct solution. If one has a good estimate about the
  // solution of a linear system, it may be worthwhile to start from that
  // vector, but as a general observation it is a fact that the starting point
  // doesn't matter very much: it has to be a very, very good guess to reduce
  // the number of iterations by more than a few. It turns out that for this
  // problem, using the previous nonlinear update as a starting point actually
  // hurts convergence and increases the number of iterations needed, so we
  // simply set it to zero.
  //
  // The function returns the number of iterations it took to converge to a
  // solution. This number will later be used to generate output on the screen
  // showing how many iterations were needed in each nonlinear iteration.
  template <int dim>
  unsigned int
  SineGordonProblem<dim>::solve ()
  {
    SolverControl solver_control (1000, 1e-12*system_rhs.l2_norm());
    SolverCG<> cg (solver_control);

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

    solution_update = 0;
    cg.solve (system_matrix, solution_update,
              system_rhs,
              preconditioner);

    return solver_control.last_step();
  }

  // @sect4{SineGordonProblem::output_results}

  // This function outputs the results to a file. It is pretty much identical
  // to the respective functions in step-23 and step-24:
  template <int dim>
  void
  SineGordonProblem<dim>::output_results (const unsigned int timestep_number) const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (solution, "u");
    data_out.build_patches ();

    const std::string filename =  "solution-" +
                                  Utilities::int_to_string (timestep_number, 3) +
                                  ".vtk";

    std::ofstream output (filename.c_str());
    data_out.write_vtk (output);
  }

  // @sect4{SineGordonProblem::run}

  // This function has the top-level control over everything: it runs the
  // (outer) time-stepping loop, the (inner) nonlinear-solver loop, and
  // outputs the solution after each time step.
  template <int dim>
  void SineGordonProblem<dim>::run ()
  {
    make_grid_and_dofs ();

    // To aknowledge the initial condition, we must use the function $u_0(x)$
    // to compute $U^0$. To this end, below we will create an object of type
    // InitialValues; note that when we create this object (which
    // is derived from the Function class), we set its internal
    // time variable to $t_0$, to indicate that the initial condition is a
    // function of space and time evaluated at $t=t_0$.
    //
    // Then we produce $U^0$ by projecting $u_0(x)$ onto the grid using
    // VectorTools::project. We have to use the same construct
    // using hanging node constraints as in step-21: the VectorTools::project
    // function requires a hanging node constraints object, but to be used we
    // first need to close it:
    {
      ConstraintMatrix constraints;
      constraints.close();
      VectorTools::project (dof_handler,
                            constraints,
                            QGauss<dim>(3),
                            InitialValues<dim> (1, time),
                            solution);
    }

    // For completeness, we output the zeroth time step to a file just like
    // any other other time step.
    output_results (0);

    // Now we perform the time stepping: at every time step we solve the
    // matrix equation(s) corresponding to the finite element discretization
    // of the problem, and then advance our solution according to the time
    // stepping formulas we discussed in the Introduction.
    unsigned int timestep_number = 1;
    for (time+=time_step; time<=final_time; time+=time_step, ++timestep_number)
      {
        old_solution = solution;

        std::cout << std::endl
                  << "Time step #" << timestep_number << "; "
                  << "advancing to t = " << time << "."
                  << std::endl;

        // At the beginning of each time step we must solve the nonlinear
        // equation in the split formulation via Newton's method ---
        // i.e. solve for $\delta U^{n,l}$ then compute $U^{n,l+1}$ and so
        // on. The stopping criterion for this nonlinear iteration is that
        // $\|F_h(U^{n,l})\|_2 \le 10^{-6} \|F_h(U^{n,0})\|_2$. Consequently,
        // we need to record the norm of the residual in the first iteration.
        //
        // At the end of each iteration, we output to the console how many
        // linear solver iterations it took us. When the loop below is done,
        // we have (an approximation of) $U^n$.
        double initial_rhs_norm = 0.;
        bool first_iteration = true;
        do
          {
            assemble_system ();

            if (first_iteration == true)
              initial_rhs_norm = system_rhs.l2_norm();

            const unsigned int n_iterations
              = solve ();

            solution += solution_update;

            if (first_iteration == true)
              std::cout << "    " << n_iterations;
            else
              std::cout << '+' << n_iterations;
            first_iteration = false;
          }
        while (system_rhs.l2_norm() > 1e-6 * initial_rhs_norm);

        std::cout << " CG iterations per nonlinear step."
                  << std::endl;

        // Upon obtaining the solution to the first equation of the problem at
        // $t=t_n$, we must update the auxiliary velocity variable
        // $V^n$. However, we do not compute and store $V^n$ since it is not a
        // quantity we use directly in the problem. Hence, for simplicity, we
        // update $MV^n$ directly:
        Vector<double> tmp_vector (solution.size());
        laplace_matrix.vmult (tmp_vector, solution);
        M_x_velocity.add (-time_step*theta, tmp_vector);

        tmp_vector = 0;
        laplace_matrix.vmult (tmp_vector, old_solution);
        M_x_velocity.add (-time_step*(1-theta), tmp_vector);

        tmp_vector = 0;
        compute_nl_term (old_solution, solution, tmp_vector);
        M_x_velocity.add (-time_step, tmp_vector);

        // Oftentimes, in particular for fine meshes, we must pick the time
        // step to be quite small in order for the scheme to be
        // stable. Therefore, there are a lot of time steps during which
        // "nothing interesting happens" in the solution. To improve overall
        // efficiency -- in particular, speed up the program and save disk
        // space -- we only output the solution every
        // output_timestep_skip time steps:
        if (timestep_number % output_timestep_skip == 0)
          output_results (timestep_number);
      }
  }
}

// @sect3{The main function}

// This is the main function of the program. It creates an object of top-level
// class and calls its principal function. Also, we suppress some of the
// library output by setting deallog.depth_console to
// zero. Furthermore, if exceptions are thrown during the execution of the run
// method of the SineGordonProblem class, we catch and report
// them here. For more information about exceptions the reader should consult
// step-6.
int main ()
{
  try
    {
      using namespace dealii;
      using namespace Step25;

      deallog.depth_console (0);

      SineGordonProblem<1> sg_problem;
      sg_problem.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;
}


/***************************************************************************************************************************
 * circle_tracking_8_tracking.cpp
 *
 * Author: Yuxi
 *
 * Update Time: 2019.9.17
 *
 * 说明: mavros无人机圆形以及8字形飞行(依据位置控制)
 *
***************************************************************************************************************************/
//头文件
#include <ros/ros.h>
//#include <tf2_ros/transform_listener.h>

#include <iostream>
#include <Eigen/Eigen>
#include <math.h>

#include <math_utils.h>
#include <Frame_tf_utils.h>
//msg 头文件
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/AccelStamped.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/UInt16.h>
#include <std_msgs/Float64.h>
#include <tf2_msgs/TFMessage.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/Range.h>

using namespace std;
//---------------------------------------vicon定位相关------------------------------------------
Eigen::Quaterniond q_uav_mocap;                           // 无人机当前姿态(四元数)
Eigen::Quaterniond q_pen_mocap;

Eigen::Vector3d Euler_uav_mocap;                              //无人机当前姿态 (vicon)
Eigen::Vector3d Euler_pen_mocap;

Eigen::Vector3d pose_pen_mocap;
Eigen::Vector3d pose_uav_mocap;

Eigen::Vector3d twist_uav_mocap;
Eigen::Vector3d twist_pen_mocap;

float error_last = 0;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>函数声明<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
float get_dt(ros::Time last);                                                        // 获取时间
float cal_error_deriv(float error_now, float dt);                                    // 求微分
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>回调函数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
void euler_uav_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    Eigen::Vector3d pose_uav_mocap_enu(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    pose_uav_mocap = pose_uav_mocap_enu;
    // Read the Quaternion from the Vrpn Package [Frame: Vicon[ENU]]
    Eigen::Quaterniond q_uav_mocap_enu(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
    q_uav_mocap = q_uav_mocap_enu;

    // Transform the Quaternion to Euler Angles
    Euler_uav_mocap = quaternion_to_euler(q_uav_mocap);

}

void euler_pen_cb(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    // Read the Quaternion from the Vrpn Package [Frame: Vicon[ENU]]
    Eigen::Quaterniond q_pen_mocap_enu(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
    q_pen_mocap = q_pen_mocap_enu;

    // Transform the Quaternion to Euler Angles
    Euler_pen_mocap = quaternion_to_euler(q_pen_mocap);

    Eigen::Vector3d pose_pen_mocap_enu(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
    pose_pen_mocap = pose_pen_mocap_enu;
}

void twist_uav_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    Eigen::Vector3d twist_uav_mocap_enu(msg->twist.angular.x, msg->twist.angular.y, msg->twist.angular.z);
    twist_uav_mocap = twist_uav_mocap_enu;
}

void twist_pen_cb(const geometry_msgs::TwistStamped::ConstPtr& msg)
{
    Eigen::Vector3d twist_pen_mocap_enu(msg->twist.angular.x, msg->twist.angular.y, msg->twist.angular.z);
    twist_pen_mocap = twist_pen_mocap_enu;
}
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, "test_demo");
    ros::NodeHandle nh("~");

    // 【订阅】vicon uav Euler角
    ros::Subscriber vicon_uav_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/UAV_6_3/pose", 10, euler_uav_cb);

    // 【订阅】vicon pendulum Euler角
    ros::Subscriber euler_sub = nh.subscribe<geometry_msgs::PoseStamped>("/vrpn_client_node/pendulum/pose", 10, euler_pen_cb);

    // 【订阅】vicon uav Euler角
    ros::Subscriber vicon_uav_twist_sub = nh.subscribe<geometry_msgs::TwistStamped>("/vrpn_client_node/UAV_6_3/twist", 10, twist_uav_cb);

    // 【订阅】vicon pendulum Euler角
    ros::Subscriber euler_pen_twist_sub = nh.subscribe<geometry_msgs::TwistStamped>("/vrpn_client_node/pendulum/twist", 10, twist_pen_cb);

    ros::Rate rate(50.0);

    ros::Time begin_time = ros::Time::now();
    float last_time = get_dt(begin_time);
    float dt = 0;
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>Main Loop<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
    while(ros::ok())
    {
        //回调一次 更新传感器状态
        ros::spinOnce();

        float cur_time = get_dt(begin_time);
        dt = cur_time  - last_time;

        if(dt < 0.01)
            dt = 0.01;

        if(dt > 0.03)
            dt = 0.03;

        last_time = cur_time;

        float uav_theta = Euler_uav_mocap[1];

	float error_pos = pose_pen_mocap[0] - pose_uav_mocap[0] - 0.012 - 0.1 * sin(uav_theta);
        float pen_theta = asin(error_pos / 0.24);

	float pen_theta_1 = Euler_pen_mocap[1] - uav_theta;

        float theta = pen_theta - uav_theta;

        float theta_dot = cal_error_deriv(theta, dt);

        float twist_theta = twist_pen_mocap[0] - twist_uav_mocap[0];

        cout << "------------------------------------------------" << endl;
	cout << "pen_theta: " << pen_theta * (180/3.14) << endl;
	cout << "uav_theta: " << uav_theta * (180/3.14) << endl;
        cout << "theta:     " << theta * (180/3.14) << endl;
        cout << "theta_dot:     " << theta_dot << endl;
        cout << "twist_theta:   " << twist_theta << endl;

        rate.sleep();
    }

    return 0;

}

//获取当前时间 单位：秒
float get_dt(ros::Time last)
{
    ros::Time time_now = ros::Time::now();
    float currTimeSec = time_now.sec-last.sec;
    float currTimenSec = time_now.nsec / 1e9 - last.nsec / 1e9;
    return (currTimeSec + currTimenSec);
}

float cal_error_deriv(float error_now, float dt)
{
    float error_dot_now;
    error_dot_now = (error_now - error_last) / dt;

    error_last = error_now;

    return error_dot_now;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_NEXTPOW2_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SIMD_COMMON_NEXTPOW2_HPP_INCLUDED

#include <boost/simd/ieee/functions/nextpow2.hpp>
#include <boost/simd/include/functions/simd/bitwise_cast.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/is_equal.hpp>
#include <boost/simd/include/functions/simd/seladd.hpp>
#include <boost/simd/include/functions/simd/frexp.hpp>
#include <boost/simd/include/functions/simd/group.hpp>
#include <boost/simd/include/functions/simd/split.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( nextpow2_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_integer<A0, unsigned>::type utype;
      return bitwise_cast<A0>(nextpow2(bitwise_cast<utype >(abs(a0))));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( nextpow2_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<unsigned_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename dispatch::meta::as_floating<A0>::type rtype;
      typedef typename dispatch::meta::as_integer<A0, signed>::type itype;
      rtype m;
      itype p;
      frexp(tofloat(a0), m, p);
      return bitwise_cast<A0>(seladd(boost::simd::is_equal(m, Half<rtype>()), p, Mone<itype>()));
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( nextpow2_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<uint16_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type  stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype,X>              ivtype;
      ivtype a0l, a0h;
      split(a0, a0l, a0h);
      return bitwise_cast<A0>(group(nextpow2(a0l),nextpow2(a0h)));
      }
  };
} } }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is uint8_t
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( nextpow2_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<uint8_<A0>,X>))
                                    )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typedef typename meta::scalar_of<A0>::type  stype;
      typedef typename dispatch::meta::upgrade<stype>::type itype;
      typedef simd::native<itype, X>             ivtype;
      ivtype a0l, a0h;
      split(a0, a0l, a0h);
      return simd::bitwise_cast<A0>(group(nextpow2(a0l),nextpow2(a0h)));
    }
  };
} } }

/////////////////////////////////////////////////////////////////////////////
// Implementation when type A0 is floating_
/////////////////////////////////////////////////////////////////////////////
namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( nextpow2_, tag::cpu_
                                    , (A0)(X)
                                    , ((simd_<floating_<A0>,X>))
                                    )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type result_type;
    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      A0 m;
      result_type p;
      boost::simd::frexp(abs(a0), m, p);
      return seladd(boost::simd::is_equal(m, Half<A0>()), p, Mone<result_type>());
    }
  };

} } }

#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/def.hpp>
#include <boost/python/args.hpp>
#include <mmtbx/max_lik/max_lik.h>

#include <boost/python/list.hpp>
#include <boost/python/detail/api_placeholder.hpp>

namespace mmtbx { namespace max_lik {
namespace {

  void init_module()
  {
    using namespace boost::python;
    class_<wat_dist>("wat_dist")
      .def("do_wat_dist", &wat_dist::do_wat_dist,
        (arg("shell"),
         arg("xyzf"),  arg("atmrad"), arg("element_symbol"),
         arg("uc"),  arg("sg"), arg("nxnynz"), arg("sel_flag"), arg("rad"), arg("nshells")))
      .def("data", &wat_dist::data)
      .def("max_number_of_shells", &wat_dist::max_number_of_shells)
      .def("as_xplor_map", &wat_dist::as_xplor_map,
        (arg("outputfile")))
    ;
    def("fo_fc_alpha_over_eps_beta",
         (af::shared<double>(*)
               (af::shared<double> const&,
                af::shared<double> const&,
                af::shared<double> const&,
                af::shared<double> const&,
                cctbx::sgtbx::space_group const&,
                af::const_ref<cctbx::miller::index<> >)) fo_fc_alpha_over_eps_beta,
                                                          (arg("f_obs"),
                                                           arg("f_model"),
                                                           arg("alpha"),
                                                           arg("beta"),
                                                           arg("space_group"),
                                                           arg("miller_indices")))
    ;
    class_<alpha_beta_est>("alpha_beta_est",
                 init<boost::python::list const&,
                      boost::python::list const&,
                      boost::python::list const&,
                      boost::python::list const&,
                      cctbx::sgtbx::space_group const& >((arg("fo_test"),
                                                          arg("fm_test"),
                                                          arg("indices"),
                                                          arg("epsilons"),
                                                          arg("space_group"))))
      .def("alpha", &alpha_beta_est::alpha)
      .def("beta",  &alpha_beta_est::beta)
    ;
    class_<fom_and_phase_error>("fom_and_phase_error",
                 init<af::shared<double> const&,
                      af::shared<double> const&,
                      af::shared<double> const&,
                      af::shared<double> const&,
                      af::shared<double> const&,
                      af::shared<bool> const&
                       >((
                                                          arg("f_obs"),
                                                          arg("f_model"),
                                                          arg("alpha"),
                                                          arg("beta"),
                                                          arg("epsilons"),
                                                          arg("centric_flags")
                                                          )))
      .def("fom",         &fom_and_phase_error::fom)
      .def("phase_error", &fom_and_phase_error::phase_error)
    ;
    class_<f_star_w_star_mu_nu>("f_star_w_star_mu_nu",
                 init<af::const_ref<double> const&,
                      af::const_ref<double> const&,
                      af::const_ref<double> const&,
                      af::const_ref<double> const&,
                      cctbx::sgtbx::space_group const&,
                      af::const_ref<cctbx::miller::index<> > const& >((
                                                          arg("f_obs"),
                                                          arg("f_model"),
                                                          arg("alpha"),
                                                          arg("beta"),
                                                          arg("space_group"),
                                                          arg("miller_indices"))))
      .def("f_star", &f_star_w_star_mu_nu::f_star)
      .def("w_star", &f_star_w_star_mu_nu::w_star)
      .def("mu",     &f_star_w_star_mu_nu::mu)
      .def("nu",     &f_star_w_star_mu_nu::nu)
      .def("k1",     &f_star_w_star_mu_nu::f_star)
      .def("k2",     &f_star_w_star_mu_nu::w_star)
      .def("ls_star_1", &f_star_w_star_mu_nu::mu)
      .def("ls_star_2", &f_star_w_star_mu_nu::nu)
      .def("number_of_f_star_zero", &f_star_w_star_mu_nu::number_of_f_star_zero)
    ;
    class_<sasha_error_calculator>("sasha_error_calculator",
                 init<af::const_ref<vec3<double> > const&,
                      af::const_ref<vec3<double> > const&,
                      af::const_ref<vec3<double> > const&,
                      af::const_ref<vec3<double> > const&,
                      af::const_ref<std::string> const&,
                      af::const_ref<std::string> const&,
                      cctbx::uctbx::unit_cell const&,
                      cctbx::sgtbx::space_group const&,
                      double >((arg("r1f"),
                                    arg("r1c"),
                                    arg("r2f"),
                                    arg("r2c"),
                                    arg("lab1"),
                                    arg("lab2"),
                                    arg("uc"),
                                    arg("sg"),
                                    arg("rad"))))
      .def("doptimal", &sasha_error_calculator::doptm)
      .def("dregular", &sasha_error_calculator::distm)
    ;
    class_<peak_clustering>("peak_clustering",
                 init<af::const_ref<vec3<double> > const&,
                      af::const_ref<vec3<double> > const&,
                      af::const_ref<double> const&,
                      af::const_ref<double> const&,
                      cctbx::uctbx::unit_cell const&,
                      double const& >((arg("r1f"),
                                       arg("r2f"),
                                       arg("h1"),
                                       arg("h2"),
                                       arg("uc"),
                                       arg("cutoff"))))
      .def("sites", &peak_clustering::sites)
      .def("heights", &peak_clustering::heights)
    ;
  }

} // namespace <anonymous>
}} // namespace mmtbx::max_lik

BOOST_PYTHON_MODULE(mmtbx_max_lik_ext)
{
  mmtbx::max_lik::init_module();
}


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2014-2017.
// Modifications copyright (c) 2014-2017, Oracle and/or its affiliates.

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

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP
#define BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP

#include <boost/geometry/core/cs.hpp>
#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/radian_access.hpp>

#include <boost/geometry/formulas/spherical.hpp>

#include <boost/geometry/util/math.hpp>
#include <boost/geometry/util/promote_floating_point.hpp>
#include <boost/geometry/util/select_calculation_type.hpp>

#include <boost/geometry/strategies/side.hpp>
//#include <boost/geometry/strategies/concepts/side_concept.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace side
{

/*!
\brief Check at which side of a Great Circle segment a point lies
         left of segment (> 0), right of segment (< 0), on segment (0)
\ingroup strategies
\tparam CalculationType \tparam_calculation
 */
template <typename CalculationType = void>
class side_by_cross_track
{

public :
    template <typename P1, typename P2, typename P>
    static inline int apply(P1 const& p1, P2 const& p2, P const& p)
    {
        typedef typename promote_floating_point
            <
                typename select_calculation_type_alt
                    <
                        CalculationType,
                        P1, P2, P
                    >::type
            >::type calc_t;

        calc_t d1 = 0.001; // m_strategy.apply(sp1, p);

        calc_t lon1 = geometry::get_as_radian<0>(p1);
        calc_t lat1 = geometry::get_as_radian<1>(p1);
        calc_t lon2 = geometry::get_as_radian<0>(p2);
        calc_t lat2 = geometry::get_as_radian<1>(p2);
        calc_t lon = geometry::get_as_radian<0>(p);
        calc_t lat = geometry::get_as_radian<1>(p);

        calc_t crs_AD = geometry::formula::spherical_azimuth<calc_t, false>
                             (lon1, lat1, lon, lat).azimuth;

        calc_t crs_AB = geometry::formula::spherical_azimuth<calc_t, false>
                             (lon1, lat1, lon2, lat2).azimuth;

        calc_t XTD = asin(sin(d1) * sin(crs_AD - crs_AB));

        return math::equals(XTD, 0) ? 0 : XTD < 0 ? 1 : -1;
    }
};

}} // namespace strategy::side

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_SPHERICAL_SIDE_BY_CROSS_TRACK_HPP


// __BEGIN_LICENSE__
//  Copyright (c) 2006-2013, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NASA Vision Workbench is licensed under the Apache License,
//  Version 2.0 (the "License"); you may not use this file except in
//  compliance with the License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

/// \file ipalign.cc
///
/// Example program demonstrating how to align two images using the
/// Interest Point module.

#include <vw/Core/Log.h>
#include <vw/Core/FundamentalTypes.h>
#include <vw/Math/Geometry.h>
#include <vw/Math/RANSAC.h>
#include <vw/Math/Vector.h>
#include <vw/Math/Matrix.h>
#include <vw/Image/ImageView.h>
#include <vw/Image/ImageMath.h>
#include <vw/Image/Transform.h>
#include <vw/Image/Manipulation.h>
#include <vw/Image/PixelTypes.h>
#include <vw/Image/ImageViewRef.h>
#include <vw/FileIO/DiskImageView.h>
#include <vw/InterestPoint/Descriptor.h>
#include <vw/InterestPoint/Detector.h>
#include <vw/InterestPoint/Matcher.h>
#include <vw/InterestPoint/IntegralDetector.h>
#include <vw/InterestPoint/IntegralInterestOperator.h>

using namespace vw;
using namespace vw::ip;

#include <boost/program_options.hpp>
namespace po = boost::program_options;
#include <boost/filesystem/path.hpp>
namespace fs = boost::filesystem;
#include <boost/foreach.hpp>

#define MAX_POINTS_TO_DRAW 1000

struct Options {
  // Input
  std::vector<std::string> input_filenames;

  // Settings
  std::string output_prefix, interest_operator, descriptor_generator;
  float matcher_threshold, detect_gain, tile_size;
  float inlier_threshold;
  int ransac_iterations;
  bool single_scale, homography, debug_images;
  bool save_intermediate;
};

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

// Draw the interest points and write as an image.
template <class ViewT>
void write_point_image(std::string out_file_name,
                       ImageViewBase<ViewT> const& src,
                       InterestPointList const& points) {

  ImageView<PixelRGB<uint8> > viz = pixel_cast<PixelRGB<uint8> >(channel_cast_rescale<uint8>(src));

  // Draw points into color planes
  int n = 0;
  for (InterestPointList::const_iterator pt = points.begin();
       pt != points.end() && n < MAX_POINTS_TO_DRAW; ++pt, ++n) {
    // Draw a red line from the point outward along the orientation
    for (int r=0; r<(int)(8*(*pt).scale); ++r){
      int i = (int)(0.5 + (*pt).x + r*cos((*pt).orientation));
      int j = (int)(0.5 + (*pt).y + r*sin((*pt).orientation));
      // red, green, blue
      viz(i,j) = PixelRGB<uint8>(255, 0, 0);
    }
    // Draw a green 3x3 filled square at the point to indicate center
    int i0 = (int)(0.5 + (*pt).x);
    int j0 = (int)(0.5 + (*pt).y);
    for (int j=j0-1; j<=j0+1; ++j){
      for (int i=i0-1; i<=i0+1; ++i){
        // red, green, blue
        viz(i,j) = PixelRGB<uint8>(0, 255, 0);
      }
    }
  }

  write_image(out_file_name, viz);
}

// Produces Interest Points from files
void find_interest_points( std::string const& image_name,
                           InterestPointList& ip,
                           Options& opt ) {
  const float IDEAL_LOG_THRESHOLD = .03;
  const float IDEAL_OBALOG_THRESHOLD = .07;
  const float IDEAL_HARRIS_THRESHOLD = 1.2e-5;

  DiskImageView<PixelRGB<uint8> > input_image( image_name );
  ip.clear();
  // Image Alignment
  //
  // Images are aligned by computing interest points, matching
  // them using a standard 2-Norm nearest-neighor metric, and then
  // rejecting outliers by fitting a similarity between the
  // putative matches using RANSAC.
  vw_out(InfoMessage) << "\nInterest Point Detection: "
                      << image_name << "\n";
  if (opt.interest_operator == "harris") {
    HarrisInterestOperator
      interest_operator(IDEAL_HARRIS_THRESHOLD/opt.detect_gain);
    if (!opt.single_scale) {
      ScaledInterestPointDetector<HarrisInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    } else {
      InterestPointDetector<HarrisInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    }
  } else if (opt.interest_operator == "log" ) {
    // Use a scale-space Laplacian of Gaussian feature detector. The
    // associated threshold is abs(interest) > interest_threshold.
    LogInterestOperator interest_operator(IDEAL_LOG_THRESHOLD/opt.detect_gain);
    if (!opt.single_scale) {
      ScaledInterestPointDetector<LogInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    } else {
      InterestPointDetector<LogInterestOperator> detector(interest_operator);
      ip = detect_interest_points(input_image, detector);
    }
  } else if (opt.interest_operator == "obalog" ) {
    OBALoGInterestOperator
      interest_operator(IDEAL_OBALOG_THRESHOLD/opt.detect_gain);
    IntegralInterestPointDetector<OBALoGInterestOperator>
      detector( interest_operator );
    ip = detect_interest_points(input_image, detector);
  } else {
    std::cout << "Unknown interest operator: " << opt.interest_operator << ".  Options are : [ Harris, LoG, OBALoG ]\n";
    exit(0);
  }
  vw_out() << "\t Found " << ip.size() << " interest points.\n";

  // Write out images with interest points marked
  if (opt.debug_images)
    write_point_image(opt.output_prefix + "-debug.tif", input_image, ip);

  // Generate descriptors for interest points.
  vw_out(InfoMessage) << "Generating descriptors using " << opt.descriptor_generator << " generator... " << std::flush;
  if (opt.descriptor_generator == "patch") {
    PatchDescriptorGenerator descriptor;
    describe_interest_points( input_image, descriptor, ip );
  } else if ( opt.descriptor_generator == "pca" ) {
    PCASIFTDescriptorGenerator descriptor("pca_basis.exr", "pca_avg.exr");
    describe_interest_points( input_image, descriptor, ip );
  } else if ( opt.descriptor_generator == "sgrad" ) {
    SGradDescriptorGenerator descriptor;
    describe_interest_points( input_image, descriptor, ip );
  } else {
    std::cout << "Unknown interest descriptor: " << opt.descriptor_generator << ".  Options are : [ patch, pca, sgrad ]\n";
    exit(0);
  }
  vw_out(InfoMessage) << "done." << std::endl;
  if ( opt.save_intermediate )
    write_binary_ip_file(fs::path(image_name).replace_extension("vwip").string(),ip);
}

void align_images( Options & opt ) {
  // Detect IP from the first image as it is the reference
  InterestPointList ref_ip;
  find_interest_points( opt.input_filenames[0], ref_ip, opt );
  std::vector<InterestPoint> ref_ip_v( ref_ip.size() );
  std::copy( ref_ip.begin(), ref_ip.end(), ref_ip_v.begin() );

  // Determine the size of the reference image
  Vector2i ref_size;
  {
    DiskImageView<PixelRGB<uint8> > ref_image( opt.input_filenames[0] );
    ref_size[0] = ref_image.cols();
    ref_size[1] = ref_image.rows();
  }
  std::string ref_name = opt.input_filenames.front();
  opt.input_filenames.erase( opt.input_filenames.begin() );

  // Detect IP and align all the other images
  BOOST_FOREACH( std::string const& input_name,
                 opt.input_filenames ) {
    InterestPointList input_ip;
    find_interest_points( input_name, input_ip, opt );
    std::vector<InterestPoint> input_ip_v( input_ip.size() );
    std::copy( input_ip.begin(), input_ip.end(), input_ip_v.begin() );

    vw_out(InfoMessage) << "\nInterest Point Matching: "
                        << input_name << "\n";

    DefaultMatcher matcher(opt.matcher_threshold);
    std::vector<InterestPoint> matched_ip1, matched_ip2;
    matcher(ref_ip_v, input_ip_v, matched_ip1, matched_ip2,
            TerminalProgressCallback( "tools.ipalign", "Matching:"));
    vw_out(InfoMessage) << "\tFound " << matched_ip1.size() << " putative matches.\n";

    // RANSAC is used to fit a similarity transform between the
    // matched sets of points
    std::vector<Vector3>
      ransac_ip1 = iplist_to_vectorlist(matched_ip1),
      ransac_ip2 = iplist_to_vectorlist(matched_ip2);
    Matrix<double> align_matrix;

    std::vector<size_t> indices;
    if ( opt.homography ) {
      math::RandomSampleConsensus<math::HomographyFittingFunctor, math::InterestPointErrorMetric> ransac(math::HomographyFittingFunctor(), math::InterestPointErrorMetric(), opt.ransac_iterations, opt.inlier_threshold, ransac_ip1.size()/2, true);
      align_matrix = ransac(ransac_ip2,ransac_ip1);
      indices = ransac.inlier_indices(align_matrix,ransac_ip2,ransac_ip1);
    } else {
      math::RandomSampleConsensus<math::AffineFittingFunctor, math::InterestPointErrorMetric> ransac(math::AffineFittingFunctor(), math::InterestPointErrorMetric(), opt.ransac_iterations, opt.inlier_threshold, ransac_ip1.size()/2, true);
      align_matrix = ransac(ransac_ip2,ransac_ip1);
      indices = ransac.inlier_indices(align_matrix,ransac_ip2,ransac_ip1);
    }

    if ( opt.save_intermediate ) {
      std::vector<InterestPoint> final_ip1, final_ip2;
      BOOST_FOREACH( size_t& index, indices ) {
        final_ip1.push_back(matched_ip1[index]);
        final_ip2.push_back(matched_ip2[index]);
      }
      std::string output_filename =
        fs::path(ref_name).replace_extension().string() + "__" +
        fs::path(input_name).stem().string() + ".match";
      write_binary_match_file(output_filename, final_ip1, final_ip2);
    }

    DiskImageView<PixelRGB<uint8> > input_image( input_name );
    ImageViewRef<PixelRGB<uint8> > aligned_image =
      transform(input_image, HomographyTransform(align_matrix),
                ref_size[0], ref_size[1]);
    std::ostringstream ostr;
    ostr << opt.output_prefix << "_" << input_name;
    write_image(ostr.str(), aligned_image,
                TerminalProgressCallback( "tools.ipalign", "Writing:") );
  }
}

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

void handle_arguments( int argc, char* argv[], Options& opt ) {
  po::options_description general_options("Options");
  general_options.add_options()
    ("help,h", "Display this help message")
    ("debug-images,d", po::bool_switch(&opt.debug_images),
     "Produce additional debugging images as well as the aligned image.")
    ("tile-size", po::value(&opt.tile_size)->default_value(1024),
     "Specify the tile size for detecting interest points.")
    ("output-prefix,o", po::value(&opt.output_prefix)->default_value("aligned"),
     "Specify the output prefix")
    ("save-intermediate,s", po::bool_switch(&opt.save_intermediate),
     "Save working VWIP and Match files")

    // Interest point detector options
    ("detector-gain,g", po::value(&opt.detect_gain)->default_value(1.0),
     "Increasing this number will increase the  gain at which interest points are detected.")
    ("interest-operator", po::value(&opt.interest_operator)->default_value("OBALoG"),
     "Choose an interest metric from [LoG, Harris, OBALoG]")
    ("single-scale", po::bool_switch(&opt.single_scale),
     "Do not use the scale-space interest point detector.")

    // Descriptor generator options
    ("descriptor-generator", po::value(&opt.descriptor_generator)->default_value("sgrad"),
     "Choose a descriptor generator from [patch,pca,sgrad]")

    // Alignment options
    ("matcher-threshold,t", po::value(&opt.matcher_threshold)->default_value(0.5),
     "Rejects points during matching if best > matcher_threshold * second_best")
    ("inlier-threshold,i", po::value(&opt.inlier_threshold)->default_value(10), "RANSAC inlier threshold.")
    ("ransac-iterations", po::value(&opt.ransac_iterations)->default_value(100), "Number of RANSAC iterations.")
    ("homography", po::bool_switch(&opt.homography),
     "Align images using a full projective transform (homography).  By default, aligment uses a more restricted Similarity transform.");

  po::options_description positional("");
  positional.add_options()
    ("input-file",  po::value(&opt.input_filenames),  "Input images");

  po::positional_options_description positional_desc;
  positional_desc.add("input-file", -1);

  po::options_description all_options;
  all_options.add(general_options).add(positional);

  po::variables_map vm;
  try {
    po::store( po::command_line_parser( argc, argv ).options(all_options).positional(positional_desc).run(), vm );
    po::notify( vm );
  } catch (const po::error& e) {
    vw_throw( ArgumentErr() << "Error parsing input:\n\t"
              << e.what() << general_options );
  }

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " <ref image> <images to be aligned ...>\n";

  if ( vm.count("help") )
    vw_throw( ArgumentErr() << usage.str() << general_options );
  if ( opt.input_filenames.empty() )
    vw_throw( ArgumentErr() << "Missing input images!\n"
              << usage.str() << general_options );

  boost::to_lower( opt.interest_operator );
  boost::to_lower( opt.descriptor_generator );
}

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

  Options opt;
  try {
    handle_arguments( argc, argv, opt );
    align_images( opt );
  } catch ( const ArgumentErr& e ) {
    vw_out() << e.what() << "\n";
    return 1;
  } catch ( const Exception& e ) {
    std::cerr << "\n\nVW Error: " << e.what() << "\n";
    return 1;
  } catch ( const std::bad_alloc& e ) {
    std::cerr << "\n\nError: Ran out of Memory!\n";
    return 1;
  } catch ( const std::exception& e ) {
    std::cerr << "\n\nError: " << e.what() << "\n";
    return 1;
  }

  return 0;
}


// Copyright (C) 2003  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <sstream>
#include <string>
#include <ctime>
#include <cstdlib>

#include <dlib/conditioning_class.h>

#include "tester.h"
#include "conditioning_class.h"

namespace
{

    class conditioning_class_tester : public tester
    {
    public:
        conditioning_class_tester (
        ) :
            tester ("test_conditioning_class",
                    "Runs tests on the conditioning_class component.")
        {}

        void perform_test (
        )
        {
            dlog << LINFO << "testing kernel_1a";
            conditioning_class_kernel_test<
                conditioning_class<256>::kernel_1a,
                conditioning_class<2>::kernel_1a
                >();
            print_spinner();

            dlog << LINFO << "testing kernel_2a";
            conditioning_class_kernel_test<
                conditioning_class<256>::kernel_2a,
                conditioning_class<2>::kernel_2a
                >();
            print_spinner();

            dlog << LINFO << "testing kernel_3a";
            conditioning_class_kernel_test<
                conditioning_class<256>::kernel_3a,
                conditioning_class<2>::kernel_3a
                >();
            print_spinner();

            dlog << LINFO << "testing kernel_4a";
            conditioning_class_kernel_test<
                conditioning_class<256>::kernel_4a,
                conditioning_class<2>::kernel_4a
                >();
            print_spinner();

            dlog << LINFO << "testing kernel_4b";
            conditioning_class_kernel_test<
                conditioning_class<256>::kernel_4b,
                conditioning_class<2>::kernel_4b
                >();
            print_spinner();

            dlog << LINFO << "testing kernel_4c";
            conditioning_class_kernel_test<
                conditioning_class<256>::kernel_4c,
                conditioning_class<2>::kernel_4c
                >();
            print_spinner();

            dlog << LINFO << "testing kernel_4d";
            conditioning_class_kernel_test<
                conditioning_class<256>::kernel_4d,
                conditioning_class<2>::kernel_4d
                >();
            print_spinner();

        }
    } a;

}


//
//  multiThreading.hpp
//  ALL_BUILD
//
//  Created by Sarah Curtit on 25/03/2018.
//

#pragma once

#include <Eigen/Dense>
#include <mutex>

#include "Correction/correcteur.hpp"
#include "Network/FT_Client.hpp"
#include "Network/Robot_Client.hpp"

using namespace std;

template <typename T>
struct shared_data{
	mutex m;
	T data;
};

class MultiThreading{
private:

	// Networking

	Robot_Client * robot_client;
	FT_Client* client_capteur;

	// Shared variables
	shared_data<Eigen::VectorXd> rotation;
	shared_data<Eigen::VectorXd> mesures; // in counts (micronewtons)
	shared_data<Eigen::VectorXd> displacement;
	shared_data<Eigen::VectorXd> objective;
    shared_data<Eigen::VectorXd> integral;

	struct MoindreCarres {
		double mCx[10000][2];
        double mCy[10000][2];
        double mCz[10000][2];
	};

    shared_data<MoindreCarres> moindreCarres;

	shared_data<double> kalman_out; // Correction in meters

	// Other mutexes
	mutex m_prompt; // For cout or other prompt actions

public:

    // Constructor
    MultiThreading(Eigen::VectorXd rot, Eigen::VectorXd y, Eigen::VectorXd x);

    //Create blank estimator
    MultiThreading();

    // Accessors
    Eigen::VectorXd getRotation();
    Eigen::VectorXd getMesures();
    Eigen::VectorXd getDisplacement();
    Eigen::VectorXd getObjective();
    Eigen::VectorXd getIntegral();

    // Modifiers
    void setRotation(Eigen::VectorXd rot);
    void setMesures(Eigen::VectorXd y);
    void setDisplacement(Eigen::VectorXd x);
    void setObjective(Eigen::VectorXd Fobj);
    void setIntegral(Eigen::VectorXd integ);

    //loop functions
    void runKalman(KalmanFilter Kf);
    void acquireData();
    void sendData();

	// Statistical analysis
	double winsorize(double *data);
};


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_CORE_FUNCTIONS_SCALAR_IND2SUB_HPP_INCLUDED
#define NT2_CORE_FUNCTIONS_SCALAR_IND2SUB_HPP_INCLUDED

#include <nt2/core/functions/ind2sub.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/functions/simd/plus.hpp>
#include <nt2/include/functions/simd/splat.hpp>
#include <nt2/include/functions/simd/modulo.hpp>
#include <nt2/include/functions/simd/divides.hpp>
#include <nt2/include/functions/simd/bitwise_cast.hpp>
#include <boost/dispatch/meta/as_signed.hpp>
#include <boost/fusion/adapted/array.hpp>
#include <boost/fusion/include/size.hpp>
#include <boost/fusion/include/at.hpp>
#include <boost/mpl/int.hpp>

namespace nt2 { namespace ext
{
  //============================================================================
  // Case for no base index
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ind2sub_, tag::cpu_
                            , (A0)(A1)
                            , (fusion_sequence_<A0>)
                              (generic_< unspecified_<A1> >)
                            )
  {
    typedef typename meta::strip<A1>::type                          base_t;
    typedef typename boost::dispatch::meta::as_signed<base_t>::type type_t;
    typedef boost::fusion::result_of::size<A0>                      dims_t;
    typedef boost::array<type_t,dims_t::value>                      result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0& size, const A1& pos) const
    {
      type_t p;
      result_type sub;

      p = bitwise_cast<type_t>(pos);
      eval(sub,p,size,boost::mpl::int_<0>(),boost::mpl::int_<dims_t::value>());
      return sub;
    }

    template<class N, class M>
    BOOST_DISPATCH_FORCE_INLINE result_type
    eval(result_type& sub,const type_t& p, const A0& s, const N&, const M& m) const
    {
      type_t ls     = splat<type_t>(boost::fusion::at_c<N::value>(s)?boost::fusion::at_c<N::value>(s):1);
      sub[N::value] = p % ls + One<type_t>();
      eval( sub, p/ls, s, boost::mpl::int_<N::value+1>(), m );

      return sub;
    }

    BOOST_DISPATCH_FORCE_INLINE void
    eval( result_type& sub, const type_t& p, const A0&
        , const boost::mpl::int_<0>&, const boost::mpl::int_<1>&
        ) const
    {
      sub[0] = p + One<type_t>();
    }

    template<class N>
    BOOST_DISPATCH_FORCE_INLINE void
    eval(result_type&, const type_t&, const A0&, const N&, const N&) const {}
  };

  //============================================================================
  // Case for specific base index
  //============================================================================
  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::ind2sub_, tag::cpu_
                            , (A0)(A1)(A2)
                            , (fusion_sequence_<A0>)
                              (generic_< unspecified_<A1> >)
                              (fusion_sequence_<A2>)
                            )
  {
    typedef typename meta::strip<A1>::type                          base_t;
    typedef typename boost::dispatch::meta::as_signed<base_t>::type type_t;
    typedef boost::fusion::result_of::size<A0>                      dims_t;
    typedef boost::array<type_t,dims_t::value>                      result_type;

    BOOST_DISPATCH_FORCE_INLINE result_type
    operator()(const A0& size, const A1& pos, const A2& base) const
    {
      type_t p;
      result_type sub;

      p = bitwise_cast<type_t>(pos);
      eval(sub,p,size,base,boost::mpl::int_<0>(),boost::mpl::int_<dims_t::value>());

      return sub;
    }

    template<class N, class M>
    BOOST_DISPATCH_FORCE_INLINE result_type
    eval( result_type& sub,const type_t& p, const A0& s, const A2& b
        , const N&, const M& m
        ) const
    {
      type_t ls     = splat<type_t>(boost::fusion::at_c<N::value>(s)?boost::fusion::at_c<N::value>(s):1);
      //      type_t ls     = splat<type_t>(boost::fusion::at_c<N::value>(s));
      sub[N::value] = p % ls + splat<type_t>(boost::fusion::at_c<N::value>(b));
      eval( sub, p/ls, s, b, boost::mpl::int_<N::value+1>(), m );

      return sub;
    }

    BOOST_DISPATCH_FORCE_INLINE void
    eval( result_type& sub, const type_t& p, const A0&, const A2& b
        , const boost::mpl::int_<0>&, const boost::mpl::int_<1>&
        ) const
    {
      sub[0] = p + splat<type_t>(boost::fusion::at_c<0>(b));
    }

    template<class N>
    BOOST_DISPATCH_FORCE_INLINE void
    eval(result_type&,const type_t&,const A0&,const A2&,const N&,const N&) const  {}
  };
} }

#endif