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

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

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

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

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

/** \example amg.cpp
*
*   This tutorial shows the use of algebraic multigrid (AMG) preconditioners.
*   \warning AMG is currently only experimentally available with the OpenCL backend and depends on Boost.uBLAS
*
*   We start with some rather general includes and preprocessor variables:
**/

#ifndef NDEBUG     //without NDEBUG the performance of sparse ublas matrices is poor.
 #define BOOST_UBLAS_NDEBUG
#endif

#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/operation_sparse.hpp>

#define VIENNACL_WITH_UBLAS 1

#include "viennacl/vector.hpp"
#include "viennacl/coordinate_matrix.hpp"
#include "viennacl/compressed_matrix.hpp"
#include "viennacl/linalg/ilu.hpp"
#include "viennacl/linalg/cg.hpp"
#include "viennacl/linalg/bicgstab.hpp"
#include "viennacl/io/matrix_market.hpp"
#include "viennacl/linalg/norm_2.hpp"

/**
* Import the AMG functionality:
**/
#include "viennacl/linalg/amg.hpp"

/**
* Some more includes:
**/
#include <iostream>
#include <vector>
#include <ctime>
#include "vector-io.hpp"

/**
Part 1: Worker routines

*
*
Run the Solver

*   Runs the provided solver specified in the `solver` object with the provided preconditioner `precond`
**/
template<typename MatrixType, typename VectorType, typename SolverTag, typename PrecondTag>
void run_solver(MatrixType const & matrix, VectorType const & rhs, VectorType const & ref_result, SolverTag const & solver, PrecondTag const & precond)
{
  VectorType result(rhs);
  VectorType residual(rhs);

  result = viennacl::linalg::solve(matrix, rhs, solver, precond);
  residual -= viennacl::linalg::prod(matrix, result);
  std::cout << "  > Relative residual: " << viennacl::linalg::norm_2(residual) / viennacl::linalg::norm_2(rhs) << std::endl;
  std::cout << "  > Iterations: " << solver.iters() << std::endl;
  result -= ref_result;
  std::cout << "  > Relative deviation from result: " << viennacl::linalg::norm_2(result) / viennacl::linalg::norm_2(ref_result) << std::endl;
}

/**
Compare AMG preconditioner for uBLAS and ViennaCL types

*
*  The AMG implementations in ViennaCL can be used with uBLAS types as well as ViennaCL types.
*  This function compares the two in terms of execution time.
**/
template<typename ScalarType>
void run_amg(viennacl::linalg::cg_tag & cg_solver,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_vec*/,
             boost::numeric::ublas::vector<ScalarType> & /*ublas_result*/,
             boost::numeric::ublas::compressed_matrix<ScalarType> & ublas_matrix,
             viennacl::vector<ScalarType> & vcl_vec,
             viennacl::vector<ScalarType> & vcl_result,
             viennacl::compressed_matrix<ScalarType> & vcl_compressed_matrix,
             std::string info,
             viennacl::linalg::amg_tag & amg_tag)
{

  viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > ublas_amg = viennacl::linalg::amg_precond<boost::numeric::ublas::compressed_matrix<ScalarType> > (ublas_matrix, amg_tag);
  boost::numeric::ublas::vector<ScalarType> avgstencil;
  unsigned int coarselevels = amg_tag.get_coarselevels();

  std::cout << "-- CG with AMG preconditioner, " << info << " --" << std::endl;

  std::cout << " * Setup phase (ublas types)..." << std::endl;

  // Coarse level measure might have been changed during setup. Reload!
  ublas_amg.tag().set_coarselevels(coarselevels);
  ublas_amg.setup();

  std::cout << " * Operator complexity: " << ublas_amg.calc_complexity(avgstencil) << std::endl;

  amg_tag.set_coarselevels(coarselevels);
  viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > vcl_amg = viennacl::linalg::amg_precond<viennacl::compressed_matrix<ScalarType> > (vcl_compressed_matrix, amg_tag);
  std::cout << " * Setup phase (ViennaCL types)..." << std::endl;
  vcl_amg.tag().set_coarselevels(coarselevels);
  vcl_amg.setup();

  std::cout << " * CG solver (ublas types)..." << std::endl;
  //run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, ublas_amg);

  std::cout << " * CG solver (ViennaCL types)..." << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, vcl_amg);

}

/**
*
Part 2: Run Solvers with AMG Preconditioners

*
*  In this
**/
int main()
{
  /**
  * Print some device info at the beginning. If there is more than one OpenCL device available, use the second device.
  **/
  std::cout << std::endl;
  std::cout << "----------------------------------------------" << std::endl;
  std::cout << "               Device Info" << std::endl;
  std::cout << "----------------------------------------------" << std::endl;

#ifdef VIENNACL_WITH_OPENCL
  // Optional: Customize OpenCL backend
  viennacl::ocl::platform pf = viennacl::ocl::get_platforms()[0];
  std::vector<viennacl::ocl::device> const & devices = pf.devices();

  // Optional: Set first device to first context:
  viennacl::ocl::setup_context(0, devices[0]);

  // Optional: Set second device for second context (use the same device for the second context if only one device available):
  if (devices.size() > 1)
    viennacl::ocl::setup_context(1, devices[1]);
  else
    viennacl::ocl::setup_context(1, devices[0]);

  std::cout << viennacl::ocl::current_device().info() << std::endl;
  viennacl::context ctx(viennacl::ocl::get_context(1));
#else
  viennacl::context ctx;
#endif

  typedef float    ScalarType;  // feel free to change this to double if supported by your device

  /**
  * Set up the matrices and vectors for the iterative solvers (cf. iterative.cpp)
  **/
  boost::numeric::ublas::vector<ScalarType> ublas_vec, ublas_result;
  boost::numeric::ublas::compressed_matrix<ScalarType> ublas_matrix;

  // Read matrix
  if (!viennacl::io::read_matrix_market_file(ublas_matrix, "../examples/testdata/mat65k.mtx"))
  {
    std::cout << "Error reading Matrix file" << std::endl;
    return EXIT_FAILURE;
  }

  // Set up rhs and result vector
  if (!readVectorFromFile("../examples/testdata/rhs65025.txt", ublas_vec))
  {
    std::cout << "Error reading RHS file" << std::endl;
    return 0;
  }

  if (!readVectorFromFile("../examples/testdata/result65025.txt", ublas_result))
  {
    std::cout << "Error reading Result file" << std::endl;
    return 0;
  }

  viennacl::vector<ScalarType> vcl_vec(ublas_vec.size(), ctx);
  viennacl::vector<ScalarType> vcl_result(ublas_vec.size(), ctx);
  viennacl::compressed_matrix<ScalarType> vcl_compressed_matrix(ublas_vec.size(), ublas_vec.size(), ctx);

  // Copy to GPU
  viennacl::copy(ublas_matrix, vcl_compressed_matrix);
  viennacl::copy(ublas_vec, vcl_vec);
  viennacl::copy(ublas_result, vcl_result);

  /**
  * Instantiate a tag for the conjugate gradient solver, the AMG preconditioner tag, and create an AMG preconditioner object:
  **/
  viennacl::linalg::cg_tag cg_solver;
  viennacl::linalg::amg_tag amg_tag;

  /**
  * Run solver without preconditioner. This serves as a baseline for comparison.
  * Note that iterative solvers without preconditioner on GPUs can be very efficient because they map well to the massively parallel hardware.
  **/
  std::cout << "-- CG solver (CPU, no preconditioner) --" << std::endl;
  run_solver(ublas_matrix, ublas_vec, ublas_result, cg_solver, viennacl::linalg::no_precond());

  std::cout << "-- CG solver (GPU, no preconditioner) --" << std::endl;
  run_solver(vcl_compressed_matrix, vcl_vec, vcl_result, cg_solver, viennacl::linalg::no_precond());

  /**
  * Generate the setup for an AMG preconditioner of Ruge-Stueben type with direct interpolation (RS+DIRECT) and run the solver:
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS,       // coarsening strategy
                                      VIENNACL_AMG_INTERPOL_DIRECT, // interpolation strategy
                                      0.25, // strength of dependence threshold
                                      0.2,  // interpolation weight
                                      0.67, // jacobi smoother weight
                                      3,    // presmoothing steps
                                      3,    // postsmoothing steps
                                      0);   // number of coarse levels to be used (0: automatically use as many as reasonable)
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of Ruge-Stueben type with classic interpolation (RS+CLASSIC) and run the solver:
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS, VIENNACL_AMG_INTERPOL_CLASSIC, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg ( cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS COARSENING, CLASSIC INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of Ruge-Stueben type with only one pass and direct interpolation (ONEPASS+DIRECT)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_ONEPASS, VIENNACL_AMG_INTERPOL_DIRECT,0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "ONEPASS COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of parallel Ruge-Stueben type with direct interpolation (RS0+DIRECT)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS0, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS0 COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner of parallel Ruge-Stueben type (with communication across domains) and use direct interpolation (RS3+DIRECT)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_RS3, VIENNACL_AMG_INTERPOL_DIRECT, 0.25, 0.2, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "RS3 COARSENING, DIRECT INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner which as aggregation-based (AG)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_AG, 0.08, 0, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, AG INTERPOLATION", amg_tag);

  /**
  * Generate the setup for an AMG preconditioner with smoothed aggregation (SA)
  **/
  amg_tag = viennacl::linalg::amg_tag(VIENNACL_AMG_COARSE_AG, VIENNACL_AMG_INTERPOL_SA, 0.08, 0.67, 0.67, 3, 3, 0);
  run_amg (cg_solver, ublas_vec, ublas_result, ublas_matrix, vcl_vec, vcl_result, vcl_compressed_matrix, "AG COARSENING, SA INTERPOLATION",amg_tag);

  /**
  *  That's it.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


//#define BZ_DISABLE_RESTRICT
#define BZ_ARRAY_2D_NEW_STENCIL_TILING

#include <blitz/array.h>
#include <blitz/timer.h>
#include <blitz/benchext.h>
#include <blitz/vector2.h>

#ifdef BZ_HAVE_STD
  #include <fstream>
#else
  #include <fstream.h>
#endif

BZ_USING_NAMESPACE(blitz)

#if defined(BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES)
 #define echo_f90           echo_f90_
 #define echo_f77           echo_f77_
 #define echo_f90_tuned     echo_f90_tuned_
 #define echo_f77tuned      echo_f77tuned_
#elif defined(BZ_FORTRAN_SYMBOLS_WITH_DOUBLE_TRAILING_UNDERSCORES)
 #define echo_f90           echo_f90__
 #define echo_f77           echo_f77__
 #define echo_f90_tuned     echo_f90_tuned__
 #define echo_f77tuned      echo_f77tuned__
#elif defined(BZ_FORTRAN_SYMBOLS_CAPS)
 #define echo_f90           ECHO_F90
 #define echo_f77           ECHO_F77
 #define echo_f90_tuned     ECHO_F90_TUNED
 #define echo_f77tuned      ECHO_F77TUNED
#endif

extern "C" {
void echo_f90(int& N, int& niters, float& check);
void echo_f77(int& N, int& niters, float& check);
void echo_f90_tuned(int& N, int& niters, float& check);
void echo_f77tuned(int& N, int& niters, float& check);
}

void f77(BenchmarkExt<int>&);
void f90(BenchmarkExt<int>&);
void f77_tuned(BenchmarkExt<int>&);
void f90_tuned(BenchmarkExt<int>&);

void echo_BlitzInterlacedCycled(BenchmarkExt<int>&);
void echo_BlitzCycled(BenchmarkExt<int>&);
void echo_BlitzRaw(BenchmarkExt<int>&);
void echo_BlitzStencil(BenchmarkExt<int>&);

int main()
{
    Timer timer;
    float check;
    int numBenchmarks = 6;
#ifdef FORTRAN_90
    numBenchmarks+=2;
#endif

    BenchmarkExt<int> bench("Acoustic 2D Benchmark", numBenchmarks);
    const int numSizes=7;
    bench.setNumParameters(numSizes);
    Vector<int> parameters(numSizes);
    parameters=10*pow(2.0,tensor::i);
    Vector<double> flops(numSizes);
    flops=(parameters-2)*(parameters-2) * 9.0;
    Vector<long> iters(numSizes);
    // iters must be divisible by 3 for tuned fortran versions
    iters=cast<long>(100000000/flops)*3;

    bench.setParameterVector(parameters);
    bench.setParameterDescription("Matrix size");
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);
    bench.setDependentVariable("flops");
    bench.beginBenchmarking();

    echo_BlitzRaw(bench);
    echo_BlitzStencil(bench);

#if 0
    echo_BlitzInterlaced(bench, c);
#endif

    echo_BlitzCycled(bench);
    echo_BlitzInterlacedCycled(bench);

#ifdef FORTRAN_90
    f90(bench);
    f90_tuned(bench);
#endif

    f77(bench);
    f77_tuned(bench);

    bench.endBenchmarking();
    bench.saveMatlabGraph("acoustic.m");

    return 0;
}

void checkArray(Array<float,2>& A, int N)
{
    float check = 0.0;
    for (int i=0; i < N; ++i)
        for (int j=0; j < N; ++j)
            check += ((i+1)*N + j + 1) * A(i,j);

    cout << "Array check: " << check << endl;
}

void setInitialConditions(Array<float,2>& c, Array<float,2>& P1,
    Array<float,2>& P2, Array<float,2>& P3, int N);

void echo_BlitzRaw(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (raw)");
    while (!bench.doneImplementationBenchmark())
    {
      int N = bench.getParameter();
      int niters = bench.getIterations();

    Array<float,2> P1(N,N), P2(N,N), P3(N,N), c(N,N);
    Range I(1,N-2), J(1,N-2);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J) = (2-4*c(I,J)) * P2(I,J)
          + c(I,J)*(P2(I-1,J) + P2(I+1,J) + P2(I,J-1) + P2(I,J+1))
          - P1(I,J);

        P1 = P2;
        P2 = P3;
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();

#if 0
ofstream ofs("testecho.m");
ofs << "A = [";
for (int i=0; i < N; ++i)
{
  for (int j=0; j < N; ++j)
  {
    ofs << int(8192*P2(i,j)+1024*c(i,j)) << " ";
  }
  if (i < N-1)
    ofs << ";" << endl;
}
ofs << "];" << endl;
#endif

}

void echo_BlitzCycled(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (cycled)");
    while (!bench.doneImplementationBenchmark())
    {
    int N = bench.getParameter();
    int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;

    Array<float,2> P1(N,N), P2(N,N), P3(N,N), c(N,N);
    Range I(1,N-2), J(1,N-2);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J) = (2-4*c(I,J)) * P2(I,J)
          + c(I,J)*(P2(I-1,J) + P2(I+1,J) + P2(I,J-1) + P2(I,J+1))
          - P1(I,J);

        cycleArrays(P1,P2,P3);
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();
}

void echo_BlitzInterlacedCycled(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (interlaced & cycled)");
    while (!bench.doneImplementationBenchmark())
    {
    int N = bench.getParameter();
    int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;

    Array<float,2> P1, P2, P3, c;
    allocateArrays(shape(N,N), P1, P2, P3, c);
    Range I(1,N-2), J(1,N-2);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        P3(I,J) = (2-4*c(I,J)) * P2(I,J)
          + c(I,J)*(P2(I-1,J) + P2(I+1,J) + P2(I,J-1) + P2(I,J+1))
          - P1(I,J);

        cycleArrays(P1,P2,P3);
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();
}

BZ_DECLARE_STENCIL4(acoustic2D,P1,P2,P3,c)
  P3 = 2 * P2 + c * Laplacian2D_stencilop(P2) - P1;
BZ_STENCIL_END

void echo_BlitzStencil(BenchmarkExt<int>&bench)
{
    bench.beginImplementation("Blitz++ (stencil)");
    while (!bench.doneImplementationBenchmark())
    {
    int N = bench.getParameter();
    int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;

    Array<float,2> P1, P2, P3, c;
    allocateArrays(shape(N,N), P1, P2, P3, c);

    setInitialConditions(c, P1, P2, P3, N);
    checkArray(P2, N);
    checkArray(c, N);

    bench.start();
    for (int iter=0; iter < niters; ++iter)
    {
        applyStencil(acoustic2D(), P1, P2, P3, c);
        cycleArrays(P1,P2,P3);
    }
    bench.stop();

    cout << P1(N/2-1,(7*N)/8-1) << endl;
    }

    bench.endImplementation();
}

void setInitialConditions(Array<float,2>& c, Array<float,2>& P1,
    Array<float,2>& P2, Array<float,2>& P3, int N)
{
    // Set the velocity field
    c = 0.2;

    // Solid block with which the pulse collides
    int blockLeft = 0;
    int blockRight = int(2*N/5.0-1);
    int blockTop = int(N/3-1);
    int blockBottom = int(2*N/3.0-1);
    c(Range(blockTop,blockBottom),Range(blockLeft,blockRight)) = 0.5;

    // Channel directing the pulse leftwards
    int channelLeft = int(4*N/5.0-1);
    int channelRight = N-1;
    int channel1Height = int(3*N/8.0-1);
    int channel2Height = int(5*N/8.0-1);
    c(channel1Height,Range(channelLeft,channelRight)) = 0.0;
    c(channel2Height,Range(channelLeft,channelRight)) = 0.0;

    // Initial pressure distribution: gaussian pulse inside the channel
    BZ_USING_NAMESPACE(blitz::tensor)
    int cr = int(N/2-1);
    int cc = int(7.0*N/8.0-1);
    // pow2 is not defined for pod types.
    float s2 = 64.0 * 9.0 / pow(N/2.0,2);
    cout << "cr = " << cr << " cc = " << cc << " s2 = " << s2 << endl;
    P1 = 0.0;
    P2 = exp(-(pow2(i-cr)+pow2(j-cc)) * s2);
    P3 = 0.0;
}

void f77(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran77");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f77(N, niters, check);
  bench.stop();
    cout << check << endl;
    }
    bench.endImplementation();
};

void f77_tuned(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran77 (tuned)");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f77tuned(N, niters, check);
  bench.stop();
    cout << check << endl;
    }

    bench.endImplementation();
};

void f90(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran90");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f90(N, niters, check);
  bench.stop();
    cout << check << endl;
    }

    bench.endImplementation();
};
void f90_tuned(BenchmarkExt<int>&bench)
{
  bench.beginImplementation("Fortran90 (tuned)");
    while (!bench.doneImplementationBenchmark())
    {
  int N = bench.getParameter();
  int niters = bench.getIterations();
    cout << bench.currentImplementation() << " N=" << N << endl;
  float check;
  bench.start();
  echo_f90_tuned(N, niters, check);
  bench.stop();
    cout << check << endl;
    }

    bench.endImplementation();
};


#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/filters/filter.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <vector>
#include <fstream>

using Point = pcl::PointXYZI;
using PointCloud = pcl::PointCloud<Point>;

void ros_callback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    PointCloud::Ptr pc(new PointCloud);
    std::vector<int> indices;
    pcl::KdTreeFLANN<Point> kdtree; // 用于搜索最近点

    // sensor_msgs::PointCloud2 -> pcl::PointCloud
    pcl::fromROSMsg(*msg, *pc);

    // is_dense 用于表示点云中的所有数据是否合法
    // 设置为 否，让后续的函数再检查一遍，把 Nan（not a number）
    // 这种不合法的数值全部去掉
    pc->is_dense = false;
    pcl::removeNaNFromPointCloud(*pc, *pc, indices);

    // 初始化 kd 树
    kdtree.setInputCloud(pc);

    // 提前将需要在循环中用到的变量初始化好，放置在循环中重复构造变量与析构，拖慢程序运行速度
    const int k = 20;                           // 临近点数量，根据作业要求设置为 20
    std::vector<int> point_idx(k);              // 用来保存临近点再原来点云中的下标
    std::vector<float> point_sq_dis(k);         // 用来保存临近点到目标点距离的平方
    std::vector<float> features(6);             // 用来保存六种点云特征
    std::vector<float> e(3);                    // 用来保存 k+1 个点经过 PCA 分析后得到的三个特征值计算得到的 e，从大到小排序
    std::ofstream file;                         // 输出计算结果的目标文件
    Eigen::Matrix<float, 3, 21> nearest_points; // 3x(k+1) 维的矩阵，用来保存点云中的点
    Eigen::Matrix3f covariance;                 // 用来保存协方差矩阵
    Eigen::Vector3f m, eigen_value;             // m 为 k+1 个点的质心，eigen_value 用来保存计算好的计算好的特征值

    // 打开文件，没有就凭空创建一个，如果有就删掉里面的内容，再写入新的
    // 一般不会出错
    file.open("wcm.txt");

    // pcl::PointCloud 中保存点的对象，我们用引用单独给他拿出来
    // 方便后续写代码
    auto& points = pc->points;
    for (size_t i = 0; i < pc->size(); i++)
    {
        // 每隔五个点计算一次特征值，作业没有要求这么做
        // 只是想这么做，希望能快点
        if(i%5 != 0) continue;

        // 重置 m，因为 m 需要累加，而其他的变量只需要赋值
        m = m.Zero();

        // 搜索目标点最近的几个点
        // https://pointclouds.org/documentation/classpcl_1_1_organized_neighbor_search.html#a3c18f38a4aad5fe6c05179906faf14cb
        kdtree.nearestKSearch(points[i], k, point_idx, point_sq_dis);

        // 累加搜索后的数据
        for (size_t j = 0; j < k; j++)
        {
            // 矩阵的块操作，将每个点作为列向量存入 nearest_points
            // http://eigen.tuxfamily.org/dox/group__TutorialBlockOperations.html
            nearest_points.col(j) << points[point_idx[j]].x, points[point_idx[j]].y, points[point_idx[j]].z;
            m[0] += points[point_idx[j]].x;
            m[1] += points[point_idx[j]].y;
            m[2] += points[point_idx[j]].z;
        }
        nearest_points.col(k) << points[i].x, points[i].y, points[i].z;
        m[0] += points[i].x;
        m[1] += points[i].y;
        m[2] += points[i].z;

        // 矩阵的广播操作，将每一列减去 k+1 个点的质心
        // http://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
        nearest_points.colwise() -= (m/(k+1));

        // 计算协方差矩阵
        covariance = nearest_points * nearest_points.transpose();

        // 对称矩阵求特征值
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(covariance);
        eigen_value = solver.eigenvalues();

        // 矩阵的 reduction 操作，计算矩阵所有元素的和
        // http://eigen.tuxfamily.org/dox/group__TutorialReductionsVisitorsBroadcasting.html
        e[0] = eigen_value[2]/eigen_value.sum();
        e[1] = eigen_value[1]/eigen_value.sum();
        e[2] = eigen_value[0]/eigen_value.sum();

        // 计算点云特征
        features[0] = (e[0]-e[1])/e[0];
        features[1] = (e[1]-e[2])/e[0];
        features[2] = e[2]/e[0];
        features[3] = std::cbrt(std::accumulate(e.begin(), e.end(), 0))*3.0f;
        features[4] = -e[0] * std::log(e[0]) - e[1] * std::log(e[1]) - e[2] * std::log(e[2]);
        features[5] = 3.0f*e[2];

        // 将结果写入文件，空格分开，最有追加一个换行
        // 这种特殊的换行有清空缓冲区的效果
        for(auto& num : features)
            file << num << " ";
        file << std::endl;
    }
    // 关闭文件
    file.close();

    std::cout << "cal done" << std::endl;

    // 关闭节点
    // 下面是官方描述
    // Disconnects everything and unregisters from the master.
    // It is generally not necessary to call this function,
    // as the node will automatically shutdown when all NodeHandles destruct.
    // However, if you want to break out of a spin() loop explicitly, this function allows that.
    ros::shutdown();
    return;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "aiimooc_wcm_node");
    ros::NodeHandle nh("~");

    auto sub_rslidar = nh.subscribe<sensor_msgs::PointCloud2>("/rslidar_points", 2, ros_callback);

    ros::spin();
    return 0;
}


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

#include "Pars.h"

#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/hermite.hpp>
#include <boost/math/special_functions/laguerre.hpp>
#include <boost/math/special_functions/factorials.hpp>

using namespace cppqedutils;
using namespace boost::math;

namespace quantumdata {

ParsFunctionScan::ParsFunctionScan(parameters::Table& p, const std::string& mod)
  : fLimitXUL(p.addTitle("Distribution function scan",mod).add("fLimitXUL","",2.)),
    fLimitYUL(p.add("fLimitYUL","",2.)),
    fLimitXL(p.add("fLimitXL","",-2.)),
    fLimitXU(p.add("fLimitXU","",2.)),
    fLimitYL(p.add("fLimitYL","",-2.)),
    fLimitYU(p.add("fLimitYU","",2.)),
    fStep(p.add("fStep","",.1)),
    fCutoff(p.add("fCutoff","",100))
{}

namespace {

const WignerFunctionKernelOld::Hermites fillWithHermite(size_t dim, double x)
{
  WignerFunctionKernelOld::Hermites res(2*dim-1);
  res(0)=hermite(0,x); res(1)=hermite(1,x);
  for (unsigned l=1; l<res.size()-1; ++l)
    res(l+1)=hermite_next(l,x,res(l),res(l-1));
  return res;
}

}

double details::w(size_t n, double r, size_t k)
{
  const double sqrR=sqr(r);
  return minusOneToThePowerOf(n)/PI*sqrt(factorial<double>(n)/factorial<double>(n+k))*exp(-2*sqrR)*pow(2*r,k)*laguerre(n,k,4*sqrR);
}

WignerFunctionKernelOld::WignerFunctionKernelOld(double x, double y, size_t dim)
  : hermite_m2x_(fillWithHermite(dim,-2*x)), hermite_2y_(fillWithHermite(dim,2*y))
{}

dcomp WignerFunctionKernelOld::operator()(size_t m, size_t n) const
{
  dcomp res(0);

  for (size_t u=0; u<=m; ++u) for (size_t v=0; v<=n; ++v)
    res+=
      binomial_coefficient<double>(m,u)*
      binomial_coefficient<double>(n,v)*
      minusOneToThePowerOf(v)*
      pow(DCOMP_I,u+v)*
      hermite_m2x_(    u+v)*
      hermite_2y_ (n+m-u-v);

  return res;

}

} // quantumdata


//  justanhduc
// Oct 2020

#include <Eigen/Core>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>

#include "TSDFVolumeCPU.hpp"
#include "Raycaster.hpp"
#include "RenderUtilities.hpp"

#define MAX_PATH 260  // for linux

int main() {
    using namespace Eigen;

    const int dim_x = 101, dim_y = 139, dim_z = 106;
    const int n_max_frame = 1;
    auto *tsdf = new float[dim_x * dim_y * dim_z * n_max_frame];
    char stmp[MAX_PATH];
    float ftemp[8];
    for (int i = 0; i < n_max_frame; i++) {
        sprintf(stmp, "../volume/_tsdf_multi_%03d.bin", i);  // for linux
        FILE *fp = nullptr;
        fp = fopen(stmp, "rb");  // for linux
        if (fp == 0) {
            printf("Cannot read file %s\n", stmp);
            exit(-1);
        }
        fread(ftemp, sizeof(float), 8, fp);
        fread(tsdf + (dim_x * dim_y * dim_z) * i, dim_x * dim_y * dim_z, sizeof(float), fp);
        fclose(fp);
    }

    TSDFVolumeCPU volume(dim_x, dim_y, dim_z, dim_x * .01, dim_y * .01, dim_z * .01);
    volume.set_distance_data(tsdf);
    volume.set_truncation_distance(.09);
    std::cout << "Read file. Rendering." << std::endl;

    uint16_t width = 1024;
    uint16_t height = 768;

    Eigen::Matrix<float, 3, Eigen::Dynamic> vertices;
    Eigen::Matrix<float, 3, Eigen::Dynamic> normals;

    Vector3f light_source{0, 3, 3};
    auto eye = Vector3f{0.5, 0, 2};  // view point
    Camera cam((float) width / 2, (float) height / 2, (float) (width - 1) / 2,
               (float) (height - 1) / 2);
    cam.move_to(eye);
    cam.look_at(0.5, 0.5, 0.5);

    Raycaster r{width, height};
    r.raycast(volume, cam, vertices, normals);
    uint8_t *scene = render_scene(width, height, vertices, normals, cam, light_source);

    cv::Mat frame(height, width, 0, scene);
    cv::imshow("Frame", frame);
    cv::waitKey(0);
    return 0;
}


/*
MIT License

Copyright (c) 2020 Rik Baehnemann, ASL, ETH Zurich, Switzerland

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/

#include "fm_trajectories/circle_trajectory.h"

#include <algorithm>
#include <cmath>
#include <limits>

#include <angles/angles.h>
#include <mav_trajectory_generation/motion_defines.h>
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <Eigen/Core>

namespace fm_trajectories {

namespace mtg = mav_trajectory_generation;

CircleTrajectory::Settings::Settings(
    double center_east, double center_north, double radius,
    double start_heading_deg, double arc_length_deg_in, double altitude,
    double velocity_in, double offset_heading_deg,
    const std::weak_ptr<BaseTrajectory>& prev_trajectory,
    const mtg::InputConstraints& input_constraints,
    double circle_deviation_ratio /* = 0.01 */)
    : BaseTrajectory::Settings(input_constraints, velocity_in,
                               offset_heading_deg, prev_trajectory),
      center_east(center_east),
      center_north(center_north),
      radius(std::max(std::fabs(radius), kZeroGuard)),
      start_heading_deg(start_heading_deg),
      arc_length_deg(std::max(std::fabs(arc_length_deg_in), kMinArcLength)),
      altitude(altitude),
      circle_deviation_ratio(
          std::max(std::min(std::fabs(circle_deviation_ratio), 0.999), 0.001)) {
  // Arc feasibility.
  ROS_WARN_COND(arc_length_deg_in < 0.0,
                "Input arc length negative. Inverted.");
  if (std::fabs(arc_length_deg_in) < kMinArcLength)
    ROS_WARN_STREAM("Arc length too small: " << arc_length_deg_in
                                             << " degrees. Clipped to: "
                                             << arc_length_deg << " degrees.");

  // Yaw rate feasibility.
  double omega_z_max = std::numeric_limits<double>::max();
  if (input_constraints.getConstraint(
          mav_trajectory_generation::InputConstraintType::kOmegaZMax,
          &omega_z_max)) {
    if (computeAngularVelocity() > omega_z_max) {
      double new_velocity =
          std::copysign(omega_z_max * radius - kVelocityResolution, velocity);
      ROS_WARN_STREAM("Trajectory yaw rate too large: "
                      << computeAngularVelocity()
                      << " rad/s. Clipping velocity to: " << new_velocity
                      << " m/s");
      velocity = new_velocity;
    }
  }
}

double CircleTrajectory::Settings::computeAngularVelocity() const {
  return velocity / radius;
}

CircleTrajectory::CircleTrajectory(
    const std::shared_ptr<CircleTrajectory::Settings>& settings)
    : BaseTrajectory(settings),
      start_rad_(angles::from_degrees(
          std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
              ->start_heading_deg -
          90)),
      arc_length_rad_(angles::from_degrees(
          std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
              ->arc_length_deg)) {
  // Angle between vertices.
  // https://stackoverflow.com/questions/11774038/how-to-render-a-circle-with-as-few-vertices-as-possible
  const double kDeltaAngleMax = std::acos(
      2.0 * std::pow(
                (1.0 -
                 std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                     ->circle_deviation_ratio),
                2.0) -
      1.0);
  const double kNumVerticesMin = std::fabs(arc_length_rad_) / kDeltaAngleMax;
  num_vertices_ = std::ceil(kNumVerticesMin);
  num_vertices_ += 1;  // To close circle.
}

bool CircleTrajectory::toYaml(YAML::Node* node) {
  CHECK_NOTNULL(node);
  *node = YAML::Node();

  Eigen::Vector3d center_wgs84;
  if (!geotf_.convert(
          "enu",
          Eigen::Vector3d(
              std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                  ->center_east,
              std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                  ->center_north,
              std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
                  ->altitude),
          "wgs84", &center_wgs84))
    return false;

  (*node)["altitude"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)->altitude;
  (*node)["velocity"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)->velocity;
  (*node)["trajectory_type"] = static_cast<int>(TrajectoryType::Hotpoint);
  (*node)["center_lat"] = center_wgs84.x();
  (*node)["center_lon"] = center_wgs84.y();
  (*node)["radius"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)->radius;
  (*node)["start_heading_deg"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->start_heading_deg;
  (*node)["arc_length_deg"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->arc_length_deg;
  (*node)["offset_heading_deg"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->offset_heading_deg;
  (*node)["circle_deviation_ratio"] =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->circle_deviation_ratio;

  return true;
}

void CircleTrajectory::samplePositionVertices() {
  const Eigen::Vector3d kOffset(
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->center_east,
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->center_north,
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->altitude);
  const double kAngularVelocity =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->computeAngularVelocity();
  const double kDeltaAngle = computeDeltaAngle(kAngularVelocity);

  position_vertices_.resize(num_vertices_, kPositionDimension);
  for (int i = 0; i < num_vertices_; ++i) {
    const double kAngle = i * kDeltaAngle + start_rad_;

    Eigen::Vector3d position;
    position.x() =
        std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
            ->radius *
        std::cos(kAngle);
    position.y() =
        std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
            ->radius *
        std::sin(kAngle);
    position.z() = 0.0;
    if (kMaxDerivativePos >= mtg::derivative_order::POSITION) {
      position_vertices_[i].addConstraint(mtg::derivative_order::POSITION,
                                          position + kOffset);
    }

    Eigen::Vector3d velocity;
    velocity.x() =
        -std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
             ->radius *
        kAngularVelocity * std::sin(kAngle);
    velocity.y() =
        std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
            ->radius *
        kAngularVelocity * std::cos(kAngle);
    velocity.z() = 0.0;
    if (kMaxDerivativePos >= mtg::derivative_order::VELOCITY) {
      position_vertices_[i].addConstraint(mtg::derivative_order::VELOCITY,
                                          velocity);
    }

    if (kMaxDerivativePos >= mtg::derivative_order::ACCELERATION) {
      Eigen::Vector3d acceleration;
      acceleration.x() = -std::pow(kAngularVelocity, 2.0) * position.x();
      acceleration.y() = -std::pow(kAngularVelocity, 2.0) * position.y();
      acceleration.z() = 0.0;
      position_vertices_[i].addConstraint(mtg::derivative_order::ACCELERATION,
                                          acceleration);
    }

    if (kMaxDerivativePos >= mtg::derivative_order::JERK) {
      Eigen::Vector3d jerk;
      jerk.x() = -std::pow(kAngularVelocity, 3.0) * velocity.x();
      jerk.y() = -std::pow(kAngularVelocity, 3.0) * velocity.y();
      jerk.z() = 0.0;
      position_vertices_[i].addConstraint(mtg::derivative_order::JERK, jerk);
    }

    if (kMaxDerivativePos >= mtg::derivative_order::SNAP) {
      Eigen::Vector3d snap;
      snap.x() = std::pow(kAngularVelocity, 4.0) * position.x();
      snap.y() = std::pow(kAngularVelocity, 4.0) * position.y();
      snap.z() = 0.0;
      position_vertices_[i].addConstraint(mtg::derivative_order::SNAP, snap);
    }
  }
}

void CircleTrajectory::sampleYawVertices() {
  const double kOffsetHeading = angles::from_degrees(
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->offset_heading_deg);
  const double kAngularVelocity =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->computeAngularVelocity();
  const double kDeltaAngle = computeDeltaAngle(kAngularVelocity);

  yaw_vertices_.resize(num_vertices_, kYawDimension);
  for (int i = 0; i < num_vertices_; ++i) {
    typedef Eigen::Matrix<double, 1, 1> Vector1d;
    if (kMaxDerivativeYaw >= mtg::derivative_order::ORIENTATION) {
      Vector1d orientation;
      const double kAngle = i * kDeltaAngle + start_rad_;
      orientation[0] = kAngle + 0.5 * M_PI + kOffsetHeading;
      yaw_vertices_[i].addConstraint(mtg::derivative_order::ORIENTATION,
                                     orientation);
    }

    if (kMaxDerivativeYaw >= mtg::derivative_order::ANGULAR_VELOCITY) {
      Vector1d angular_velocity;
      angular_velocity[0] = kAngularVelocity;
      yaw_vertices_[i].addConstraint(mtg::derivative_order::ANGULAR_VELOCITY,
                                     angular_velocity);
    }

    if (kMaxDerivativeYaw >= mtg::derivative_order::ANGULAR_ACCELERATION) {
      Vector1d angular_acceleration;
      angular_acceleration[0] = 0.0;
      yaw_vertices_[i].addConstraint(
          mtg::derivative_order::ANGULAR_ACCELERATION, angular_acceleration);
    }
  }
}

double CircleTrajectory::computeDeltaAngle(double angular_velocity) const {
  return std::copysign(arc_length_rad_ / (num_vertices_ - 1), angular_velocity);
}

void CircleTrajectory::sampleTimes() {
  const double kAngularVelocity =
      std::static_pointer_cast<CircleTrajectory::Settings>(settings_)
          ->computeAngularVelocity();
  const double kDeltaAngle = computeDeltaAngle(kAngularVelocity);
  const double kSamplePeriod = std::fabs(kDeltaAngle / kAngularVelocity);

  segment_times_.resize(position_vertices_.size() - 1);
  for (double& t : segment_times_) t = kSamplePeriod;
}

}  // namespace fm_trajectories


/*

Copyright (c) 2005-2019, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef TYSONNOVAK2001ODESYSTEM_HPP_
#define TYSONNOVAK2001ODESYSTEM_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include <cmath>
#include <iostream>

#include "AbstractOdeSystemWithAnalyticJacobian.hpp"

/**
 * Represents the Tyson & Novak (2001) system of ODEs.
 * [doi:10.1006/jtbi.2001.2293]
 */
class TysonNovak2001OdeSystem : public AbstractOdeSystemWithAnalyticJacobian
{
private:

    /**
     * Parameters for the Tyson & Novak (2001) model.
     */

    /** Dimensional parameter k_1. */
    double mK1;
    /** Dimensional parameter k_2'. */
    double mK2d;
    /** Dimensional parameter k_2''. */
    double mK2dd;
    /** Dimensional parameter k_2'''. */
    double mK2ddd;
    /** Dimensionless parameter [CycB]_threshold. */
    double mCycB_threshold;
    /** Dimensional parameter k_3'. */
    double mK3d;
    /** Dimensional parameter k_3''. */
    double mK3dd;
    /** Dimensional parameter k_4'. */
    double mK4d;
    /** Dimensional parameter k_4. */
    double mK4;
    /** Dimensionless parameter J_3. */
    double mJ3;
    /** Dimensionless parameter J_4. */
    double mJ4;
    /** Dimensional parameter k_5'. */
    double mK5d;
    /** Dimensional parameter k_5''. */
    double mK5dd;
    /** Dimensional parameter k_6. */
    double mK6;
    /** Dimensionless parameter J_5. */
    double mJ5;
    /** Dimensionless parameter n. */
    unsigned mN;
    /** Dimensional parameter k_7. */
    double mK7;
    /** Dimensional parameter k_8. */
    double mK8;
    /** Dimensionless parameter J_7. */
    double mJ7;
    /** Dimensionless parameter J_8. */
    double mJ8;
    /** Dimensionless parameter [Mad]. */
    double mMad;
    /** Dimensional parameter k_9. */
    double mK9;
    /** Dimensional parameter k_10. */
    double mK10;
    /** Dimensional parameter k_11. */
    double mK11;
    /** Dimensional parameter k_12'. */
    double mK12d;
    /** Dimensional parameter k_12''. */
    double mK12dd;
    /** Dimensional parameter k_12'''. */
    double mK12ddd;
    /** Dimensionless parameter K_eq. */
    double mKeq;
    /** Dimensional parameter k_13. */
    double mK13;
    /** Dimensional parameter k_14. */
    double mK14;
    /** Dimensional parameter k_15'. */
    double mK15d;
    /** Dimensional parameter k_15''. */
    double mK15dd;
    /** Dimensional parameter k_16'. */
    double mK16d;
    /** Dimensional parameter k_16''. */
    double mK16dd;
    /** Dimensionless parameter J_15. */
    double mJ15;
    /** Dimensionless parameter J_16. */
    double mJ16;
    /** Dimensional parameter mu. */
    double mMu;
    /** Dimensionless parameter m_star. */
    double mMstar;

    friend class boost::serialization::access;
    /**
     * Serialize the object and its member variables.
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractOdeSystem>(*this);
    }

public:

    /**
     * Constructor.
     *
     * @param stateVariables optional initial conditions for state variables (only used in archiving)
     */
    TysonNovak2001OdeSystem(std::vector<double> stateVariables=std::vector<double>());

    /**
     * Destructor.
     */
    ~TysonNovak2001OdeSystem();

    /**
     * Initialise parameter values.
     */
    void Init();

    /**
     * Compute the RHS of the Alarcon et al. (2004) system of ODEs.
     *
     * Returns a vector representing the RHS of the ODEs at each time step, y' = [y1' ... yn'].
     * An ODE solver will call this function repeatedly to solve for y = [y1 ... yn].
     *
     * @param time used to evaluate the RHS.
     * @param rY value of the solution vector used to evaluate the RHS.
     * @param rDY filled in with the resulting derivatives (using Alarcons et al. (2004) system of equations).
     */
    void EvaluateYDerivatives(double time, const std::vector<double>& rY, std::vector<double>& rDY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     * (Used by Chaste solvers to find whether or not to stop solving)
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS.
     *
     * @return whether or not stopping conditions have been met
     */
    bool CalculateStoppingEvent(double time, const std::vector<double>& rY);

    /**
     * Calculate whether the conditions for the cell cycle to finish have been met.
     * (Used by CVODE solver to find exact stopping position)
     *
     * @param time at which to calculate whether the stopping event has occurred
     * @param rY value of the solution vector used to evaluate the RHS.
     *
     * @return How close we are to the root of the stopping condition
     */
    double CalculateRootFunction(double time, const std::vector<double>& rY);

    /**
     * Compute the Jacobian of the ODE system.
     *
     * @param rSolutionGuess initial guess for the solution vector.
     * @param jacobian the Jacobian of the ODE system.
     * @param time at which to calculate the Jacobian.
     * @param timeStep used to calculate the Jacobian.
     */
    virtual void AnalyticJacobian(const std::vector<double>& rSolutionGuess, double** jacobian, double time, double timeStep);
};

// Declare identifier for the serializer
#include "SerializationExportWrapper.hpp"
CHASTE_CLASS_EXPORT(TysonNovak2001OdeSystem)

namespace boost
{
namespace serialization
{
/**
 * Serialize information required to construct a TysonNovak2001OdeSystem.
 */
template<class Archive>
inline void save_construct_data(
    Archive & ar, const TysonNovak2001OdeSystem * t, const unsigned int file_version)
{
    // Save data required to construct instance
    const std::vector<double>& state_variables = t->rGetConstStateVariables();
    ar & state_variables;
}

/**
 * De-serialize constructor parameters and initialise a TysonNovak2001OdeSystem.
 */
template<class Archive>
inline void load_construct_data(
    Archive & ar, TysonNovak2001OdeSystem * t, const unsigned int file_version)
{
    // Retrieve data from archive required to construct new instance
    std::vector<double> state_variables;
    ar & state_variables;

    // Invoke inplace constructor to initialise instance
    ::new(t)TysonNovak2001OdeSystem(state_variables);
}
}
} // namespace ...

#endif /*TYSONNOVAK2001ODESYSTEM_HPP_*/


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GESDD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GESDD_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // singular value decomposition
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * (divide and conquer driver)
     * gesdd() computes the singular value decomposition (SVD) of
     * M-by-N matrix A, optionally computing the left and/or right
     * singular vectors, by using divide-and-conquer method.
     * The SVD is written
     *
     *     A = U * S * V^T    or    A = U * S * V^H
     *
     * where S is an M-by-N matrix which is zero except for its min(m,n)
     * diagonal elements, U is an M-by-M orthogonal/unitary matrix, and V
     * is an N-by-N orthogonal/unitary matrix. The diagonal elements of S
     * are the singular values of A; they are real and non-negative, and
     * are returnede in descending  order. The first min(m,n) columns of
     * U and V are the left and right singular vectors of A. (Note that
     * the routine returns V^T or V^H, not V.
     */

    namespace detail {

      inline
      void gesdd (char const jobz, int const m, int const n,
                  float* a, int const lda,
                  float* s, float* u, int const ldu,
                  float* vt, int const ldvt,
                  float* work, int const lwork, float* /* dummy */,
                  int* iwork, int* info)
      {
        LAPACK_SGESDD (&jobz, &m, &n, a, &lda, s,
                       u, &ldu, vt, &ldvt, work, &lwork, iwork, info);
      }

      inline
      void gesdd (char const jobz, int const m, int const n,
                  double* a, int const lda,
                  double* s, double* u, int const ldu,
                  double* vt, int const ldvt,
                  double* work, int const lwork, double* /* dummy */,
                  int* iwork, int* info)
      {
        LAPACK_DGESDD (&jobz, &m, &n, a, &lda, s,
                       u, &ldu, vt, &ldvt, work, &lwork, iwork, info);
      }

      inline
      void gesdd (char const jobz, int const m, int const n,
                  traits::complex_f* a, int const lda,
                  float* s, traits::complex_f* u, int const ldu,
                  traits::complex_f* vt, int const ldvt,
                  traits::complex_f* work, int const lwork,
                  float* rwork, int* iwork, int* info)
      {
        LAPACK_CGESDD (&jobz, &m, &n,
                       traits::complex_ptr (a), &lda, s,
                       traits::complex_ptr (u), &ldu,
                       traits::complex_ptr (vt), &ldvt,
                       traits::complex_ptr (work), &lwork,
                       rwork, iwork, info);
      }

      inline
      void gesdd (char const jobz, int const m, int const n,
                  traits::complex_d* a, int const lda,
                  double* s, traits::complex_d* u, int const ldu,
                  traits::complex_d* vt, int const ldvt,
                  traits::complex_d* work, int const lwork,
                  double* rwork, int* iwork, int* info)
      {
        LAPACK_ZGESDD (&jobz, &m, &n,
                       traits::complex_ptr (a), &lda, s,
                       traits::complex_ptr (u), &ldu,
                       traits::complex_ptr (vt), &ldvt,
                       traits::complex_ptr (work), &lwork,
                       rwork, iwork, info);
      }

      inline
      int gesdd_min_work (float, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m3 = 3 * minmn;
        int m4 = 4 * minmn;
        int minw;
        if (jobz == 'N') {
          // leading comments:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 6*min(M,N))
          // code:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 7*min(M,N))
          int m7 = 7 * minmn;
          minw = maxmn < m7 ? m7 : maxmn;
          minw += m3;
        }
        if (jobz == 'O') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 5*min(M,N)*min(M,N)+4*min(M,N))
          int m5 = 5 * minmn * minmn + m4;
          minw = maxmn < m5 ? m5 : maxmn;
          minw += m3 * minmn;
        }
        if (jobz == 'S' || jobz == 'A') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 4*min(M,N)*min(M,N)+4*min(M,N)).
          int m44 = m4 * minmn + m4;
          minw = maxmn < m44 ? m44 : maxmn;
          minw += m3 * minmn;
        }
        return minw;
      }
      inline
      int gesdd_min_work (double, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m3 = 3 * minmn;
        int m4 = 4 * minmn;
        int minw;
        if (jobz == 'N') {
          // leading comments:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 6*min(M,N))
          // code:
          //   LWORK >= 3*min(M,N) + max(max(M,N), 7*min(M,N))
          int m7 = 7 * minmn;
          minw = maxmn < m7 ? m7 : maxmn;
          minw += m3;
        }
        else if (jobz == 'O') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 5*min(M,N)*min(M,N)+4*min(M,N))
          int m5 = 5 * minmn * minmn + m4;
          minw = maxmn < m5 ? m5 : maxmn;
          minw += m3 * minmn;
        }
        else if (jobz == 'S' || jobz == 'A') {
          // LWORK >= 3*min(M,N)*min(M,N)
          //          + max(max(M,N), 4*min(M,N)*min(M,N)+4*min(M,N)).
          int m44 = m4 * minmn + m4;
          minw = maxmn < m44 ? m44 : maxmn;
          minw += m3 * minmn;
        }
        else {
          std::cerr << "Invalid option passed to gesdd" << std::endl ;
          return 0 ;
        }
        return minw;
      }
      inline
      int gesdd_min_work (traits::complex_f, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m2 = 2 * minmn;
        int minw = m2 + maxmn;
        if (jobz == 'N')
          // LWORK >= 2*min(M,N)+max(M,N)
          ;
        if (jobz == 'O')
          // LWORK >= 2*min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += m2 * minmn;
        if (jobz == 'S' || jobz == 'A')
          // LWORK >= min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += minmn * minmn;
        return minw;
      }
      inline
      int gesdd_min_work (traits::complex_d, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m2 = 2 * minmn;
        int minw = m2 + maxmn;
        if (jobz == 'N')
          // LWORK >= 2*min(M,N)+max(M,N)
          ;
        if (jobz == 'O')
          // LWORK >= 2*min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += m2 * minmn;
        if (jobz == 'S' || jobz == 'A')
          // LWORK >= min(M,N)*min(M,N) + 2*min(M,N) + max(M,N)
          minw += minmn * minmn;
        return minw;
      }

      inline
      int gesdd_rwork (float, char, int, int) { return 1; }
      inline
      int gesdd_rwork (double, char, int, int) { return 1; }
      inline
      int gesdd_rwork (traits::complex_f, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int minw;
        if (jobz == 'N')
          // LWORK >= 7*min(M,N)
          minw = 7 * minmn;
        else
          // LRWORK >= 5*min(M,N)*min(M,N) + 5*min(M,N)
          minw = 5 * (minmn * minmn + minmn);
        return minw;
      }
      inline
      int gesdd_rwork (traits::complex_d, char jobz, int m, int n) {
        int minmn = m < n ? m : n;
        int minw;
        if (jobz == 'N')
          // LWORK >= 7*min(M,N)
          minw = 7 * minmn;
        else
          // LRWORK >= 5*min(M,N)*min(M,N) + 5*min(M,N)
          minw = 5 * (minmn * minmn + minmn);
        return minw;
      }

      inline
      int gesdd_iwork (int m, int n) {
        int minmn = m < n ? m : n;
        return 8 * minmn;
      }

    } // detail

    template <typename MatrA>
    inline
    int gesdd_work (char const q, char const jobz, MatrA const& a)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

#ifdef BOOST_NUMERIC_BINDINGS_LAPACK_2
      assert (q == 'M');
#else
      assert (q == 'M' || q == 'O');
#endif
      assert (jobz == 'N' || jobz == 'O' || jobz == 'A' || jobz == 'S');

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int lw = -13;

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif

      if (q == 'M')
        lw = detail::gesdd_min_work (val_t(), jobz, m, n);

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
      MatrA& a2 = const_cast<MatrA&> (a);
      if (q == 'O') {
        // traits::detail::array<val_t> w (1);
        val_t w;
        int info;
        detail::gesdd (jobz, m, n,
                       traits::matrix_storage (a2),
                       traits::leading_dimension (a2),
                       0, // traits::vector_storage (s),
                       0, // traits::matrix_storage (u),
                       m, // traits::leading_dimension (u),
                       0, // traits::matrix_storage (vt),
                       n, // traits::leading_dimension (vt),
                       &w, // traits::vector_storage (w),
                       -1, // traits::vector_size (w),
                       0, // traits::vector_storage (rw),
                       0, // traits::vector_storage (iw),
                       &info);
        assert (info == 0);

        lw = traits::detail::to_int (w);
        // // lw = traits::detail::to_int (w[0]);
        /*
         * is there a bug in LAPACK? or in Mandrake's .rpm?
         * if m == 3, n == 4 and jobz == 'N' (real A),
         * gesdd() returns optimal size == 1 while minimum size == 27
         */
        // int lwo = traits::detail::to_int (w);
        // int lwmin = detail::gesdd_min_work (val_t(), jobz, m, n);
        // lw = lwo < lwmin ? lwmin : lwo;
      }
#endif

      return lw;
    }

    template <typename MatrA>
    inline
    int gesdd_rwork (char jobz, MatrA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      assert (jobz == 'N' || jobz == 'O' || jobz == 'A' || jobz == 'S');

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif

      return detail::gesdd_rwork (val_t(), jobz,
                                  traits::matrix_size1 (a),
                                  traits::matrix_size2 (a));
    }

    template <typename MatrA>
    inline
    int gesdd_iwork (MatrA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      return detail::gesdd_iwork (traits::matrix_size1 (a),
                                  traits::matrix_size2 (a));
    }

    template <typename MatrA, typename VecS,
              typename MatrU, typename MatrV, typename VecW, typename VecIW>
    inline
    int gesdd (char const jobz, MatrA& a,
               VecS& s, MatrU& u, MatrV& vt, VecW& w, VecIW& iw)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT(
        (boost::is_same<
          typename traits::matrix_traits<MatrA>::value_type, float
        >::value
        ||
        boost::is_same<
          typename traits::matrix_traits<MatrA>::value_type, double
        >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int const minmn = m < n ? m : n;

      assert (minmn == traits::vector_size (s));
      assert ((jobz == 'N')
              || ((jobz == 'O' || jobz == 'A') && m >= n)
              || ((jobz == 'O' || jobz == 'A')
                  && m < n
                  && m == traits::matrix_size2 (u))
              || (jobz == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobz == 'N' && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (u) >= m)
              || (jobz == 'A' && traits::leading_dimension (u) >= m)
              || (jobz == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobz == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (vt) >= n)
              || (jobz == 'A' && traits::leading_dimension (vt) >= n)
              || (jobz == 'S' && traits::leading_dimension (vt) >= minmn));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      assert (traits::vector_size (w)
              >= detail::gesdd_min_work (val_t(), jobz, m, n));
      assert (traits::vector_size (iw) >= detail::gesdd_iwork (m, n));

      int info;
      detail::gesdd (jobz, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     0, // dummy argument
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

    template <typename MatrA, typename VecS, typename MatrU,
              typename MatrV, typename VecW, typename VecRW, typename VecIW>
    inline
    int gesdd (char const jobz, MatrA& a,
               VecS& s, MatrU& u, MatrV& vt, VecW& w, VecRW& rw, VecIW& iw)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int const minmn = m < n ? m : n;

      assert (minmn == traits::vector_size (s));
      assert ((jobz == 'N')
              || ((jobz == 'O' || jobz == 'A') && m >= n)
              || ((jobz == 'O' || jobz == 'A')
                  && m < n
                  && m == traits::matrix_size2 (u))
              || (jobz == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobz == 'N' && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (u) >= m)
              || (jobz == 'A' && traits::leading_dimension (u) >= m)
              || (jobz == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobz == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (vt) >= n)
              || (jobz == 'A' && traits::leading_dimension (vt) >= n)
              || (jobz == 'S' && traits::leading_dimension (vt) >= minmn));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      assert (traits::vector_size (w)
              >= detail::gesdd_min_work (val_t(), jobz, m, n));
      assert (traits::vector_size (rw)
              >= detail::gesdd_rwork (val_t(), jobz, m, n));
      assert (traits::vector_size (iw) >= detail::gesdd_iwork (m, n));

      int info;
      detail::gesdd (jobz, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     traits::vector_storage (rw),
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesdd (char const opt, char const jobz,
               MatrA& a, VecS& s, MatrU& u, MatrV& vt)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
#ifndef NDEBUG
      int const minmn = m < n ? m : n;
#endif // NDEBUG

      assert (minmn == traits::vector_size (s));
      assert ((jobz == 'N')
              || ((jobz == 'O' || jobz == 'A') && m >= n)
              || ((jobz == 'O' || jobz == 'A')
                  && m < n
                  && m == traits::matrix_size2 (u))
              || (jobz == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobz == 'N' && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (u) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (u) >= m)
              || (jobz == 'A' && traits::leading_dimension (u) >= m)
              || (jobz == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobz == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m < n
                  && traits::leading_dimension (vt) >= 1)
              || (jobz == 'O'
                  && m >= n
                  && traits::leading_dimension (vt) >= n)
              || (jobz == 'A' && traits::leading_dimension (vt) >= n)
              || (jobz == 'S' && traits::leading_dimension (vt) >= minmn));

#ifdef BOOST_NUMERIC_BINDINGS_LAPACK_2
      assert (opt == 'M');
#else
      assert (opt == 'M' || opt == 'O');
#endif

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;

      int const lw = gesdd_work (opt, jobz, a);
      traits::detail::array<val_t> w (lw);
      if (!w.valid()) return -101;

      int const lrw = gesdd_rwork (jobz, a);
      traits::detail::array<real_t> rw (lrw);
      if (!rw.valid()) return -102;

      int const liw = gesdd_iwork (a);
      traits::detail::array<int> iw (liw);
      if (!iw.valid()) return -103;

      int info;
      detail::gesdd (jobz, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     lw, //traits::vector_size (w),
                     traits::vector_storage (rw),
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesdd (char const jobz, MatrA& a, VecS& s, MatrU& u, MatrV& vt) {
      return gesdd ('O', jobz, a, s, u, vt);
    }

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesdd (MatrA& a, VecS& s, MatrU& u, MatrV& vt) {
      return gesdd ('O', 'S', a, s, u, vt);
    }

    template <typename MatrA, typename VecS>
    inline
    int gesdd (MatrA& a, VecS& s) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int const minmn = m < n ? m : n;

      assert (minmn == traits::vector_size (s));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;

      int const lw = gesdd_work ('O', 'N', a);
      traits::detail::array<val_t> w (lw);
      if (!w.valid()) return -101;

      int const lrw = gesdd_rwork ('N', a);
      traits::detail::array<real_t> rw (lrw);
      if (!rw.valid()) return -102;

      int const liw = gesdd_iwork (a);
      traits::detail::array<int> iw (liw);
      if (!iw.valid()) return -103;

      int info;
      detail::gesdd ('N', m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     0, // traits::matrix_storage (u),
                     1, // traits::leading_dimension (u),
                     0, // traits::matrix_storage (vt),
                     1, // traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     traits::vector_storage (rw),
                     traits::vector_storage (iw),
                     &info);
      return info;
    }

#endif

  } // namespace lapack

}}}

#endif


//            Copyright Daniel Trebbien 2010.
// Distributed under the Boost Software License, Version 1.0.
//   (See accompanying file LICENSE_1_0.txt or the copy at
//         http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP
#define BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP 1

#include <boost/assert.hpp>
#include <set>
#include <vector>
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/maximum_adjacency_search.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/one_bit_color_map.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/utility/result_of.hpp>
#include <boost/graph/iteration_macros.hpp>

namespace boost {

  namespace detail {
    template < typename ParityMap, typename WeightMap, typename IndexMap >
    class mas_min_cut_visitor : public boost::default_mas_visitor {
      typedef one_bit_color_map <IndexMap> InternalParityMap;
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
    public:
      template < typename Graph >
      mas_min_cut_visitor(const Graph& g,
                          ParityMap parity,
                          weight_type& cutweight,
                          const WeightMap& weight_map,
                          IndexMap index_map)
        : m_bestParity(parity),
          m_parity(make_one_bit_color_map(num_vertices(g), index_map)),
          m_bestWeight(cutweight),
          m_cutweight(0),
          m_visited(0),
          m_weightMap(weight_map)
      {
        // set here since the init list sets the reference
        m_bestWeight = (std::numeric_limits<weight_type>::max)();
      }

      template < typename Vertex, typename Graph >
      void initialize_vertex(Vertex u, const Graph & g)
      {
        typedef typename boost::property_traits<ParityMap>::value_type parity_type;
        typedef typename boost::property_traits<InternalParityMap>::value_type internal_parity_type;

        put(m_parity, u, internal_parity_type(0));
        put(m_bestParity, u, parity_type(0));
      }

      template < typename Edge, typename Graph >
      void examine_edge(Edge e, const Graph & g)
      {
        weight_type w = get(m_weightMap, e);

        // if the target of e is already marked then decrease cutweight
        // otherwise, increase it
        if (get(m_parity, boost::target(e, g))) {
          m_cutweight -= w;
        } else {
          m_cutweight += w;
        }
      }

      template < typename Vertex, typename Graph >
      void finish_vertex(Vertex u, const Graph & g)
      {
        typedef typename boost::property_traits<ParityMap>::value_type parity_type;
        typedef typename boost::property_traits<InternalParityMap>::value_type internal_parity_type;

        ++m_visited;
        put(m_parity, u, internal_parity_type(1));

        if (m_cutweight < m_bestWeight && m_visited < num_vertices(g)) {
          m_bestWeight = m_cutweight;
          BGL_FORALL_VERTICES_T(i, g, Graph) {
            put(m_bestParity,i, get(m_parity,i));
          }
        }
      }

      inline void clear() {
        m_bestWeight = (std::numeric_limits<weight_type>::max)();
        m_visited = 0;
        m_cutweight = 0;
      }

    private:
      ParityMap m_bestParity;
      InternalParityMap m_parity;
      weight_type& m_bestWeight;
      weight_type m_cutweight;
      unsigned m_visited;
      const WeightMap& m_weightMap;
    };

    /**
     * \brief Computes a min-cut of the input graph
     *
     * Computes a min-cut of the input graph using the Stoer-Wagner algorithm.
     *
     * \pre \p g is a connected, undirected graph
     * \pre pq.empty()
     * \param[in] g the input graph
     * \param[in] weights a readable property map from each edge to its weight (a non-negative value)
     * \param[out] parities a writable property map from each vertex to a bool type object for
     *     distinguishing the two vertex sets of the min-cut
     * \param[out] assignments a read/write property map from each vertex to a \c vertex_descriptor object. This
     *     map serves as work space, and no particular meaning should be derived from property values
     *     after completion of the algorithm.
     * \param[out] pq a keyed, updatable max-priority queue
     * \returns the cut weight of the min-cut
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.114.6687&rep=rep1&type=pdf
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.31.614&rep=rep1&type=pdf
     *
     * \author Daniel Trebbien
     * \date 2010-09-11
     */
    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue, class IndexMap>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq, IndexMap index_map) {
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::graph_traits<UndirectedGraph>::vertices_size_type vertices_size_type;
      typedef typename boost::graph_traits<UndirectedGraph>::edge_descriptor edge_descriptor;
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
      typedef typename boost::property_traits<ParityMap>::value_type parity_type;

      typename graph_traits<UndirectedGraph>::vertex_iterator u_iter, u_end;

      weight_type bestW = (std::numeric_limits<weight_type>::max)();
      weight_type bestThisTime = (std::numeric_limits<weight_type>::max)();
      vertex_descriptor bestStart = boost::graph_traits<UndirectedGraph>::null_vertex();

      detail::mas_min_cut_visitor<ParityMap, WeightMap, IndexMap>
        vis(g, parities, bestThisTime, weights, index_map);

      // for each node in the graph,
      for (boost::tie(u_iter, u_end) = vertices(g); u_iter != u_end; ++u_iter) {
        // run the MAS and find the min cut
        vis.clear();
        boost::maximum_adjacency_search(g,
            boost::weight_map(weights).
            visitor(vis).
            root_vertex(*u_iter).
            vertex_assignment_map(assignments).
            max_priority_queue(pq));
        if (bestThisTime < bestW) {
          bestW = bestThisTime;
          bestStart = *u_iter;
        }
      }

      // Run one more time, starting from the best start location, to
      // ensure the visitor has the best values.
      vis.clear();
      boost::maximum_adjacency_search(g,
        boost::vertex_assignment_map(assignments).
        weight_map(weights).
        visitor(vis).
        root_vertex(bestStart).
        max_priority_queue(pq));

      return bestW;
    }
  } // end `namespace detail` within `namespace boost`

    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue, class IndexMap>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq, IndexMap index_map) {
      BOOST_CONCEPT_ASSERT((boost::IncidenceGraphConcept<UndirectedGraph>));
      BOOST_CONCEPT_ASSERT((boost::VertexListGraphConcept<UndirectedGraph>));
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::graph_traits<UndirectedGraph>::vertices_size_type vertices_size_type;
      typedef typename boost::graph_traits<UndirectedGraph>::edge_descriptor edge_descriptor;
      BOOST_CONCEPT_ASSERT((boost::Convertible<typename boost::graph_traits<UndirectedGraph>::directed_category, boost::undirected_tag>));
      BOOST_CONCEPT_ASSERT((boost::ReadablePropertyMapConcept<WeightMap, edge_descriptor>));
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
      BOOST_CONCEPT_ASSERT((boost::WritablePropertyMapConcept<ParityMap, vertex_descriptor>));
      typedef typename boost::property_traits<ParityMap>::value_type parity_type;
      BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<VertexAssignmentMap, vertex_descriptor>));
      BOOST_CONCEPT_ASSERT((boost::Convertible<vertex_descriptor, typename boost::property_traits<VertexAssignmentMap>::value_type>));
      BOOST_CONCEPT_ASSERT((boost::KeyedUpdatableQueueConcept<KeyedUpdatablePriorityQueue>));

      vertices_size_type n = num_vertices(g);
      if (n < 2)
        throw boost::bad_graph("the input graph must have at least two vertices.");
      else if (!pq.empty())
        throw std::invalid_argument("the max-priority queue must be empty initially.");

      return detail::stoer_wagner_min_cut(g, weights,
                                          parities, assignments, pq, index_map);
    }

namespace graph {
  namespace detail {
    template <class UndirectedGraph, class WeightMap>
    struct stoer_wagner_min_cut_impl {
      typedef typename boost::property_traits<WeightMap>::value_type result_type;
      template <typename ArgPack>
      result_type operator() (const UndirectedGraph& g, WeightMap weights, const ArgPack& arg_pack) const {
        using namespace boost::graph::keywords;
        typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
        typedef typename boost::property_traits<WeightMap>::value_type weight_type;

        typedef boost::detail::make_priority_queue_from_arg_pack_gen<boost::graph::keywords::tag::max_priority_queue, weight_type, vertex_descriptor, std::greater<weight_type> > gen_type;

        gen_type gen(choose_param(get_param(arg_pack, boost::distance_zero_t()), weight_type(0)));

        typename boost::result_of<gen_type(const UndirectedGraph&, const ArgPack&)>::type pq = gen(g, arg_pack);

        return boost::stoer_wagner_min_cut(g,
          weights,
          arg_pack [_parity_map | boost::dummy_property_map()],
          boost::detail::make_property_map_from_arg_pack_gen<tag::vertex_assignment_map, vertex_descriptor>(vertex_descriptor())(g, arg_pack),
          pq,
          boost::detail::override_const_property(arg_pack, _vertex_index_map, g, vertex_index)
        );
      }
    };
  }
  BOOST_GRAPH_MAKE_FORWARDING_FUNCTION(stoer_wagner_min_cut,2,4)
}

  // Named parameter interface
  BOOST_GRAPH_MAKE_OLD_STYLE_PARAMETER_FUNCTION(stoer_wagner_min_cut, 2)
namespace graph {
    // version without IndexMap kept for backwards compatibility
    // (but requires vertex_index_t to be defined in the graph)
    // Place after the macro to avoid compilation errors
    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq) {

      return stoer_wagner_min_cut(g, weights,
                                  parities, assignments, pq,
                                  get(vertex_index, g));
    }
} // end `namespace graph`
} // end `namespace boost`

#include <boost/graph/iteration_macros_undef.hpp>

#endif // !BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP


/**
 * August 8th, 2018
 * source: https://zhuanlan.zhihu.com/p/38745950
 * Constant velocity prediction Kalman filter for shield global position
 */

#include <iostream>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <queue>
#include "rm_cv/ArmorRecord.h"

using namespace std;
using namespace Eigen;

string attitude_topic, publisher_topic, debug_topic, real_visual_topic, transform_topic;
string debug_angle_topic;
ros::Publisher filter_pub, debug_pub, transform_pub, debug_angle_pub;
MatrixXd gimbal_R_camera = MatrixXd::Identity(3, 3); // rotation matrix from camera to imu
MatrixXd init_R_gimbal= MatrixXd::Identity(3, 3);
Vector3d gimbal_T_camera = MatrixXd::Zero(3, 1);

VectorXd x(6);      // state
MatrixXd P = MatrixXd::Identity(6, 6); // covariance
MatrixXd Q = MatrixXd::Identity(6, 6); // prediction noise covariance
MatrixXd R = MatrixXd::Identity(3, 3); // observation noise covariance
MatrixXd A = MatrixXd::Identity(6, 6); // state transfer function

bool visual_initialized = false, visual_valid = false;

ros::Time t_prev;
double R_pos, Q_pos, Q_vel, P_weight;
const double CV_UPDATE_TIME_MAX = 0.2; // maxium allowed update time
const double DELAY_MAX = 0.05;
const int ROS_FREQ = 100;
const int MAX_VISUAL_QUEUE_SIZE = 5;
const int MAX_IMU_QUEUE_SIZE = 1000;
double OUTLIER_THRESHOLD = 10000.0;

Vector3d init_T_shield_prev = MatrixXd::Zero(3, 1);
Vector3d imu_T_shield_prev = MatrixXd::Zero(3, 1);
bool pos_is_outlier = false;
double chi_square = 0;
double outlier_l2_norm_ratio = 1.5;
double yaw_delay = 0.0;
double pitch_delay = 0.0;
Vector3d OUTPUT_BOUND = MatrixXd::Zero(3, 1);

// synchronization
queue<geometry_msgs::QuaternionStamped::ConstPtr> imu_queue;
queue<Vector3d> visual_queue;
int imu_back_time = 10;

double angle_diff = 0;
bool angle_diff_not_inited = true;

static void pub_result(const ros::Time &stamp, double delay_dt)
{
//    geometry_msgs::TwistStamped odom;
    rm_cv::ArmorRecord odom;
    odom.header.stamp = stamp;
    double predict_x = x(0) + (delay_dt + yaw_delay)   * x(3);
    double predict_y = x(1) + (delay_dt + pitch_delay) * x(4);
    double predict_z = x(2) + delay_dt * x(5);
    odom.armorPose.linear.x  = (predict_x < OUTPUT_BOUND[0]) ? predict_x * 1000 : OUTPUT_BOUND[0] * 1000;
    odom.armorPose.linear.y  = (predict_y < OUTPUT_BOUND[1]) ? predict_y * 1000 : OUTPUT_BOUND[1] * 1000;
    odom.armorPose.linear.z  = (predict_z < OUTPUT_BOUND[2]) ? predict_z * 1000 : OUTPUT_BOUND[2] * 1000;
    odom.armorPose.angular.x = x(3) * 1000;
    odom.armorPose.angular.y = x(4) * 1000;
    odom.armorPose.angular.z = x(5) * 1000;
    filter_pub.publish(odom);
}

static void pub_preprocessed(const std_msgs::Header &header,
                      const Ref<const Vector3d> p,
                      const Ref<const Vector3d> v,
                      ros::Publisher &publisher)
{
    geometry_msgs::TwistStamped debug;
    debug.header = header;
    debug.twist.linear.x  = p[0];
    debug.twist.linear.y  = p[1];
    debug.twist.linear.z  = p[2];
    debug.twist.angular.x = v[0];
    debug.twist.angular.y = v[1];
    debug.twist.angular.z = v[2];
    publisher.publish(debug);
}

static void pub_angle_debug(const std_msgs::Header &header,
                            const double yaw,
                            const Ref<const Vector3d> p,
                            const double imuz)
{
    geometry_msgs::TwistStamped debug_angle;
    debug_angle.header = header;
    debug_angle.twist.angular.x = yaw;
    debug_angle.twist.angular.y = -atan2(p[1], p[0]);
    debug_angle.twist.angular.z = imuz;
    debug_angle_pub.publish(debug_angle);
}

static double imu_to_yaw_angle(const geometry_msgs::QuaternionStamped::ConstPtr &imu)
{
    // Quaterniond q = Quaterniond( imu->quaternion.w, imu->quaternion.x, imu->quaternion.y, imu->quaternion.z );
    double qw = imu->quaternion.w;
    double qx = imu->quaternion.x;
    double qy = imu->quaternion.y;
    double qz = imu->quaternion.z;

    // convert to ZYX Euler angle yaw angle
    // double euler_angle_0 = atan2(2.0 * (qw * qx + qy * qz), 1.0 - 2.0 * (qx * qx + qy * qy) );
    // double euler_angle_1 = asin( 2.0 * (qw * qy - qz * qx) );
    return atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz) );
}

// Chi-square test for outlier rejection
static bool translation_is_outlier(const Vector3d &pos)
{
    VectorXd r = MatrixXd::Zero(3, 1);
    VectorXd z = MatrixXd::Zero(3, 1);
    MatrixXd S = MatrixXd::Zero(3, 3);
    z << pos[0], pos[1], pos[2];

    double pos_norm   = z.segment<3>(0).norm();
    double state_norm = x.segment<3>(0).norm();
    // ROS_INFO("pos_norm %f, state_norm %f", pos_norm, state_norm);
    if (pos_norm > state_norm * outlier_l2_norm_ratio) {
        return true;
    } else if (pos_norm < state_norm / outlier_l2_norm_ratio) {
        return true;
    }

    // r = z - H * x, residual
    // S = H P H' + R, residual covariance
    MatrixXd H = MatrixXd::Identity(3, 6); // observation matrix

//    MatrixXd K_next = (H * P * H.transpose() + R).ldlt().solve(P * H.transpose());
//    MatrixXd x_next = x + K_next * (z - H * x);
//    MatrixXd P_next = P - K_next * H * P;

    r = z - H * x;
    S = H * P * H.transpose() + R;
    chi_square = r.transpose() * S * r;
    // cout << "chi_square " << endl << chi_square << endl;

    return (chi_square > OUTLIER_THRESHOLD);
}

static void preprocess_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist,
                              Vector3d &pos, double dt_update)
{
    Vector3d camera_T_shield, gimbal_T_shield, init_T_shield;
    Vector3d camera_vel_shield, gimbal_vel_shield, init_vel_shield;
    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;
    gimbal_T_shield = gimbal_R_camera * camera_T_shield + gimbal_T_camera;
    gimbal_T_shield *= 0.001; // Convert millimeter to meter

    Matrix3d world_R_gimbal;// = MatrixXd::Identity(3, 3);
    world_R_gimbal << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    double yaw = 0;
    double imuz= 0;

/*    int pop_time = 0;
    if (!imu_queue.empty()) {
        pop_time = imu_queue.size() - imu_back_time;

        if (pop_time > 0) {
            for (int i = 0; i < pop_time; ++i) {
                imu_queue.pop();
            }
            imuz = imu_queue.front()->quaternion.z;
            yaw = imu_to_yaw_angle(imu_queue.front());
            world_R_gimbal << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1;

            if (angle_diff_not_inited) {
                angle_diff =  yaw + atan2(gimbal_T_shield[1], gimbal_T_shield[0]);
                angle_diff_not_inited = false;
                ROS_INFO("angle_diff is, %f", angle_diff);
            }
        }
    }*/
    init_T_shield = world_R_gimbal * gimbal_T_shield;

    // calculate and check the velocity
    init_vel_shield = (init_T_shield - init_T_shield_prev) / dt_update;
    init_T_shield_prev = init_T_shield;

    pos_is_outlier = translation_is_outlier(init_T_shield);

    if (pos_is_outlier) {
        pos = x.segment<3>(0);
        x.segment<3>(3) *= 0.5;
        ROS_INFO("outlier rejected");
    }
    else {
        pos = init_T_shield;
    }

    // store the state
    pub_preprocessed(header, gimbal_T_shield, gimbal_vel_shield, debug_pub);

    pub_preprocessed(header, init_T_shield_prev, init_vel_shield, transform_pub);

    pub_angle_debug(header, yaw - angle_diff, gimbal_T_shield, imuz);
}

/**
 * Core Kalman Filter math, propagate and update
 */
static void propagate(const double &dt) {
    A.topRightCorner(3, 3) = dt * MatrixXd::Identity(3, 3);
    x = A * x;
    P = A * P * A.transpose() + Q;
}

static void update(const Vector3d &pos)
{
    MatrixXd H, K, z;

    H = MatrixXd::Identity(3, 6); // observation matrix
    K = P * H.transpose() * (H * P * H.transpose() + R).inverse();
    z = MatrixXd::Zero(3, 1);
    z << pos[0], pos[1], pos[2];

    x = x + K * (z - H * x);
    P = P - K * H * P;
}

/**
 * initialization of the state and convariance from visual
 * @param pnp
 */
static void initialize_visual(const std_msgs::Header &header,
                              const geometry_msgs::Twist &twist)
{
    ros::Time t_update = header.stamp;
    ROS_INFO("visual init at %f", t_update.toSec());

    Vector3d camera_T_shield, gimbal_T_shield, init_T_shield;
    camera_T_shield[0] = twist.linear.x;
    camera_T_shield[1] = twist.linear.y;
    camera_T_shield[2] = twist.linear.z;

    gimbal_T_shield = gimbal_R_camera * camera_T_shield + gimbal_T_camera;
    gimbal_T_shield *= 0.001; // Convert millimeter to meter

    Matrix3d world_R_gimbal;// = MatrixXd::Identity(3, 3);
    world_R_gimbal << 1, 0, 0, 0, 1, 0, 0, 0, 1;
    double yaw = 0;

/*    int pop_time = 0;
    if (!imu_queue.empty()) {
        pop_time = imu_queue.size() - imu_back_time;

        if (pop_time > 0) {
            for (int i = 0; i < pop_time; ++i) {
                imu_queue.pop();
            }
            yaw = imu_to_yaw_angle(imu_queue.front());
            // double euler_angle_2 = imu_to_yaw_angle(imu_queue.back());
            world_R_gimbal << cos(yaw), -sin(yaw), 0, sin(yaw), cos(yaw), 0, 0, 0, 1;
            double DEBUG_imu_delay_time = header.stamp.toSec() - imu_queue.front()->header.stamp.toSec();
            cout << "imu to visual time difference " << DEBUG_imu_delay_time << endl;
        }
        cout << "final poped times " << pop_time << endl;
    }*/

    cout << "world_R_gimbal " << endl << world_R_gimbal << endl;

    init_T_shield = world_R_gimbal * gimbal_T_shield;

    // TODO: using RANSAC instead of averaging filter
    visual_queue.push(init_T_shield);
    if (visual_queue.size() >= MAX_VISUAL_QUEUE_SIZE) {
        Vector3d T_sum;
        T_sum.setZero();

        for (int i = 0; i < MAX_VISUAL_QUEUE_SIZE; ++i) {
            T_sum += visual_queue.front();
            visual_queue.pop();
        }

        x.segment<3>(0) = T_sum / MAX_VISUAL_QUEUE_SIZE; // init velocity with zero
        x.segment<3>(3).setZero(); // init velocity with zero

        P.setZero();
        P.topLeftCorner(3, 3)     = P_weight * MatrixXd::Identity(3, 3);
        P.bottomRightCorner(3, 3) = 2 * P_weight * MatrixXd::Identity(3, 3);

        cout << "DEBUG: x initialized with " << endl << x.transpose() << endl;
        cout << "P " << endl << P << endl;

        init_T_shield_prev = init_T_shield;
        visual_initialized = true;
    }
    else {
        cout << "DEBUG: current x reading: " << init_T_shield.transpose() << endl;
    }
}

/**
 * handle and save attitude of the gimbal
 * @param imu
 */
void attitude_cb(const geometry_msgs::QuaternionStamped::ConstPtr &imu)
{
    // synchronize the timestamp
    imu_queue.push(imu);
    if (imu_queue.size() > MAX_IMU_QUEUE_SIZE) imu_queue.pop();
}

/**
 * handle, save, and process visual messages
 * @param armor
 */
void real_visual_cb(const rm_cv::ArmorRecord::ConstPtr &armor)
{
    visual_valid = !(armor->armorPose.linear.x == 0 &&
                     armor->armorPose.linear.y == 0 &&
                     armor->armorPose.linear.z == 0 );

    if (visual_valid) {
        ros::Time t_update = armor->header.stamp;
        double dt_update = (t_update - t_prev).toSec();

        if (!visual_initialized) {
            initialize_visual(armor->header, armor->armorPose);
        }
        else {
            if (dt_update < CV_UPDATE_TIME_MAX) {
                Vector3d pos;

                preprocess_visual(armor->header, armor->armorPose, pos, dt_update);

                update(pos);

                propagate(dt_update);

                double running_time = ros::Time::now().toSec();
                double delay_dt = running_time - armor->header.stamp.toSec();
                delay_dt = (delay_dt < DELAY_MAX) ? delay_dt : DELAY_MAX;

                pub_result(armor->header.stamp, delay_dt);
            }
            else {
                visual_queue.push(x.segment<3>(0));

                visual_initialized = false;

                initialize_visual(armor->header, armor->armorPose);
            }
        }
        t_prev = t_update;
    }

}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "prediction_kalman_filter");
    ros::NodeHandle n("~");

    n.param("attitude_topic", attitude_topic, string("/can_receive_node/attitude"));
    n.param("real_visual_topic", real_visual_topic, string("/detected_armor"));
    n.param("publisher_topic", publisher_topic, string("/prediction_kf_global/predict"));
    n.param("debug_topic", debug_topic, string("/prediction_kf_global/preprocessed"));
    n.param("transform_topic", transform_topic, string("/prediction_kf_global/transformed"));
    n.param("debug_angle_topic", debug_angle_topic, string("/prediction_kf/debug_angle"));
    n.param("chi_square_threshold", OUTLIER_THRESHOLD, 10000.0);
    n.param("outlier_l2_norm_ratio", outlier_l2_norm_ratio, 1.5);
    n.param("imu_back_time", imu_back_time, 10);
    n.param("R_pos", R_pos, 16.0);
    n.param("Q_pos", Q_pos, 0.2);
    n.param("Q_vel", Q_vel, 1.0);
    n.param("P_matrix_weight", P_weight, 1.0);
    n.param("yaw_delay", yaw_delay, 0.0);
    n.param("pitch_delay", pitch_delay, 0.0);

    // For chassis reading only
    gimbal_R_camera <<  0, 0, 1,
                    -1, 0, 0,
                     0,-1, 0;
    gimbal_T_camera <<  30, 0, -120; // in millimeter

    OUTPUT_BOUND << 10.0, 10.0, 20.0;

    x.setZero();
    R = R_pos * MatrixXd::Identity(3, 3);
    Q.topLeftCorner(3, 3)     = Q_pos * MatrixXd::Identity(3, 3);
    Q.bottomRightCorner(3, 3) = Q_vel * MatrixXd::Identity(3, 3);
    cout << "R " << endl << R << endl;
    cout << "Q " << endl << Q << endl;

    ros::Subscriber s1 = n.subscribe(attitude_topic, 100, attitude_cb);
    ros::Subscriber s2 = n.subscribe(real_visual_topic, 40, real_visual_cb);

    filter_pub = n.advertise<rm_cv::ArmorRecord>(publisher_topic, 40);
    debug_pub  = n.advertise<geometry_msgs::TwistStamped>(debug_topic, 40);
    transform_pub = n.advertise<geometry_msgs::TwistStamped>(transform_topic, 40);
    debug_angle_pub = n.advertise<geometry_msgs::TwistStamped>(debug_angle_topic, 100);
    ros::Rate r(ROS_FREQ);

    while (ros::ok()) {
        if (visual_initialized) {

        }

        r.sleep();
        ros::spinOnce();
    }
}