//
// Created by chen-tian on 7/26/17.
//

#include "EdgeSE3ProjectDirect.h"
#include <Eigen/Core>
#include <g2o/core/base_unary_edge.h>
#include <g2o/types/sba/types_sba.h>
#include <g2o/types/sba/types_six_dof_expmap.h>

using namespace cv;
using namespace g2o;

EdgeSE3ProjectDirect::EdgeSE3ProjectDirect(Eigen::Vector3d point, float fx, float fy, float cx, float cy, Mat *image)
        :x_world_( point ), fx_( fx ), fy_( fy ), cx_( cx ), cy_(cy), image_(image) {};

void EdgeSE3ProjectDirect::computeError()
{
    const VertexSE3Expmap* v = static_cast<const VertexSE3Expmap*> (_vertices[0]);
    Eigen::Vector3d x_local = v->estimate().map( x_world_ );
    float x = x_local[0]*fx_/x_local[2] + cx_;
    float y = x_local[1]*fy_/x_local[2] + cy_;
    //check x,y is in the image
    if ( x-4<0 || (x+4)>image_->cols || (y-4)<0 || (y+4)>image_->rows )
    {
        _error( 0, 0 ) = 0.0;
        this->setLevel(1);
    }

    else
    {
        _error( 0,0 ) = getPixelValue(x, y) - _measurement;
    }

    //plus in manifold
}

void EdgeSE3ProjectDirect::linearizeOplus()
{
    if ( level() == 1 )
    {
        _jacobianOplusXi = Eigen::Matrix< double , 1, 6>::Zero();
        return;
    }

    VertexSE3Expmap* vtx = static_cast<VertexSE3Expmap*>(_vertices[0] );
    Eigen::Vector3d xyz_trans = vtx->estimate().map( x_world_ ); //q in book

    double x = xyz_trans[0];
    double y = xyz_trans[1];
    double invz = 1.0/xyz_trans[2];
    double invz_2 = invz*invz;

    float u = x*fx_*invz + cx_;
    float v = y*fy_*invz + cy_;

    //jacobian from se3 to u,v
    //NOTE that in g2o the Lie algebra is(\omega, \epsilon), where \omega is so(3) and \epsilon the transilation

    Eigen::Matrix<double, 2, 6> jacobian_uv_ksai;

    jacobian_uv_ksai ( 0,0 ) = - x*y*invz_2 *fx_;
    jacobian_uv_ksai ( 0,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
    jacobian_uv_ksai ( 0,2 ) = - y*invz *fx_;
    jacobian_uv_ksai ( 0,3 ) = x*invz *fx_;
    jacobian_uv_ksai ( 0,4 ) = 0;
    jacobian_uv_ksai ( 0,5 ) = - x*invz_2 *fx_;

    jacobian_uv_ksai ( 1,0 ) = - ( 1+y*y*invz_2 ) *fy_;
    jacobian_uv_ksai ( 1,1 ) = ( 1+ ( x*x*invz_2 ) ) *fx_;
    jacobian_uv_ksai ( 1,2 ) = x*invz *fx_;
    jacobian_uv_ksai ( 1,3 ) = 0;
    jacobian_uv_ksai ( 1,4 ) = - invz *fy_;
    jacobian_uv_ksai ( 1,5 ) = -y*invz_2*fy_;

    Eigen::Matrix<double, 1, 2> jacobian_pixel_uv;

    jacobian_pixel_uv ( 0,0 ) = ( getPixelValue( u+1,v )-getPixelValue(u-1,v))/2;
    jacobian_pixel_uv ( 0,1 ) = ( getPixelValue( u,v+1 )-getPixelValue(u,v-1))/2;

    _jacobianOplusXi = jacobian_pixel_uv*jacobian_uv_ksai;
}

//Bilinear interpolation
inline float EdgeSE3ProjectDirect::getPixelValue(float x, float y)
{
    uchar* data = & image_->data[ int ( y ) * image_->step + int ( x ) ];
    float xx = x -floor( x );
    float yy = y -floor( y );
    return float (
            (1-xx) * (1-yy) * data[0] +
            xx* (1-yy) * data[1] +
            (1-xx) * yy * data[ image_->step ] +
            xx*yy*data[image_->step+1]
    );
}


/* ---------------------------------------------------------------------
 *
 * Copyright (C) 2006 - 2020 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: Wolfgang Bangerth, Texas A&M University, 2006
 */

// @sect3{Include files}

// 我们从通常的各种各样的包含文件开始，我们在以前的许多测试中都看到过。

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/function.h>

#include <deal.II/lac/vector.h>
#include <deal.II/lac/sparse_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/solver_cg.h>
#include <deal.II/lac/precondition.h>
#include <deal.II/lac/affine_constraints.h>

#include <deal.II/grid/tria.h>
#include <deal.II/grid/grid_generator.h>

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

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

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

#include <fstream>
#include <iostream>

// 这里是仅有的三个有一些新兴趣的包含文件。第一个文件已经被使用了，例如，用于 VectorTools::interpolate_boundary_values 和 MatrixTools::apply_boundary_values 函数。然而，我们在这里使用该类中的另一个函数， VectorTools::project 来计算我们的初始值，作为连续初始值的 $L^2$ 投影。此外，我们使用  VectorTools::create_right_hand_side  来生成积分  $(f^n,\phi^n_i)$  。这些以前总是由 assemble_system 或应用程序代码中的类似函数手工生成。然而，我们太懒了，不能在这里这么做，所以干脆使用库函数。

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

// 与此非常相似，我们也懒得写代码来组装质量矩阵和拉普拉斯矩阵，尽管这只需要从以前的任何一个教程程序中复制相关代码。相反，我们想把重点放在这个程序中真正新的东西上，因此使用了 MatrixCreator::create_mass_matrix 和 MatrixCreator::create_laplace_matrix 函数。它们被声明在这里。

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

// 最后，这里有一个include文件，它包含了人们有时需要的各种工具函数。特别是，我们需要 Utilities::int_to_string 类，该类在给定一个整数参数后，返回它的字符串表示。它特别有用，因为它允许第二个参数，表明我们希望结果用前导零填充的数字数。我们将用它来写输出文件，其形式为 solution-XXX.vtu where XXX 表示时间步数，并且总是由三位数组成，即使我们仍然处于个位或两位数的时间步数中。

#include <deal.II/base/utilities.h>

// 最后一步和以前所有的程序一样。

namespace Step23
{
  using namespace dealii;
// @sect3{The WaveEquation class}

// 接下来是主类的声明。它的公共函数接口与其他大多数教程程序一样。值得一提的是，我们现在必须存储四个矩阵，而不是一个：质量矩阵  $M$  ，拉普拉斯矩阵  $A$  ，用于求解  $U^n$  的矩阵  $M+k^2\theta^2A$  ，以及用于求解  $V^n$  的带有边界条件的质量矩阵副本。请注意，在周围有一个额外的质量矩阵副本是有点浪费的。我们将在可能的改进部分讨论如何避免这种情况的策略。

// 同样，我们需要 $U^n,V^n$ 的解向量，以及前一个时间步骤 $U^{n-1},V^{n-1}$ 的相应向量。 system_rhs 将用于我们在每个时间步骤中求解两个线性系统之一时的任何右手向量。这些将在两个函数  solve_u  和  solve_v  中解决。

// 最后，变量 theta 用来表示参数 $\theta$ ，该参数用于定义使用哪种时间步进方案，这在介绍中已经说明。剩下的就不言而喻了。

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

  private:
    void setup_system();
    void solve_u();
    void solve_v();
    void output_results() const;

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

    AffineConstraints<double> constraints;

    SparsityPattern      sparsity_pattern;
    SparseMatrix<double> mass_matrix;
    SparseMatrix<double> laplace_matrix;
    SparseMatrix<double> matrix_u;
    SparseMatrix<double> matrix_v;

    Vector<double> solution_u, solution_v;
    Vector<double> old_solution_u, old_solution_v;
    Vector<double> system_rhs;

    double       time_step;
    double       time;
    unsigned int timestep_number;
    const double theta;
  };

//  @sect3{Equation data}

// 在我们继续填写主类的细节之前，让我们定义与问题相对应的方程数据，即解 $u$ 及其时间导数 $v$ 的初始值和边界值，以及一个右手类。我们使用从Function类模板派生出来的类来做这件事，这个模板以前已经用过很多次了，所以下面的内容不应该是一个惊喜。

// 我们从初始值开始，对数值 $u$ 以及它的时间导数，即速度 $v$ 都选择零。

  template <int dim>
  class InitialValuesU : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & /*p*/,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));
      return 0;
    }
  };

  template <int dim>
  class InitialValuesV : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & /*p*/,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));
      return 0;
    }
  };

// 其次，我们有右手边的强制项。无聊的是，我们在这里也选择零。

  template <int dim>
  class RightHandSide : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & /*p*/,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));
      return 0;
    }
  };

// 最后，我们有  $u$  和  $v$  的边界值。它们与介绍中描述的一样，一个是另一个的时间导数。

  template <int dim>
  class BoundaryValuesU : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));

      if ((this->get_time() <= 0.5) && (p[0] < 0) && (p[1] < 1. / 3) &&
          (p[1] > -1. / 3))
        return std::sin(this->get_time() * 4 * numbers::PI);
      else
        return 0;
    }
  };

  template <int dim>
  class BoundaryValuesV : public Function<dim>
  {
  public:
    virtual double value(const Point<dim> & p,
                         const unsigned int component = 0) const override
    {
      (void)component;
      Assert(component == 0, ExcIndexRange(component, 0, 1));

      if ((this->get_time() <= 0.5) && (p[0] < 0) && (p[1] < 1. / 3) &&
          (p[1] > -1. / 3))
        return (std::cos(this->get_time() * 4 * numbers::PI) * 4 * numbers::PI);
      else
        return 0;
    }
  };

//  @sect3{Implementation of the WaveEquation class}

// 实际逻辑的实现实际上是相当短的，因为我们把组装矩阵和右手边的向量等事情交给了库。其余的实际代码不超过130行，其中相当一部分是可以从以前的例子程序中获取的模板代码（例如，解决线性系统的函数，或生成输出的函数）。

// 我们从构造函数开始（关于时间步长的选择的解释，请参见介绍中关于Courant, Friedrichs, and Lewy的部分）。

  template <int dim>
  WaveEquation<dim>::WaveEquation()
    : fe(1)
    , dof_handler(triangulation)
    , time_step(1. / 64)
    , time(time_step)
    , timestep_number(1)
    , theta(0.5)
  {}
// @sect4{WaveEquation::setup_system}

// 下一个函数是在程序开始时，也就是在第一个时间步骤之前，设置网格、DoFHandler以及矩阵和向量。如果你已经阅读了至少到 step-6 为止的教程程序，那么前几行是相当标准的。

  template <int dim>
  void WaveEquation<dim>::setup_system()
  {
    GridGenerator::hyper_cube(triangulation, -1, 1);
    triangulation.refine_global(7);

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

    dof_handler.distribute_dofs(fe);

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

    DynamicSparsityPattern dsp(dof_handler.n_dofs(), dof_handler.n_dofs());
    DoFTools::make_sparsity_pattern(dof_handler, dsp);
    sparsity_pattern.copy_from(dsp);

// 然后，我们必须初始化程序过程中需要的3个矩阵：质量矩阵、拉普拉斯矩阵和在每个时间步长中求解 $M+k^2\theta^2A$ 时使用的矩阵 $U^n$ 。

// 在设置这些矩阵时，请注意它们都是利用了相同的稀疏模式对象。最后，在deal.II中矩阵和稀疏模式是独立对象的原因（与其他许多有限元或线性代数类不同）变得很清楚：在相当一部分应用中，我们必须持有几个恰好具有相同稀疏模式的矩阵，它们没有理由不共享这一信息，而不是重新建立并多次浪费内存。

// 在初始化所有这些矩阵后，我们调用库函数来建立拉普拉斯和质量矩阵。它们所需要的只是一个DoFHandler对象和一个将用于数值积分的正交公式对象。请注意，在许多方面，这些函数比我们通常在应用程序中做的要好，例如，如果一台机器有多个处理器，它们会自动并行构建矩阵：更多信息见WorkStream的文档或 @ref threads "多处理器并行计算 "模块。解决线性系统的矩阵将在run()方法中被填充，因为我们需要在每个时间步长中重新应用边界条件。

    mass_matrix.reinit(sparsity_pattern);
    laplace_matrix.reinit(sparsity_pattern);
    matrix_u.reinit(sparsity_pattern);
    matrix_v.reinit(sparsity_pattern);

    MatrixCreator::create_mass_matrix(dof_handler,
                                      QGauss<dim>(fe.degree + 1),
                                      mass_matrix);
    MatrixCreator::create_laplace_matrix(dof_handler,
                                         QGauss<dim>(fe.degree + 1),
                                         laplace_matrix);

// 该函数的其余部分用于将矢量大小设置为正确的值。最后一行关闭了悬挂的节点约束对象。由于我们在一个均匀细化的网格上工作，所以不存在或没有计算过约束条件（即没有必要像其他程序那样调用 DoFTools::make_hanging_node_constraints ），但无论如何，我们需要在下面的一个地方进一步设置一个约束对象。

    solution_u.reinit(dof_handler.n_dofs());
    solution_v.reinit(dof_handler.n_dofs());
    old_solution_u.reinit(dof_handler.n_dofs());
    old_solution_v.reinit(dof_handler.n_dofs());
    system_rhs.reinit(dof_handler.n_dofs());

    constraints.close();
  }

//  @sect4{WaveEquation::solve_u and WaveEquation::solve_v}

// 接下来的两个函数是解决与  $U^n$  和  $V^n$  的方程有关的线性系统。这两个函数并不特别有趣，因为它们基本沿用了前面所有教程程序中的方案。

// 我们可以对我们要反转的两个矩阵的预处理程序做一些小实验。然而，事实证明，对于这里的矩阵，使用雅可比或SSOR预处理器可以稍微减少解决线性系统所需的迭代次数，但由于应用预处理器的成本，在运行时间方面并不占优势。这也不是什么损失，但让我们保持简单，只做不做。

  template <int dim>
  void WaveEquation<dim>::solve_u()
  {
    SolverControl            solver_control(1000, 1e-8 * system_rhs.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    cg.solve(matrix_u, solution_u, system_rhs, PreconditionIdentity());

    std::cout << "   u-equation: " << solver_control.last_step()
              << " CG iterations." << std::endl;
  }

  template <int dim>
  void WaveEquation<dim>::solve_v()
  {
    SolverControl            solver_control(1000, 1e-8 * system_rhs.l2_norm());
    SolverCG<Vector<double>> cg(solver_control);

    cg.solve(matrix_v, solution_v, system_rhs, PreconditionIdentity());

    std::cout << "   v-equation: " << solver_control.last_step()
              << " CG iterations." << std::endl;
  }

//  @sect4{WaveEquation::output_results}

// 同样地，下面的函数也和我们之前做的差不多。唯一值得一提的是，这里我们使用 Utilities::int_to_string 函数的第二个参数，生成了一个用前导零填充的时间步长的字符串表示，长度为3个字符。

  template <int dim>
  void WaveEquation<dim>::output_results() const
  {
    DataOut<dim> data_out;

    data_out.attach_dof_handler(dof_handler);
    data_out.add_data_vector(solution_u, "U");
    data_out.add_data_vector(solution_v, "V");

    data_out.build_patches();

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

// 像  step-15  一样，由于我们在每个时间步长写输出（而且我们要解决的系统相对简单），我们指示DataOut使用zlib压缩算法，该算法针对速度而不是磁盘使用进行了优化，因为否则绘制输出会成为一个瓶颈。

    DataOutBase::VtkFlags vtk_flags;
    vtk_flags.compression_level =
      DataOutBase::VtkFlags::ZlibCompressionLevel::best_speed;
    data_out.set_flags(vtk_flags);
    std::ofstream output(filename);
    data_out.write_vtu(output);
  }

//  @sect4{WaveEquation::run}

// 下面是程序中唯一有趣的功能。它包含了所有时间步骤的循环，但在这之前我们必须设置网格、DoFHandler和矩阵。此外，我们必须以某种方式从初始值开始。为此，我们使用 VectorTools::project 函数，该函数接收一个描述连续函数的对象，并计算该函数在DoFHandler对象所描述的有限元空间的 $L^2$ 投影。没有比这更简单的了。

  template <int dim>
  void WaveEquation<dim>::run()
  {
    setup_system();

    VectorTools::project(dof_handler,
                         constraints,
                         QGauss<dim>(fe.degree + 1),
                         InitialValuesU<dim>(),
                         old_solution_u);
    VectorTools::project(dof_handler,
                         constraints,
                         QGauss<dim>(fe.degree + 1),
                         InitialValuesV<dim>(),
                         old_solution_v);

// 接下来是循环所有的时间步骤，直到我们到达结束时间（本例中为 $T=5$ ）。在每个时间步骤中，我们首先要解决 $U^n$ ，使用方程  $(M^n + k^2\theta^2 A^n)U^n =$  。
// $(M^{n,n-1} - k^2\theta(1-\theta) A^{n,n-1})U^{n-1} + kM^{n,n-1}V^{n-1} +$
// $k\theta \left[k \theta F^n + k(1-\theta) F^{n-1} \right]$  . 请注意，我们在所有的时间步骤中使用相同的网格，因此， $M^n=M^{n,n-1}=M$  和  $A^n=A^{n,n-1}=A$  。因此，我们首先要做的是将 $MU^{n-1} - k^2\theta(1-\theta) AU^{n-1} + kMV^{n-1}$ 和强制项相加，并将结果放入 system_rhs 向量中。(对于这些加法，我们需要在循环之前声明一个临时向量，以避免在每个时间步骤中重复分配内存)。

// 这里需要意识到的是我们如何将时间变量传达给描述右手边的对象：每个从函数类派生出来的对象都有一个时间字段，可以用 Function::set_time 来设置，用 Function::get_time. 来读取。 实质上，使用这种机制，所有空间和时间的函数因此被认为是在某个特定时间评估的空间的函数。这与我们在有限元程序中的典型需求非常吻合，在有限元程序中，我们几乎总是在一个时间步长上工作，而且从来没有发生过，例如，人们想在任何给定的空间位置上为所有时间评估一个时空函数。

    Vector<double> tmp(solution_u.size());
    Vector<double> forcing_terms(solution_u.size());

    for (; time <= 5; time += time_step, ++timestep_number)
      {
        std::cout << "Time step " << timestep_number << " at t=" << time
                  << std::endl;

        mass_matrix.vmult(system_rhs, old_solution_u);

        mass_matrix.vmult(tmp, old_solution_v);
        system_rhs.add(time_step, tmp);

        laplace_matrix.vmult(tmp, old_solution_u);
        system_rhs.add(-theta * (1 - theta) * time_step * time_step, tmp);

        RightHandSide<dim> rhs_function;
        rhs_function.set_time(time);
        VectorTools::create_right_hand_side(dof_handler,
                                            QGauss<dim>(fe.degree + 1),
                                            rhs_function,
                                            tmp);
        forcing_terms = tmp;
        forcing_terms *= theta * time_step;

        rhs_function.set_time(time - time_step);
        VectorTools::create_right_hand_side(dof_handler,
                                            QGauss<dim>(fe.degree + 1),
                                            rhs_function,
                                            tmp);

        forcing_terms.add((1 - theta) * time_step, tmp);

        system_rhs.add(theta * time_step, forcing_terms);

// 如此构建了第一个方程的右手向量后，我们要做的就是应用正确的边界值。至于右手边，这是一个在特定时间评估的时空函数，我们在边界节点插值，然后像通常那样用结果来应用边界值。然后将结果交给solve_u()函数。

        {
          BoundaryValuesU<dim> boundary_values_u_function;
          boundary_values_u_function.set_time(time);

          std::map<types::global_dof_index, double> boundary_values;
          VectorTools::interpolate_boundary_values(dof_handler,
                                                   0,
                                                   boundary_values_u_function,
                                                   boundary_values);

// solve_u()的矩阵在每个时间步骤中都是相同的，所以人们可以认为只在模拟开始时做一次就足够了。然而，由于我们需要对线性系统应用边界值（消除了一些矩阵的行和列，并对右手边做出了贡献），在实际应用边界数据之前，我们必须在每个时间步骤中重新填充该矩阵。实际内容非常简单：它是质量矩阵和加权拉普拉斯矩阵的总和。

          matrix_u.copy_from(mass_matrix);
          matrix_u.add(theta * theta * time_step * time_step, laplace_matrix);
          MatrixTools::apply_boundary_values(boundary_values,
                                             matrix_u,
                                             solution_u,
                                             system_rhs);
        }
        solve_u();

// 第二步，即求解 $V^n$ ，工作原理类似，只是这次左边的矩阵是质量矩阵（我们再次复制，以便能够应用边界条件，而右边是 $MV^{n-1} - k\left[ \theta A U^n + (1-\theta) AU^{n-1}\right]$ 加上强制项。边界值的应用方式与之前相同，只是现在我们必须使用BoundaryValuesV类。

        laplace_matrix.vmult(system_rhs, solution_u);
        system_rhs *= -theta * time_step;

        mass_matrix.vmult(tmp, old_solution_v);
        system_rhs += tmp;

        laplace_matrix.vmult(tmp, old_solution_u);
        system_rhs.add(-time_step * (1 - theta), tmp);

        system_rhs += forcing_terms;

        {
          BoundaryValuesV<dim> boundary_values_v_function;
          boundary_values_v_function.set_time(time);

          std::map<types::global_dof_index, double> boundary_values;
          VectorTools::interpolate_boundary_values(dof_handler,
                                                   0,
                                                   boundary_values_v_function,
                                                   boundary_values);
          matrix_v.copy_from(mass_matrix);
          MatrixTools::apply_boundary_values(boundary_values,
                                             matrix_v,
                                             solution_v,
                                             system_rhs);
        }
        solve_v();

// 最后，在计算完两个解的组成部分后，我们输出结果，计算解中的能量，并在将现在的解移入持有上一个时间步长的解的向量后，继续下一个时间步长。注意函数 SparseMatrix::matrix_norm_square 可以在一个步骤中计算 $\left<V^n,MV^n\right>$ 和 $\left$ ，为我们节省了一个临时向量和几行代码的费用。

        output_results();

        std::cout << "   Total energy: "
                  << (mass_matrix.matrix_norm_square(solution_v) +
                      laplace_matrix.matrix_norm_square(solution_u)) /
                       2
                  << std::endl;

        old_solution_u = solution_u;
        old_solution_v = solution_v;
      }
  }
} // namespace Step23
// @sect3{The main function}

//剩下的就是程序的主要功能了。这里没有什么是在前面几个程序中没有展示过的。

int main()
{
  try
    {
      using namespace Step23;

      WaveEquation<2> wave_equation_solver;
      wave_equation_solver.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;
}


/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/
/*
------------------ Author: Valentino Zuccarelli  -------------------------------------------------
 ------------------ E-mail: (Valentino.Zuccarelli@gmail.com) ----------------------------
 */

#include "math.h"
#include "Astro-Core/propagateTHREEbody.h"
#include "Astro-Core/threebodyParametersComputation.h"
#include "halorbitcomputation.h"
#include "trajectoryprinting.h"
#include "Astro-Core/EODE/eode.h"
#include <QDebug>
#include <QString>
#include <ctime>
#include <QFile>
#include <QTextStream>
#include <Eigen/LU>
#include <Eigen/QR>

using namespace Eigen;
extern double Grav_Param, bodies_distance;
int halo_num_positions;
#define Pi 3.14159265358979
QString halo_input_data;

void HaloOrbit::lpointsComputation ()
{
    double ni, csi_1, csi_2;

    ni=pow(Grav_Param/(3*(1-Grav_Param)), 1.0/3.0);
    csi_1=ni*(1-ni/3.0-(pow(ni,2.0))/9.0-23*(pow(ni,3.0))/81.0+151*(pow(ni,4.0))/243.0-(pow(ni,5.0))/9.0);
    L1_xposition=(1-Grav_Param-csi_1);		//L1 coordinate
    csi_2=ni*(1+ni/3.0-(pow(ni,2.0))/9.0-31*(pow(ni,3.0))/81.0-119*(pow(ni,4.0))/243.0-(pow(ni,5.0))/9.0);
    L2_xposition=(1+csi_2-Grav_Param);	//L2 coordinate
}

void HaloOrbit::getAnalyticalApprox ()

{
        double gammaL, c[4], r1; int i;
        r1=trajectory.Lpoint_Xposition/bodies_distance;
        ////qdebug()<<bodies_distance;

        if (trajectory.Lpoint_selected==1)
                {
                double oi=1.0-Grav_Param-r1;
                gammaL=fabs(oi);
                for (i=2;i<5;i++)
                        c[i]=(pow(1.0,i)*Grav_Param+pow(-1.0,i)*(1-Grav_Param)*pow(gammaL,i+1)/pow(1.0-gammaL,i+1))/pow(gammaL,3);
                }
        else if (trajectory.Lpoint_selected==2)
                {
                gammaL=fabs(1-Grav_Param-r1);
                for (i=2;i<5;i++)
                        c[i]=(pow(-1.0,i)*Grav_Param+pow(-1.0,i)*(1-Grav_Param)*pow(gammaL,i+1)/pow(1.0+gammaL,i+1))/pow(gammaL,3);
                }
        else if (trajectory.Lpoint_selected==3)
                {
                gammaL=fabs(Grav_Param+r1);
                for (i=2;i<5;i++)
                        c[i]=(1-Grav_Param+(Grav_Param)*pow(gammaL,i+1)/pow(1.0+gammaL,i+1))/pow(gammaL,3);
                }

        double b, cc, lambda, k, delta, d1, d2;

        b=c[2]-2; cc=-(c[2]-1)*(c[2]*2+1);
        lambda=sqrt((-b+sqrt(pow(b,2.0)-4*cc))/2);
        k=(pow(lambda,2)+1+2*c[2])/(2*lambda);
        delta=pow(lambda,2)-c[2];
        d1=3*pow(lambda,2)*(k*(6*pow(lambda,2.0)-1)-2*lambda)/k;
        d2=8*pow(lambda,2)*(k*(11*pow(lambda,2.0)-1)-2*lambda)/k;

        double a21, a22, a23, a24, a31, a32, b21, b22, b31, b32, d21, d31, d32;

        a21=3*c[3]*(pow(k,2.0)-2)/(4*(1+2*c[2]));
        a22=3*c[3]/(4*(1+2*c[2]));
        a23=-3*c[3]*lambda*(3*lambda*pow(k,3.0)-6*k*(k-lambda)+4)/(4*k*d1);
        a24=-3*c[3]*lambda*(3*lambda*k+2)/(4*k*d1);

        b21=-3*c[3]*lambda*(3*k*lambda-4)/(2*d1);
        b22=3*c[3]*lambda/d1;
        a31=-9*lambda*(4*c[3]*(k*a23-b21)+k*c[4]*(4+pow(k,2)))/(4*d2)+((9*pow(lambda,2)+1-c[2])/(2*d2))*(3*c[3]*(2*a23-k*b21)+c[4]*(2+3*pow(k,2)));
        d21=-c[3]/(2*pow(lambda,2));
        a32=-(9*lambda*(4*c[3]*(k*a24-b22)+k*c[4])/4+3*(9*pow(lambda,2)+1-c[2])*(c[3]*(k*b22+d21-2*a24)-c[4])/2)/d2;
        b31=3*(8*lambda*(3*c[3]*(k*b21-2*a23)-c[4]*(2+3*pow(k,2)))+(9*pow(lambda,2)+1+2*c[2])*(4*c[3]*(k*a23-b21)+k*c[4]*(4+pow(k,2))))/(8*d2);
        b32=(9*lambda*(c[3]*(k*b22+d21-2*a24)-c[4])+3*(9*pow(lambda,2)+1+2*c[2])*(4*c[3]*(k*a24-b22)+k*c[4])/8)/d2;
        d31=3*(4*c[3]*a24+c[4])/(64*pow(lambda,2));
        d32=3*(4*c[3]*(a23-d21)+c[4]*(4+pow(k,2)))/(64*pow(lambda,2));

        double teta, s1, s2, l1, l2, a1, a2;

        a1=-3*c[3]*(2*a21+a23+5*d21)/2-3*c[4]*(12-pow(k,2))/8;
        a2=3*c[3]*(a24-2*a22)/2+9*c[4]/8;
        s1=(3*c[3]*(2*a21*(pow(k,2)-2)-a23*(pow(k,2)+2)-2*k*b21)/2-3*c[4]*(3*pow(k,4)-8*pow(k,2)+8)/8)/(2*lambda*(lambda*(1+pow(k,2))-2*k));
        s2=(3*c[3]*(2*a22*(pow(k,2)-2)+a24*(pow(k,2)+2)+2*k*b22+5*d21)/2+3*c[4]*(12-pow(k,2))/8)/(2*lambda*(lambda*(1+pow(k,2))-2*k));
        l1=a1+2*s1*pow(lambda,2);
        l2=a2+2*s2*pow(lambda,2);

        double Axmin, R1;

        R1=bodies_distance*gammaL;
        Axmin=(sqrt(fabs(delta/l1)))*R1;

        if (trajectory.init_data.Ax_amplitude<Axmin)
                trajectory.init_data.Ax_amplitude=Axmin;
        double Ax, Az;
        Az=trajectory.init_data.Az_amplitude/R1;
        Ax=trajectory.init_data.Ax_amplitude/R1;

        teta=0; i=0;

        double temporary_Zposition=((Az*cos(teta)+d21*Ax*Az*(cos(2*teta)-3)+cos(3*teta)*(d32*Az*pow(Ax,2)-d31*pow(Az,3)))*R1)/bodies_distance;
        if (temporary_Zposition<0)
                i=20;
        teta=Pi*0.05*i;
        double temporary_Xposition=((a21*pow(Ax,2)+a22*pow(Az,2)-Ax*cos(teta)+(a23*pow(Ax,2)-a24*pow(Az,2))*cos(2*teta)+(a31*pow(Ax,3)-a32*Ax*pow(Az,2))*cos(3*teta))*R1)/bodies_distance;

        trajectory.init_data.State.position.x()=r1+temporary_Xposition;
        trajectory.init_data.State.position.z()=((Az*cos(teta)+d21*Ax*Az*(cos(2*teta)-3)+cos(3*teta)*(d32*Az*pow(Ax,2)-d31*pow(Az,3)))*R1)/bodies_distance;
        trajectory.init_data.State.velocity.y()=((k*Ax*cos(teta)*lambda+(b21*pow(Ax,2)-b22*pow(Az,2))*cos(teta*2)*2*lambda+(b31*pow(Ax,3)-b32*Ax*pow(Az,2))*cos(3*teta)*3*lambda))*R1/bodies_distance;
}

MatrixXd STM(6,6);

void HaloOrbit::getNumericalApprox (int system, int &MAX_integration_time)//, const Eigen::MatrixXd STM)

{
    clock_t start, end;
    start= clock();
    time_t t0, t1, diff;
    double time_elapsed;

    double paramv[1]={0}; double deriv[42]; double state[42];
    trajectory.init_data.State.velocity.x()=trajectory.init_data.State.velocity.z()=trajectory.init_data.State.position.y()=trajectory.init_data.period=0;
    trajectory.final_data.State.velocity.x()=trajectory.final_data.State.velocity.z()=1;

    double timein, timefin, initialtimefin, relerr, abserr, accuracy, count, count2, T, step; int flag, k, q, p, l=0;
    relerr=abserr=accuracy=1E-9; timefin=0; initialtimefin=0.25; //period   // 0.25!!!!!!!!!!!!!!!!!!!!!!! per velocizzare 1.0

    double correction[6]; double z_correction, x_correction; double deriv_vel[3];

    if (trajectory.init_data.State.velocity.y()<0) //check the initial y-velocity sign
    {l=1;}

    while (fabs(trajectory.final_data.State.velocity.x())>10e-8 || fabs( trajectory.final_data.State.velocity.z())>10e-8) //stop condition
    {
        T=0; step=0.01;
        ////qdebug()<<"before change"<<timefin;
        //if (timefin>initialtimefin)
            //timefin=timefin-0.8;
        //else
//            timefin=initialtimefin;
        timefin=0.3;
        count=1;

       while (count>0)
       {
           flag=1; timein=0; timefin=timefin+step;
           for (p=0; p<42; p++)
           {
               state[p]=0;
               deriv[p]=0;
           }
               state[1]=trajectory.init_data.State.velocity.y(); state[5]=trajectory.init_data.State.position.z();
               state[0]=trajectory.init_data.State.velocity.x(); state[2]=trajectory.init_data.State.velocity.z();
               state[3]=trajectory.init_data.State.position.x(); state[4]=trajectory.init_data.State.position.y();

               state[41]=state[6]=state[13]=state[20]=state[27]=state[34]=1;
               //qDebug()<<"step0"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];

               while (flag!=2)
               {
                   flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
                   if (flag==4)
                       flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
               }
               //qDebug()<<"step1"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];
               count=state[4];
               if (l==1)
                   count=-state[4];
       }

       count2=1;
       end= clock(); time_elapsed=(double(end-start))/CLOCKS_PER_SEC;

       if (time_elapsed>MAX_integration_time)
       {
           ////qdebug()<<"Max time allowed:"<<MAX_integration_time<<"time elapsed"<<time_elapsed;
           trajectory.error=1;
           return;
       }

       while (fabs(count2)>10e-12)
       {
           timefin=timefin-step;
           step=step/10.0;
           count2=1;
           while (count2>0)
           {
               for (p=0; p<42; p++)
               {
                   state[p]=0;
                   deriv[p]=0;
               }
                   flag=1; timein=0; timefin=timefin+step;
                   state[1]=trajectory.init_data.State.velocity.y(); state[5]=trajectory.init_data.State.position.z();
                   state[0]=trajectory.init_data.State.velocity.x(); state[2]=trajectory.init_data.State.velocity.z();
                   state[3]=trajectory.init_data.State.position.x(); state[4]=trajectory.init_data.State.position.y();
                   state[41]=state[6]=state[13]=state[20]=state[27]=state[34]=1;
                   //qDebug()<<"step2"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];

                   while (flag!=2)
                   {
                       flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
                       if (flag==4)
                           flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
                   }
                   //qDebug()<<"step3"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];

                   count2=state[4];
                   if (l==1)
                       count2=-state[4];

                   end=clock(); time_elapsed=(double(end-start))/CLOCKS_PER_SEC;

                   if (time_elapsed>MAX_integration_time)
                   {
                       ////qdebug()<<"Max time allowed:"<<MAX_integration_time<<"time elapsed"<<time_elapsed;
                       trajectory.error=1;
                       return;
                   }
            }

           trajectory.final_data.State.velocity.x()=state[0]; trajectory.final_data.State.velocity.y()=state[1]; trajectory.final_data.State.velocity.z()=state[2];
           trajectory.final_data.State.position.x()=state[3]; trajectory.final_data.State.position.y()=state[4]; trajectory.final_data.State.position.z()=state[5];
           trajectory.final_data.period=T=2*timefin;
           correction[0]=state[12]; correction[1]=state[14]; correction[2]=state[24]; correction[3]=state[26]; correction[4]=state[36]; correction[5]=state[38];
       }

       ////qdebug()<<"initial state"<<trajectory.init_data.State.position.x()<<trajectory.init_data.State.position.z()<<trajectory.init_data.State.velocity.y();
       ////qdebug()<<"final state"<<trajectory.final_data.State.position.y()<<trajectory.final_data.State.velocity.x()<<trajectory.final_data.State.velocity.z()<<timefin;
       double C;
       Jacobi (Grav_Param, trajectory.final_data.State.position.x(), trajectory.final_data.State.position.y(), trajectory.final_data.State.position.z(), trajectory.final_data.State.velocity.x(), trajectory.final_data.State.velocity.y(), trajectory.final_data.State.velocity.z(),C);
       ////qdebug()<<"C"<<C;
       if (fabs(trajectory.final_data.State.velocity.x())<10e-8 && fabs(trajectory.final_data.State.velocity.z())<10e-8)
       {
           for (k=0;k<6;k++)
           {
               for (p=0;p<6;p++)
               {
                   STM(k,p)=state[6*(k+1)+p];
               }
           }
       }
       else
       {
           double r1, r2;
           r1=sqrt(pow(trajectory.final_data.State.position.x()+Grav_Param,2)+pow(trajectory.final_data.State.position.y(),2)+pow(trajectory.final_data.State.position.z(),2));
           r2=sqrt(pow(trajectory.final_data.State.position.x()-1+Grav_Param,2)+pow(trajectory.final_data.State.position.y(),2)+pow(trajectory.final_data.State.position.z(),2));
           deriv_vel[0]=2*trajectory.final_data.State.velocity.y()+trajectory.final_data.State.position.x()-(1-Grav_Param)*(trajectory.final_data.State.position.x()+Grav_Param)/(pow(r1,3))+ Grav_Param*(1-Grav_Param-trajectory.final_data.State.position.x())/(pow(r2,3));
           deriv_vel[2]=-(1-Grav_Param)*trajectory.final_data.State.position.z()/(pow(r1,3))-Grav_Param*trajectory.final_data.State.position.z()/(pow(r2,3));

           MatrixXd correction_matrix(2,2);
           for (k=0; k<4; k++)
           {
               q=(k+2)/2;
               int col = (k+1)/2-(q-1);
               correction_matrix(q-1,col)=correction[k+2]-deriv_vel[2*(q-1)]*correction[k-2*(q-1)]/trajectory.final_data.State.velocity.y();
               //correction_matrix[k]=correction[k+2]-deriv_vel[2*(q-1)]*correction[k-2*(q-1)]/trajectory.final_data.State.velocity.y();
           }
           //correction_matrix.inverse();
           ////qdebug()<<"inverse!"<<correction_matrix.inverse()(0,0)<<correction_matrix.inverse()(0,1)<<correction_matrix.inverse()(1,0)<<correction_matrix.inverse()(1,1);
           //determinant= correction_matrix[0]*correction_matrix[3]-correction_matrix[2]*correction_matrix[1];
           //inverse_correction_matrix[0]=correction_matrix[3]/determinant;
           //inverse_correction_matrix[1]=-correction_matrix[1]/determinant;
           //inverse_correction_matrix[2]=-correction_matrix[2]/determinant;
           //inverse_correction_matrix[3]=correction_matrix[0]/determinant;

               //x_correction=-inverse_correction_matrix[0]*trajectory.final_data.State.velocity.x()-inverse_correction_matrix[1]*trajectory.final_data.State.velocity.z();
               //z_correction=-inverse_correction_matrix[2]*trajectory.final_data.State.velocity.x()-inverse_correction_matrix[3]*trajectory.final_data.State.velocity.z();
           x_correction=-correction_matrix.inverse()(0,0)*trajectory.final_data.State.velocity.x()-correction_matrix.inverse()(0,1)*trajectory.final_data.State.velocity.z();
           z_correction=-correction_matrix.inverse()(1,0)*trajectory.final_data.State.velocity.x()-correction_matrix.inverse()(1,1)*trajectory.final_data.State.velocity.z();
               trajectory.init_data.State.position.x()=trajectory.init_data.State.position.x()+x_correction;
               trajectory.init_data.State.position.z()=trajectory.init_data.State.position.z()+z_correction;
       }

       }

//qdebug()<<"here|";
        int trajectory_error=0;
        state[6]=T; state[0]=trajectory.init_data.State.velocity.x(); state[1]=trajectory.init_data.State.velocity.y(); state[2]=trajectory.init_data.State.velocity.z();
        state[3]=trajectory.init_data.State.position.x(); state[4]=trajectory.init_data.State.position.y(); state[5]=trajectory.init_data.State.position.z();
        QString save_path = "3BMhalorbit";
        halo_input_data=save_path=save_path+QString::number(trajectory.init_data.Ax_amplitude,'f',0)+QString::number(trajectory.init_data.Az_amplitude,'f',0)+".stae";
        halo_num_positions=trajectory.Num_positions;
        trajectory_integration(state, trajectory.Num_positions, save_path, trajectory_error);
        ////qdebug() << "halo file: " << save_path;

        if (trajectory_error==2) //if the halo trajectory.txt has been not opened a message error must be given
        {
            trajectory.error=trajectory_error;
            return;
        }

        //file 3BMsystem.stam
        QString file = "3BMsystem.stam";
        QFile threeBMsystemFile(file);
        threeBMsystemFile.open(QIODevice::WriteOnly | QIODevice::Text);

        QTextStream outsys(&threeBMsystemFile);
        outsys.setRealNumberNotation(QTextStream::FixedNotation);
        outsys.setRealNumberPrecision(16);
        if (system==12)
            system=30; //user system, as ICD states

        outsys<<system<<"\n";
        outsys<<Grav_Param<<"\n";
        outsys<<bodies_distance<<"\n";
        outsys<<L1_xposition<<"\n";
        outsys<<L2_xposition<<"\n";
        threeBMsystemFile.close();
        ////qdebug()<<file;

        //computation of the real amplitudes obtained for the halo orbit
        QFile haloFile1(save_path);
        if (!haloFile1.open(QIODevice::ReadOnly | QIODevice::Text))
            {trajectory.error=3;
                return;
            }

        QTextStream in(&haloFile1);
        double t,Vx,Vy,Vz,x,y,z,X_max=0, X_min=10,Z_max=0, Z_min=10;
        for (p=0;p<trajectory.Num_positions;p++) //search for the maximum and minimum values of x and z coordinates
                {
                in>>t>>x>>y>>z>>Vx>>Vy>>Vz;
                //qDebug()<<x<<z;
                if (x<X_min)
                        X_min=x;
                if (x>X_max)
                        X_max=x;
                if (z<Z_min)
                        Z_min=z;
                if (z>Z_max)
                        Z_max=z;
                }
        haloFile1.close();

        trajectory.final_data.Ax_amplitude=(fabs(X_max)-fabs(X_min))/2;	trajectory.final_data.Az_amplitude=(fabs(Z_max)+fabs(Z_min))/2; //computation of the amplitudes obtained
        if (trajectory.init_data.Az_amplitude<0)
            trajectory.final_data.Az_amplitude=-trajectory.final_data.Az_amplitude;
        //qDebug()<<trajectory.final_data.Az_amplitude<<trajectory.final_data.Ax_amplitude<<bodies_distance;

        //check on the real amplitudes obtained, if too different the manifolds computation will be not allowed
        if (fabs((trajectory.final_data.Az_amplitude*bodies_distance-trajectory.init_data.Az_amplitude)/trajectory.init_data.Az_amplitude)>15 || fabs((trajectory.final_data.Ax_amplitude*bodies_distance-trajectory.init_data.Ax_amplitude)/trajectory.init_data.Ax_amplitude)>15)
            trajectory.error=4;
}

void HaloOrbit::getManifolds (double eigen, int points_number, double period_manifolds, int &first)

{
        double initial_condition_perturbed[7], initial_condition_0[7]; const char * manifolds_save_path;
        initial_condition_perturbed[6]=period_manifolds;

        QFile file_manifolds_settings ("3BMmanifolds_settings.stam");
        if (first==1)
                {if (!file_manifolds_settings.open(QIODevice::WriteOnly | QIODevice::Text))
                        {manifolds.error=3;
                        return;}
                }
        else
                {if (!file_manifolds_settings.open(QIODevice::Append | QIODevice::Text))
                        {manifolds.error=3;
                        return;}
                }

        QTextStream settings(&file_manifolds_settings);

        if (manifolds.kind==1)	//information about the manifolds required
                manifolds_save_path="3BMmanifold_rightStable.stae";
        else if (manifolds.kind==2)
                manifolds_save_path="3BMmanifold_rightUnstable.stae";
        else if (manifolds.kind==3)
                manifolds_save_path="3BMmanifold_leftStable.stae";
        else if (manifolds.kind==4)
                manifolds_save_path="3BMmanifold_leftUnstable.stae";
        else
                {manifolds.error=5;
                return;
                }
        settings<<manifolds.kind<<"\n"<<trajectory.Num_positions<<"\n";	//the kind of manifold and the number of orbits are printed

        if (period_manifolds!=0)
                settings<<points_number<<"\n";	//the number of positions per orbit is printed in case a time of integration has been specified
        file_manifolds_settings.close();

        int i;
       /////// if(trajectory.Num_positions>0) //normal manifolds computation

             QString halo_data = "3BMhalorbit";
        halo_data=halo_data+QString::number(trajectory.init_data.Ax_amplitude,'f',0)+QString::number(trajectory.init_data.Az_amplitude,'f',0)+".stae";
        QFile file_halo2(halo_data);
        if (!file_halo2.open(QIODevice::ReadOnly | QIODevice::Text))
        {
            manifolds.error=1;
            return;
        }
        QTextStream in(&file_halo2);
        for (i=1;i<trajectory.Num_positions+1;i++)
                {
                in>>initial_condition_0[6]>>initial_condition_0[3]>>initial_condition_0[4]>>initial_condition_0[5]>>initial_condition_0[0]>>initial_condition_0[1]>>initial_condition_0[2];

                for (int j=0;j<6;j++)
                        {initial_condition_perturbed[j]=initial_condition_0[j]-manifolds.epsilon*eigen*initial_condition_0[j];}
                if (eigen<1) //condition in case of stable manifolds
                {
                    initial_condition_perturbed[0]=-initial_condition_perturbed[0];
                    initial_condition_perturbed[4]=-initial_condition_perturbed[4];
                    initial_condition_perturbed[5]=-initial_condition_perturbed[5];
                    i=-i;
                }
                ////qdebug()<<"Debug| Bon"<<initial_condition_perturbed[0]<<initial_condition_perturbed[1]<<initial_condition_perturbed[2]<<initial_condition_perturbed[3]<<initial_condition_perturbed[4]<<initial_condition_perturbed[5]<<initial_condition_perturbed[6]<<points_number<<manifolds_save_path<<manifolds.error<<i<<trajectory.Lpoint_Xposition<<manifolds.kind;
                manifolds_integration(initial_condition_perturbed, points_number, manifolds_save_path, manifolds.error, i, trajectory.Lpoint_Xposition, manifolds.kind);
                i=abs(i);
                if (manifolds.error!=0)
                    return;
                }

                file_halo2.close();
                if (i!=trajectory.Num_positions+1)
                manifolds.error=5;

        /* manifolds computation in case of transfer optimization
           in this case only on trajectory is randomly computed
        */

       /*
 else

        {
            double paramv[1]={0}; double deriv[5]; double timefin, timein=0;
            double relerr, abserr, accuracy; abserr=relerr=accuracy=1E-9;

            for (int p=0; p<6; p++)
                deriv[p]=0;
            int flag=1;

            extern double fractionOrbit;
            timefin=trajectory.final_data.period*fractionOrbit;
            //qdebug()<<"time"<<timefin<<trajectory.final_data.period<<fractionOrbit;
            initial_condition_0[3]=trajectory.final_data.State.position.x();
            initial_condition_0[4]=trajectory.final_data.State.position.y();
            initial_condition_0[5]=trajectory.final_data.State.position.z();
            initial_condition_0[0]=trajectory.final_data.State.velocity.x();
            initial_condition_0[1]=trajectory.final_data.State.velocity.y();
            initial_condition_0[2]=trajectory.final_data.State.velocity.z();
//qdebug()<<"Debug| 1"<<initial_condition_0[0]<<initial_condition_0[1]<<initial_condition_0[2]<<initial_condition_0[3]<<initial_condition_0[4]<<initial_condition_0[5];
//qdebug()<<timein;
double initial_condition_1[5];
        while (flag!=2)
        {
            flag=Runge_Kutta_Fehlberg(threebody_EOM, 6, initial_condition_0, paramv, deriv, &timein, timefin, &relerr, abserr, flag );
            if (flag==4)
                flag=Runge_Kutta_Fehlberg(threebody_EOM, 6, initial_condition_0, paramv, deriv, &timein, timefin, &relerr, abserr, flag );
        //qdebug()<<timein;
        }
        for (int u=0; u<6; u++)
            initial_condition_1[u]=initial_condition_0[u];

//qdebug()<<"Debug| 2"<<initial_condition_1[0]<<initial_condition_1[1]<<initial_condition_1[2]<<initial_condition_1[3]<<initial_condition_1[4]<<initial_condition_1[5];
        initial_condition_1[6]=0; //required intersection with a main body
                for (int j=0;j<6;j++)
                        {initial_condition_perturbed[j]=initial_condition_1[j]-manifolds.epsilon*eigen*initial_condition_1[j];}
                i=1;
                //qdebug()<<"Debug| 3"<<initial_condition_perturbed[0]<<initial_condition_perturbed[1]<<initial_condition_perturbed[2]<<initial_condition_perturbed[3]<<initial_condition_perturbed[4]<<initial_condition_perturbed[5];
                if (eigen<1) //condition in case of stable manifolds
                {
                    initial_condition_perturbed[0]=-initial_condition_perturbed[0];
                    initial_condition_perturbed[4]=-initial_condition_perturbed[4];
                    initial_condition_perturbed[5]=-initial_condition_perturbed[5];
                    i=-1;
                }
                //qdebug()<<"Debug| final"<<initial_condition_perturbed[0]<<initial_condition_perturbed[1]<<initial_condition_perturbed[2]<<initial_condition_perturbed[3]<<initial_condition_perturbed[4]<<initial_condition_perturbed[5]<<initial_condition_perturbed[6]<<points_number<<manifolds_save_path<<manifolds.error<<i<<trajectory.Lpoint_Xposition<<manifolds.kind;
                manifolds_integration(initial_condition_perturbed, points_number, manifolds_save_path, manifolds.error, i, trajectory.Lpoint_Xposition, manifolds.kind);
                if (manifolds.error!=0)
                    return;
        }
        */

        //qdebug()<<manifolds_save_path;
        //qdebug()<<"deviation:"<<manifolds.epsilon;

        ////qdebug()<<manifolds_save_path;
        ////qdebug()<<"deviation:"<<manifolds.epsilon;

}

//subroutine to compute Eigenvalues
void HaloOrbit::getEigenvalues ()

{
        MatrixXd mat1(6,6), mat2(6,6), mat3(6,6), fi(6,6);//, STM_inv(6,6);
        mat2.setZero();
        mat3.setZero();
        mat1.setIdentity();
        mat1=-mat1;

        mat2(0,3)=mat2(1,4)=mat2(2,5)=-1;
        mat2(3,0)=mat2(4,1)=mat2(5,2)=1;
        mat2(3,4)=-2; mat2(4,3)=2;

        mat3(0,1)=-2; mat3(1,0)=2;
        mat3(0,3)=mat3(1,4)=mat3(2,5)=1;
        mat3(3,0)=mat3(4,1)=mat3(5,2)=-1;
        fi=mat1*STM.inverse()*mat2*mat3;

        //Eigen libray doesn't support the eigenvalues computation of any matrix, but only of selfAdjoint matrix.
        //because the STM is not such a matrix, the eigenvalues are computed in an alternative way.
        //When the eigenvalues computation will be available also for this kind of matrix, it's stronly recommended
        //to change this subroutine, by using the Eigen library.
        //the following hidden part is the code able to compute eigenvalues for selfAdjoint matrix. By replacing that
        //specific line, it will be possible to use the new subroutine of the Eigen library, when available.
#if 0
        //
        VectorXd eigenvalues_vector(6);
        eigenvalues_vector=fi.marked<SelfAdjoint>().eigenvalues();
        //VectorXd eigenvalues(6);
//cout<<fi;

        //fi.eigenvalues();
        //qdebug()<<"fi";
        //qdebug()<<fi(0,0)<<fi(0,1)<<fi(0,2)<<fi(0,3)<<fi(0,4)<<fi(0,5);
        //qdebug()<<fi(1,0)<<fi(1,1)<<fi(1,2)<<fi(1,3)<<fi(1,4)<<fi(1,5);
        //qdebug()<<fi(2,0)<<fi(2,1)<<fi(2,2)<<fi(2,3)<<fi(2,4)<<fi(2,5);
        //qdebug()<<fi(3,0)<<fi(3,1)<<fi(3,2)<<fi(3,3)<<fi(3,4)<<fi(3,5);
        //qdebug()<<fi(4,0)<<fi(4,1)<<fi(4,2)<<fi(4,3)<<fi(4,4)<<fi(4,5);
        //qdebug()<<fi(5,0)<<fi(5,1)<<fi(5,2)<<fi(5,3)<<fi(5,4)<<fi(5,5);
        //qdebug()<<"eigen";
        //qdebug()<<evve(0)<<evve(1)<<evve(2)<<evve(3)<<evve(4)<<evve(5);
        ////qdebug()<<eigenvalues(0)<<eigenvalues(1)<<eigenvalues(2)<<eigenvalues(3)<<eigenvalues(4)<<eigenvalues(5);

#endif

         MatrixXd mm1(6,6);
         mm1=fi;
         double step=0.001;
         double l=0; double h=1; int i;

         while (abs(h)>0.00001)
         {
             if (l>1)
             {
                 l=0;
                 step=step/10;}
                 l=step+l;
                 for (i=0;i<6;i++)
                 {
                     mm1(i,i)=fi(i,i)-l;}
                     h=mm1.determinant();
                     if (step<0.0000000001)
                     {
                         manifolds.error=4;
                         return;}
                     }

         manifolds.eigen1=l;
         manifolds.eigen2=1/l;
         //double index=0.5*(manifolds.eigen1+1/manifolds.eigen1); //stability index
}


//#include "Aggregate_Gen.hpp"
#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <chrono>
#include <Eigen/Dense>
#include <omp.h>
#include "aggregate_gen_SA.hpp"
using namespace Eigen;
using namespace std;

//return pos_sph matrix
vector<MatrixXd> aggregate_gen_CCA(const double& a, const int& num_sph_SA, const int& levels, const double& kf, const double& Df, const double& tol){

  vector<MatrixXd> pos_sph_agg_output; // vector of aggregate geometry from level 0 to "levels".
  int iter1max {1000};
  int iter2max {10000};

  int num_SA_agg= pow(2,levels);
  int final_totnum_sph= num_SA_agg*num_sph_SA;
  cout << "final totnum_sph= " << final_totnum_sph << endl;

  vector<MatrixXd> pos_sph_SAagg;
  for(int i= 0; i<num_SA_agg; ++i ) pos_sph_SAagg.push_back(aggregate_gen_SA(a,num_sph_SA,kf,Df,tol));

  pos_sph_agg_output.push_back(pos_sph_SAagg[0]); // level 0 aggregate

  vector<MatrixXd> pos_sph_agg_old;

  // start time
  chrono::time_point<chrono::steady_clock> CCA_start_time= chrono::steady_clock::now();

  for(int k= 1; k <= levels; ++k){
      cout << "Generating aggregates in " << k << "th level ... please wait ..." << endl;
      vector<MatrixXd> pos_sph_agg;

      for(int L= 1; L <= pow(2,levels-k); ++L){
          //cout << "generating " << L << " th " << "aggregate in level" << k << endl;
          int N1= num_sph_SA*pow(2,k-1);
          int N2= N1;

          // aggregation process in k-th level
          MatrixXd pos_sph_agg1(3,N1);
          MatrixXd pos_sph_agg2(3,N2);

          if(k==1){
            pos_sph_agg1= pos_sph_SAagg[2*L-2];
            pos_sph_agg2= pos_sph_SAagg[2*L-1];
          } else {
            pos_sph_agg1= pos_sph_agg_old[2*L-2];
            pos_sph_agg2= pos_sph_agg_old[2*L-1];
          }

          // random rotation of aggregate 1 around the centroid (origin)
          double theta,phi; // polar angles
          theta= acos(2.0f*urf(re)-1.0f);
          phi= 2*M_PI*urf(re);

          Vector3d nvec; // unit vector of rotation axis
          nvec << sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta); //axis of rotation randomly choosen over unit sphere

          double psi; // angle of rotation
          psi= 2*M_PI*urf(re); // randomly choosen rotation angle

          Matrix3d rot_mat; // 3D rotation matrix
          rot_mat= AngleAxisd(psi,nvec);

          for(int i= 0; i < pos_sph_agg1.cols(); ++i) pos_sph_agg1.col(i)=rot_mat*pos_sph_agg1.col(i); // random rotation of agg1

          double R1_2; // square of gyration radius of agg1
          double R2_2; // square of gyration radius of agg2
          R1_2= a*a + pos_sph_agg1.colwise().squaredNorm().mean();
          R2_2= a*a + pos_sph_agg2.colwise().squaredNorm().mean();

          double rN;
          rN= a*a*(N1+N2)*(N1+N2)/(N1*N2)*pow((N1+N2)/kf,2/Df)-(N1+N2)/N2*R1_2-(N1+N2)/N1*R2_2;
          rN= sqrt(rN);

          bool found {false};

          //--- translation of agg1 to random position on the sphere of radius rN
          for(int iter1= 0; iter1 != iter1max; ++iter1){

              pos_sph_agg1.colwise() -= pos_sph_agg1.rowwise().mean(); // translate the centroid of agg1 to origin

              double phi;
              phi= 2*M_PI*urf(re);
              double u; // uniform random number [-1.0,1.0)
              u= 2*urf(re)-1.0;

              Vector3d cen_pos_agg1; // centroid of agg1
              cen_pos_agg1 << sqrt(1.0-u*u)*cos(phi), sqrt(1.0-u*u)*sin(phi), u;
              cen_pos_agg1 *= rN; // cen_pos_agg1 is set to a uniform random cartesian position (x,y,z) over the spherical surface of radius rN
              pos_sph_agg1.colwise() += cen_pos_agg1;

              double theta;
              theta= acos(2.0*urf(re)-1.0);
              phi= 2*M_PI*urf(re);
              Vector3d nvec; // unit vector of rotation axis
              nvec << sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta); //axis of rotation randomly choosen over unit sphere

              // random rotation of aggregate 2 around the centroid (origin)
              #pragma omp parallel for
              for(int iter2= 0; iter2 < iter2max; ++iter2){
                  bool found_local;
                  #pragma omp atomic read
                  found_local= found;
                  if(found) continue;

                  MatrixXd pos_sph_agg1_thread(3,N1); // thread local copy of pos_sph_agg1
                  MatrixXd pos_sph_agg2_thread(3,N2); // thread local copy of pos_sph_agg2
                  pos_sph_agg1_thread= pos_sph_agg1;
                  pos_sph_agg2_thread= pos_sph_agg2;

                  double psi; // angle of rotation

                  #pragma omp critical
                  {
                     psi= 2*M_PI*urf(re);
                  }

                  Matrix3d rot_mat; // 3D rotation matrix
                  rot_mat= AngleAxisd(psi,nvec);
                  for(int i= 0; i < N2; ++i) pos_sph_agg2_thread.col(i)=rot_mat*pos_sph_agg2_thread.col(i); // random rotation of agg2
                  //  cout << "iter2 pos_sph_agg2 " << endl << pos_sph_agg2 << endl;

                  bool attached {false};
                  for(int i= 0; i < N2; ++i){
                      RowVectorXd dist_pair(N1);
                      dist_pair=(pos_sph_agg1_thread.colwise()-pos_sph_agg2_thread.col(i)).colwise().norm();
                    //  cout << "dist_pair" << endl << dist_pair << endl;
                      double mindist= dist_pair.minCoeff();
                      //cout << "2*a, mindist " << 2*a << ' ' << mindist << endl;
                      bool overrapped {mindist <= 2*a};
                      if(overrapped) {
                        //exit; // 20180813 removed
                        attached= false; // 20180813 added
                        break; // 20180813 added
                      } else {
                        attached= attached || (mindist < 2*a+tol);
                      }
                  }

                  // construction of new aggregate if attached condition is satisfied
                  if(attached){
                      //cout << "attached " << attached << endl;
                      MatrixXd pos_sph_agg_tmp(3,N1+N2);
                      pos_sph_agg_tmp.block(0,0,3,N1)= pos_sph_agg1_thread;
                      pos_sph_agg_tmp.block(0,N1,3,N2)= pos_sph_agg2_thread;
                      pos_sph_agg_tmp.colwise() -= pos_sph_agg_tmp.rowwise().mean(); // translate the centroid of new agg to origin
                      #pragma omp critical
                      {
                          pos_sph_agg.push_back(pos_sph_agg_tmp); // update the aggregate
                          found = true;
                          //break; // exit iter2 loop
                          if(k==levels)  cout << "number of threads for parallel search: " << omp_get_num_threads() << endl;
                      }

                  }
              }// iter2 loop

              if(found) break; // exit iter1 loop

              if(iter1 == iter1max){
                  cout << "calculation failed: iter1 reachs iter1max! " << endl;
                  abort();
              }
          }// iter1 loop
      }// L loop

      pos_sph_agg_old= move(pos_sph_agg);
      pos_sph_agg_output.push_back(pos_sph_agg_old[0]);

  }// k loop

  auto CCA_end_time= chrono::steady_clock::now();

  cout << "CCA computation time: "
  << chrono::duration_cast<chrono::milliseconds>(CCA_end_time - CCA_start_time).count() << " milliseconds" << endl;

  // geometry check for output
  bool non_overlapped_all {true};
  bool attached_at_least_1pair_all {true};

  for(int k= 0; k <= levels; ++k){
      MatrixXd pos_sph_agg_k_out {pos_sph_agg_output[k]};
      int num_sph = num_sph_SA*pow(2,k);
      assert(num_sph == pos_sph_agg_k_out.cols());
      // final check of the attached condition
      bool non_overlapped;
      ArrayXd min_diff_from2a(num_sph);
      for(int i= 0; i< num_sph; ++i){
          ArrayXd distance_from_this_sph(num_sph);
          distance_from_this_sph= (pos_sph_agg_k_out.colwise()-pos_sph_agg_k_out.col(i)).colwise().norm().array();
          non_overlapped= {(distance_from_this_sph-2*a >= 0)(i) == false};
          for(int j= 0; j< num_sph; ++j){
              if(j != i) {
                non_overlapped= non_overlapped && ((distance_from_this_sph-2*a > 0)(j) == true);
              }
          }
         //cout << i << ", " << (distance_from_this_sph-(2*a-buf_factor*tol) >= 0).transpose() << endl ;
          min_diff_from2a(i)=((pos_sph_agg_k_out.colwise()-pos_sph_agg_k_out.col(i)).colwise().norm().array()-2*a).array().abs().minCoeff();
      }
      bool attached_at_least_1pair {min_diff_from2a.maxCoeff() < tol};
      string non_overlapped_test= {non_overlapped ? "Success" : "Failed"};
      string attached_at_least_1pair_test= {attached_at_least_1pair ? "Success" : "Failed"};
      cout << k << "th level, non_overlapped_check         : " << non_overlapped_test << endl;
      cout << k << "th level, attached_at_least_1pair_check: " << attached_at_least_1pair_test << endl;

      non_overlapped_all = non_overlapped_all && non_overlapped;
      attached_at_least_1pair_all = attached_at_least_1pair_all && attached_at_least_1pair;
  }

  if(non_overlapped_all && attached_at_least_1pair_all){
    return pos_sph_agg_output;
  }else{
    cout << "Geometry check failed! Please change tol." << endl;
    exit(1);
  }

}


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

// Copyright (c) 2016-2017 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, 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_GEOGRAPHIC_AREA_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP

#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/formulas/area_formulas.hpp>
#include <boost/geometry/formulas/flattening.hpp>

#include <boost/geometry/strategies/geographic/parameters.hpp>

#include <boost/math/special_functions/atanh.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Geographic area calculation
\ingroup strategies
\details Geographic area calculation by trapezoidal rule plus integral
         approximation that gives the ellipsoidal correction
\tparam PointOfSegment \tparam_segment_point
\tparam FormulaPolicy Formula used to calculate azimuths
\tparam SeriesOrder The order of approximation of the geodesic integral
\tparam Spheroid The spheroid model
\tparam CalculationType \tparam_calculation
\author See
- Danielsen JS, The area under the geodesic. Surv Rev 30(232): 61–66, 1989
- Charles F.F Karney, Algorithms for geodesics, 2011 https://arxiv.org/pdf/1109.4448.pdf

\qbk{
[heading See also]
[link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
}
*/
template
<
    typename PointOfSegment,
    typename FormulaPolicy = strategy::andoyer,
    std::size_t SeriesOrder = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
{
    // Switch between two kinds of approximation(series in eps and n v.s.series in k ^ 2 and e'^2)
    static const bool ExpandEpsN = true;
    // LongSegment Enables special handling of long segments
    static const bool LongSegment = false;

    //Select default types in case they are not set

    typedef typename boost::mpl::if_c
    <
        boost::is_void<CalculationType>::type::value,
        typename select_most_precise
            <
                typename coordinate_type<PointOfSegment>::type,
                double
            >::type,
        CalculationType
    >::type CT;

protected :
    struct spheroid_constants
    {
        Spheroid m_spheroid;
        CT const m_a2;  // squared equatorial radius
        CT const m_e2;  // squared eccentricity
        CT const m_ep2; // squared second eccentricity
        CT const m_ep;  // second eccentricity
        CT const m_c2;  // squared authalic radius

        inline spheroid_constants(Spheroid const& spheroid)
            : m_spheroid(spheroid)
            , m_a2(math::sqr(get_radius<0>(spheroid)))
            , m_e2(formula::flattening<CT>(spheroid)
                 * (CT(2.0) - CT(formula::flattening<CT>(spheroid))))
            , m_ep2(m_e2 / (CT(1.0) - m_e2))
            , m_ep(math::sqrt(m_ep2))
            , m_c2(authalic_radius(spheroid, m_a2, m_e2))
        {}
    };

    static inline CT authalic_radius(Spheroid const& sph, CT const& a2, CT const& e2)
    {
        CT const c0 = 0;

        if (math::equals(e2, c0))
        {
            return a2;
        }

        CT const sqrt_e2 = math::sqrt(e2);
        CT const c2 = 2;

        return (a2 / c2) +
                  ((math::sqr(get_radius<2>(sph)) * boost::math::atanh(sqrt_e2))
                   / (c2 * sqrt_e2));
    }

    struct area_sums
    {
        CT m_excess_sum;
        CT m_correction_sum;

        // Keep track if encircles some pole
        std::size_t m_crosses_prime_meridian;

        inline area_sums()
            : m_excess_sum(0)
            , m_correction_sum(0)
            , m_crosses_prime_meridian(0)
        {}
        inline CT area(spheroid_constants spheroid_const) const
        {
            CT result;

            CT sum = spheroid_const.m_c2 * m_excess_sum
                   + spheroid_const.m_e2 * spheroid_const.m_a2 * m_correction_sum;

            // If encircles some pole
            if (m_crosses_prime_meridian % 2 == 1)
            {
                std::size_t times_crosses_prime_meridian
                        = 1 + (m_crosses_prime_meridian / 2);

                result = CT(2.0)
                         * geometry::math::pi<CT>()
                         * spheroid_const.m_c2
                         * CT(times_crosses_prime_meridian)
                         - geometry::math::abs(sum);

                if (geometry::math::sign<CT>(sum) == 1)
                {
                    result = - result;
                }

            }
            else
            {
                result = sum;
            }

            return result;
        }
    };

public :
    typedef CT return_type;
    typedef PointOfSegment segment_point_type;
    typedef area_sums state_type;

    explicit inline geographic(Spheroid const& spheroid = Spheroid())
        : m_spheroid_constants(spheroid)
    {}

    inline void apply(PointOfSegment const& p1,
                      PointOfSegment const& p2,
                      area_sums& state) const
    {

        if (! geometry::math::equals(get<0>(p1), get<0>(p2)))
        {

            typedef geometry::formula::area_formulas
                <
                    CT, SeriesOrder, ExpandEpsN
                > area_formulas;

            typename area_formulas::return_type_ellipsoidal result =
                     area_formulas::template ellipsoidal<FormulaPolicy::template inverse>
                                             (p1, p2, m_spheroid_constants);

            state.m_excess_sum += result.spherical_term;
            state.m_correction_sum += result.ellipsoidal_term;

            // Keep track whenever a segment crosses the prime meridian
            geometry::formula::area_formulas<CT>
                    ::crosses_prime_meridian(p1, p2, state);
        }
    }

    inline return_type result(area_sums const& state) const
    {
        return state.area(m_spheroid_constants);
    }

private:
    spheroid_constants m_spheroid_constants;

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Point>
struct default_strategy<geographic_tag, Point>
{
    typedef strategy::area::geographic<Point> type;
};

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP


#include <iostream>

#include "sandbox/external/NanoGUI.h"
#include <nanogui/opengl.h>
#include <nanogui/window.h>
#include <nanogui/layout.h>
#include <nanogui/slider.h>
#include <nanogui/combobox.h>
#include <nanogui/textbox.h>
#include <nanogui/glcanvas.h>
#include <nanogui/opengl.h>
#include <nanogui/glutil.h>
#include "sandbox/SceneNode.h"
#include "sandbox/base/RenderCallback.h"
#include "sandbox/base/Transform.h"
#include "sandbox/base/Camera.h"
#include "sandbox/base/NodeRenderer.h"
#include "sandbox/geometry/algorithms/SmoothNormals.h"
#include "sandbox/geometry/shapes/Cylinder.h"
#include "sandbox/geometry/shapes/Grid.h"
#include "sandbox/geometry/Material.h"
#include "sandbox/graphics/MeshRenderer.h"
#include "sandbox/graphics/shaders/MaterialShader.h"
#include "sandbox/graphics/Viewport.h"
#include "sandbox/graphics/Window.h"
#include "sandbox/graphics/RenderState.h"
#include "sandbox/data/KdTree.h"
#include "glm/glm.hpp"
#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/quaternion.hpp>
#include <cstdlib>

#include <iostream>
#include <Eigen/Dense>
#include "src/LeastSquares.h"
#include <algorithm>

using namespace sandbox;

float xFrequency = 1.0;

float function(float x, float y, float t) {
	return 0.5*std::cos(2.0f*3.1415*y)*std::cos(xFrequency*2.0f*3.1415*(x+t));
}

float fx(float x, float y) {
	return 0.5*std::cos(2.0f*3.1415*y)*std::sin(xFrequency*2.0f*3.1415*x)*-xFrequency*2.0f*3.1415;
}

float fy(float x, float y) {
	return 0.5*std::sin(2.0f*3.1415*y)*std::cos(xFrequency*2.0f*3.1415*x)*-xFrequency*3.1415;
}

class TestApp : public NanoguiScreen {
public:

	//TestApp() : NanoguiScreen(Eigen::Vector2i(1180, 980), "Test App") {
	TestApp() : NanoguiScreen(Eigen::Vector2i(1150, 600), "Test App") {
		using namespace nanogui;

		Grid* grid = new Grid(30, 30);
		Grid* estGrid = new Grid(30, 30);

		SceneNode* eventNode = new SceneNode(&scene);
			NanoguiResizeEvent* resize = new NanoguiResizeEvent(this);
			eventNode->addComponent(resize);
			ResizeEventHandler* resizeCallback = (new ResizeCallback())->init(this);
			resize->subscribe(resizeCallback);
			eventNode->addComponent(resizeCallback);
		SceneNode* geometryNode = new SceneNode(&scene);
			SceneNode* arrowNode = new SceneNode(geometryNode);
				arrowNode->addComponent(new Transform(glm::scale(glm::mat4(1.0f),glm::vec3(1.0f/1.25))*glm::translate(glm::mat4(1.0f),glm::vec3(0,2.0,0))));
				SceneNode* arrowTipNode = new SceneNode(arrowNode);
					glm::mat4 tipTrans = glm::translate(glm::mat4(1.0f),glm::vec3(0,0.25,0));
					tipTrans = glm::scale(tipTrans,glm::vec3(0.25f));
					arrowTipNode->addComponent(new Transform(tipTrans));
					arrowTipNode->addComponent(new Cylinder(20, 1.0f, 0.0f));
					arrowTipNode->addComponent(new MeshRenderer());
					Material* material = new Material();
					material->setColor(glm::vec4(1.0f, 0, 0, 1));
					arrowTipNode->addComponent(material);
				SceneNode* cylNode = new SceneNode(arrowNode);
					glm::mat4 cylTrans = glm::translate(glm::mat4(1.0f),glm::vec3(0,-1.0,0));
					cylTrans = glm::scale(cylTrans,glm::vec3(0.1f, 1.0f, 0.1f));
					cylNode->addComponent(new Transform(cylTrans));
					cylNode->addComponent(new Cylinder(20));
					cylNode->addComponent(new MeshRenderer());
					material = new Material();
					material->setColor(glm::vec4(1.0f, 0, 0, 1));
					cylNode->addComponent(material);
			SceneNode* gridNode = new SceneNode(geometryNode);
				gridNode->addComponent(grid);
				gridNode->addComponent(new SmoothNormals());
				gridNode->addComponent(new MeshRenderer());
				gridNode->addComponent(new Material());
			SceneNode* estGridNode = new SceneNode(geometryNode);
				estGridNode->addComponent(estGrid);
				estGridNode->addComponent(new SmoothNormals());
				estGridNode->addComponent(new MeshRenderer());
				estGridNode->addComponent(new Material());
			SceneNode* sampleNode = new SceneNode(geometryNode);
				Mesh* sampleMesh = new Mesh();
				sampleNode->addComponent(sampleMesh);
				sampleNode->addComponent(new MeshRenderer(GL_POINTS));
				Material* sampleMaterial = new Material();
				sampleMaterial->setColor(glm::vec4(1.0f,0,0,1));
				sampleNode->addComponent(sampleMaterial);

		graphicsNode = new SceneNode(&scene);
			graphicsNode->addComponent(new sandbox::Window(eventNode));
			graphicsNode->addComponent((new OpenGLCallback())->init(this));
			SceneNode* functionViewNode = new SceneNode(graphicsNode);
				functionViewNode->addComponent(new PercentViewport(glm::vec4(0.0, 0.0, 0.5, 1.0)));
				functionViewNode->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,2.5,5.0))));
				functionViewNode->addComponent(new Camera());
				functionViewNode->addComponent(new MaterialShader());
				SceneNode* functionNode = new SceneNode(functionViewNode);
					functionNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f*1.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f,3.0f,1.0f*3.0f))));
					functionNode->addComponent(new NodeRenderer(gridNode));
				SceneNode* pointsNode = new SceneNode(functionViewNode);
					pointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f*1.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f,3.0f,1.0f*3.0f))));
					//pointsNode->addComponent(new NodeRenderer(sampleNode));
			SceneNode* flatGraph = new SceneNode(graphicsNode);
				flatGraph->addComponent(new PercentViewport(glm::vec4(0.0, 0.5, 0.5, 0.5)));
				flatGraph->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,0,3))));
				flatGraph->addComponent(new Camera());
				flatGraph->addComponent(new MaterialShader());
				SceneNode* flatPointsNode = new SceneNode(flatGraph);
					flatPointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), 3.1415f*4.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(2.0f,2.0f,0.0f*2.0f))));
			functionViewNode = new SceneNode(graphicsNode);
				//functionViewNode->addComponent(new PercentViewport(glm::vec4(0.5, 0.0, 0.5, 0.5)));
				functionViewNode->addComponent(new PercentViewport(glm::vec4(0.5, 0.0, 0.5, 1.0)));
				functionViewNode->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,2.5,5.0))));
				functionViewNode->addComponent(new Camera());
				functionViewNode->addComponent(new MaterialShader());
				functionNode = new SceneNode(functionViewNode);
					functionNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f))));
					functionNode->addComponent(new NodeRenderer(estGridNode));
				SceneNode* estPointsNode = new SceneNode(functionViewNode);
					estPointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f*1.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f,3.0f,1.0f*3.0f))));
			SceneNode* estFlatGraph = new SceneNode(graphicsNode);
				estFlatGraph->addComponent(new PercentViewport(glm::vec4(0.5, 0.5, 0.5, 0.5)));
				estFlatGraph->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,0,3))));
				estFlatGraph->addComponent(new Camera());
				estFlatGraph->addComponent(new MaterialShader());
				SceneNode* estFlatPointsNode = new SceneNode(estFlatGraph);
					estFlatPointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), 3.1415f*4.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(2.0f,2.0f,0.0f*2.0f))*glm::rotate(glm::mat4(1.0f), 3.1415f*0.0f/2.0f, glm::vec3(0.0f,0.0f,1.0f))));

		float time = 0.25;

		for (int x = 0; x < grid->getWidth(); x++) {
			for (int y = 0; y < grid->getHeight(); y++) {
				grid->getNode(x,y).z = function(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1), time);
			}
		}

		int numSamples = 4000;
		std::vector<glm::vec4> samplePoints;

		/*std::vector<int> xDim;
		for (int f = 0; f < 10; f++) {
			cards.push_back(f);
		}
		std::random_shuffle(cards.begin(), cards.end());
		for (int f = 0; f < 10; f++) {
			std::cout << cards[f] << std::endl;
		}*/

		std::vector<float> xDim;
		std::vector<float> yDim;
		std::vector<float> tDim;

		int numSplits = 10;//grid->getWidth();
		float step = 1.0f/numSplits;

		for (int f = 0; f < numSamples/numSplits; f++) {
			for (int i = 0; i < numSplits; i++) {
				xDim.push_back(step*i + step*float(std::rand())/RAND_MAX);
				yDim.push_back(step*i + step*float(std::rand())/RAND_MAX);
				tDim.push_back(step*i + step*float(std::rand())/RAND_MAX);
			}
			std::random_shuffle(xDim.begin(), xDim.end());
			std::random_shuffle(yDim.begin(), yDim.end());
			std::random_shuffle(tDim.begin(), tDim.end());

			for (int i = 0; i < numSplits; i++) {
				float x = xDim[i];
				float y = yDim[i];
				float t = tDim[i];
				float z = function(x,y,t);
				samplePoints.push_back(glm::vec4(x,y,t,z));
			}
			/* random sampling
			float x = float(std::rand())/RAND_MAX;
			float y = float(std::rand())/RAND_MAX;
			float t = float(std::rand())/RAND_MAX;
			float z = function(x,y,t);
			samplePoints.push_back(glm::vec4(x,y,t,z));*/
		}

		for (int iteration = 0; iteration < 1; iteration++) {

			PointCollection pc(samplePoints);

			for (int x = 0; x < grid->getWidth(); x++) {
				for (int y = 0; y < grid->getHeight(); y++) {
					//estGrid->getNode(x,y).z = function(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1));
					float residual;
					estGrid->getNode(x,y).z = calculateFromSamples(pc, glm::vec3(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1), time), &residual);
					estGrid->getCoord(x,y).x = residual;
				}
			}

			glm::vec2 minMax;
			for (int f = 0; f < pc.getPoints().size(); f++) {
				if (f == 0) {
					minMax = glm::vec2(pc.getEstResiduals()[f]);
				}
				else {
					minMax.x = minMax.x < pc.getEstResiduals()[f] ? minMax.x : pc.getEstResiduals()[f];
					minMax.y = minMax.y > pc.getEstResiduals()[f] ? minMax.y : pc.getEstResiduals()[f];
				}

			}
			std::cout << minMax.x << ", "<< minMax.y << std::endl;

			for (int x = 0; x < grid->getWidth(); x++) {
				for (int y = 0; y < grid->getHeight(); y++) {
					estGrid->getCoord(x,y).x = (estGrid->getCoord(x,y).x - minMax.x)/(minMax.y-minMax.x);
				}
			}

			std::vector<float> splits;
			std::vector<int> splitSizes;
			for (int f = 0; f < numSplits*numSplits*numSplits; f++) {
				splits.push_back(0.0f);
				splitSizes.push_back(0);
			}

			for (int f = 0; f < pc.getPoints().size(); f++) {
				glm::vec4 splitIndex = pc.getActualPoints()[f];
				splitIndex = splitIndex / step;
				int index = (int(splitIndex.x)*numSplits+int(splitIndex.y))*numSplits+int(splitIndex.z);
				splits[index] += pc.getEstResiduals()[f];
				splitSizes[index]++;
			}

			float totalResidual = 0.0f;
			for (int f = 0; f < splits.size(); f++) {
				if (splitSizes[f] > 0) {
					splits[f] = splits[f]/splitSizes[f];
				}
				totalResidual += splits[f];
			}

			int numS = 0;
			for (int f = 0; f < splits.size(); f++) {
				splits[f] /= totalResidual;
				//std::cout << splits[f]*numSamples << std::endl;
				numS += std::round(splits[f]*numSamples);
			}

			//std::cout << numS << std::endl;

			/*for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {

					}
				}
			}*/

			for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {
						int index = (x*numSplits+y)*numSplits+t;
						for (int f = 0; f < std::round(1.0f*splits[index]*numSamples); f++) {
							float x2 = 1.0f*step*x + step*float(std::rand())/RAND_MAX;
							float y2 = 1.0f*step*y + step*float(std::rand())/RAND_MAX;
							float t2 = 1.0f*step*t + step*float(std::rand())/RAND_MAX;
							float z2 = function(x2,y2,t2);
							//std::cout << samplePoints.size() << " " << x << " " << x2 << " " << y2 << " " << t2 << " " << z2 << " " << splits[index] << " " << step << std::endl;
							samplePoints.push_back(glm::vec4(x2,y2,t2,z2));
						}
					}
				}
			}

			// vector estimate
			/*std::vector<glm::vec4> newPoints;
			for (int f = 0; f < pc.getPoints().size(); f++) {
				float randPercent = 2.0f*(float(std::rand())/RAND_MAX - 0.5);
				glm::vec4 newPoint = pc.getActualPoints()[f] + glm::vec4(pc.getEstGradients()[f]*randPercent,0.0);
				newPoint.w = function(newPoint.x, newPoint.y, newPoint.z);
				newPoints.push_back(newPoint);
			}

			for (int f = 0; f < newPoints.size(); f++) {
				samplePoints.push_back(newPoints[f]);
			}*/

			/*for (int f = 0; f < numSamples; f++) {
				float x = float(std::rand())/RAND_MAX;
				float y = float(std::rand())/RAND_MAX;
				float t = float(std::rand())/RAND_MAX;
				float z = function(x,y,t);
				samplePoints.push_back(glm::vec4(x,y,t,z));
			}*/

			/*float totalResidual = 0.0f;
			float dr = 1.0f/pc.getPoints().size();
			for (int f = 0; f < pc.getPoints().size(); f++) {
				totalResidual += (pc.getEstResiduals()[f]-minMax.x)/(pc.getEstResiduals()[f]-minMax.y - pc.getEstResiduals()[f]-minMax.x);
			}*/

			/*splits.clear();
			totalResidual = 0.0f;
			for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {
						float newX = 1.0f*step*x + 0.5*step;
						float newY = 1.0f*step*y + 0.5*step;
						float newT = 1.0f*step*t + 0.5*step;
						//estGrid->getNode(x,y).z = function(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1));

						int numNearest = 1;
						std::vector<float> point;
						point.push_back(newX);
						point.push_back(newY);
						point.push_back(newT);
						std::vector<KdTree<float>::KdValue> nearest = pc.getKdTree().getNearestSorted(point, numNearest);

						float residual = pc.getEstResiduals()[nearest[0].index];
						//std::cout << newX << " " << newY << " " << newT << " " << pc.getEstResiduals()[nearest[0].index] << std::endl;
						splits.push_back(residual);
						totalResidual += residual;
					}
				}
			}

			for (int f = 0; f < splits.size(); f++) {
				splits[f] /= totalResidual;
			}

			int splitIndex = 0;
			for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {
						for (int f = 0; f < int(splits[splitIndex]*numSamples); f++) {
							float newX = 1.0f*step*x + step*float(std::rand())/RAND_MAX;
							float newY = 1.0f*step*y + step*float(std::rand())/RAND_MAX;
							float newT = 1.0f*step*t + step*float(std::rand())/RAND_MAX;
							float newZ = function(newX, newY, newT);
							samplePoints.push_back(glm::vec4(newX, newY, newT, newZ));
						}
						splitIndex++;
					}
				}
			}

			/*for (int f = 0; f < numSamples; f++) {
				float x = float(std::rand())/RAND_MAX;
				float y = float(std::rand())/RAND_MAX;
				float t = float(std::rand())/RAND_MAX;
				float z = function(x,y,t);
				samplePoints.push_back(glm::vec4(x,y,t,z));
			}*/
			std::cout << samplePoints.size() << std::endl;
		}

    	resizeEvent(Eigen::Vector2i(width(), height()));
	}

	class PointCollection : public KdSearchable<float> {
	public:

		class ObjectiveFunction {
		public:
			virtual ~ObjectiveFunction() {}
			virtual float getValue(int sampleNum, const glm::vec4& point) const {
				return point.w;
			}
		};

		class GradObjectiveFunction : public ObjectiveFunction {
		public:
			GradObjectiveFunction(int dim, PointCollection* pc) : dim(dim), pc(pc) {}
			virtual ~GradObjectiveFunction() {}
			virtual float getValue(int sampleNum, const glm::vec4& point) const {
				return pc->estGradients[sampleNum][dim];
			}
		private:
			int dim;
			PointCollection* pc;
		};

		const glm::vec3 calculateGradient(int numNearest, glm::vec4 estPoint, float value, bool pointIsSample, const ObjectiveFunction* objFunction, float* residual = NULL) const {
			int startNearest = pointIsSample ? 1 : 0;

			Eigen::MatrixXf A = Eigen::MatrixXf(numNearest-startNearest, 3);
			Eigen::VectorXf b = Eigen::VectorXf(numNearest-startNearest);

			std::vector<float> point;
			point.push_back(estPoint[0]);
			point.push_back(estPoint[1]);
			point.push_back(estPoint[2]);
			//point.push_back(estPoint[3]);

			std::vector<KdTree<float>::KdValue> nearest = kdTree->getNearestSorted(point, numNearest);

			for (int f = startNearest; f < nearest.size(); f++) {
				glm::vec3 diff = glm::vec3(points[nearest[f].index]-estPoint);
				glm::vec3 dir = normalize(diff);
				float dirDeriv = (objFunction->getValue(nearest[f].index, points[nearest[f].index])-value)/(glm::length(diff));

				Eigen::VectorXf d(3);
				d[0] = dir[0];
				d[1] = dir[1];
				d[2] = dir[2];

				b[f-startNearest] = dirDeriv;
				A.block(f-startNearest, 0, 1, 3) = d.transpose();
			}

			Eigen::VectorXf sol = calculateLeastSquares(A,b);
			if (residual) {
				*residual = (A * sol - b).norm();
			}

			return glm::vec3(sol[0], sol[1], sol[2]);
		}

		PointCollection(const std::vector<glm::vec4>& points) : points(points) {
			for (int f = 0; f < points.size(); f++) {
				indices.push_back(f);
			}

			std::vector<unsigned int> dimensions;
			dimensions.push_back(0);
			dimensions.push_back(1);
			dimensions.push_back(2);
			//dimensions.push_back(3);
			kdTree = new KdTree<float>(dimensions, *this, new EuclideanDistance<float>());

			int numNearest = 11;
//			Eigen::MatrixXf A = Eigen::MatrixXf(numNearest-1, 3);
//			Eigen::VectorXf b = Eigen::VectorXf(numNearest-1);

			ObjectiveFunction fun;

			for (int sampleNum = 0; sampleNum < points.size(); sampleNum++) {
				float residual;
				glm::vec3 gradient = calculateGradient(numNearest, points[sampleNum], points[sampleNum].w, true, &fun, &residual);
				estGradients.push_back(gradient);
				estResiduals.push_back(residual);

				/*std::vector<float> point;
				point.push_back(points[sampleNum][0]);
				point.push_back(points[sampleNum][1]);
				point.push_back(points[sampleNum][2]);
				point.push_back(points[sampleNum][3]);
				std::vector<KdTree<float>::KdValue> nearest = kdTree->getNearestSorted(point, numNearest);

				for (int f = 1; f < nearest.size(); f++) {
					glm::vec4 diff = points[nearest[f].index]-points[sampleNum];
					glm::vec3 dir = normalize(glm::vec3(diff));
					float dirDeriv = diff.w/(glm::length(glm::vec3(diff)));

					Eigen::VectorXf d(3);
					d[0] = dir[0];
					d[1] = dir[1];
					d[2] = dir[2];

					b[f-1] = dirDeriv;
					A.block(f-1, 0, 1, 3) = d.transpose();
				}

				Eigen::VectorXf sol = calculateLeastSquares(A,b);
				float residual = (A * sol - b).norm();
				estGradients.push_back(glm::vec3(sol[0], sol[1], sol[2]));
				estResiduals.push_back(residual);*/
			}

			// calculate Hessians
			std::vector<GradObjectiveFunction> dimFunctions;
			dimFunctions.push_back(GradObjectiveFunction(0, this));
			dimFunctions.push_back(GradObjectiveFunction(1, this));
			dimFunctions.push_back(GradObjectiveFunction(2, this));
			for (int sampleNum = 0; sampleNum < points.size(); sampleNum++) {
				glm::mat3 hessian(1.0f);
				for (int f = 0; f < 3; f++) {
					glm::vec3 gradient = calculateGradient(numNearest, points[sampleNum], estGradients[sampleNum][f], true, &(dimFunctions[f]));
					hessian = glm::row(hessian, f, gradient);
					//std::cout << "Grad: " << f << " "  << gradient[0] << " " << gradient[1] << " " << gradient[2] << std::endl;
					//std::cout << "Hessian: " << f << " "  << glm::row(hessian, f)[0] << " " << glm::row(hessian, f)[1] << " " << glm::row(hessian, f)[2] << std::endl;
				}

				estHessians.push_back(hessian);
			}

			dimensions.clear();
			dimensions.push_back(0);
			dimensions.push_back(1);
			dimensions.push_back(2);
			queryKdTree = new KdTree<float>(dimensions, *this, new EuclideanDistance<float>());

		}
		~PointCollection() {
			delete kdTree;
			delete queryKdTree;
		}

		const std::vector<unsigned int>& getPoints() const { return indices; }
		float getDimension(unsigned int index, unsigned int dimension) const { return points[index][dimension]; }
		const std::vector<glm::vec4>& getActualPoints() const { return points; }
		const std::vector<glm::vec3>& getEstGradients() const { return estGradients; }
		const std::vector<float>& getEstResiduals() const { return estResiduals; }
		const std::vector<glm::mat3>& getEstHessians() const { return estHessians; }
		const KdTree<float>& getKdTree() const { return *queryKdTree; }
	private:
		std::vector<glm::vec4> points;
		std::vector<unsigned int> indices;
		std::vector<glm::vec3> estGradients;
		std::vector<float> estResiduals;
		std::vector<glm::mat3> estHessians;
		KdTree<float>* kdTree;
		KdTree<float>* queryKdTree;
	};

	float calculateFromSamples(const PointCollection& pc, const glm::vec3& pos, float* residual) {
		//return 0.0f;
				/*if (true) {//recalculateZ_FirstHalf) { // nearest directional deriv
					numAlgorithms++;
					glm::vec3 diff = samplePoints[sampleNum]-samplePoints[nearest[0].index];
					glm::vec2 dir = normalize(glm::vec2(diff));
					float dirDeriv = glm::dot(estGradients[nearest[0].index],dir);
					zEstimate = samplePoints[nearest[0].index].z + dirDeriv*glm::length(glm::vec2(diff));

					finalEstimate += zEstimate;
				}*/
		int numNearest = 10;

		std::vector<float> point;
		point.push_back(pos[0]);
		point.push_back(pos[1]);
		point.push_back(pos[2]);

		std::vector<KdTree<float>::KdValue> nearest = pc.getKdTree().getNearestSorted(point, numNearest);

		glm::vec3 diff = pos-glm::vec3(pc.getActualPoints()[nearest[0].index]);
		glm::vec3 dir = normalize(diff);
		float dirDeriv = glm::dot(pc.getEstGradients()[nearest[0].index],dir);

		*residual = pc.getEstResiduals()[nearest[0].index];

		//return pc.getActualPoints()[nearest[0].index].w + dirDeriv*glm::length(diff);

		glm::vec3 gradient = pc.getEstGradients()[nearest[0].index];
		glm::vec4 newPoint = pc.getActualPoints()[nearest[0].index];

		float integrationSteps = 1.0f;
		glm::vec3 intDiff = diff/integrationSteps;
		PointCollection::ObjectiveFunction fun;

		for (int f = 0; f < integrationSteps; f++) {
			//float hessCalc =  0.5*glm::dot(diff, pc.getEstHessians()[nearest[0].index]*diff)
			newPoint += glm::vec4(intDiff, glm::dot(gradient, intDiff));

			gradient = pc.calculateGradient(5, newPoint, newPoint.w, false, &fun);
		}

		//return newPoint.w;

		//return pc.getActualPoints()[nearest[0].index].w;

		return pc.getActualPoints()[nearest[0].index].w + glm::dot(pc.getEstGradients()[nearest[0].index],diff);

		//return f(x+dx) ~= f(x) + df(x)*dx + (1/2)*dx*H(x)*dx
		return pc.getActualPoints()[nearest[0].index].w + glm::dot(pc.getEstGradients()[nearest[0].index],diff)
		 + 0.5*glm::dot(diff, pc.getEstHessians()[nearest[0].index]*diff);

		// mean gradient
		float value = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			diff = pos-glm::vec3(pc.getActualPoints()[nearest[f].index]);
			value += (pc.getActualPoints()[nearest[f].index].w + glm::dot(pc.getEstGradients()[nearest[f].index],diff));
		}
		return value/nearest.size();

		// inverse weighted
		float totalWeight = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			totalWeight += 1.0f/nearest[f].distance;
		}

		// inverse weighted gradient
		value = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			diff = pos-glm::vec3(pc.getActualPoints()[nearest[f].index]);
			value += (pc.getActualPoints()[nearest[f].index].w + glm::dot(pc.getEstGradients()[nearest[f].index],diff)) * ((1.0f/nearest[f].distance)/totalWeight);
		}
		return value;

		// inverse weighted distance
		value = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			value += pc.getActualPoints()[nearest[f].index].w * ((1.0f/nearest[f].distance)/totalWeight);
		}
		return value;
	}

	glm::quat rotationBetweenVectors(glm::vec3 start, glm::vec3 dest){
		start = normalize(start);
		dest = normalize(dest);

		float cosTheta = dot(start, dest);
		glm::vec3 rotationAxis;

		if (cosTheta < -1 + 0.001f){
			// special case when vectors in opposite directions:
			// there is no "ideal" rotation axis
			// So guess one; any will do as long as it's perpendicular to start
			rotationAxis = glm::cross(glm::vec3(0.0f, 0.0f, 1.0f), start);
			if (glm::length(rotationAxis) < 0.01 ) // bad luck, they were parallel, try again!
				rotationAxis = glm::cross(glm::vec3(1.0f, 0.0f, 0.0f), start);

			rotationAxis = glm::normalize(rotationAxis);
			return glm::angleAxis(glm::radians(180.0f), rotationAxis);
		}

		rotationAxis = glm::cross(start, dest);

		float s = sqrt( (1+cosTheta)*2 );
		float invs = 1 / s;

		return glm::quat(
			s * 0.5f,
			rotationAxis.x * invs,
			rotationAxis.y * invs,
			rotationAxis.z * invs
		);

	}

	~TestApp() {
	}

	void drawContents() {
		scene.updateModel();
		scene.updateSharedContext(context);
		scene.updateContext(context);
		graphicsNode->render(context);
	}

private:

	class ResizeCallback : public StateEventHandlerCallback<ResizeState, TestApp> {
		void onEvent(const ResizeState& state, TestApp* app) {
			std::cout << state.width << " " << state.height << " " << app << std::endl;
		}
	};

	class OpenGLCallback : public RenderCallback<TestApp> {
		void renderCallback(const SceneContext& sceneContext, TestApp* app) {
			//std::cout << "Clear screen" << std::endl;
			glClearColor(0.75,0.75,0.75,1);
            //glClearDepth(1.0f);
			glPointSize(10);
			glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
			glEnable(GL_DEPTH_TEST);
            glDepthFunc(GL_LESS);
            //glDepthFunc(GL_LEQUAL);
            glPatchParameteri(GL_PATCH_VERTICES, 3);
            glEnable(GL_CULL_FACE);
            glDisable(GL_BLEND);
            glCullFace(GL_BACK);
            glDisable(GL_CULL_FACE);
		}
	};

	class TestCallback : public RenderCallback<TestApp> {
	public:
		TestCallback(std::string name) : name(name) {}
		void renderCallback(const SceneContext& sceneContext, TestApp* app) {
			RenderState& renderState = RenderState::get(sceneContext);
			glm::mat4 model = renderState.getModelMatrix().get();
			glm::vec3 v(1.0);
			//glm::vec3 v(0.0f);
			v = model*glm::vec4(v, 1.0);
			std::cout << name << ": " << v.x << " " << v.y << " " << v.z << std::endl;
		}
		std::string name;
	};

	SceneContext context;
	SceneNode scene;
	SceneNode* graphicsNode;
};

int main(int argc, char**argv) {
	nanogui::init();

	nanogui::Screen* screen = new TestApp();

	screen->performLayout();
	screen->drawAll();
	screen->setVisible(true);

	nanogui::mainloop();
	nanogui::shutdown();

	return 0;
}


#include <iostream>
#include <cassert>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor>>>>
    Graph;
typedef Graph::vertex_descriptor Vertex;

template <typename T>
void print_vec(std::vector<T> &vec, std::ostream &stream)
{
  for (int i = 0; i < int(vec.size()); i++)
  {
    if (i > 0)
    {
      stream << " ";
    }
    stream << vec.at(i);
  }
  stream << "\n";
}

void testcase()
{
  int l, p;
  std::cin >> l >> p;
  assert(l >= 1 && l <= 500 && p >= 1 && p <= l * l);

  std::vector<int> start_pops_by_town(l), end_pops_by_town(l);
  int total_start_pops = 0, total_end_pops = 0;
  for (int i = 0; i < l; i++)
  {
    int g, d;
    std::cin >> g >> d;
    assert(g >= 1 && g <= 1e6 && d >= 1 && d <= 1e6);
    start_pops_by_town.at(i) = g;
    total_start_pops += g;
    end_pops_by_town.at(i) = d;
    total_end_pops += d;
  }
  DEBUG(2, "total_start_pops " << total_start_pops << " total_end_pops " << total_end_pops);

  std::vector<int> starts_by_path(p), ends_by_path(p), min_caps_by_path(p), max_caps_by_path(p);
  for (int i = 0; i < p; i++)
  {
    int f, t, c, C;
    std::cin >> f >> t >> c >> C;
    assert(f >= 0 && f < l && t >= 0 && t < l);
    assert(c >= 0 && c <= C && C <= 1e6);
    starts_by_path.at(i) = f;
    ends_by_path.at(i) = t;
    min_caps_by_path.at(i) = c;
    max_caps_by_path.at(i) = C;
  }

  if (total_end_pops > total_start_pops)
  {
    std::cout << "no\n";
    return;
  }

  int num_nodes = l + p + 2, next_free_node = 0;
  Vertex source = next_free_node++;
  Vertex sink = next_free_node++;
  auto town_at = [l, next_free_node](int i) { assert(i >= 0 && i < l);  return next_free_node + i; };
  next_free_node += l;
  auto path_mid_node_at = [p, next_free_node](int i) { assert(i >= 0 && i < p);  return next_free_node + i; };
  next_free_node += p;
  assert(next_free_node == num_nodes);
  Graph G(num_nodes);

  auto add_edge = [&G](int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  };

  std::vector<int> demands_by_town(l);
  for (int i = 0; i < l; i++)
  {
    demands_by_town.at(i) = end_pops_by_town.at(i) - start_pops_by_town.at(i);
  }
  if (debug_level >= 2)
  {
    std::cerr << "demands_by_town before adjustment ";
    print_vec(demands_by_town, std::cerr);
  }

  for (int i = 0; i < p; i++)
  {
    int s = starts_by_path.at(i), e = ends_by_path.at(i), mid = path_mid_node_at(i), min = min_caps_by_path.at(i), max = max_caps_by_path.at(i);
    demands_by_town.at(s) += min;
    demands_by_town.at(e) -= min;
    add_edge(town_at(s), mid, max - min);
    add_edge(mid, town_at(e), max - min);
  }
  if (debug_level >= 2)
  {
    std::cerr << "demands_by_town after adjustment ";
    print_vec(demands_by_town, std::cerr);
  }

  for (int i = 0; i < l; i++)
  {
    int d = demands_by_town.at(i);
    if (d > 0)
    {
      add_edge(town_at(i), sink, d);
    }
    else if (d < 0)
    {
      add_edge(source, town_at(i), -d);
    }
  }

  int target_flow = 0;
  for (int d : demands_by_town)
  {
    if (d > 0)
    {
      target_flow += d;
    }
  }
  DEBUG(2, "target_flow " << target_flow);

  long flow = boost::push_relabel_max_flow(G, source, sink);
  DEBUG(2, "flow " << flow);
  assert(flow >= 0 && flow <= target_flow);
  std::cout << (flow == target_flow ? "yes\n" : "no\n");
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}